From b8260fdf408c622030f3004eafb874e0d54fa1fb Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 2 Oct 2016 15:18:24 +0100 Subject: [PATCH 1/8] Better split between MSP and serial --- src/main/io/serial_msp.c | 44 ++++-------- src/main/io/serial_msp.h | 5 -- src/main/msp/msp.h | 7 +- src/main/msp/msp_server_fc.c | 128 +++++++++++++++++++---------------- 4 files changed, 88 insertions(+), 96 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 914f89ff8c..67831a34c0 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -38,8 +38,6 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; -serialPort_t *mspSerialPort; -mspPort_t *currentPort; bufWriter_t *writer; @@ -94,12 +92,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 +99,36 @@ 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)) { + 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) { + 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) { + mspPostProcessFn(mspPort); + mspPostProcessFn = NULL; } } } diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 2d9d89afd8..8859b44411 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -45,14 +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); diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index c1058326c8..54d9e2e454 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -18,6 +18,9 @@ #pragma once +typedef void (*mspPostProcessFuncPtr)(mspPort_t *); // msp post process function, used for gracefully handling reboots, etc. +extern mspPostProcessFuncPtr mspPostProcessFn; + void mspInit(void); -bool mspProcessReceivedData(uint8_t c); -void mspProcessReceivedCommand(void); +bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c); +void mspProcessReceivedCommand(mspPort_t *mspPort); diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index cd3750392d..4a8e8b80b1 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -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 +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,19 @@ typedef enum { #define DATAFLASH_BUFFER_SIZE 4096 +void mspRebootFn(mspPort_t *mspPort) +{ + waitForSerialPortToFinishTransmitting(mspPort->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. + systemReset(); + + // control should never return here. + while(1) ; +} + static void serialize8(uint8_t a) { bufWriterAppend(writer, a); @@ -234,7 +244,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 +274,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) @@ -1135,7 +1145,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 (len >= sizeof(uint32_t) + sizeof(uint16_t)) { readLength = read16(); useLegacyFormat = false; } else { @@ -1285,11 +1295,12 @@ static bool processOutCommand(uint8_t cmdMSP) return true; } -static bool processInCommand(void) +static bool processInCommand(uint8_t cmdMSP) { uint32_t i; uint16_t tmp; uint8_t value; + const unsigned int len = currentPort->dataSize; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; @@ -1297,7 +1308,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) { @@ -1323,7 +1334,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 = len / sizeof(uint16_t); if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { headSerialError(0); } else { @@ -1403,7 +1414,7 @@ static bool processInCommand(void) break; case MSP_SET_RC_TUNING: - if (currentPort->dataSize >= 10) { + if (len >= 10) { currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcExpo8 = read8(); for (i = 0; i < 3; i++) { @@ -1415,10 +1426,10 @@ static bool processInCommand(void) currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->tpa_breakpoint = read16(); - if (currentPort->dataSize >= 11) { + if (len >= 11) { currentControlRateProfile->rcYawExpo8 = read8(); } - if (currentPort->dataSize >= 12) { + if (len >= 12) { currentControlRateProfile->rcYawRate8 = read8(); } } else { @@ -1462,7 +1473,7 @@ static bool processInCommand(void) break; case MSP_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS - if (currentPort->dataSize != 1 + sizeof(servoParam_t)) { + if (len != 1 + sizeof(servoParam_t)) { headSerialError(0); break; } @@ -1559,7 +1570,7 @@ static bool processInCommand(void) #ifdef TRANSPONDER case MSP_SET_TRANSPONDER_CONFIG: - if (currentPort->dataSize != sizeof(masterConfig.transponderData)) { + if (len != sizeof(masterConfig.transponderData)) { headSerialError(0); break; } @@ -1691,16 +1702,16 @@ static bool processInCommand(void) masterConfig.rxConfig.midrc = read16(); masterConfig.rxConfig.mincheck = read16(); masterConfig.rxConfig.spektrum_sat_bind = read8(); - if (currentPort->dataSize > 8) { + if (len > 8) { masterConfig.rxConfig.rx_min_usec = read16(); masterConfig.rxConfig.rx_max_usec = read16(); } - if (currentPort->dataSize > 12) { + if (len > 12) { masterConfig.rxConfig.rcInterpolation = read8(); masterConfig.rxConfig.rcInterpolationInterval = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); } - if (currentPort->dataSize > 16) { + if (len > 16) { masterConfig.rxConfig.rx_spi_protocol = read8(); masterConfig.rxConfig.rx_spi_id = read32(); masterConfig.rxConfig.rx_spi_rf_channel_count = read8(); @@ -1760,12 +1771,12 @@ static bool processInCommand(void) { uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); - if (currentPort->dataSize % portConfigSize != 0) { + if (len % portConfigSize != 0) { headSerialError(0); break; } - uint8_t remainingPortsInPacket = currentPort->dataSize / portConfigSize; + uint8_t remainingPortsInPacket = len / portConfigSize; while (remainingPortsInPacket--) { uint8_t identifier = read8(); @@ -1799,7 +1810,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 || len != (1 + 4)) { headSerialError(0); break; } @@ -1821,7 +1832,7 @@ static bool processInCommand(void) break; #endif case MSP_REBOOT: - isRebootScheduled = true; + mspPostProcessFn = mspRebootFn; break; #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -1858,13 +1869,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 (len > 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 (len > 13) { serialize16(masterConfig.gyro_soft_notch_hz_2); serialize16(masterConfig.gyro_soft_notch_cutoff_2); } @@ -1891,7 +1902,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, len); i++) { masterConfig.name[i] = read8(); } break; @@ -1903,51 +1914,52 @@ static bool processInCommand(void) return true; } -void mspProcessReceivedCommand(void) +void mspProcessReceivedCommand(mspPort_t *mspPort) { - if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) { + currentPort = mspPort; + if (!(processOutCommand(mspPort->cmdMSP) || processInCommand(mspPort->cmdMSP))) { headSerialError(0); } tailSerialReply(); - currentPort->c_state = IDLE; + mspPort->c_state = IDLE; } -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; From 1bc9cc83fe5ac4da721d6a506c429bdffaace964 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 2 Oct 2016 16:13:42 +0100 Subject: [PATCH 2/8] Improved MSP post process function handling --- src/main/io/serial_msp.c | 4 ++-- src/main/msp/msp.h | 9 ++++----- src/main/msp/msp_server_fc.c | 6 ++++-- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 67831a34c0..bc97ad02c1 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -109,6 +109,7 @@ void mspSerialProcess(void) uint8_t buf[sizeof(bufWriter_t) + 20]; writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port); + mspPostProcessFuncPtr mspPostProcessFn = NULL; while (serialRxBytesWaiting(mspPort->port)) { const uint8_t c = serialRead(mspPort->port); @@ -119,7 +120,7 @@ void mspSerialProcess(void) } if (mspPort->c_state == COMMAND_RECEIVED) { - mspProcessReceivedCommand(mspPort); + mspPostProcessFn = mspProcessReceivedCommand(mspPort); break; // process one command at a time so as not to block. } } @@ -128,7 +129,6 @@ void mspSerialProcess(void) if (mspPostProcessFn) { mspPostProcessFn(mspPort); - mspPostProcessFn = NULL; } } } diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 54d9e2e454..e49dd6bb77 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -17,10 +17,9 @@ #pragma once - -typedef void (*mspPostProcessFuncPtr)(mspPort_t *); // msp post process function, used for gracefully handling reboots, etc. -extern mspPostProcessFuncPtr mspPostProcessFn; +struct mspPort_s; +typedef void (*mspPostProcessFuncPtr)(struct mspPort_s *); // msp post process function, used for gracefully handling reboots, etc. void mspInit(void); -bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c); -void mspProcessReceivedCommand(mspPort_t *mspPort); +bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c); +mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort); diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index 4a8e8b80b1..131ab2f437 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -112,7 +112,7 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c extern void resetProfile(profile_t *profile); // cause reboot after MSP processing complete -mspPostProcessFuncPtr mspPostProcessFn = NULL; +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. @@ -1914,14 +1914,16 @@ static bool processInCommand(uint8_t cmdMSP) return true; } -void mspProcessReceivedCommand(mspPort_t *mspPort) +mspPostProcessFuncPtr mspProcessReceivedCommand(mspPort_t *mspPort) { currentPort = mspPort; + mspPostProcessFn = NULL; if (!(processOutCommand(mspPort->cmdMSP) || processInCommand(mspPort->cmdMSP))) { headSerialError(0); } tailSerialReply(); mspPort->c_state = IDLE; + return mspPostProcessFn; } bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c) From 345f9cfe0345a886ae523688414a8aed4237ae8d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 2 Oct 2016 22:36:11 +0100 Subject: [PATCH 3/8] Removed unnecessary #includes --- src/main/io/serial_msp.c | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index bc97ad02c1..90051d010f 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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" @@ -50,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) { @@ -63,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++; @@ -75,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); From 445d3f31dcf4c6ba470f8b862b69f3bc765848e3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 2 Oct 2016 23:20:17 +0100 Subject: [PATCH 4/8] Better mspPostProcessFn handling --- src/main/io/serial_msp.c | 1 + src/main/msp/msp_server_fc.c | 22 +++++++++++++++------- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 90051d010f..3fc7cbc55b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -120,6 +120,7 @@ void mspSerialProcess(void) bufWriterFlush(writer); if (mspPostProcessFn) { + waitForSerialPortToFinishTransmitting(mspPort->port); mspPostProcessFn(mspPort); } } diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index 131ab2f437..ee776edf76 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -192,9 +192,22 @@ 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 + void mspRebootFn(mspPort_t *mspPort) { - waitForSerialPortToFinishTransmitting(mspPort->port); + UNUSED(mspPort); + 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 @@ -1849,12 +1862,7 @@ static bool processInCommand(uint8_t cmdMSP) 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 + msp4WayIfFn(currentPort); break; #endif From dc733127755c706091508c56be60dad7afd3a702 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 2 Oct 2016 23:33:23 +0100 Subject: [PATCH 5/8] Fixed len initialisation in processOutCommand --- src/main/msp/msp_server_fc.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index ee776edf76..8dd830cd9a 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -613,7 +613,7 @@ static uint32_t packFlightModeFlags(void) static bool processOutCommand(uint8_t cmdMSP) { uint32_t i; - uint8_t len; + const unsigned int len = currentPort->dataSize; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0; @@ -707,10 +707,12 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_NAME: - len = strlen(masterConfig.name); - headSerialReply(len); - for (uint8_t i=0; i Date: Sun, 2 Oct 2016 23:38:43 +0100 Subject: [PATCH 6/8] Renamed len to dataSize --- src/main/msp/msp_server_fc.c | 38 ++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index 8dd830cd9a..60465f6f80 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -613,7 +613,7 @@ static uint32_t packFlightModeFlags(void) static bool processOutCommand(uint8_t cmdMSP) { uint32_t i; - const unsigned int len = currentPort->dataSize; + const unsigned int dataSize = currentPort->dataSize; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0; @@ -708,7 +708,7 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_NAME: { - const int nameLen = strlen(masterConfig.name); + const unsigned int nameLen = strlen(masterConfig.name); headSerialReply(nameLen); for (i = 0; i < nameLen; i++) { serialize8(masterConfig.name[i]); @@ -1160,7 +1160,7 @@ static bool processOutCommand(uint8_t cmdMSP) uint32_t readAddress = read32(); uint16_t readLength; bool useLegacyFormat; - if (len >= sizeof(uint32_t) + sizeof(uint16_t)) { + if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) { readLength = read16(); useLegacyFormat = false; } else { @@ -1315,7 +1315,7 @@ static bool processInCommand(uint8_t cmdMSP) uint32_t i; uint16_t tmp; uint8_t value; - const unsigned int len = currentPort->dataSize; + const unsigned int dataSize = currentPort->dataSize; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; @@ -1349,7 +1349,7 @@ static bool processInCommand(uint8_t cmdMSP) case MSP_SET_RAW_RC: #ifndef SKIP_RX_MSP { - uint8_t channelCount = len / sizeof(uint16_t); + uint8_t channelCount = dataSize / sizeof(uint16_t); if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { headSerialError(0); } else { @@ -1429,7 +1429,7 @@ static bool processInCommand(uint8_t cmdMSP) break; case MSP_SET_RC_TUNING: - if (len >= 10) { + if (dataSize >= 10) { currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcExpo8 = read8(); for (i = 0; i < 3; i++) { @@ -1441,10 +1441,10 @@ static bool processInCommand(uint8_t cmdMSP) currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->tpa_breakpoint = read16(); - if (len >= 11) { + if (dataSize >= 11) { currentControlRateProfile->rcYawExpo8 = read8(); } - if (len >= 12) { + if (dataSize >= 12) { currentControlRateProfile->rcYawRate8 = read8(); } } else { @@ -1488,7 +1488,7 @@ static bool processInCommand(uint8_t cmdMSP) break; case MSP_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS - if (len != 1 + sizeof(servoParam_t)) { + if (dataSize != 1 + sizeof(servoParam_t)) { headSerialError(0); break; } @@ -1585,7 +1585,7 @@ static bool processInCommand(uint8_t cmdMSP) #ifdef TRANSPONDER case MSP_SET_TRANSPONDER_CONFIG: - if (len != sizeof(masterConfig.transponderData)) { + if (dataSize != sizeof(masterConfig.transponderData)) { headSerialError(0); break; } @@ -1717,16 +1717,16 @@ static bool processInCommand(uint8_t cmdMSP) masterConfig.rxConfig.midrc = read16(); masterConfig.rxConfig.mincheck = read16(); masterConfig.rxConfig.spektrum_sat_bind = read8(); - if (len > 8) { + if (dataSize > 8) { masterConfig.rxConfig.rx_min_usec = read16(); masterConfig.rxConfig.rx_max_usec = read16(); } - if (len > 12) { + if (dataSize > 12) { masterConfig.rxConfig.rcInterpolation = read8(); masterConfig.rxConfig.rcInterpolationInterval = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); } - if (len > 16) { + if (dataSize > 16) { masterConfig.rxConfig.rx_spi_protocol = read8(); masterConfig.rxConfig.rx_spi_id = read32(); masterConfig.rxConfig.rx_spi_rf_channel_count = read8(); @@ -1786,12 +1786,12 @@ static bool processInCommand(uint8_t cmdMSP) { uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); - if (len % portConfigSize != 0) { + if (dataSize % portConfigSize != 0) { headSerialError(0); break; } - uint8_t remainingPortsInPacket = len / portConfigSize; + uint8_t remainingPortsInPacket = dataSize / portConfigSize; while (remainingPortsInPacket--) { uint8_t identifier = read8(); @@ -1825,7 +1825,7 @@ static bool processInCommand(uint8_t cmdMSP) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= LED_MAX_STRIP_LENGTH || len != (1 + 4)) { + if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) { headSerialError(0); break; } @@ -1879,13 +1879,13 @@ static bool processInCommand(uint8_t cmdMSP) masterConfig.gyro_soft_lpf_hz = read8(); currentProfile->pidProfile.dterm_lpf_hz = read16(); currentProfile->pidProfile.yaw_lpf_hz = read16(); - if (len > 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 (len > 13) { + if (dataSize > 13) { serialize16(masterConfig.gyro_soft_notch_hz_2); serialize16(masterConfig.gyro_soft_notch_cutoff_2); } @@ -1912,7 +1912,7 @@ static bool processInCommand(uint8_t cmdMSP) case MSP_SET_NAME: memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); - for (i = 0; i < MIN(MAX_NAME_LENGTH, len); i++) { + for (i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) { masterConfig.name[i] = read8(); } break; From ea85428e23ddf14f21545b3593c0219135126200 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 3 Oct 2016 08:01:13 +0100 Subject: [PATCH 7/8] Use mspPostProcessFn to do esc4way processing --- src/main/msp/msp_server_fc.c | 40 +++++++++++++++--------------------- 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index 60465f6f80..1fd10c76db 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -209,9 +209,6 @@ void mspRebootFn(mspPort_t *mspPort) UNUSED(mspPort); 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. systemReset(); // control should never return here. @@ -1304,6 +1301,22 @@ 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; } @@ -1846,27 +1859,6 @@ static bool processInCommand(uint8_t cmdMSP) } break; #endif - case MSP_REBOOT: - mspPostProcessFn = mspRebootFn; - 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); - msp4WayIfFn(currentPort); - break; -#endif case MSP_SET_ADVANCED_CONFIG : masterConfig.gyro_sync_denom = read8(); From c2dbaa4330f288396310e6239799486c98303a71 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 3 Oct 2016 22:52:34 +0100 Subject: [PATCH 8/8] Minor tidy --- src/main/io/serial_msp.h | 1 - src/main/msp/msp_server_fc.c | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 8859b44411..dd552da664 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -48,7 +48,6 @@ typedef struct mspPort_s { struct bufWriter_s; extern struct bufWriter_s *writer; - void mspSerialInit(void); void mspSerialProcess(void); void mspSerialAllocatePorts(void); diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c index 1fd10c76db..dbea23c3b7 100755 --- a/src/main/msp/msp_server_fc.c +++ b/src/main/msp/msp_server_fc.c @@ -204,7 +204,7 @@ void msp4WayIfFn(mspPort_t *mspPort) } #endif -void mspRebootFn(mspPort_t *mspPort) +static void mspRebootFn(mspPort_t *mspPort) { UNUSED(mspPort);