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

Merge pull request #1330 from martinbudden/bf_msp_streambuf

Browse source 
Converted MSP to use streambuf
This commit is contained in:
J Blackman 2016-10-21 15:26:07 +11:00 • committed by GitHub
commit bd452b58b8
4 changed files with 692 additions and 809 deletions
Download patch file Download diff file Expand all files Collapse all files

1394
src/main/fc/fc_msp.c
View file

File diff suppressed because it is too large Load diff

10
src/main/msp/msp.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "common/streambuf.h"
// return positive for ACK, negative on error, zero for no reply
typedef enum {
MSP_RESULT_ACK = 1,
@ -24,8 +26,12 @@ typedef enum {
MSP_RESULT_NO_REPLY = 0
} mspResult_e;
typedef struct mspPacket_s {
sbuf_t buf;
int16_t cmd;
int16_t result;
} mspPacket_t;
struct serialPort_s;
typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc.
struct mspPort_s;
typedef mspResult_e (*mspProcessCommandFnPtr)(struct mspPort_s *mspPort, mspPostProcessFnPtr *mspPostProcessFn);
typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);

79
src/main/msp/msp_serial.c
View file

@ -21,22 +21,18 @@
#include "platform.h"
#include "common/streambuf.h"
#include "common/utils.h"
#include "drivers/buf_writer.h"
#include "drivers/serial.h"
#include "fc/runtime_config.h"
#include "io/serial.h"
#include "msp/msp.h"
#include "msp/msp_serial.h"
static mspProcessCommandFnPtr mspProcessCommandFn;
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
bufWriter_t *writer;
static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
@ -78,7 +74,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
}
}
bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
if (mspPort->c_state == MSP_IDLE) {
if (c == '$') {
@ -93,12 +89,10 @@ bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
} else if (mspPort->c_state == MSP_HEADER_ARROW) {
if (c > MSP_PORT_INBUF_SIZE) {
mspPort->c_state = MSP_IDLE;
} else {
mspPort->dataSize = c;
mspPort->offset = 0;
mspPort->checksum = 0;
mspPort->indRX = 0;
mspPort->checksum ^= c;
mspPort->c_state = MSP_HEADER_SIZE;
}
@ -119,12 +113,64 @@ bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
return true;
}
static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort)
static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len)
{
mspPostProcessFnPtr mspPostProcessFn = NULL;
mspProcessCommandFn(mspPort, &mspPostProcessFn);
while (len-- > 0) {
checksum ^= *data++;
}
return checksum;
}
mspPort->c_state = MSP_IDLE;
#define JUMBO_FRAME_SIZE_LIMIT 255
static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
{
serialBeginWrite(msp->port);
const int len = sbufBytesRemaining(&packet->buf);
const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
serialWriteBuf(msp->port, hdr, sizeof(hdr));
uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field
if (len >= JUMBO_FRAME_SIZE_LIMIT) {
serialWrite(msp->port, len & 0xff);
checksum ^= len & 0xff;
serialWrite(msp->port, (len >> 8) & 0xff);
checksum ^= (len >> 8) & 0xff;
}
if (len > 0) {
serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
}
serialWrite(msp->port, checksum);
serialEndWrite(msp->port);
}
static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
{
static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
mspPacket_t reply = {
.buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
.cmd = -1,
.result = 0,
};
uint8_t *outBufHead = reply.buf.ptr;
mspPacket_t command = {
.buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
.cmd = msp->cmdMSP,
.result = 0,
};
mspPostProcessFnPtr mspPostProcessFn = NULL;
const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
if (status != MSP_RESULT_NO_REPLY) {
sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
mspSerialEncode(msp, &reply);
}
msp->c_state = MSP_IDLE;
return mspPostProcessFn;
}
@ -140,11 +186,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
if (!mspPort->port) {
continue;
}
// Big enough to fit a MSP_STATUS in one write.
uint8_t buf[sizeof(bufWriter_t) + 20];
writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
mspPostProcessFnPtr mspPostProcessFn = NULL;
while (serialRxBytesWaiting(mspPort->port)) {
@ -160,9 +201,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
break; // process one command at a time so as not to block.
}
}
bufWriterFlush(writer);
if (mspPostProcessFn) {
waitForSerialPortToFinishTransmitting(mspPort->port);
mspPostProcessFn(mspPort->port);
@ -176,3 +214,4 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
memset(mspPorts, 0, sizeof(mspPorts));
mspSerialAllocatePorts();
}

18
src/main/msp/msp_serial.h
View file

@ -17,7 +17,6 @@
#pragma once
#include "msp/msp.h"
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
@ -38,7 +37,14 @@ typedef enum {
MSP_SKIP_NON_MSP_DATA
} mspEvaluateNonMspData_e;
#define MSP_PORT_INBUF_SIZE 64
#define MSP_PORT_INBUF_SIZE 192
#ifdef USE_FLASHFS
#define MSP_PORT_DATAFLASH_BUFFER_SIZE 4096
#define MSP_PORT_DATAFLASH_INFO_SIZE 16
#define MSP_PORT_OUTBUF_SIZE (MSP_PORT_DATAFLASH_BUFFER_SIZE + MSP_PORT_DATAFLASH_INFO_SIZE)
#else
#define MSP_PORT_OUTBUF_SIZE 256
#endif
struct serialPort_s;
typedef struct mspPort_s {
@ -46,16 +52,12 @@ typedef struct mspPort_s {
uint8_t offset;
uint8_t dataSize;
uint8_t checksum;
uint8_t indRX;
uint8_t inBuf[MSP_PORT_INBUF_SIZE];
mspState_e c_state;
uint8_t cmdMSP;
mspState_e c_state;
uint8_t inBuf[MSP_PORT_INBUF_SIZE];
} mspPort_t;
struct bufWriter_s;
extern struct bufWriter_s *writer;
void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData);
void mspSerialAllocatePorts(void);