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

Applied some ledvinap’s suggestions.

Browse source
This commit is contained in:
Raphael Coeffic 2016-10-27 13:37:24 +02:00
parent 083222dcff
commit 86c4195b4f
1 changed files with 12 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/telemetry/smartport.c
View file

@ -172,16 +172,16 @@ typedef struct smartPortFrame_s {
#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t)
#define SMARTPORT_TX_BUF_SIZE 256
#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - 3*sizeof(uint8_t))
#define SMARTPORT_PAYLOAD_OFFSET 2
#define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId)
#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1)
static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE];
static smartPortFrame_t smartPortRxBuffer;
static uint8_t smartPortRxBytes = 0;
static bool smartPortFrameReceived = false;
#define SMARTPORT_MSP_VERSION 1
#define SMARTPORT_MSP_VER_SHIFT 5
#define SMARTPORT_MSP_VER_MASK 0xE0
#define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT)
#define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT)
#define SMARTPORT_MSP_START_FLAG (1 << 4)
@ -211,6 +211,7 @@ static void smartPortDataReceive(uint16_t c)
return;
}
uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer;
if (smartPortRxBytes == 0) {
if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) {
@ -219,7 +220,7 @@ static void smartPortDataReceive(uint16_t c)
smartPortHasRequest = 1;
}
else if (c == FSSP_SENSOR_ID2) {
smartPortRxBuffer[smartPortRxBytes++] = c;
rxBuffer[smartPortRxBytes++] = c;
checksum = 0;
}
else {
@ -238,7 +239,7 @@ static void smartPortDataReceive(uint16_t c)
byteStuffing = false;
}
smartPortRxBuffer[smartPortRxBytes++] = c;
rxBuffer[smartPortRxBytes++] = c;
if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) {
if (c == (0xFF - checksum)) {
@ -405,7 +406,7 @@ static void processMspPacket(mspPacket_t* packet)
* - payload...
* - CRC
*/
void handleSmartPortMspFrame(uint8_t* frame, uint8_t size)
void handleSmartPortMspFrame(smartPortFrame_t* sp_frame)
{
static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
static uint8_t mspStarted = 0;
@ -419,8 +420,8 @@ void handleSmartPortMspFrame(uint8_t* frame, uint8_t size)
};
// re-assemble MSP frame & forward to MSP port when complete
uint8_t* p = frame;
uint8_t* end = frame + size;
uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET;
uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE;
uint8_t head = *p++;
uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK;
@ -561,11 +562,10 @@ void handleSmartPortTelemetry(void)
smartPortFrameReceived = false;
// do not check the physical ID here again
// unless we start receiving other sensors' packets
smartPortFrame_t* frame = (smartPortFrame_t*)smartPortRxBuffer;
if(frame->frameId == FSSP_MSPC_FRAME) {
if(smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
// Pass only the payload: skip sensorId & frameId
handleSmartPortMspFrame(smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET, SMARTPORT_PAYLOAD_SIZE);
handleSmartPortMspFrame(&smartPortRxBuffer);
}
}