Rebased onto #3469

src/main/drivers/serial_escserial.c

@@ -21,12 +21,6 @@
 
 #include "platform.h"
 
-typedef enum {
-    BAUDRATE_NORMAL = 19200,
-    BAUDRATE_KISS   = 38400,
-    BAUDRATE_CASTLE = 18880
-} escBaudRate_e;
-
 #if defined(USE_ESCSERIAL)
 
 #include "build/build_config.h"
@@ -38,17 +32,25 @@ typedef enum {
 #include "config/parameter_group_ids.h"
 
 #include "drivers/io.h"
-#include "drivers/nvic.h"
-#include "drivers/time.h"
-#include "timer.h"
-
-#include "serial.h"
-#include "serial_escserial.h"
 #include "drivers/light_led.h"
+#include "drivers/nvic.h"
 #include "drivers/pwm_output.h"
-#include "io/serial.h"
+#include "drivers/serial.h"
+#include "drivers/serial_escserial.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+
 #include "flight/mixer.h"
 
+#include "io/serial.h"
+
+
+typedef enum {
+    BAUDRATE_NORMAL = 19200,
+    BAUDRATE_KISS   = 38400,
+    BAUDRATE_CASTLE = 18880
+} escBaudRate_e;
+
 #define RX_TOTAL_BITS 10
 #define TX_TOTAL_BITS 10
 
@@ -107,9 +109,8 @@ typedef struct {
 escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS];
 
 extern timerHardware_t* serialTimerHardware;
-extern escSerial_t escSerialPorts[];
 
-extern const struct serialPortVTable escSerialVTable[];
+const struct serialPortVTable escSerialVTable[];
 
 escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS];
 
@@ -123,10 +124,13 @@ PG_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig,
     .ioTag = IO_TAG(ESCSERIAL_TIMER_TX_PIN),
 );
 
-void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+enum {
-void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+    TRAILING,
-void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+    LEADING
-void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+};
+
+#define STOP_BIT_MASK (1 << 0)
+#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
 
 // XXX No TIM_DeInit equivalent in HAL driver???
 #ifdef USE_HAL_DRIVER
@@ -136,7 +140,7 @@ static void TIM_DeInit(TIM_TypeDef *tim)
 }
 #endif
 
-void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
+static void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
 {
     if (escSerial->mode == PROTOCOL_KISSALL)
     {
@@ -183,7 +187,7 @@ static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg)
 #endif
 }
 
-void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
+static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
 {
 #ifdef STM32F10X
     escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
@@ -200,6 +204,150 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
     return timerPeriod > 0xFFFF;
 }
 
+static bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance)
+{
+    // start listening
+    return instance->txBufferHead == instance->txBufferTail;
+}
+
+static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
+{
+    escSerialGPIOConfig(timerHardwarePtr, IOCFG_OUT_PP);
+    timerChITConfig(timerHardwarePtr,DISABLE);
+}
+
+static void processTxStateBL(escSerial_t *escSerial)
+{
+    uint8_t mask;
+    if (escSerial->isReceivingData) {
+        return;
+    }
+
+    if (!escSerial->isTransmittingData) {
+        char byteToSend;
+        if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
+            // canreceive
+            return;
+        }
+
+        // data to send
+        byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
+        if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
+            escSerial->port.txBufferTail = 0;
+        }
+
+        // build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
+        escSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
+        escSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
+        escSerial->isTransmittingData = true;
+
+
+        //set output
+        if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) {
+            escSerialOutputPortConfig(escSerial->rxTimerHardware);
+        }
+        return;
+    }
+
+    if (escSerial->bitsLeftToTransmit) {
+        mask = escSerial->internalTxBuffer & 1;
+        escSerial->internalTxBuffer >>= 1;
+
+        setTxSignalEsc(escSerial, mask);
+        escSerial->bitsLeftToTransmit--;
+        return;
+    }
+
+    escSerial->isTransmittingData = false;
+    if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
+        if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE)
+        {
+            escSerialInputPortConfig(escSerial->rxTimerHardware);
+        }
+    }
+}
+
+static void extractAndStoreRxByteBL(escSerial_t *escSerial)
+{
+    if ((escSerial->port.mode & MODE_RX) == 0) {
+        return;
+    }
+
+    uint8_t haveStartBit = (escSerial->internalRxBuffer & START_BIT_MASK) == 0;
+    uint8_t haveStopBit = (escSerial->internalRxBuffer & STOP_BIT_MASK) == 1;
+
+    if (!haveStartBit || !haveStopBit) {
+        escSerial->receiveErrors++;
+        return;
+    }
+
+    uint8_t rxByte = (escSerial->internalRxBuffer >> 1) & 0xFF;
+
+    if (escSerial->port.rxCallback) {
+        escSerial->port.rxCallback(rxByte);
+    } else {
+        escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
+        escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
+    }
+}
+
+static void prepareForNextRxByteBL(escSerial_t *escSerial)
+{
+    // prepare for next byte
+    escSerial->rxBitIndex = 0;
+    escSerial->isSearchingForStartBit = true;
+    if (escSerial->rxEdge == LEADING) {
+        escSerial->rxEdge = TRAILING;
+        timerChConfigIC(
+            escSerial->rxTimerHardware,
+            (escSerial->port.options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0
+        );
+    }
+}
+
+static void applyChangedBitsBL(escSerial_t *escSerial)
+{
+    if (escSerial->rxEdge == TRAILING) {
+        uint8_t bitToSet;
+        for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) {
+            escSerial->internalRxBuffer |= 1 << bitToSet;
+        }
+    }
+}
+
+static void processRxStateBL(escSerial_t *escSerial)
+{
+    if (escSerial->isSearchingForStartBit) {
+        return;
+    }
+
+    escSerial->rxBitIndex++;
+
+    if (escSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
+        applyChangedBitsBL(escSerial);
+        return;
+    }
+
+    if (escSerial->rxBitIndex == RX_TOTAL_BITS) {
+
+        if (escSerial->rxEdge == TRAILING) {
+            escSerial->internalRxBuffer |= STOP_BIT_MASK;
+        }
+
+        extractAndStoreRxByteBL(escSerial);
+        prepareForNextRxByteBL(escSerial);
+    }
+}
+
+static void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
+{
+    UNUSED(capture);
+    escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
+
+    processTxStateBL(escSerial);
+    processRxStateBL(escSerial);
+}
+
 static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud)
 {
     uint32_t clock = SystemCoreClock/2;
@@ -222,6 +370,55 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
 }
 
