Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Overhaul VTX_CONFIG processing. #222

Merged
merged 4 commits into from
Nov 4, 2024
Merged

Overhaul VTX_CONFIG processing. #222

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0d6a901
Overhaul VTX_CONFIG processing.
geoffsim Sep 19, 2024
493aed3
Ensure the correct power limits are sent to the FC.
geoffsim Sep 20, 2024
9b8599e
Send band/channel/power max values to FC.
geoffsim Sep 26, 2024
eeb6b58
Merge branch 'hd-zero:main' into VTX-Config-Processing
geoffsim Sep 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
192 changes: 82 additions & 110 deletions src/msp_displayport.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,8 @@ uint16_t msp_rcv_tick_8hz = 0;
uint8_t msp_rbuf[64];

uint8_t mspVtxLock = 0;
uint8_t init_table_supported = 0;
uint8_t init_table_done = 0;
uint8_t init_table_unsupported = 0;

#ifdef USE_TP9950
uint16_t cam_menu_timeout_sec = 0;
Expand Down Expand Up @@ -305,7 +305,7 @@ uint8_t msp_read_one_frame() {
else if (cur_cmd == CUR_FC_VARIANT)
parse_variant();
else if (cur_cmd == CUR_VTX_CONFIG)
parse_vtx_config();
parse_vtx_config(state);
else if (cur_cmd == CUR_GET_OSD_CANVAS)
parse_get_osd_canvas();
else if (cur_cmd == CUR_DISPLAYPORT)
Expand All @@ -315,11 +315,11 @@ uint8_t msp_read_one_frame() {
fc_lock |= FC_INIT_VTX_TABLE_LOCK;
if (msp_cmp_fc_variant("BTFL") || msp_cmp_fc_variant("QUIC")) {
#ifdef INIT_VTX_TABLE
init_table_supported = 1;
InitVtxTable();
#endif
msp_set_osd_canvas();
} else if (msp_cmp_fc_variant("INAV")) {
init_table_unsupported = 1;
msp_set_inav_osd_canvas();
}
}
Expand Down Expand Up @@ -402,8 +402,9 @@ uint8_t msp_read_one_frame() {
case MSP_VTX_GET_HW_FAULTS:
msp_send_vtx_hw_faults();
break;
case MSP_GET_VTX_CONFIG:
parseMspVtx_V2(state);
default:
parseMspVtx_V2(cmd_u16);
break;
}
msp_lst_rcv_sec = seconds;
Expand Down Expand Up @@ -934,16 +935,15 @@ void msp_set_vtx_config(uint8_t power, uint8_t save) {
crc ^= 0x08; // channel count
#if defined HDZERO_FREESTYLE_V1 || HDZERO_FREESTYLE_V2
if (powerLock) {
msp_tx(3);
crc ^= (3); // power count
} else {
msp_tx(5);
crc ^= (5); // power count
msp_tx((POWER_MAX & 0x1) + 1);
crc ^= ((POWER_MAX & 0x1) + 1); // power locked to 25/200mW
}
#else
msp_tx(POWER_MAX + 2);
crc ^= (POWER_MAX + 2); // power count
else
#endif
{
msp_tx(POWER_MAX + 1);
crc ^= (POWER_MAX + 1); // power count
}
msp_tx(0x00);
crc ^= 0x00; // disable/clear vtx table
msp_tx(crc);
Expand Down Expand Up @@ -998,49 +998,7 @@ uint8_t msp_vtx_set_channel(uint8_t const channel) {
DM6300_SetChannel(channel);
return 1;
}

void parse_vtx_config() {
uint8_t nxt_ch;
// uint8_t const power = msp_rx_buf[3];
// uint8_t const pitmode = msp_rx_buf[4];

/*
nxt_pwr:
0: 25mW
1:200mW
.....
POWER_MAX+1: 0mw
*/
// uint8_t nxt_pwr = 0;
// uint8_t pit_update = 0;
// uint8_t needSaveEEP = 0;

fc_lock |= FC_VTX_CONFIG_LOCK;

if (!msp_cmp_fc_variant("BTFL") && !msp_cmp_fc_variant("EMUF") && !msp_cmp_fc_variant("QUIC") && !msp_cmp_fc_variant("INAV")) {
return;
}
if (!msp_rx_buf[0]) {
// Invalid VTX type
return;
}

fc_pwr_rx = msp_rx_buf[3] - 1;
if (fc_pwr_rx > (POWER_MAX + 2))
fc_pwr_rx = 0;

// Check configured band
nxt_ch = bfChannel_to_channel(((msp_rx_buf[1] - 1) << 3) + (msp_rx_buf[2] - 1));
if (nxt_ch == INVALID_CHANNEL) {
// Validate frequency
uint16_t const fc_frequency = ((uint16_t)msp_rx_buf[6] << 8) + msp_rx_buf[5];
nxt_ch = DM6300_GetChannelByFreq(fc_frequency);
}
msp_vtx_set_channel(nxt_ch);
// PIT_MODE ?
// RF_POWER ?
}


void msp_set_osd_canvas(void) {
uint8_t crc = 0;
if (msp_cmp_fc_variant("BTFL")) {
Expand Down Expand Up @@ -1074,47 +1032,48 @@ void parse_get_osd_canvas(void) {
}
}

void parseMspVtx_V2(uint16_t const cmd_u16) {
void parse_vtx_settings(uint8_t ident) {
uint8_t nxt_ch = INVALID_CHANNEL;
uint8_t nxt_pwr = 0;
uint8_t nxt_pwr;
uint8_t nxt_pit;
uint8_t needSaveEEP = 0;
static uint8_t last_pwr = 255;
static uint8_t last_lp = 255;
static uint8_t last_pit = 255;

if (tramp_lock)
if (SA_lock || tramp_lock || init_table_supported) {
return;
}

if (cmd_u16 != MSP_GET_VTX_CONFIG)
return;
if (ident == MSP_CRC1) { // V1 (initial) config

if (!(fc_lock & FC_VTX_CONFIG_LOCK))
return;
fc_pwr_rx = msp_rx_buf[3];
fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = msp_rx_buf[6];

fc_pwr_rx = msp_rx_buf[3];
fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = msp_rx_buf[8];
} else { // MSP_CRC2 (V2 config)

pwr_lmt_done = 1;
mspVtxLock |= 1;
fc_pwr_rx = msp_rx_buf[3];
fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = msp_rx_buf[8];
}

if (SA_lock || (init_table_done == 0 && !init_table_unsupported)) {
return;
if (fc_pwr_rx == 0) {
fc_pwr_rx = POWER_MAX+2;
}

// update LP_MODE
if (fc_lp_rx != last_lp) {
last_lp = fc_lp_rx;
if (fc_lp_rx < 2) {
mspVtxLock = 1;
pwr_lmt_done = 1;

// Update LP_MODE
if (fc_lp_rx != LP_MODE) {
if (fc_lp_rx < 3) {
LP_MODE = fc_lp_rx;
}
needSaveEEP = 1;
}
// update channel
// Update channel
nxt_ch = bfChannel_to_channel(((msp_rx_buf[1] - 1) << 3) + (msp_rx_buf[2] - 1));
if (nxt_ch == INVALID_CHANNEL) {
// Note: BF sends band index 0 when frequency setting is used.
// Won't harm to validate frequency is all cases.
// BF sends band index 0 when frequency setting is used.
// Won't harm to validate frequency in all cases.
uint16_t const fc_frequency = ((uint16_t)msp_rx_buf[6] << 8) + msp_rx_buf[5];
nxt_ch = DM6300_GetChannelByFreq(fc_frequency);
if (nxt_ch == INVALID_CHANNEL) {
Expand All @@ -1124,12 +1083,13 @@ void parseMspVtx_V2(uint16_t const cmd_u16) {
}
needSaveEEP |= msp_vtx_set_channel(nxt_ch);

// update pit
// Update PIT_MODE
nxt_pwr = fc_pwr_rx - 1;
nxt_pit = fc_pit_rx & 1;

if ((boot_0mw_done == 0) && TEAM_RACE) {
msp_set_vtx_config(POWER_MAX + 1, 0);
// sometimes FC delay to receive 0mW, so check fc reply power and resend 0mW.
// Sometimes FC is delayed in receive 0mW, so check fc reply power and resend 0mW.
if (nxt_pwr == POWER_MAX + 1) {
dm6300_init_done = 0;
cur_pwr = POWER_MAX + 2;
Expand All @@ -1140,27 +1100,27 @@ void parseMspVtx_V2(uint16_t const cmd_u16) {
return;
}

if ((nxt_pwr != POWER_MAX + 1) && (!dm6300_init_done)) {
if ((nxt_pwr != (POWER_MAX + 1)) && !dm6300_init_done) {
Init_6300RF(RF_FREQ, RF_POWER);
DM6300_AUXADC_Calib();
}

PIT_MODE = fc_pit_rx & 1;
if (fc_pit_rx != last_pit) {
if (nxt_pit != PIT_MODE) {
PIT_MODE = nxt_pit;
if (PIT_MODE) {
DM6300_SetPower(POWER_MAX + 1, RF_FREQ, pwr_offset);
cur_pwr = POWER_MAX + 1;
vtx_pit_save = PIT_MODE;
} else {
#ifndef VIDEO_PAT
#if defined HDZERO_FREESTYLE_V1 || HDZERO_FREESTYLE_V2
if ((RF_POWER == 3) && (!g_IS_ARMED))
if ((RF_POWER == 3) && !g_IS_ARMED)
pwr_lmt_done = 0;
else
#endif
#endif
if (nxt_pwr == POWER_MAX + 1) {
WriteReg(0, 0x8F, 0x10);
if (nxt_pwr == (POWER_MAX + 1)) {
WriteReg(0, 0x8F, 0x10); // 5680 reset low (image tx off).
dm6300_init_done = 0;
cur_pwr = POWER_MAX + 2;
vtx_pit_save = PIT_0MW;
Expand All @@ -1172,15 +1132,14 @@ void parseMspVtx_V2(uint16_t const cmd_u16) {
vtx_pit_save = PIT_MODE;
}
}
last_pit = fc_pit_rx;
needSaveEEP = 1;
}

// update power
if (last_pwr != nxt_pwr) {
if (last_pwr == POWER_MAX + 1) {
// Update power
if (cur_pwr != nxt_pwr) {
if (cur_pwr == (POWER_MAX + 1)) { // Exit 0mW
// Exit 0mW
if (cur_pwr == POWER_MAX + 2) {
if (cur_pwr == (POWER_MAX + 2)) {
if (PIT_MODE)
Init_6300RF(RF_FREQ, POWER_MAX + 1);
else
Expand All @@ -1189,24 +1148,26 @@ void parseMspVtx_V2(uint16_t const cmd_u16) {
needSaveEEP = 1;
}
vtx_pit_save = PIT_MODE;
} else if (nxt_pwr == POWER_MAX + 1) {
// Enter 0mW
if (cur_pwr != POWER_MAX + 2) {
} else if (nxt_pwr == POWER_MAX + 1) { // Enter 0mW
if (cur_pwr != (POWER_MAX + 2)) {
WriteReg(0, 0x8F, 0x10);
dm6300_init_done = 0;
cur_pwr = POWER_MAX + 2;
vtx_pit_save = PIT_0MW;
vtx_pit = PIT_0MW;
temp_err = 1;
}
} else if (nxt_pwr <= POWER_MAX) {
RF_POWER = nxt_pwr;
RF_POWER = nxt_pwr;

if (PIT_MODE)
nxt_pwr = POWER_MAX + 1;

if (dm6300_init_done) {
if (cur_pwr != nxt_pwr) {
#ifndef VIDEO_PAT
#if defined HDZERO_FREESTYLE_V1 || HDZERO_FREESTYLE_V2
if ((nxt_pwr == 3) && (!g_IS_ARMED))
if ((nxt_pwr == 3) && !g_IS_ARMED)
pwr_lmt_done = 0;
else
#endif
Expand All @@ -1223,11 +1184,23 @@ void parseMspVtx_V2(uint16_t const cmd_u16) {
}
vtx_pit_save = PIT_MODE;
}
last_pwr = nxt_pwr;
}

if (needSaveEEP)
; // Setting_Save();
if (needSaveEEP) {
Setting_Save();
// msp_set_vtx_config(RF_POWER, (ident == MSP_CRC1));
}
}

void parse_vtx_config(uint8_t ident) {
fc_lock |= FC_VTX_CONFIG_LOCK;
parse_vtx_settings(ident);
}

void parseMspVtx_V2(uint8_t ident) {
if (fc_lock & FC_VTX_CONFIG_LOCK) {
parse_vtx_settings(ident);
}
}

uint8_t parse_displayport(uint8_t len) {
Expand Down Expand Up @@ -1454,7 +1427,6 @@ void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throt
DM6300_AUXADC_Calib();
}
}

if (!(IS_HI_yaw && IS_LO_throttle && IS_LO_roll && IS_LO_pitch))
cms_state = CMS_OSD;
break;
Expand Down Expand Up @@ -1923,8 +1895,8 @@ void save_vtx_param() {
TEAM_RACE = vtx_team_race;
SHORTCUT = vtx_shortcut;
CFG_Back();
Setting_Save();
Imp_RF_Param();
Setting_Save(); // Write to EEPROM
Imp_RF_Param(); // Set Band/Channel/Power

// init pitmode status and first_arm after setting_save
pit_mode_cfg_done = 0;
Expand Down Expand Up @@ -2021,12 +1993,12 @@ void set_vtx_param() {
}
} else if (heat_protect) {
#if defined HDZERO_FREESTYLE_V1 || HDZERO_FREESTYLE_V2
WriteReg(0, 0x8F, 0x00);
WriteReg(0, 0x8F, 0x01);
DM6300_Init(RF_FREQ, RF_BW);
DM6300_SetChannel(RF_FREQ);
DM6300_SetPower(0, RF_FREQ, 0);
WriteReg(0, 0x8F, 0x11);
WriteReg(0, 0x8F, 0x00); // dm6300 reset low
WriteReg(0, 0x8F, 0x01); // dm6300 reset high
DM6300_Init(RF_FREQ, RF_BW); // Set bandwidth
DM6300_SetChannel(RF_FREQ); // Set band/channel
DM6300_SetPower(0, RF_FREQ, 0); // Set low power (25mW)
WriteReg(0, 0x8F, 0x11); // enable 5680 video engine
#else
DM6300_SetPower(RF_POWER, RF_FREQ, pwr_offset);
cur_pwr = RF_POWER;
Expand Down Expand Up @@ -2116,7 +2088,7 @@ void InitVtxTable() {
if (TEAM_RACE)
msp_set_vtx_config(POWER_MAX + 1, 0);
else
msp_set_vtx_config(fc_pwr_rx, 0);
msp_set_vtx_config(RF_POWER, 0);

// set band/channel
for (i = 0; i < 5; i++) {
Expand Down
5 changes: 3 additions & 2 deletions src/msp_displayport.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,8 +138,9 @@ void msp_send_vtx_hw_faults();
void parse_status();
void parse_rc();
void parse_variant();
void parse_vtx_config();
void parseMspVtx_V2(uint16_t const cmd_u16);
void parse_vtx_params(uint8_t ident);
void parse_vtx_config(uint8_t ident);
void parseMspVtx_V2(uint8_t ident);
uint8_t parse_displayport(uint8_t len);
void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throttle);
void vtx_menu_init();
Expand Down