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

Merge pull request #1297 from martinbudden/bf_msp_split_stage4

Browse source 
MSP split stage4 - Better split between MSP and serial
This commit is contained in:
J Blackman 2016-10-16 19:59:03 +11:00 • committed by GitHub
commit 84c6672967
9 changed files with 151 additions and 87 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/fc_tasks.c
View file

@ -96,7 +96,7 @@ static void taskHandleSerial(uint32_t currentTime)
return;
}
#endif
mspSerialProcess();
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA);
}
#ifdef BEEPER

2
src/main/io/serial.c
View file

@ -433,7 +433,7 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)
void cliEnter(serialPort_t *serialPort);
void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar)
void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar)
{
#ifndef USE_CLI
UNUSED(serialPort);

2
src/main/io/serial.h
View file

@ -147,5 +147,5 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate);
//
// msp/cli/bootloader
//
void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar);
void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar);
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC);

75
src/main/io/serial_msp.c
View file

@ -34,6 +34,7 @@
#include "msp/msp.h"
static mspProcessCommandFnPtr mspProcessCommandFn;
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
bufWriter_t *writer;
@ -77,11 +78,54 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
}
}
void mspSerialInit(void)
bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
mspInit();
memset(mspPorts, 0, sizeof(mspPorts));
mspSerialAllocatePorts();
if (mspPort->c_state == MSP_IDLE) {
if (c == '$') {
mspPort->c_state = MSP_HEADER_START;
} else {
return false;
}
} else if (mspPort->c_state == MSP_HEADER_START) {
mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
} else if (mspPort->c_state == MSP_HEADER_M) {
mspPort->c_state = (c == '<') ? MSP_HEADER_ARROW : MSP_IDLE;
} 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;
}
} else if (mspPort->c_state == MSP_HEADER_SIZE) {
mspPort->cmdMSP = c;
mspPort->checksum ^= c;
mspPort->c_state = MSP_HEADER_CMD;
} else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset < mspPort->dataSize) {
mspPort->checksum ^= c;
mspPort->inBuf[mspPort->offset++] = c;
} else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
if (mspPort->checksum == c) {
mspPort->c_state = MSP_COMMAND_RECEIVED;
} else {
mspPort->c_state = MSP_IDLE;
}
}
return true;
}
static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort)
{
mspPostProcessFnPtr mspPostProcessFn = NULL;
mspProcessCommandFn(mspPort, &mspPostProcessFn);
mspPort->c_state = MSP_IDLE;
return mspPostProcessFn;
}
/*
@ -89,7 +133,7 @@ void mspSerialInit(void)
*
* Called periodically by the scheduler.
*/
void mspSerialProcess(void)
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
{
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
mspPort_t * const mspPort = &mspPorts[portIndex];
@ -101,18 +145,18 @@ void mspSerialProcess(void)
uint8_t buf[sizeof(bufWriter_t) + 20];
writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
mspPostProcessFuncPtr mspPostProcessFn = NULL;
mspPostProcessFnPtr mspPostProcessFn = NULL;
while (serialRxBytesWaiting(mspPort->port)) {
const uint8_t c = serialRead(mspPort->port);
const bool consumed = mspProcessReceivedData(mspPort, c);
const bool consumed = mspSerialProcessReceivedData(mspPort, c);
if (!consumed && !ARMING_FLAG(ARMED)) {
evaluateOtherData(mspPort->port, c);
if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
serialEvaluateNonMspData(mspPort->port, c);
}
if (mspPort->c_state == COMMAND_RECEIVED) {
mspPostProcessFn = mspProcessReceivedCommand(mspPort);
if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort);
break; // process one command at a time so as not to block.
}
}
@ -121,7 +165,14 @@ void mspSerialProcess(void)
if (mspPostProcessFn) {
waitForSerialPortToFinishTransmitting(mspPort->port);
mspPostProcessFn(mspPort);
mspPostProcessFn(mspPort->port);
}
}
}
void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
{
mspProcessCommandFn = mspProcessCommandFnToUse;
memset(mspPorts, 0, sizeof(mspPorts));
mspSerialAllocatePorts();
}

26
src/main/io/serial_msp.h
View file

@ -17,19 +17,27 @@
#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.
#define MAX_MSP_PORT_COUNT 2
typedef enum {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
COMMAND_RECEIVED
MSP_IDLE,
MSP_HEADER_START,
MSP_HEADER_M,
MSP_HEADER_ARROW,
MSP_HEADER_SIZE,
MSP_HEADER_CMD,
MSP_COMMAND_RECEIVED
} mspState_e;
typedef enum {
MSP_EVALUATE_NON_MSP_DATA,
MSP_SKIP_NON_MSP_DATA
} mspEvaluateNonMspData_e;
#define MSP_PORT_INBUF_SIZE 64
struct serialPort_s;
@ -48,7 +56,7 @@ typedef struct mspPort_s {
struct bufWriter_s;
extern struct bufWriter_s *writer;
void mspSerialInit(void);
void mspSerialProcess(void);
void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData);
void mspSerialAllocatePorts(void);
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);

11
src/main/main.c
View file