+static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
+{
+    UNUSED(capture);
+
+    escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
+    bool inverted = escSerial->port.options & SERIAL_INVERTED;
+
+    if ((escSerial->port.mode & MODE_RX) == 0) {
+        return;
+    }
+
+    if (escSerial->isSearchingForStartBit) {
+        // Adjust the timing so it will interrupt on the middle.
+        // This is clobbers transmission, but it is okay because we are
+        // always half-duplex.
+#ifdef USE_HAL_DRIVER
+        __HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2);
+#else
+        TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2);
+#endif
+        if (escSerial->isTransmittingData) {
+            escSerial->transmissionErrors++;
+        }
+
+        timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
+        escSerial->rxEdge = LEADING;
+
+        escSerial->rxBitIndex = 0;
+        escSerial->rxLastLeadingEdgeAtBitIndex = 0;
+        escSerial->internalRxBuffer = 0;
+        escSerial->isSearchingForStartBit = false;
+        return;
+    }
+
+    if (escSerial->rxEdge == LEADING) {
+        escSerial->rxLastLeadingEdgeAtBitIndex = escSerial->rxBitIndex;
+    }
+
+    applyChangedBitsBL(escSerial);
+
+    if (escSerial->rxEdge == TRAILING) {
+        escSerial->rxEdge = LEADING;
+        timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
+    } else {
+        escSerial->rxEdge = TRAILING;
+        timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
+    }
+}
+
 static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
 {
     // start bit is usually a FALLING signal
@@ -232,164 +429,7 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
 }
 
