From 287a45659d88f7bc5f97cb265eb47a5d811cc61e Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 20 Sep 2017 10:43:51 +0100 Subject: [PATCH] CRSF code tidy --- src/main/fc/fc_core.c | 4 +++- src/main/fc/fc_rc.c | 6 ++++-- src/main/rx/crsf.c | 33 +++++++++++++++---------------- src/main/rx/crsf.h | 2 +- src/main/telemetry/crsf.c | 18 +++++++++-------- src/main/telemetry/crsf.h | 4 ++-- src/main/telemetry/msp_shared.c | 22 ++++++++++----------- src/main/telemetry/msp_shared.h | 2 +- src/test/unit/rx_crsf_unittest.cc | 4 ++-- 9 files changed, 50 insertions(+), 45 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 7c0f14d979..eb179c9893 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -368,7 +368,9 @@ void updateMagHold(void) } #endif - +/* + * processRx called from taskUpdateRxMain + */ void processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index f0ac7e669b..94299c0ebe 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -134,7 +134,8 @@ static void calculateSetpointRate(int axis) setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) } -static void scaleRcCommandToFpvCamAngle(void) { +static void scaleRcCommandToFpvCamAngle(void) +{ //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed static uint8_t lastFpvCamAngleDegrees = 0; static float cosFactor = 1.0; @@ -155,7 +156,8 @@ static void scaleRcCommandToFpvCamAngle(void) { #define THROTTLE_BUFFER_MAX 20 #define THROTTLE_DELTA_MS 100 - static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) { + static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) + { static int index; static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; const int rxRefreshRateMs = rxRefreshRate / 1000; diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 1c598ca537..1e82f0a8d7 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -59,7 +59,7 @@ STATIC_UNIT_TESTED crsfFrame_t crsfFrame; STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; static serialPort_t *serialPort; -static uint32_t crsfFrameStartAt = 0; +static uint32_t crsfFrameStartAtUs = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; @@ -75,13 +75,13 @@ static uint8_t telemetryBufLen = 0; * 1 Stop bit * Big endian * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte - * Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes - * A 36 byte frame can be transmitted in 771 microseconds. + * Max frame size is 64 bytes + * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds. * - * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds + * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds * * Every frame has the structure: - * < Type> < CRC> + * * * Device address: (uint8_t) * Frame length: length in bytes including Type (uint8_t) @@ -117,20 +117,20 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t; STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c) { static uint8_t crsfFramePosition = 0; - const uint32_t now = micros(); + const uint32_t currentTimeUs = micros(); #ifdef DEBUG_CRSF_PACKETS - debug[2] = now - crsfFrameStartAt; + debug[2] = currentTimeUs - crsfFrameStartAtUs; #endif - if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) { + if (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. crsfFramePosition = 0; } if (crsfFramePosition == 0) { - crsfFrameStartAt = now; + crsfFrameStartAtUs = currentTimeUs; } // assume frame is 5 bytes long until we have received the frame length // full frame length includes the length of the address and framelength fields @@ -167,7 +167,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) } crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; // unpack the RC channels - const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; + const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; crsfChannelData[0] = rcChannels->chan0; crsfChannelData[1] = rcChannels->chan1; crsfChannelData[2] = rcChannels->chan2; @@ -188,15 +188,15 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) } else { if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) { // TODO: CRC CHECK - scheduleDeviceInfoResponse(); + crsfScheduleDeviceInfoResponse(); return RX_FRAME_COMPLETE; #if defined(USE_MSP_OVER_TELEMETRY) } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) { // TODO: CRC CHECK uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - if(handleMspFrame(frameStart, frameEnd)) { - scheduleMspResponse(); + if (handleMspFrame(frameStart, frameEnd)) { + crsfScheduleMspResponse(); } return RX_FRAME_COMPLETE; #endif @@ -234,9 +234,9 @@ void crsfRxSendTelemetryData(void) // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame) // and that there is time to send the telemetry frame before the next RX frame arrives if (rxConfig()->halfDuplex) { - const uint32_t timeSinceStartOfFrame = micros() - crsfFrameStartAt; - if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) || - (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { + const uint32_t timeSinceStartOfFrameUs = micros() - crsfFrameStartAtUs; + if ((timeSinceStartOfFrameUs < CRSF_TIME_NEEDED_PER_FRAME_US) || + (timeSinceStartOfFrameUs > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { return; } } @@ -247,7 +247,6 @@ void crsfRxSendTelemetryData(void) bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { - for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408; } diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 0b547cbc67..8342a29917 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -69,7 +69,7 @@ enum { CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE }; -#define CRSF_PAYLOAD_SIZE_MAX 60 // Size confirmed by Remo +#define CRSF_PAYLOAD_SIZE_MAX 60 #define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4) typedef struct crsfFrameDef_s { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d7d18bf692..64e054afd4 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -63,9 +63,6 @@ static bool crsfTelemetryEnabled; static bool deviceInfoReplyPending; -#if defined(USE_MSP_OVER_TELEMETRY) -static bool mspReplyPending; -#endif static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; static void crsfInitializeFrame(sbuf_t *dst) @@ -232,10 +229,6 @@ void crsfFrameFlightMode(sbuf_t *dst) *lengthPtr = sbufPtr(dst) - lengthPtr; } -void scheduleDeviceInfoResponse() { - deviceInfoReplyPending = true; -} - /* 0x29 Device Info Payload: @@ -283,7 +276,11 @@ static uint8_t crsfScheduleCount; static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) -void scheduleMspResponse() { + +static bool mspReplyPending; + +void crsfScheduleMspResponse(void) +{ mspReplyPending = true; } @@ -336,6 +333,11 @@ static void processCrsf(void) crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } +void crsfScheduleDeviceInfoResponse(void) +{ + deviceInfoReplyPending = true; +} + void initCrsfTelemetry(void) { // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index b183d2927b..3ccfc76cac 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -24,7 +24,7 @@ void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); -void scheduleDeviceInfoResponse(); -void scheduleMspResponse(); +void crsfScheduleDeviceInfoResponse(void); +void crsfScheduleMspResponse(void); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 074b9f92b0..d80ec2a168 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -46,7 +46,8 @@ static mspTxBuffer_t mspTxBuffer; static mspPacket_t mspRxPacket; static mspPacket_t mspTxPacket; -void initSharedMsp() { +void initSharedMsp(void) +{ mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer; mspPackage.requestPacket = &mspRxPacket; mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer; @@ -58,7 +59,7 @@ void initSharedMsp() { mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; } -static void processMspPacket() +static void processMspPacket(void) { mspPackage.responsePacket->cmd = 0; mspPackage.responsePacket->result = 0; @@ -102,9 +103,9 @@ bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) mspPacket_t *packet = mspPackage.requestPacket; sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd); sbuf_t *rxBuf = &mspPackage.requestPacket->buf; - uint8_t header = sbufReadU8(frameBuf); - uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK; - uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; + const uint8_t header = sbufReadU8(frameBuf); + const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK; + const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; if (version != TELEMETRY_MSP_VERSION) { sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0); @@ -131,8 +132,8 @@ bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) return false; } - uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); - uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); + const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); + const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); uint8_t payload[frameBytesRemaining]; if (bufferBytesRemaining >= frameBytesRemaining) { @@ -185,14 +186,13 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) uint8_t size = sbufBytesRemaining(txBuf); sbufWriteU8(payloadBuf, size); - } - else { + } else { // header sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK)); } - uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); - uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); + const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); + const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); uint8_t frame[payloadBytesRemaining]; if (bufferBytesRemaining >= payloadBytesRemaining) { diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index 315c47a615..85a3e2c703 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -24,6 +24,6 @@ typedef union mspTxBuffer_u { uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; } mspTxBuffer_t; -void initSharedMsp(); +void initSharedMsp(void); bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 0b41b35325..1aa2e89642 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -286,7 +286,7 @@ serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, seria serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; -void scheduleDeviceInfoResponse(void) {}; -void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; +void crsfScheduleDeviceInfoResponse(void) {}; +void crsfScheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; bool handleMspFrame(uint8_t *, uint8_t *) { return false; } }