diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 881a3f65dc..f85ac5fd7b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -783,6 +783,9 @@ const clivalue_t valueTable[] = { #endif #if defined(USE_SERIALRX_CRSF) { "crsf_use_rx_snr", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, crsf_use_rx_snr) }, +#if defined(USE_CRSF_V3) + { "crsf_use_negotiated_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, crsf_use_negotiated_baud) }, +#endif #endif { "airmode_start_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index e1025ad641..0cf557d13c 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -71,6 +71,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .sbus_baud_fast = false, .crsf_use_rx_snr = false, .msp_override_channels_mask = 0, + .crsf_use_negotiated_baud = false, ); #ifdef RX_CHANNELS_TAER diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index a5812f99ad..1be7ecdb75 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -62,6 +62,7 @@ typedef struct rxConfig_s { uint8_t sbus_baud_fast; // Select SBus fast baud rate uint8_t crsf_use_rx_snr; // Use RX SNR (in dB) instead of RSSI dBm for CRSF uint32_t msp_override_channels_mask; // Channels to override when the MSP override mode is enabled + uint8_t crsf_use_negotiated_baud; // Use negotiated baud rate for CRSF V3 } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 9154dce3ad..d4e249ff70 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -656,6 +656,11 @@ void crsfRxUpdateBaudrate(uint32_t baudrate) { serialSetBaudRate(serialPort, baudrate); } + +bool crsfRxUseNegotiatedBaud(void) +{ + return rxConfig()->crsf_use_negotiated_baud; +} #endif bool crsfRxIsActive(void) diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index d3f8e57a01..a1ae3c80be 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -86,4 +86,5 @@ struct rxConfig_s; struct rxRuntimeState_s; bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeState_s *rxRuntimeState); void crsfRxUpdateBaudrate(uint32_t baudrate); +bool crsfRxUseNegotiatedBaud(void); bool crsfRxIsActive(void); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index b64e4f951f..52be8f7ee8 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -431,7 +431,7 @@ void speedNegotiationProcess(uint32_t currentTime) crsfRxSendTelemetryData(); } else { if (crsfSpeed.hasPendingReply) { - bool found = crsfSpeed.index < BAUD_COUNT ? true : false; + bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false; sbuf_t crsfSpeedNegotiationBuf; sbuf_t *dst = &crsfSpeedNegotiationBuf; crsfInitializeFrame(dst); @@ -440,7 +440,7 @@ void speedNegotiationProcess(uint32_t currentTime) crsfFinalize(dst); crsfRxSendTelemetryData(); crsfSpeed.hasPendingReply = false; - crsfSpeed.isNewSpeedValid = true; + crsfSpeed.isNewSpeedValid = found; crsfSpeed.confirmationTime = currentTime; return; } else if (crsfSpeed.isNewSpeedValid) {