-static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
+static void processTxStateEsc(escSerial_t *escSerial)
-{
-    uint32_t timerPeriod = 34;
-    TIM_DeInit(timerHardwarePtr->tim);
-    timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
-    timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
-    timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
-}
-
-static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
-{
-    // start bit is usually a FALLING signal
-    TIM_DeInit(timerHardwarePtr->tim);
-    timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
-    timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0);
-    timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
-    timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
-}
-
-static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
-{
-    escSerialGPIOConfig(timerHardwarePtr, IOCFG_OUT_PP);
-    timerChITConfig(timerHardwarePtr,DISABLE);
-}
-
-static void resetBuffers(escSerial_t *escSerial)
-{
-    escSerial->port.rxBufferSize = ESCSERIAL_BUFFER_SIZE;
-    escSerial->port.rxBuffer = escSerial->rxBuffer;
-    escSerial->port.rxBufferTail = 0;
-    escSerial->port.rxBufferHead = 0;
-
-    escSerial->port.txBuffer = escSerial->txBuffer;
-    escSerial->port.txBufferSize = ESCSERIAL_BUFFER_SIZE;
-    escSerial->port.txBufferTail = 0;
-    escSerial->port.txBufferHead = 0;
-}
-
-serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode)
-{
-    escSerial_t *escSerial = &(escSerialPorts[portIndex]);
-
-    if (mode != PROTOCOL_KISSALL) {
-        escSerial->rxTimerHardware = &(timerHardware[output]);
-#ifdef USE_HAL_DRIVER
-        escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
-#endif
-    }
-
-    escSerial->mode = mode;
-    escSerial->txTimerHardware = timerGetByTag(escSerialConfig()->ioTag, TIM_USE_ANY);
-
-    if (!escSerial->txTimerHardware) {
-        return NULL;
-    }
-
-#ifdef USE_HAL_DRIVER
-    escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
-#endif
-
-    escSerial->port.vTable = escSerialVTable;
-    escSerial->port.baudRate = baud;
-    escSerial->port.mode = MODE_RXTX;
-    escSerial->port.options = options;
-    escSerial->port.rxCallback = callback;
-
-    resetBuffers(escSerial);
-
-    escSerial->isTransmittingData = false;
-
-    escSerial->isSearchingForStartBit = true;
-    escSerial->rxBitIndex = 0;
-
-    escSerial->transmissionErrors = 0;
-    escSerial->receiveErrors = 0;
-    escSerial->receiveTimeout = 0;
-
-    escSerial->escSerialPortIndex = portIndex;
-
-    if (mode != PROTOCOL_KISSALL)
-    {
-        escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag);
-        escSerialInputPortConfig(escSerial->rxTimerHardware);
-        setTxSignalEsc(escSerial, ENABLE);
-    }
-    delay(50);
-
-    if (mode==PROTOCOL_SIMONK) {
-        escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
-        escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
-    }
-    else if (mode==PROTOCOL_BLHELI) {
-        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
-        serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
-    }
-    else if (mode==PROTOCOL_KISS) {
-        escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used
-        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
-    }
-    else if (mode==PROTOCOL_KISSALL) {
-        escSerial->outputCount = 0;
-        memset(&escOutputs, 0, sizeof(escOutputs));
-        pwmOutputPort_t *pwmMotors = pwmGetMotors();
-        for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
-            if (pwmMotors[i].enabled) {
-                if (pwmMotors[i].io != IO_NONE) {
-                    for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) {
-                        if (pwmMotors[i].io == IOGetByTag(timerHardware[j].tag))
-                        {
-                            escSerialOutputPortConfig(&timerHardware[j]);
-                            if (timerHardware[j].output & TIMER_OUTPUT_INVERTED) {
-                                escOutputs[escSerial->outputCount].inverted = 1;
-                            }
-                            break;
-                        }
-                    }
-                    escOutputs[escSerial->outputCount].io = pwmMotors[i].io;
-                    escSerial->outputCount++;
-                }
-            }
-        }
-        setTxSignalEsc(escSerial, ENABLE);
-        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
-    }
-    else if (mode == PROTOCOL_CASTLE){
-        escSerialOutputPortConfig(escSerial->rxTimerHardware);
-        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
-        serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
-    }
-    return &escSerial->port;
-}
-
-
-void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
-{
-    timerChClearCCFlag(timerHardwarePtr);
-    timerChITConfig(timerHardwarePtr,DISABLE);
-    escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
-}
-
-
-void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode)
-{
-    escSerial_t *escSerial = &(escSerialPorts[portIndex]);
-
-    if (mode != PROTOCOL_KISSALL) {
-        escSerialInputPortDeConfig(escSerial->rxTimerHardware);
-        timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
-        TIM_DeInit(escSerial->rxTimerHardware->tim);
-    }
-
-    timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
-    TIM_DeInit(escSerial->txTimerHardware->tim);
-}
-
-/*********************************************/
-
-void processTxStateEsc(escSerial_t *escSerial)
 {
     uint8_t mask;
     static uint8_t bitq=0, transmitStart=0;
@@ -483,218 +523,7 @@ reload:
     }
 }
 
