Merge pull request #1257 from martinbudden/bf_msp_split_stage3
MSP split stage3 - Better split between MSP and serial
This commit is contained in:
commit
b37c718ef7
|
@ -25,12 +25,9 @@
|
|||
|
||||
#include "drivers/buf_writer.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/serial_msp.h"
|
||||
|
||||
|
@ -38,8 +35,6 @@
|
|||
|
||||
|
||||
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
|
||||
serialPort_t *mspSerialPort;
|
||||
mspPort_t *currentPort;
|
||||
bufWriter_t *writer;
|
||||
|
||||
|
||||
|
@ -52,12 +47,8 @@ static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
|
|||
|
||||
void mspSerialAllocatePorts(void)
|
||||
{
|
||||
serialPort_t *serialPort;
|
||||
|
||||
uint8_t portIndex = 0;
|
||||
|
||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);
|
||||
|
||||
while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
|
||||
mspPort_t *mspPort = &mspPorts[portIndex];
|
||||
if (mspPort->port) {
|
||||
|
@ -65,7 +56,7 @@ void mspSerialAllocatePorts(void)
|
|||
continue;
|
||||
}
|
||||
|
||||
serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
|
||||
serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
|
||||
if (serialPort) {
|
||||
resetMspPort(mspPort, serialPort);
|
||||
portIndex++;
|
||||
|
@ -77,8 +68,7 @@ void mspSerialAllocatePorts(void)
|
|||
|
||||
void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
|
||||
{
|
||||
uint8_t portIndex;
|
||||
for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
mspPort_t *candidateMspPort = &mspPorts[portIndex];
|
||||
if (candidateMspPort->port == serialPort) {
|
||||
closeSerialPort(serialPort);
|
||||
|
@ -94,12 +84,6 @@ void mspSerialInit(void)
|
|||
mspSerialAllocatePorts();
|
||||
}
|
||||
|
||||
static void setCurrentPort(mspPort_t *port)
|
||||
{
|
||||
currentPort = port;
|
||||
mspSerialPort = currentPort->port;
|
||||
}
|
||||
|
||||
/*
|
||||
* Process MSP commands from serial ports configured as MSP ports.
|
||||
*
|
||||
|
@ -107,46 +91,37 @@ static void setCurrentPort(mspPort_t *port)
|
|||
*/
|
||||
void mspSerialProcess(void)
|
||||
{
|
||||
uint8_t portIndex;
|
||||
mspPort_t *candidatePort;
|
||||
|
||||
for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
candidatePort = &mspPorts[portIndex];
|
||||
if (!candidatePort->port) {
|
||||
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
|
||||
mspPort_t * const mspPort = &mspPorts[portIndex];
|
||||
if (!mspPort->port) {
|
||||
continue;
|
||||
}
|
||||
|
||||
setCurrentPort(candidatePort);
|
||||
// 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, currentPort->port);
|
||||
writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
|
||||
|
||||
while (serialRxBytesWaiting(mspSerialPort)) {
|
||||
mspPostProcessFuncPtr mspPostProcessFn = NULL;
|
||||
while (serialRxBytesWaiting(mspPort->port)) {
|
||||
|
||||
uint8_t c = serialRead(mspSerialPort);
|
||||
bool consumed = mspProcessReceivedData(c);
|
||||
const uint8_t c = serialRead(mspPort->port);
|
||||
const bool consumed = mspProcessReceivedData(mspPort, c);
|
||||
|
||||
if (!consumed && !ARMING_FLAG(ARMED)) {
|
||||
evaluateOtherData(mspSerialPort, c);
|
||||
evaluateOtherData(mspPort->port, c);
|
||||
}
|
||||
|
||||
if (currentPort->c_state == COMMAND_RECEIVED) {
|
||||
mspProcessReceivedCommand();
|
||||
if (mspPort->c_state == COMMAND_RECEIVED) {
|
||||
mspPostProcessFn = mspProcessReceivedCommand(mspPort);
|
||||
break; // process one command at a time so as not to block.
|
||||
}
|
||||
}
|
||||
|
||||
bufWriterFlush(writer);
|
||||
|
||||
if (isRebootScheduled) {
|
||||
waitForSerialPortToFinishTransmitting(candidatePort->port);
|
||||
stopPwmAllMotors();
|
||||
// On real flight controllers, systemReset() will do a soft reset of the device,
|
||||
// reloading the program. But to support offline testing this flag needs to be
|
||||
// cleared so that the software doesn't continuously attempt to reboot itself.
|
||||
isRebootScheduled = false;
|
||||
systemReset();
|
||||
if (mspPostProcessFn) {
|
||||
waitForSerialPortToFinishTransmitting(mspPort->port);
|
||||
mspPostProcessFn(mspPort);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -45,15 +45,9 @@ typedef struct mspPort_s {
|
|||
} mspPort_t;
|
||||
|
||||
|
||||
extern struct serialPort_s *mspSerialPort;
|
||||
extern mspPort_t *currentPort;
|
||||
|
||||
struct bufWriter_s;
|
||||
extern struct bufWriter_s *writer;
|
||||
|
||||
extern bool isRebootScheduled;
|
||||
|
||||
|
||||
void mspSerialInit(void);
|
||||
void mspSerialProcess(void);
|
||||
void mspSerialAllocatePorts(void);
|
||||
|
|
|
@ -17,7 +17,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
struct mspPort_s;
|
||||
typedef void (*mspPostProcessFuncPtr)(struct mspPort_s *); // msp post process function, used for gracefully handling reboots, etc.
|
||||
|
||||
void mspInit(void);
|
||||
bool mspProcessReceivedData(uint8_t c);
|
||||
void mspProcessReceivedCommand(void);
|
||||
bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c);
|
||||
mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort);
|
||||
|
|
|
@ -69,6 +69,7 @@
|
|||
#include "io/vtx.h"
|
||||
|
||||
#include "msp/msp_protocol.h"
|
||||
#include "msp/msp.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/msp.h"
|
||||
|
@ -110,9 +111,11 @@
|
|||
extern uint16_t cycleTime; // FIXME dependency on mw.c
|
||||
extern void resetProfile(profile_t *profile);
|
||||
|
||||
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse);
|
||||
// cause reboot after MSP processing complete
|
||||
static mspPostProcessFuncPtr mspPostProcessFn = NULL;
|
||||
static mspPort_t *currentPort;
|
||||
|
||||
const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
|
||||
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
|
||||
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
||||
|
||||
typedef struct box_e {
|
||||
|
@ -161,12 +164,6 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
|
||||
// this is the number of filled indexes in above array
|
||||
static uint8_t activeBoxIdCount = 0;
|
||||
// from mixer.c
|
||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
|
||||
// cause reboot after MSP processing complete
|
||||
bool isRebootScheduled = false;
|
||||
|
||||
|
||||
static const char pidnames[] =
|
||||
"ROLL;"
|
||||
|
@ -195,6 +192,29 @@ typedef enum {
|
|||
|
||||
#define DATAFLASH_BUFFER_SIZE 4096
|
||||
|
||||
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
void msp4WayIfFn(mspPort_t *mspPort)
|
||||
{
|
||||
// rem: App: Wait at least appx. 500 ms for BLHeli to jump into
|
||||
// bootloader mode before try to connect any ESC
|
||||
// Start to activate here
|
||||
esc4wayProcess(mspPort->port);
|
||||
// former used MSP uart is still active
|
||||
// proceed as usual with MSP commands
|
||||
}
|
||||
#endif
|
||||
|
||||
static void mspRebootFn(mspPort_t *mspPort)
|
||||
{
|
||||
UNUSED(mspPort);
|
||||
|
||||
stopPwmAllMotors();
|
||||
systemReset();
|
||||
|
||||
// control should never return here.
|
||||
while(1) ;
|
||||
}
|
||||
|
||||
static void serialize8(uint8_t a)
|
||||
{
|
||||
bufWriterAppend(writer, a);
|
||||
|
@ -234,7 +254,7 @@ static uint32_t read32(void)
|
|||
|
||||
static void headSerialResponse(uint8_t err, uint16_t responseBodySize)
|
||||
{
|
||||
serialBeginWrite(mspSerialPort);
|
||||
serialBeginWrite(currentPort->port);
|
||||
|
||||
serialize8('$');
|
||||
serialize8('M');
|
||||
|
@ -264,7 +284,7 @@ static void headSerialError(uint8_t responseBodySize)
|
|||
static void tailSerialReply(void)
|
||||
{
|
||||
serialize8(currentPort->checksum);
|
||||
serialEndWrite(mspSerialPort);
|
||||
serialEndWrite(currentPort->port);
|
||||
}
|
||||
|
||||
static void s_struct(uint8_t *cb, uint8_t siz)
|
||||
|
@ -591,7 +611,7 @@ static uint32_t packFlightModeFlags(void)
|
|||
static bool processOutCommand(uint8_t cmdMSP)
|
||||
{
|
||||
uint32_t i;
|
||||
uint8_t len;
|
||||
const unsigned int dataSize = currentPort->dataSize;
|
||||
#ifdef GPS
|
||||
uint8_t wp_no;
|
||||
int32_t lat = 0, lon = 0;
|
||||
|
@ -685,10 +705,12 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
break;
|
||||
|
||||
case MSP_NAME:
|
||||
len = strlen(masterConfig.name);
|
||||
headSerialReply(len);
|
||||
for (uint8_t i=0; i<len; i++) {
|
||||
serialize8(masterConfig.name[i]);
|
||||
{
|
||||
const unsigned int nameLen = strlen(masterConfig.name);
|
||||
headSerialReply(nameLen);
|
||||
for (i = 0; i < nameLen; i++) {
|
||||
serialize8(masterConfig.name[i]);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
|
@ -1136,7 +1158,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
uint32_t readAddress = read32();
|
||||
uint16_t readLength;
|
||||
bool useLegacyFormat;
|
||||
if (currentPort->dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
|
||||
if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
|
||||
readLength = read16();
|
||||
useLegacyFormat = false;
|
||||
} else {
|
||||
|
@ -1280,17 +1302,34 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(masterConfig.mag_hardware);
|
||||
break;
|
||||
|
||||
case MSP_REBOOT:
|
||||
headSerialReply(0);
|
||||
mspPostProcessFn = mspRebootFn;
|
||||
break;
|
||||
|
||||
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
case MSP_SET_4WAY_IF:
|
||||
headSerialReply(1);
|
||||
// get channel number
|
||||
// switch all motor lines HI
|
||||
// reply with the count of ESC found
|
||||
serialize8(esc4wayInit());
|
||||
mspPostProcessFn = msp4WayIfFn;
|
||||
break;
|
||||
#endif
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool processInCommand(void)
|
||||
static bool processInCommand(uint8_t cmdMSP)
|
||||
{
|
||||
uint32_t i;
|
||||
uint16_t tmp;
|
||||
uint8_t value;
|
||||
const unsigned int dataSize = currentPort->dataSize;
|
||||
#ifdef GPS
|
||||
uint8_t wp_no;
|
||||
int32_t lat = 0, lon = 0, alt = 0;
|
||||
|
@ -1298,7 +1337,7 @@ static bool processInCommand(void)
|
|||
#ifdef OSD
|
||||
uint8_t addr, font_data[64];
|
||||
#endif
|
||||
switch (currentPort->cmdMSP) {
|
||||
switch (cmdMSP) {
|
||||
case MSP_SELECT_SETTING:
|
||||
value = read8();
|
||||
if ((value & RATEPROFILE_MASK) == 0) {
|
||||
|
@ -1324,7 +1363,7 @@ static bool processInCommand(void)
|
|||
case MSP_SET_RAW_RC:
|
||||
#ifndef SKIP_RX_MSP
|
||||
{
|
||||
uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t);
|
||||
uint8_t channelCount = dataSize / sizeof(uint16_t);
|
||||
if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
|
||||
headSerialError(0);
|
||||
} else {
|
||||
|
@ -1404,7 +1443,7 @@ static bool processInCommand(void)
|
|||
break;
|
||||
|
||||
case MSP_SET_RC_TUNING:
|
||||
if (currentPort->dataSize >= 10) {
|
||||
if (dataSize >= 10) {
|
||||
currentControlRateProfile->rcRate8 = read8();
|
||||
currentControlRateProfile->rcExpo8 = read8();
|
||||
for (i = 0; i < 3; i++) {
|
||||
|
@ -1416,10 +1455,10 @@ static bool processInCommand(void)
|
|||
currentControlRateProfile->thrMid8 = read8();
|
||||
currentControlRateProfile->thrExpo8 = read8();
|
||||
currentControlRateProfile->tpa_breakpoint = read16();
|
||||
if (currentPort->dataSize >= 11) {
|
||||
if (dataSize >= 11) {
|
||||
currentControlRateProfile->rcYawExpo8 = read8();
|
||||
}
|
||||
if (currentPort->dataSize >= 12) {
|
||||
if (dataSize >= 12) {
|
||||
currentControlRateProfile->rcYawRate8 = read8();
|
||||
}
|
||||
} else {
|
||||
|
@ -1463,7 +1502,7 @@ static bool processInCommand(void)
|
|||
break;
|
||||
case MSP_SET_SERVO_CONFIGURATION:
|
||||
#ifdef USE_SERVOS
|
||||
if (currentPort->dataSize != 1 + sizeof(servoParam_t)) {
|
||||
if (dataSize != 1 + sizeof(servoParam_t)) {
|
||||
headSerialError(0);
|
||||
break;
|
||||
}
|
||||
|
@ -1560,7 +1599,7 @@ static bool processInCommand(void)
|
|||
|
||||
#ifdef TRANSPONDER
|
||||
case MSP_SET_TRANSPONDER_CONFIG:
|
||||
if (currentPort->dataSize != sizeof(masterConfig.transponderData)) {
|
||||
if (dataSize != sizeof(masterConfig.transponderData)) {
|
||||
headSerialError(0);
|
||||
break;
|
||||
}
|
||||
|
@ -1692,16 +1731,16 @@ static bool processInCommand(void)
|
|||
masterConfig.rxConfig.midrc = read16();
|
||||
masterConfig.rxConfig.mincheck = read16();
|
||||
masterConfig.rxConfig.spektrum_sat_bind = read8();
|
||||
if (currentPort->dataSize > 8) {
|
||||
if (dataSize > 8) {
|
||||
masterConfig.rxConfig.rx_min_usec = read16();
|
||||
masterConfig.rxConfig.rx_max_usec = read16();
|
||||
}
|
||||
if (currentPort->dataSize > 12) {
|
||||
if (dataSize > 12) {
|
||||
masterConfig.rxConfig.rcInterpolation = read8();
|
||||
masterConfig.rxConfig.rcInterpolationInterval = read8();
|
||||
masterConfig.rxConfig.airModeActivateThreshold = read16();
|
||||
}
|
||||
if (currentPort->dataSize > 16) {
|
||||
if (dataSize > 16) {
|
||||
masterConfig.rxConfig.rx_spi_protocol = read8();
|
||||
masterConfig.rxConfig.rx_spi_id = read32();
|
||||
masterConfig.rxConfig.rx_spi_rf_channel_count = read8();
|
||||
|
@ -1761,12 +1800,12 @@ static bool processInCommand(void)
|
|||
{
|
||||
uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
|
||||
|
||||
if (currentPort->dataSize % portConfigSize != 0) {
|
||||
if (dataSize % portConfigSize != 0) {
|
||||
headSerialError(0);
|
||||
break;
|
||||
}
|
||||
|
||||
uint8_t remainingPortsInPacket = currentPort->dataSize / portConfigSize;
|
||||
uint8_t remainingPortsInPacket = dataSize / portConfigSize;
|
||||
|
||||
while (remainingPortsInPacket--) {
|
||||
uint8_t identifier = read8();
|
||||
|
@ -1800,7 +1839,7 @@ static bool processInCommand(void)
|
|||
case MSP_SET_LED_STRIP_CONFIG:
|
||||
{
|
||||
i = read8();
|
||||
if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) {
|
||||
if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
|
||||
headSerialError(0);
|
||||
break;
|
||||
}
|
||||
|
@ -1821,32 +1860,6 @@ static bool processInCommand(void)
|
|||
}
|
||||
break;
|
||||
#endif
|
||||
case MSP_REBOOT:
|
||||
isRebootScheduled = true;
|
||||
break;
|
||||
|
||||
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
case MSP_SET_4WAY_IF:
|
||||
// get channel number
|
||||
// switch all motor lines HI
|
||||
// reply the count of ESC found
|
||||
headSerialReply(1);
|
||||
serialize8(esc4wayInit());
|
||||
// because we do not come back after calling Process4WayInterface
|
||||
// proceed with a success reply first
|
||||
tailSerialReply();
|
||||
// flush the transmit buffer
|
||||
bufWriterFlush(writer);
|
||||
// wait for all data to send
|
||||
waitForSerialPortToFinishTransmitting(currentPort->port);
|
||||
// rem: App: Wait at least appx. 500 ms for BLHeli to jump into
|
||||
// bootloader mode before try to connect any ESC
|
||||
// Start to activate here
|
||||
esc4wayProcess(currentPort->port);
|
||||
// former used MSP uart is still active
|
||||
// proceed as usual with MSP commands
|
||||
break;
|
||||
#endif
|
||||
|
||||
case MSP_SET_ADVANCED_CONFIG :
|
||||
masterConfig.gyro_sync_denom = read8();
|
||||
|
@ -1859,13 +1872,13 @@ static bool processInCommand(void)
|
|||
masterConfig.gyro_soft_lpf_hz = read8();
|
||||
currentProfile->pidProfile.dterm_lpf_hz = read16();
|
||||
currentProfile->pidProfile.yaw_lpf_hz = read16();
|
||||
if (currentPort->dataSize > 5) {
|
||||
if (dataSize > 5) {
|
||||
masterConfig.gyro_soft_notch_hz_1 = read16();
|
||||
masterConfig.gyro_soft_notch_cutoff_1 = read16();
|
||||
currentProfile->pidProfile.dterm_notch_hz = read16();
|
||||
currentProfile->pidProfile.dterm_notch_cutoff = read16();
|
||||
}
|
||||
if (currentPort->dataSize > 13) {
|
||||
if (dataSize > 13) {
|
||||
serialize16(masterConfig.gyro_soft_notch_hz_2);
|
||||
serialize16(masterConfig.gyro_soft_notch_cutoff_2);
|
||||
}
|
||||
|
@ -1892,7 +1905,7 @@ static bool processInCommand(void)
|
|||
|
||||
case MSP_SET_NAME:
|
||||
memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
|
||||
for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) {
|
||||
for (i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
|
||||
masterConfig.name[i] = read8();
|
||||
}
|
||||
break;
|
||||
|
@ -1904,51 +1917,54 @@ static bool processInCommand(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mspProcessReceivedCommand(void)
|
||||
mspPostProcessFuncPtr mspProcessReceivedCommand(mspPort_t *mspPort)
|
||||
{
|
||||
if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) {
|
||||
currentPort = mspPort;
|
||||
mspPostProcessFn = NULL;
|
||||
if (!(processOutCommand(mspPort->cmdMSP) || processInCommand(mspPort->cmdMSP))) {
|
||||
headSerialError(0);
|
||||
}
|
||||
tailSerialReply();
|
||||
currentPort->c_state = IDLE;
|
||||
mspPort->c_state = IDLE;
|
||||
return mspPostProcessFn;
|
||||
}
|
||||
|
||||
bool mspProcessReceivedData(uint8_t c)
|
||||
bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c)
|
||||
{
|
||||
if (currentPort->c_state == IDLE) {
|
||||
if (mspPort->c_state == IDLE) {
|
||||
if (c == '$') {
|
||||
currentPort->c_state = HEADER_START;
|
||||
mspPort->c_state = HEADER_START;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
} else if (currentPort->c_state == HEADER_START) {
|
||||
currentPort->c_state = (c == 'M') ? HEADER_M : IDLE;
|
||||
} else if (currentPort->c_state == HEADER_M) {
|
||||
currentPort->c_state = (c == '<') ? HEADER_ARROW : IDLE;
|
||||
} else if (currentPort->c_state == HEADER_ARROW) {
|
||||
} else if (mspPort->c_state == HEADER_START) {
|
||||
mspPort->c_state = (c == 'M') ? HEADER_M : IDLE;
|
||||
} else if (mspPort->c_state == HEADER_M) {
|
||||
mspPort->c_state = (c == '<') ? HEADER_ARROW : IDLE;
|
||||
} else if (mspPort->c_state == HEADER_ARROW) {
|
||||
if (c > MSP_PORT_INBUF_SIZE) {
|
||||
currentPort->c_state = IDLE;
|
||||
mspPort->c_state = IDLE;
|
||||
|
||||
} else {
|
||||
currentPort->dataSize = c;
|
||||
currentPort->offset = 0;
|
||||
currentPort->checksum = 0;
|
||||
currentPort->indRX = 0;
|
||||
currentPort->checksum ^= c;
|
||||
currentPort->c_state = HEADER_SIZE;
|
||||
mspPort->dataSize = c;
|
||||
mspPort->offset = 0;
|
||||
mspPort->checksum = 0;
|
||||
mspPort->indRX = 0;
|
||||
mspPort->checksum ^= c;
|
||||
mspPort->c_state = HEADER_SIZE;
|
||||
}
|
||||
} else if (currentPort->c_state == HEADER_SIZE) {
|
||||
currentPort->cmdMSP = c;
|
||||
currentPort->checksum ^= c;
|
||||
currentPort->c_state = HEADER_CMD;
|
||||
} else if (currentPort->c_state == HEADER_CMD && currentPort->offset < currentPort->dataSize) {
|
||||
currentPort->checksum ^= c;
|
||||
currentPort->inBuf[currentPort->offset++] = c;
|
||||
} else if (currentPort->c_state == HEADER_CMD && currentPort->offset >= currentPort->dataSize) {
|
||||
if (currentPort->checksum == c) {
|
||||
currentPort->c_state = COMMAND_RECEIVED;
|
||||
} else if (mspPort->c_state == HEADER_SIZE) {
|
||||
mspPort->cmdMSP = c;
|
||||
mspPort->checksum ^= c;
|
||||
mspPort->c_state = HEADER_CMD;
|
||||
} else if (mspPort->c_state == HEADER_CMD && mspPort->offset < mspPort->dataSize) {
|
||||
mspPort->checksum ^= c;
|
||||
mspPort->inBuf[mspPort->offset++] = c;
|
||||
} else if (mspPort->c_state == HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
|
||||
if (mspPort->checksum == c) {
|
||||
mspPort->c_state = COMMAND_RECEIVED;
|
||||
} else {
|
||||
currentPort->c_state = IDLE;
|
||||
mspPort->c_state = IDLE;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
|
|
Loading…
Reference in New Issue