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

Fix BetaFlight Issues #229

Merged
merged 1 commit into from
Nov 26, 2024
+42 −52
Merged

Fix BetaFlight Issues #229

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
88 changes: 39 additions & 49 deletions src/msp_displayport.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -305,13 +305,13 @@ 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(state);
parse_vtx_config();
else if (cur_cmd == CUR_GET_OSD_CANVAS)
parse_get_osd_canvas();
else if (cur_cmd == CUR_DISPLAYPORT)
ret = parse_displayport(osd_len);
full_frame = 1;
if ((fc_lock & FC_VTX_CONFIG_LOCK) && (fc_lock & FC_INIT_VTX_TABLE_LOCK) == 0 && (fc_lock & FC_VARIANT_LOCK)) {
if ((fc_lock & FC_VTX_CONFIG_LOCK) && (fc_lock & FC_VARIANT_LOCK) && (fc_lock & FC_INIT_VTX_TABLE_LOCK) == 0) {
fc_lock |= FC_INIT_VTX_TABLE_LOCK;
if (msp_cmp_fc_variant("BTFL") || msp_cmp_fc_variant("QUIC")) {
#ifdef INIT_VTX_TABLE
Expand Down Expand Up @@ -403,7 +403,7 @@ uint8_t msp_read_one_frame() {
msp_send_vtx_hw_faults();
break;
case MSP_GET_VTX_CONFIG:
parseMspVtx_V2(state);
parseMspVtx_V2();
default:
break;
}
Expand Down Expand Up @@ -772,31 +772,33 @@ uint8_t msp_send_header_v2(uint16_t len, uint16_t msg) {
// Send commands to the FC.
// Ensure VARIANT is processed first, followed by CONFIG, then the others.
void msp_cmd_tx()
{
uint8_t i, start = 0, count = 1;

// Wait for variant, then config, then continue with status/rx/osd_canvas
if (fc_lock & FC_VARIANT_LOCK) {
start = 1;
if (fc_lock & FC_VTX_CONFIG_LOCK) {
start = 2;
count = (msp_cmp_fc_variant("BTFL")) ? 3 : 2;
}
}

{
uint8_t const msp_cmd[5] = {
MSP_FC_VARIANT,
MSP_GET_VTX_CONFIG,
MSP_STATUS,
MSP_RC,
MSP_GET_OSD_CANVAS,
MSP_STATUS,
MSP_RC
};

for (i = start; count--; i++) {
uint8_t idx, start = 0, end = 0;

// Process in strict order: VARIANT; VTX_CONFIG; then STATUS/RC/OSD_CANVAS
if (fc_lock & FC_VARIANT_LOCK) {
start = 1; end = 1; // Config only
if (fc_lock & FC_VTX_CONFIG_LOCK) {
end = 4; // the rest...
if (msp_cmp_fc_variant("INAV")) {
start = 3; // OSD_CANVAS not reuired for iNav
}
}
}

for (idx = start; idx <= end; idx++) {
msp_send_command(0, MSP_HEADER_V1);
msp_tx(0x00); // len
msp_tx(msp_cmd[i]); // function
msp_tx(msp_cmd[i]); // crc
msp_tx(msp_cmd[idx]); // function
msp_tx(msp_cmd[idx]); // crc
}
}

Expand Down Expand Up @@ -946,14 +948,14 @@ 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((POWER_MAX & 0x1) + 1);
crc ^= ((POWER_MAX & 0x1) + 1); // power locked to 25/200mW
msp_tx(3); // power locked to 25/200/0mW
crc ^= (3);
}
else
#endif
{
msp_tx(POWER_MAX + 1);
crc ^= (POWER_MAX + 1); // power count
msp_tx(POWER_MAX + 2); // power count (including 0mW)
crc ^= (POWER_MAX + 2);
}
msp_tx(0x00);
crc ^= 0x00; // disable/clear vtx table
Expand Down Expand Up @@ -1043,33 +1045,26 @@ void parse_get_osd_canvas(void) {
}
}

void parse_vtx_settings(uint8_t ident) {
void parse_vtx_params(uint8_t isMSP_V2) {
uint8_t nxt_ch = INVALID_CHANNEL;
uint8_t nxt_pwr;
uint8_t nxt_pit;
uint8_t needSaveEEP = 0;

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

if (ident == MSP_CRC1) { // V1 (initial) config

fc_pwr_rx = msp_rx_buf[3];
fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = msp_rx_buf[6];

} else { // MSP_CRC2 (V2 config)

fc_pwr_rx = msp_rx_buf[3];
fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = msp_rx_buf[8];
}

fc_pwr_rx = msp_rx_buf[3];
if (fc_pwr_rx == 0) {
fc_pwr_rx = POWER_MAX+2;
fc_pwr_rx = POWER_MAX+2; // 0mW
} else if (fc_pwr_rx > (POWER_MAX+2)) {
fc_pwr_rx = 1; // min power if invalid
}

fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = (isMSP_V2) ? msp_rx_buf[6] : msp_rx_buf[8];

mspVtxLock = 1;
pwr_lmt_done = 1;

Expand All @@ -1087,10 +1082,6 @@ void parse_vtx_settings(uint8_t ident) {
// 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) {
// Invalid config requested -> ignore
return;
}
}
needSaveEEP |= msp_vtx_set_channel(nxt_ch);

Expand Down Expand Up @@ -1199,18 +1190,17 @@ void parse_vtx_settings(uint8_t ident) {

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

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

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

Expand Down
6 changes: 3 additions & 3 deletions src/msp_displayport.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,9 +138,9 @@ void msp_send_vtx_hw_faults();
void parse_status();
void parse_rc();
void parse_variant();
void parse_vtx_params(uint8_t ident);
void parse_vtx_config(uint8_t ident);
void parseMspVtx_V2(uint8_t ident);
void parse_vtx_params(uint8_t isMSP_V2);
void parse_vtx_config();
void parseMspVtx_V2();
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