-/*-----------------------BL*/
+static void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
-/*********************************************/
-
-void processTxStateBL(escSerial_t *escSerial)
-{
-    uint8_t mask;
-    if (escSerial->isReceivingData) {
-        return;
-    }
-
-    if (!escSerial->isTransmittingData) {
-        char byteToSend;
-        if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
-            // canreceive
-            return;
-        }
-
-        // data to send
-        byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
-        if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
-            escSerial->port.txBufferTail = 0;
-        }
-
-        // build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
-        escSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
-        escSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
-        escSerial->isTransmittingData = true;
-
-
-        //set output
-        if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) {
-            escSerialOutputPortConfig(escSerial->rxTimerHardware);
-        }
-        return;
-    }
-
-    if (escSerial->bitsLeftToTransmit) {
-        mask = escSerial->internalTxBuffer & 1;
-        escSerial->internalTxBuffer >>= 1;
-
-        setTxSignalEsc(escSerial, mask);
-        escSerial->bitsLeftToTransmit--;
-        return;
-    }
-
-    escSerial->isTransmittingData = false;
-    if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
-        if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE)
-        {
-            escSerialInputPortConfig(escSerial->rxTimerHardware);
-        }
-    }
-}
-
-
-
-enum {
-    TRAILING,
-    LEADING
-};
-
-void applyChangedBitsBL(escSerial_t *escSerial)
-{
-    if (escSerial->rxEdge == TRAILING) {
-        uint8_t bitToSet;
-        for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) {
-            escSerial->internalRxBuffer |= 1 << bitToSet;
-        }
-    }
-}
-
-void prepareForNextRxByteBL(escSerial_t *escSerial)
-{
-    // prepare for next byte
-    escSerial->rxBitIndex = 0;
-    escSerial->isSearchingForStartBit = true;
-    if (escSerial->rxEdge == LEADING) {
-        escSerial->rxEdge = TRAILING;
-        timerChConfigIC(
-            escSerial->rxTimerHardware,
-            (escSerial->port.options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0
-        );
-    }
-}
-
-#define STOP_BIT_MASK (1 << 0)
-#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
-
-void extractAndStoreRxByteBL(escSerial_t *escSerial)
-{
-    if ((escSerial->port.mode & MODE_RX) == 0) {
-        return;
-    }
-
-    uint8_t haveStartBit = (escSerial->internalRxBuffer & START_BIT_MASK) == 0;
-    uint8_t haveStopBit = (escSerial->internalRxBuffer & STOP_BIT_MASK) == 1;
-
-    if (!haveStartBit || !haveStopBit) {
-        escSerial->receiveErrors++;
-        return;
-    }
-
-    uint8_t rxByte = (escSerial->internalRxBuffer >> 1) & 0xFF;
-
-    if (escSerial->port.rxCallback) {
-        escSerial->port.rxCallback(rxByte);
-    } else {
-        escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
-        escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
-    }
-}
-
-void processRxStateBL(escSerial_t *escSerial)
-{
-    if (escSerial->isSearchingForStartBit) {
-        return;
-    }
-
-    escSerial->rxBitIndex++;
-
-    if (escSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
-        applyChangedBitsBL(escSerial);
-        return;
-    }
-
-    if (escSerial->rxBitIndex == RX_TOTAL_BITS) {
-
-        if (escSerial->rxEdge == TRAILING) {
-            escSerial->internalRxBuffer |= STOP_BIT_MASK;
-        }
-
-        extractAndStoreRxByteBL(escSerial);
-        prepareForNextRxByteBL(escSerial);
-    }
-}
-
-void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
-{
-    UNUSED(capture);
-    escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
-
-    processTxStateBL(escSerial);
-    processRxStateBL(escSerial);
-}
-
-void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
-{
-    UNUSED(capture);
-
-    escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
-    bool inverted = escSerial->port.options & SERIAL_INVERTED;
-
-    if ((escSerial->port.mode & MODE_RX) == 0) {
-        return;
-    }
-
-    if (escSerial->isSearchingForStartBit) {
-        // Adjust the timing so it will interrupt on the middle.
-        // This is clobbers transmission, but it is okay because we are
-        // always half-duplex.
-#ifdef USE_HAL_DRIVER
-        __HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2);
-#else
-        TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2);
-#endif
-        if (escSerial->isTransmittingData) {
-            escSerial->transmissionErrors++;
-        }
-
-        timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
-        escSerial->rxEdge = LEADING;
-
-        escSerial->rxBitIndex = 0;
-        escSerial->rxLastLeadingEdgeAtBitIndex = 0;
-        escSerial->internalRxBuffer = 0;
-        escSerial->isSearchingForStartBit = false;
-        return;
-    }
-
-    if (escSerial->rxEdge == LEADING) {
-        escSerial->rxLastLeadingEdgeAtBitIndex = escSerial->rxBitIndex;
-    }
-
-    applyChangedBitsBL(escSerial);
-
-    if (escSerial->rxEdge == TRAILING) {
-        escSerial->rxEdge = LEADING;
-        timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
-    } else {
-        escSerial->rxEdge = TRAILING;
-        timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
-    }
-}
-/*-------------------------BL*/
-
-void extractAndStoreRxByteEsc(escSerial_t *escSerial)
-{
-    if ((escSerial->port.mode & MODE_RX) == 0) {
-        return;
-    }
-
-    uint8_t rxByte = (escSerial->internalRxBuffer) & 0xFF;
-
-    if (escSerial->port.rxCallback) {
-        escSerial->port.rxCallback(rxByte);
-    } else {
-        escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
-        escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
-    }
-}
-
-void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 {
     UNUSED(capture);
     escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
@@ -710,11 +539,35 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
         }
     }
 
-
     processTxStateEsc(escSerial);
 }
 
-void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
+static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
+{
+    uint32_t timerPeriod = 34;
+    TIM_DeInit(timerHardwarePtr->tim);
+    timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
+    timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
+    timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
+}
+
+static void extractAndStoreRxByteEsc(escSerial_t *escSerial)
+{
+    if ((escSerial->port.mode & MODE_RX) == 0) {
+        return;
+    }
+
+    uint8_t rxByte = (escSerial->internalRxBuffer) & 0xFF;
+
+    if (escSerial->port.rxCallback) {
+        escSerial->port.rxCallback(rxByte);
+    } else {
+        escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
+        escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
+    }
+}
+
+static void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 {
     UNUSED(capture);
     static uint8_t zerofirst=0;
@@ -778,7 +631,143 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture
 
 }
 
-uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
+static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
+{
+    // start bit is usually a FALLING signal
+    TIM_DeInit(timerHardwarePtr->tim);
+    timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
+    timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0);
+    timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
+    timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
+}
+
+static void resetBuffers(escSerial_t *escSerial)
+{
+    escSerial->port.rxBufferSize = ESCSERIAL_BUFFER_SIZE;
+    escSerial->port.rxBuffer = escSerial->rxBuffer;
+    escSerial->port.rxBufferTail = 0;
+    escSerial->port.rxBufferHead = 0;
+
+    escSerial->port.txBuffer = escSerial->txBuffer;
+    escSerial->port.txBufferSize = ESCSERIAL_BUFFER_SIZE;
+    escSerial->port.txBufferTail = 0;
+    escSerial->port.txBufferHead = 0;
+}
+
+static serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode)
+{
+    escSerial_t *escSerial = &(escSerialPorts[portIndex]);
+
+    if (mode != PROTOCOL_KISSALL) {
+        escSerial->rxTimerHardware = &(timerHardware[output]);
+#ifdef USE_HAL_DRIVER
+        escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
+#endif
+    }
+
+    escSerial->mode = mode;
+    escSerial->txTimerHardware = timerGetByTag(escSerialConfig()->ioTag, TIM_USE_ANY);
+
+#ifdef USE_HAL_DRIVER
+    escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
+#endif
+
+    escSerial->port.vTable = escSerialVTable;
+    escSerial->port.baudRate = baud;
+    escSerial->port.mode = MODE_RXTX;
+    escSerial->port.options = options;
+    escSerial->port.rxCallback = callback;
+
+    resetBuffers(escSerial);
+
+    escSerial->isTransmittingData = false;
+
+    escSerial->isSearchingForStartBit = true;
+    escSerial->rxBitIndex = 0;
+
+    escSerial->transmissionErrors = 0;
+    escSerial->receiveErrors = 0;
+    escSerial->receiveTimeout = 0;
+
+    escSerial->escSerialPortIndex = portIndex;
+
+    if (mode != PROTOCOL_KISSALL)
+    {
+        escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag);
+        escSerialInputPortConfig(escSerial->rxTimerHardware);
+        setTxSignalEsc(escSerial, ENABLE);
+    }
+    delay(50);
+
+    if (mode==PROTOCOL_SIMONK) {
+        escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
+        escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
+    }
+    else if (mode==PROTOCOL_BLHELI) {
+        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
+        serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
+    }
+    else if (mode==PROTOCOL_KISS) {
+        escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used
+        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
+    }
+    else if (mode==PROTOCOL_KISSALL) {
+        escSerial->outputCount = 0;
+        memset(&escOutputs, 0, sizeof(escOutputs));
+        pwmOutputPort_t *pwmMotors = pwmGetMotors();
+        for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+            if (pwmMotors[i].enabled) {
+                if (pwmMotors[i].io != IO_NONE) {
+                    for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) {
+                        if (pwmMotors[i].io == IOGetByTag(timerHardware[j].tag))
+                        {
+                            escSerialOutputPortConfig(&timerHardware[j]);
+                            if (timerHardware[j].output & TIMER_OUTPUT_INVERTED) {
+                                escOutputs[escSerial->outputCount].inverted = 1;
+                            }
+                            break;
+                        }
+                    }
+                    escOutputs[escSerial->outputCount].io = pwmMotors[i].io;
+                    escSerial->outputCount++;
+                }
+            }
+        }
+        setTxSignalEsc(escSerial, ENABLE);
+        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
+    }
+    else if (mode == PROTOCOL_CASTLE){
+        escSerialOutputPortConfig(escSerial->rxTimerHardware);
+        serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
+        serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
+    }
+    return &escSerial->port;
+}
+
+
+static void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
+{
+    timerChClearCCFlag(timerHardwarePtr);
+    timerChITConfig(timerHardwarePtr,DISABLE);
+    escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
+}
+
+
+static void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode)
+{
+    escSerial_t *escSerial = &(escSerialPorts[portIndex]);
+
+    if (mode != PROTOCOL_KISSALL) {
+        escSerialInputPortDeConfig(escSerial->rxTimerHardware);
+        timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
+        TIM_DeInit(escSerial->rxTimerHardware->tim);
+    }
+
+    timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
+    TIM_DeInit(escSerial->txTimerHardware->tim);
+}
+
+static uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
 {
     if ((instance->mode & MODE_RX) == 0) {
         return 0;
@@ -789,7 +778,7 @@ uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
     return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1);
 }
 
-uint8_t escSerialReadByte(serialPort_t *instance)
+static uint8_t escSerialReadByte(serialPort_t *instance)
 {
     uint8_t ch;
 
@@ -806,7 +795,7 @@ uint8_t escSerialReadByte(serialPort_t *instance)
     return ch;
 }
 
