diff --git a/src/main/config/config.c b/src/main/config/config.c index 378a9f6b58..2ec00c1de6 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -97,6 +97,8 @@ static bool configIsDirty; /* someone indicated that the config is modified and static bool rebootRequired = false; // set if a config change requires a reboot to take effect +static bool eepromWriteInProgress = false; + pidProfile_t *currentPidProfile; #ifndef RX_SPI_DEFAULT_PROTOCOL @@ -126,6 +128,11 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .enableStickArming = false, ); +bool isEepromWriteInProgress(void) +{ + return eepromWriteInProgress; +} + uint8_t getCurrentPidProfileIndex(void) { return systemConfig()->pidProfileIndex; @@ -738,9 +745,9 @@ void writeUnmodifiedConfigToEEPROM(void) validateAndFixConfig(); suspendRxSignal(); - + eepromWriteInProgress = true; writeConfigToEEPROM(); - + eepromWriteInProgress = false; resumeRxSignal(); configIsDirty = false; } diff --git a/src/main/config/config.h b/src/main/config/config.h index 4d7fff7ad5..afdcd9bf5a 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -90,3 +90,4 @@ void targetValidateConfiguration(void); bool isSystemConfigured(void); void setRebootRequired(void); bool getRebootRequired(void); +bool isEepromWriteInProgress(void); diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index a7057eecd9..b8da676726 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -429,7 +429,7 @@ task_attribute_t task_attributes[TASK_COUNT] = { #endif #ifdef USE_CRSF_V3 - [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOWEST), + [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), #endif }; diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index d4e249ff70..981eb6e212 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -34,6 +34,8 @@ #include "common/maths.h" #include "common/utils.h" +#include "config/config.h" + #include "pg/rx.h" #include "drivers/serial.h" @@ -58,7 +60,7 @@ #define CRSF_LINK_STATUS_UPDATE_TIMEOUT_US 250000 // 250ms, 4 Hz mode 1 telemetry -#define CRSF_FRAME_ERROR_COUNT_THRESHOLD 100 +#define CRSF_FRAME_ERROR_COUNT_THRESHOLD 3 STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; @@ -362,6 +364,12 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) { // We've received a character after max time needed to complete a frame, // so this must be the start of a new frame. +#if defined(USE_CRSF_V3) + if (crsfFramePosition > 0) { + // count an error if full valid frame not received within the allowed time. + crsfFrameErrorCnt++; + } +#endif crsfFramePosition = 0; } @@ -456,14 +464,12 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) crsfFrameErrorCnt++; #endif } - } else { -#if defined(USE_CRSF_V3) - if (crsfFrameErrorCnt < CRSF_FRAME_ERROR_COUNT_THRESHOLD) - crsfFrameErrorCnt++; -#endif } #if defined(USE_CRSF_V3) - if (crsfFrameErrorCnt >= CRSF_FRAME_ERROR_COUNT_THRESHOLD) { + if (crsfBaudNegotiationInProgress() || isEepromWriteInProgress()) { + // don't count errors when negotiation or eeprom write is in progress + crsfFrameErrorCnt = 0; + } else if (crsfFrameErrorCnt >= CRSF_FRAME_ERROR_COUNT_THRESHOLD) { // fall back to default speed if speed mismatch detected setCrsfDefaultSpeed(); crsfFrameErrorCnt = 0; @@ -630,11 +636,17 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) return false; } + uint32_t crsfBaudrate = CRSF_BAUDRATE; + +#if defined(USE_CRSF_V3) + crsfBaudrate = (isMPUSoftReset() && rxConfig->crsf_use_negotiated_baud) ? getCrsfCachedBaudrate() : CRSF_BAUDRATE; +#endif + serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, crsfDataReceive, rxRuntimeState, - CRSF_BAUDRATE, + crsfBaudrate, CRSF_PORT_MODE, CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) ); diff --git a/src/main/rx/crsf_protocol.h b/src/main/rx/crsf_protocol.h index 395c3a378e..99fef0d1a1 100644 --- a/src/main/rx/crsf_protocol.h +++ b/src/main/rx/crsf_protocol.h @@ -37,6 +37,7 @@ enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 }; typedef enum { CRSF_FRAMETYPE_GPS = 0x02, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_HEARTBEAT = 0x0B, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED = 0x17, @@ -83,6 +84,7 @@ enum { enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE = 6, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 52be8f7ee8..2fbb3a2421 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -32,13 +32,14 @@ #include "cms/cms.h" +#include "config/config.h" #include "config/feature.h" -#include "config/config.h" #include "common/crc.h" #include "common/maths.h" #include "common/printf.h" #include "common/streambuf.h" +#include "common/time.h" #include "common/utils.h" #include "drivers/nvic.h" @@ -88,6 +89,9 @@ typedef struct mspBuffer_s { static mspBuffer_t mspRxBuffer; #if defined(USE_CRSF_V3) + +#define CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US 20000 // 20ms + static bool isCrsfV3Running = false; typedef struct { uint8_t hasPendingReply:1; @@ -99,6 +103,19 @@ typedef struct { static crsfSpeedControl_s crsfSpeed = {0}; +uint32_t crsfCachedBaudrate __attribute__ ((section (".noinit"))); // Used for retaining negotiated baudrate after soft reset + +uint32_t getCrsfCachedBaudrate(void) +{ + // check if valid first. return default baudrate if not + for (unsigned i = 0; i < BAUD_COUNT; i++) { + if (crsfCachedBaudrate == baudRates[i] && baudRates[i] >= CRSF_BAUDRATE) { + return crsfCachedBaudrate; + } + } + return CRSF_BAUDRATE; +} + bool checkCrsfCustomizedSpeed(void) { return crsfSpeed.index < BAUD_COUNT ? true : false; @@ -116,7 +133,13 @@ void setCrsfDefaultSpeed(void) crsfSpeed.confirmationTime = 0; crsfSpeed.index = BAUD_COUNT; isCrsfV3Running = false; - crsfRxUpdateBaudrate(getCrsfDesiredSpeed()); + crsfCachedBaudrate = getCrsfDesiredSpeed(); + crsfRxUpdateBaudrate(crsfCachedBaudrate); +} + +bool crsfBaudNegotiationInProgress(void) +{ + return crsfSpeed.hasPendingReply || crsfSpeed.isNewSpeedValid; } #endif @@ -248,6 +271,18 @@ void crsfFrameBatterySensor(sbuf_t *dst) sbufWriteU8(dst, batteryRemainingPercentage); } +/* +0x0B Heartbeat +Payload: +int16_t origin_add ( Origin Device address ) +*/ +void crsfFrameHeartbeat(sbuf_t *dst) +{ + sbufWriteU8(dst, CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + sbufWriteU8(dst, CRSF_FRAMETYPE_HEARTBEAT); + sbufWriteU16BigEndian(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -418,40 +453,37 @@ void crsfScheduleSpeedNegotiationResponse(void) crsfSpeed.isNewSpeedValid = false; } -void speedNegotiationProcess(uint32_t currentTime) +void speedNegotiationProcess(timeUs_t currentTimeUs) { - if (!featureIsEnabled(FEATURE_TELEMETRY) && getCrsfDesiredSpeed() == CRSF_BAUDRATE) { - // to notify the RX to fall back to default baud rate by sending device info frame if telemetry is disabled - sbuf_t crsfPayloadBuf; - sbuf_t *dst = &crsfPayloadBuf; + if (crsfSpeed.hasPendingReply) { + bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false; + sbuf_t crsfSpeedNegotiationBuf; + sbuf_t *dst = &crsfSpeedNegotiationBuf; crsfInitializeFrame(dst); - crsfFrameDeviceInfo(dst); + crsfFrameSpeedNegotiationResponse(dst, found); crsfRxSendTelemetryData(); // prevent overwriting previous data crsfFinalize(dst); crsfRxSendTelemetryData(); - } else { - if (crsfSpeed.hasPendingReply) { - bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false; - sbuf_t crsfSpeedNegotiationBuf; - sbuf_t *dst = &crsfSpeedNegotiationBuf; - crsfInitializeFrame(dst); - crsfFrameSpeedNegotiationResponse(dst, found); - crsfRxSendTelemetryData(); // prevent overwriting previous data - crsfFinalize(dst); - crsfRxSendTelemetryData(); - crsfSpeed.hasPendingReply = false; - crsfSpeed.isNewSpeedValid = found; - crsfSpeed.confirmationTime = currentTime; - return; - } else if (crsfSpeed.isNewSpeedValid) { - if (currentTime - crsfSpeed.confirmationTime >= 4000) { - // delay 4ms before applying the new baudrate - crsfRxUpdateBaudrate(getCrsfDesiredSpeed()); - crsfSpeed.isNewSpeedValid = false; - isCrsfV3Running = true; - return; - } + crsfSpeed.hasPendingReply = false; + crsfSpeed.isNewSpeedValid = found; + crsfSpeed.confirmationTime = currentTimeUs; + } else if (crsfSpeed.isNewSpeedValid) { + if (cmpTimeUs(currentTimeUs, crsfSpeed.confirmationTime) >= 4000) { + // delay 4ms before applying the new baudrate + crsfCachedBaudrate = getCrsfDesiredSpeed(); + crsfRxUpdateBaudrate(crsfCachedBaudrate); + crsfSpeed.isNewSpeedValid = false; + isCrsfV3Running = true; } + } else if (!featureIsEnabled(FEATURE_TELEMETRY) && crsfRxUseNegotiatedBaud()) { + // Send heartbeat if telemetry is disabled to allow RX to detect baud rate mismatches + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + crsfInitializeFrame(dst); + crsfFrameHeartbeat(dst); + crsfRxSendTelemetryData(); // prevent overwriting previous data + crsfFinalize(dst); + crsfRxSendTelemetryData(); } } #endif @@ -552,6 +584,7 @@ typedef enum { CRSF_FRAME_BATTERY_SENSOR_INDEX, CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, + CRSF_FRAME_HEARTBEAT_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -621,6 +654,15 @@ static void processCrsf(void) crsfFinalize(dst); } #endif + +#if defined(USE_CRSF_V3) + if (currentSchedule & BIT(CRSF_FRAME_HEARTBEAT_INDEX)) { + crsfInitializeFrame(dst); + crsfFrameHeartbeat(dst); + crsfFinalize(dst); + } +#endif + crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -661,6 +703,14 @@ void initCrsfTelemetry(void) crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX); } #endif + +#if defined(USE_CRSF_V3) + while (index < (CRSF_CYCLETIME_US / CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US) && index < CRSF_SCHEDULE_COUNT_MAX) { + // schedule heartbeat to ensure that telemetry/heartbeat frames are sent at minimum 50Hz + crsfSchedule[index++] = BIT(CRSF_FRAME_HEARTBEAT_INDEX); + } +#endif + crsfScheduleCount = (uint8_t)index; #if defined(USE_CRSF_CMS_TELEMETRY) @@ -730,6 +780,13 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) if (!crsfTelemetryEnabled) { return; } + +#if defined(USE_CRSF_V3) + if (crsfBaudNegotiationInProgress()) { + return; + } +#endif + // Give the receiver a chance to send any outstanding telemetry data. // This needs to be done at high frequency, to enable the RX to send the telemetry frame // in between the RX frames. diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index c52f958cfd..bd33bee3f5 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -48,4 +48,6 @@ int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize) #endif #if defined(USE_CRSF_V3) void speedNegotiationProcess(uint32_t currentTime); +bool crsfBaudNegotiationInProgress(void); +uint32_t getCrsfCachedBaudrate(void); #endif