@ -22,6 +22,8 @@
#include "platform.h"
#include "blackbox/blackbox.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/maths.h"
@ -63,6 +65,9 @@
#include "fc/fc_tasks.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "msp/msp_server_fc.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
@ -73,7 +78,6 @@
#include "io/gps.h"
#include "io/motors.h"
#include "io/servos.h"
#include "fc/rc_controls.h"
#include "io/gimbal.h"
#include "io/ledstrip.h"
#include "io/display.h"
@ -97,7 +101,6 @@
#include "sensors/initialisation.h"
#include "telemetry/telemetry.h"
#include "blackbox/blackbox.h"
#include "flight/pid.h"
#include "flight/imu.h"
@ -105,8 +108,6 @@
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "fc/runtime_config.h"
#include "config/config.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
@ -442,7 +443,7 @@ void init(void)
imuInit();
mspSerialInit();
mspSerialInit(mspFcInit());
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);

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

@ -17,9 +17,15 @@
#pragma once
struct mspPort_s;
typedef void (*mspPostProcessFuncPtr)(struct mspPort_s *); // msp post process function, used for gracefully handling reboots, etc.
// return positive for ACK, negative on error, zero for no reply
typedef enum {
MSP_RESULT_ACK = 1,
MSP_RESULT_ERROR = -1,
MSP_RESULT_NO_REPLY = 0
} mspResult_e;
void mspInit(void);
bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c);
mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort);
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);

82
src/main/msp/msp_server_fc.c
View file

@ -112,7 +112,6 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
extern void resetProfile(profile_t *profile);
// cause reboot after MSP processing complete
static mspPostProcessFuncPtr mspPostProcessFn = NULL;
static mspPort_t *currentPort;
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
@ -193,26 +192,26 @@ typedef enum {
#define DATAFLASH_BUFFER_SIZE 4096
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
void msp4WayIfFn(mspPort_t *mspPort)
static void msp4WayIfFn(serialPort_t *serialPort)
{
// 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);
esc4wayProcess(serialPort);
// former used MSP uart is still active
// proceed as usual with MSP commands
}
#endif
static void mspRebootFn(mspPort_t *mspPort)
static void mspRebootFn(serialPort_t *serialPort)
{
UNUSED(mspPort);
UNUSED(serialPort);
stopPwmAllMotors();
systemReset();
// control should never return here.
while(1) ;
while (true) ;
}
static void serialize8(uint8_t a)
@ -463,7 +462,7 @@ static void serializeDataflashReadReply(uint32_t address, uint16_t size, bool us
}
#endif
void mspInit(void)
void initActiveBoxIds(void)
{
// calculate used boxes based on features and fill availableBoxes[] array
memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
@ -608,9 +607,12 @@ static uint32_t packFlightModeFlags(void)
return junk;
}
static bool processOutCommand(uint8_t cmdMSP)
static bool processOutCommand(uint8_t cmdMSP, mspPostProcessFnPtr *mspPostProcessFn)
{
uint32_t i;
#ifdef USE_FLASHFS
const unsigned int dataSize = currentPort->dataSize;
#endif
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0;
@ -1157,7 +1159,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 {
@ -1303,7 +1305,9 @@ static bool processOutCommand(uint8_t cmdMSP)
case MSP_REBOOT:
headSerialReply(0);
mspPostProcessFn = mspRebootFn;
if (mspPostProcessFn) {
*mspPostProcessFn = mspRebootFn;
}
break;
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
@ -1313,7 +1317,9 @@ static bool processOutCommand(uint8_t cmdMSP)
// switch all motor lines HI
// reply with the count of ESC found
serialize8(esc4wayInit());
mspPostProcessFn = msp4WayIfFn;
if (mspPostProcessFn) {
*mspPostProcessFn = mspRebootFn;
}
break;
#endif
@ -1916,55 +1922,25 @@ static bool processInCommand(uint8_t cmdMSP)
return true;
}
mspPostProcessFuncPtr mspProcessReceivedCommand(mspPort_t *mspPort)
mspResult_e mspFcProcessCommand(mspPort_t *mspPort, mspPostProcessFnPtr *mspPostProcessFn)
{
mspResult_e ret = MSP_RESULT_ACK;
currentPort = mspPort;
mspPostProcessFn = NULL;
if (!(processOutCommand(mspPort->cmdMSP) || processInCommand(mspPort->cmdMSP))) {
if (!(processOutCommand(mspPort->cmdMSP, mspPostProcessFn) || processInCommand(mspPort->cmdMSP))) {
headSerialError(0);
ret = MSP_RESULT_ERROR;
}
tailSerialReply();
mspPort->c_state = IDLE;
return mspPostProcessFn;
mspPort->c_state = MSP_IDLE;
return ret;
}
bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c)
/*
* Return a pointer to the process command function
*/
mspProcessCommandFnPtr mspFcInit(void)
{
if (mspPort->c_state == IDLE) {
if (c == '$') {
mspPort->c_state = HEADER_START;
} else {
return false;
}
} 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) {
mspPort->c_state = IDLE;
} else {
mspPort->dataSize = c;
mspPort->offset = 0;
mspPort->checksum = 0;
mspPort->indRX = 0;
mspPort->checksum ^= c;
mspPort->c_state = HEADER_SIZE;
}
} 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 {
mspPort->c_state = IDLE;
}
}
return true;
initActiveBoxIds();
return mspFcProcessCommand;
}

22
src/main/msp/msp_server_fc.h Normal file
View file

@ -0,0 +1,22 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "msp/msp.h"
mspProcessCommandFnPtr mspFcInit(void);