-void escSerialWriteByte(serialPort_t *s, uint8_t ch)
+static void escSerialWriteByte(serialPort_t *s, uint8_t ch)
 {
     if ((s->mode & MODE_TX) == 0) {
         return;
@@ -816,24 +805,18 @@ void escSerialWriteByte(serialPort_t *s, uint8_t ch)
     s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
 }
 
-void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
+static void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
 {
     UNUSED(s);
     UNUSED(baudRate);
 }
 
-void escSerialSetMode(serialPort_t *instance, portMode_t mode)
+static void escSerialSetMode(serialPort_t *instance, portMode_t mode)
 {
     instance->mode = mode;
 }
 
-bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance)
+static uint32_t escSerialTxBytesFree(const serialPort_t *instance)
-{
-    // start listening
-    return instance->txBufferHead == instance->txBufferTail;
-}
-
-uint32_t escSerialTxBytesFree(const serialPort_t *instance)
 {
     if ((instance->mode & MODE_TX) == 0) {
         return 0;
@@ -861,20 +844,6 @@ const struct serialPortVTable escSerialVTable[] = {
     }
 };
 
-void escSerialInitialize()
-{
-   //StopPwmAllMotors();
-   pwmDisableMotors();
-
-   for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
-       // set outputs to pullup
-       if (timerHardware[i].output & TIMER_OUTPUT_ENABLED)
-       {
-           escSerialGPIOConfig(&timerHardware[i], IOCFG_IPU); //GPIO_Mode_IPU
-       }
-   }
-}
-
 typedef enum {
     IDLE,
     HEADER_START,
@@ -897,7 +866,7 @@ typedef struct mspPort_s {
 
 static mspPort_t currentPort;
 
-static bool ProcessExitCommand(uint8_t c)
+static bool processExitCommand(uint8_t c)
 {
     if (currentPort.c_state == IDLE) {
         if (c == '$') {
@@ -945,7 +914,6 @@ static bool ProcessExitCommand(uint8_t c)
 }
 
 
-// mode 0=sk, 1=bl, 2=ki   output=timerHardware PWM channel.
 void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode)
 {
     bool exitEsc = false;
@@ -975,19 +943,19 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
     }
     else {
         uint8_t first_output = 0;
-        for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+        for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
-            if (timerHardware[i].output & TIMER_OUTPUT_ENABLED)
+            if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) {
-            {
+                first_output = i;
-                first_output=i;
                 break;
             }
         }
 
         //doesn't work with messy timertable
-        motor_output=first_output+output-1;
+        motor_output = first_output + output;
-        if (motor_output >=USABLE_TIMER_CHANNEL_COUNT)
+        if (motor_output >= USABLE_TIMER_CHANNEL_COUNT) {
             return;
         }
+    }
 
     escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode);
 
@@ -1014,7 +982,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
             while (serialRxBytesWaiting(escPassthroughPort))
             {
                 ch = serialRead(escPassthroughPort);
-                exitEsc = ProcessExitCommand(ch);
+                exitEsc = processExitCommand(ch);
                 if (exitEsc)
                 {
                     serialWrite(escPassthroughPort, 0x24);
@@ -1039,5 +1007,4 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
     }
 }
 
-
 #endif

src/main/drivers/serial_escserial.h

@@ -33,19 +33,7 @@ typedef enum {
     PROTOCOL_COUNT
 } escProtocol_e;
 
-#define ALL_ESCS 255
-
-serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode);
-
 // serialPort API
-void escSerialWriteByte(serialPort_t *instance, uint8_t ch);
-uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance);
-uint32_t escSerialTxBytesFree(const serialPort_t *instance);
-uint8_t escSerialReadByte(serialPort_t *instance);
-void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
-bool isEscSerialTransmitBufferEmpty(const serialPort_t *s);
-
-void escSerialInitialize();
 void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode);
 
 typedef struct escSerialConfig_s {

src/main/fc/cli.c

@@ -2097,31 +2097,41 @@ static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfi
     cliDumpPrintLinef(dumpMask, equalsDefault, formatMap, buf);
 }
 
+
 static void cliMap(char *cmdline)
 {
-    uint32_t len;
+    uint32_t i;
-    char out[9];
+    char buf[RX_MAPPABLE_CHANNEL_COUNT + 1];
 
-    len = strlen(cmdline);
+    uint32_t len = strlen(cmdline);
+    if (len == RX_MAPPABLE_CHANNEL_COUNT) {
 
-    if (len == 8) {
+        for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
-        // uppercase it
+            buf[i] = toupper((unsigned char)cmdline[i]);
-        for (uint32_t i = 0; i < 8; i++)
+        }
-            cmdline[i] = toupper((unsigned char)cmdline[i]);
+        buf[i] = '\0';
-        for (uint32_t i = 0; i < 8; i++) {
+
-            if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
+        for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
+            buf[i] = toupper((unsigned char)cmdline[i]);
+
+            if (strchr(rcChannelLetters, buf[i]) && !strchr(buf + i + 1, buf[i]))
                 continue;
+
             cliShowParseError();
             return;
         }
-        parseRcChannels(cmdline, rxConfigMutable());
+        parseRcChannels(buf, rxConfigMutable());
+    } else if (len > 0) {
+        cliShowParseError();
+        return;
     }
-    cliPrint("Map: ");
+
-    uint32_t i;
+    for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
-    for (i = 0; i < 8; i++)
+        buf[rxConfig()->rcmap[i]] = rcChannelLetters[i];
-        out[rxConfig()->rcmap[i]] = rcChannelLetters[i];
+    }
-    out[i] = '\0';
+
-    cliPrintLine(out);
+    buf[i] = '\0';
+    cliPrintLinef("map %s", buf);
 }
 
 static char *checkCommand(char *cmdLine, const char *command)
