Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Merge pull request #4196 from martinbudden/bf_crsf_tidy

Browse source 
CRSF code tidy
This commit is contained in:
Martin Budden 2017-09-20 12:25:03 +01:00 • committed by GitHub
commit cf33acf21d
9 changed files with 50 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/fc/fc_core.c
View file

@ -368,7 +368,9 @@ void updateMagHold(void)
}
#endif
/*
* processRx called from taskUpdateRxMain
*/
void processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;

6
src/main/fc/fc_rc.c
View file

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

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

@ -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:
* <Device address> <Frame length> < Type> <Payload> < CRC>
* <Device address><Frame length><Type><Payload><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;
}

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

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

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

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

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

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

22
src/main/telemetry/msp_shared.c
View file

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

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

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

4
src/test/unit/rx_crsf_unittest.cc
View file

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