Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Merge pull request #11500 from klutvott123/crsf-baud-negotiation-improvements-2

Browse source 
Fix CRSF baud negotiation
This commit is contained in:
J Blackman 2022-04-22 10:31:25 +10:00 • committed by GitHub
commit eb31068614
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 122 additions and 41 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/config/config.c
View file

@ -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;
}

1
src/main/config/config.h
View file

@ -90,3 +90,4 @@ void targetValidateConfiguration(void);
bool isSystemConfigured(void);
void setRebootRequired(void);
bool getRebootRequired(void);
bool isEepromWriteInProgress(void);

2
src/main/fc/tasks.c
View file

@ -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
};

28
src/main/rx/crsf.c
View file

@ -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)
);

2
src/main/rx/crsf_protocol.h
View file

@ -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.

117
src/main/telemetry/crsf.c
View file

@ -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.

2
src/main/telemetry/crsf.h
View file

@ -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