@@ -2186,27 +2196,20 @@ static void cliGpsPassthrough(char *cmdline)
 }
 #endif
 
-#if defined(USE_ESCSERIAL) || defined(USE_DSHOT)
+static int parseOutputIndex(char *pch, bool allowAllEscs) {
-
+    int outputIndex = atoi(pch);
-#ifndef ALL_ESCS
+    if ((outputIndex >= 0) && (outputIndex < getMotorCount())) {
-#define ALL_ESCS 255
+        tfp_printf("Using output %d.\r\n", outputIndex);
-#endif
+    } else if (allowAllEscs && outputIndex == ALL_MOTORS) {
-
+        tfp_printf("Using all outputs.\r\n");
-static int parseEscNumber(char *pch, bool allowAllEscs) {
-    int escNumber = atoi(pch);
-    if ((escNumber >= 0) && (escNumber < getMotorCount())) {
-        tfp_printf("Programming on ESC %d.\r\n", escNumber);
-    } else if (allowAllEscs && escNumber == ALL_ESCS) {
-        tfp_printf("Programming on all ESCs.\r\n");
     } else {
-        tfp_printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1);
+        tfp_printf("Invalid output number, range: 0 to %d.\r\n", getMotorCount() - 1);
 
         return -1;
     }
 
-    return escNumber;
+    return outputIndex;
 }
-#endif
 
 #ifdef USE_DSHOT
 static void cliDshotProg(char *cmdline)
@@ -2220,12 +2223,12 @@ static void cliDshotProg(char *cmdline)
     char *saveptr;
     char *pch = strtok_r(cmdline, " ", &saveptr);
     int pos = 0;
-    int escNumber = 0;
+    int escIndex = 0;
     while (pch != NULL) {
         switch (pos) {
             case 0:
-                escNumber = parseEscNumber(pch, true);
+                escIndex = parseOutputIndex(pch, true);
-                if (escNumber == -1) {
+                if (escIndex == -1) {
                     return;
                 }
 
@@ -2235,12 +2238,12 @@ static void cliDshotProg(char *cmdline)
 
                 int command = atoi(pch);
                 if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
-                    if (escNumber == ALL_ESCS) {
+                    if (escIndex == ALL_MOTORS) {
                         for (unsigned i = 0; i < getMotorCount(); i++) {
                             pwmWriteDshotCommand(i, command);
                         }
                     } else {
-                        pwmWriteDshotCommand(escNumber, command);
+                        pwmWriteDshotCommand(escIndex, command);
                     }
 
                     if (command <= 5) {
@@ -2276,7 +2279,7 @@ static void cliEscPassthrough(char *cmdline)
     char *pch = strtok_r(cmdline, " ", &saveptr);
     int pos = 0;
     uint8_t mode = 0;
-    int escNumber = 0;
+    int escIndex = 0;
     while (pch != NULL) {
         switch (pos) {
             case 0:
@@ -2295,8 +2298,8 @@ static void cliEscPassthrough(char *cmdline)
                 }
                 break;
             case 1:
-                escNumber = parseEscNumber(pch, mode == PROTOCOL_KISS);
+                escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS);
-                if (escNumber == -1) {
+                if (escIndex == -1) {
                     return;
                 }
 
@@ -2313,7 +2316,7 @@ static void cliEscPassthrough(char *cmdline)
         pch = strtok_r(NULL, " ", &saveptr);
     }
 
-    escEnablePassthrough(cliPort, escNumber, mode);
+    escEnablePassthrough(cliPort, escIndex, mode);
 }
 #endif
 
@@ -2355,46 +2358,57 @@ static void cliMixer(char *cmdline)
 
 static void cliMotor(char *cmdline)
 {
-    int motor_index = 0;
-    int motor_value = 0;
-    int index = 0;
-    char *pch = NULL;
-    char *saveptr;
-
     if (isEmpty(cmdline)) {
         cliShowParseError();
+
         return;
     }
 
-    pch = strtok_r(cmdline, " ", &saveptr);
+    int motorIndex;
+    int motorValue;
+
+    char *saveptr;
+    char *pch = strtok_r(cmdline, " ", &saveptr);
+    int index = 0;
     while (pch != NULL) {
         switch (index) {
         case 0:
-                motor_index = atoi(pch);
+            motorIndex = parseOutputIndex(pch, true);
+            if (motorIndex == -1) {
+                return;
+            }
+
             break;
         case 1:
-                motor_value = atoi(pch);
+            motorValue = atoi(pch);
+
             break;
         }
         index++;
         pch = strtok_r(NULL, " ", &saveptr);
     }
 
-    if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) {
-        cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1);
-        return;
-    }
-
     if (index == 2) {
-        if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
+        if (motorValue < PWM_RANGE_MIN || motorValue > PWM_RANGE_MAX) {
             cliShowArgumentRangeError("value", 1000, 2000);
         } else {
-            motor_disarmed[motor_index] = convertExternalToMotor(motor_value);
+            uint32_t motorOutputValue = convertExternalToMotor(motorValue);
 
-            cliPrintLinef("motor %d: %d", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
+            if (motorIndex != ALL_MOTORS) {
-        }
+                motor_disarmed[motorIndex] = motorOutputValue;
+
+                cliPrintLinef("motor %d: %d", motorIndex, motorOutputValue);
+            } else  {
+                for (int i = 0; i < getMotorCount(); i++) {
+                    motor_disarmed[i] = motorOutputValue;
                 }
 
+                cliPrintLinef("all motors: %d", motorOutputValue);
+            }
+        }
+    } else {
+        cliShowParseError();
+    }
 }
 
 #ifndef MINIMAL_CLI

src/main/fc/fc_core.c

@@ -220,9 +220,8 @@ void tryArm(void)
             return;
         }
 #ifdef USE_DSHOT
-        if (!feature(FEATURE_3D)) {
+        if (isMotorProtocolDshot()) {
-            //TODO: Use BOXDSHOTREVERSE here
+            if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) {
-            if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
                 reverseMotors = false;
                 for (unsigned index = 0; index < getMotorCount(); index++) {
                     pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);

src/main/fc/fc_msp.c

@@ -150,12 +150,13 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXBLACKBOX, "BLACKBOX", 26 },
     { BOXFAILSAFE, "FAILSAFE", 27 },
     { BOXAIRMODE, "AIR MODE", 28 },
-    { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
+    { BOX3DDISABLE, "DISABLE 3D", 29},
     { BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
     { BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
     { BOXCAMERA1, "CAMERA CONTROL 1", 32},
     { BOXCAMERA2, "CAMERA CONTROL 2", 33},
     { BOXCAMERA3, "CAMERA CONTROL 3", 34 },
+    { BOXDSHOTREVERSE, "DSHOT REVERSE MOTORS", 35 },
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -193,7 +194,7 @@ typedef enum {
 #define ESC_4WAY 0xff
 
 uint8_t escMode;
-uint8_t escPortIndex = 0;
+uint8_t escPortIndex;
 
 #ifdef USE_ESCSERIAL
 static void mspEscPassthroughFn(serialPort_t *serialPort)
@@ -232,7 +233,7 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
     case PROTOCOL_KISS:
     case PROTOCOL_KISSALL:
     case PROTOCOL_CASTLE:
-        if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_ESCS)) {
+        if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_MOTORS)) {
             sbufWriteU8(dst, 1);
 
             if (mspPostProcessFn) {
@@ -396,8 +397,13 @@ void initActiveBoxIds(void)
 
     BME(BOXFPVANGLEMIX);
 
-    //TODO: Split this into BOX3DDISABLESWITCH and BOXDSHOTREVERSE
+    if (feature(FEATURE_3D)) {
-    BME(BOX3DDISABLESWITCH);
+        BME(BOX3DDISABLE);
+    }
+
+    if (isMotorProtocolDshot()) {
+        BME(BOXDSHOTREVERSE);
+    }
 
     if (feature(FEATURE_SERVO_TILT)) {
         BME(BOXCAMSTAB);
@@ -469,7 +475,7 @@ static int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
     const uint64_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON)
         | BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD)
         | BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE)
-        | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX);
+        | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXDSHOTREVERSE) | BM(BOX3DDISABLE);
     STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes);
     for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
         if ((rcModeCopyMask & BM(i))    // mode copy is enabled

src/main/fc/fc_rc.c

@@ -306,7 +306,7 @@ void updateRcCommands(void)
 
     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 
-    if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
+    if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLE) && !failsafeIsActive()) {
         fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
         rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
     }

src/main/fc/rc_controls.c

@@ -103,7 +103,7 @@ bool areSticksInApModePosition(uint16_t ap_mode)
 
 throttleStatus_e calculateThrottleStatus(void)
 {
-    if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
+    if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLE)) {
         if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)))
             return THROTTLE_LOW;
     } else {

src/main/fc/rc_modes.h

@@ -51,12 +51,13 @@ typedef enum {
     BOXBLACKBOX,
     BOXFAILSAFE,
     BOXAIRMODE,
-    BOX3DDISABLESWITCH,
+    BOX3DDISABLE,
     BOXFPVANGLEMIX,
     BOXBLACKBOXERASE,
     BOXCAMERA1,
     BOXCAMERA2,
     BOXCAMERA3,
+    BOXDSHOTREVERSE,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/flight/mixer.h

@@ -99,6 +99,8 @@ PG_DECLARE(motorConfig_t, motorConfig);
 
 #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
 
+#define ALL_MOTORS 255
+
 extern const mixer_t mixers[];
 extern float motor[MAX_SUPPORTED_MOTORS];
 extern float motor_disarmed[MAX_SUPPORTED_MOTORS];