diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index b3eb46d564..44e924411c 100644 --- a/src/main/drivers/serial_escserial.c +++ b/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); -void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); -void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); -void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +enum { + TRAILING, + LEADING +}; + +#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) -{ - 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) +static void processTxStateEsc(escSerial_t *escSerial) { uint8_t mask; static uint8_t bitq=0, transmitStart=0; @@ -483,218 +523,7 @@ reload: } } -/*-----------------------BL*/ -/*********************************************/ - -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) +static 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) -{ - // start listening - return instance->txBufferHead == instance->txBufferTail; -} - -uint32_t escSerialTxBytesFree(const serialPort_t *instance) +static 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,18 +943,18 @@ 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++) { - if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) - { - first_output=i; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) { + first_output = i; break; } } //doesn't work with messy timertable - motor_output=first_output+output-1; - if (motor_output >=USABLE_TIMER_CHANNEL_COUNT) + motor_output = first_output + output; + 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 diff --git a/src/main/drivers/serial_escserial.c.orig b/src/main/drivers/serial_escserial.c.orig new file mode 100644 index 0000000000..f4095f8b99 --- /dev/null +++ b/src/main/drivers/serial_escserial.c.orig @@ -0,0 +1,1028 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_ESCSERIAL) + +#include "build/build_config.h" +#include "build/atomic.h" + +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/io.h" +#include "drivers/light_led.h" +#include "drivers/nvic.h" +#include "drivers/pwm_output.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 + +#define MAX_ESCSERIAL_PORTS 1 +static serialPort_t *escPort = NULL; +static serialPort_t *passPort = NULL; + +#define ICPOLARITY_RISING true +#define ICPOLARITY_FALLING false + +typedef struct escSerial_s { + serialPort_t port; + + IO_t rxIO; + IO_t txIO; + + const timerHardware_t *rxTimerHardware; + volatile uint8_t rxBuffer[ESCSERIAL_BUFFER_SIZE]; + const timerHardware_t *txTimerHardware; + volatile uint8_t txBuffer[ESCSERIAL_BUFFER_SIZE]; + +#ifdef USE_HAL_DRIVER + const TIM_HandleTypeDef *txTimerHandle; + const TIM_HandleTypeDef *rxTimerHandle; +#endif + + uint8_t isSearchingForStartBit; + uint8_t rxBitIndex; + uint8_t rxLastLeadingEdgeAtBitIndex; + uint8_t rxEdge; + + uint8_t isTransmittingData; + uint8_t isReceivingData; + int8_t bitsLeftToTransmit; + + uint16_t internalTxBuffer; // includes start and stop bits + uint16_t internalRxBuffer; // includes start and stop bits + + uint16_t receiveTimeout; + uint16_t transmissionErrors; + uint16_t receiveErrors; + + uint8_t escSerialPortIndex; + uint8_t mode; + uint8_t outputCount; + + timerCCHandlerRec_t timerCb; + timerCCHandlerRec_t edgeCb; +} escSerial_t; + +typedef struct { + IO_t io; + uint8_t inverted; +} escOutputs_t; + +escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS]; + +extern timerHardware_t* serialTimerHardware; + +<<<<<<< HEAD +escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS]; + +PG_REGISTER_WITH_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig, PG_ESCSERIAL_CONFIG, 0); + +#ifndef ESCSERIAL_TIMER_TX_PIN +#define ESCSERIAL_TIMER_TX_PIN NONE +#endif + +PG_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig, + .ioTag = IO_TAG(ESCSERIAL_TIMER_TX_PIN), +); + +void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +======= +const struct serialPortVTable escSerialVTable[]; + +escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS]; + +enum { + TRAILING, + LEADING +}; + +#define STOP_BIT_MASK (1 << 0) +#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1)) +>>>>>>> betaflight/master + +// XXX No TIM_DeInit equivalent in HAL driver??? +#ifdef USE_HAL_DRIVER +static void TIM_DeInit(TIM_TypeDef *tim) +{ + UNUSED(tim); +} +#endif + +static void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) +{ + if (escSerial->mode == PROTOCOL_KISSALL) + { + for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) { + uint8_t state_temp = state; + if (escOutputs[i].inverted) { + state_temp ^= ENABLE; + } + + if (state_temp) { + IOHi(escOutputs[i].io); + } else { + IOLo(escOutputs[i].io); + } + } + } + else + { + if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) { + state ^= ENABLE; + } + + if (state) { + IOHi(escSerial->txIO); + } else { + IOLo(escSerial->txIO); + } + } +} + +static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg) +{ + ioTag_t tag = timhw->tag; + + if (!tag) { + return; + } + + IOInit(IOGetByTag(tag), OWNER_MOTOR, 0); +#ifdef STM32F7 + IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction); +#else + IOConfigGPIO(IOGetByTag(tag), cfg); +#endif +} + +static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr) +{ +#ifdef STM32F10X + escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU); +#else + escSerialGPIOConfig(timerHardwarePtr, IOCFG_AF_PP_UP); +#endif + timerChClearCCFlag(timerHardwarePtr); + timerChITConfig(timerHardwarePtr,ENABLE); +} + + +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; + +<<<<<<< HEAD + escSerial->mode = mode; + escSerial->txTimerHardware = timerGetByTag(escSerialConfig()->ioTag, TIM_USE_ANY); + + if (!escSerial->txTimerHardware) { + return NULL; + } +======= +>>>>>>> betaflight/master + + //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; + uint32_t timerPeriod; + TIM_DeInit(timerHardwarePtr->tim); + do { + timerPeriod = clock / baud; + if (isTimerPeriodTooLarge(timerPeriod)) { + if (clock > 1) { + clock = clock / 2; // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200) + } else { + // TODO unable to continue, unable to determine clock and timerPeriods for the given baud + } + + } + } while (isTimerPeriodTooLarge(timerPeriod)); + + timerConfigure(timerHardwarePtr, timerPeriod, clock); + timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL); + 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 + TIM_DeInit(timerHardwarePtr->tim); + timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2); + timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); + timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); + timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); +} + +static void processTxStateEsc(escSerial_t *escSerial) +{ + uint8_t mask; + static uint8_t bitq=0, transmitStart=0; + if (escSerial->isReceivingData) { + return; + } + + if (transmitStart==0) + { + setTxSignalEsc(escSerial, 1); + } + if (!escSerial->isTransmittingData) { + char byteToSend; +reload: + if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { + // canreceive + transmitStart=0; + return; + } + + if (transmitStart<3) + { + if (transmitStart==0) + byteToSend = 0xff; + if (transmitStart==1) + byteToSend = 0xff; + if (transmitStart==2) + byteToSend = 0x7f; + transmitStart++; + } + else{ + // data to send + byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++]; + if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) { + escSerial->port.txBufferTail = 0; + } + } + + + // build internal buffer, data bits (MSB to LSB) + escSerial->internalTxBuffer = byteToSend; + escSerial->bitsLeftToTransmit = 8; + escSerial->isTransmittingData = true; + + //set output + escSerialOutputPortConfig(escSerial->rxTimerHardware); + return; + } + + if (escSerial->bitsLeftToTransmit) { + mask = escSerial->internalTxBuffer & 1; + if (mask) + { + if (bitq==0 || bitq==1) + { + setTxSignalEsc(escSerial, 1); + } + if (bitq==2 || bitq==3) + { + setTxSignalEsc(escSerial, 0); + } + } + else + { + if (bitq==0 || bitq==2) + { + setTxSignalEsc(escSerial, 1); + } + if (bitq==1 ||bitq==3) + { + setTxSignalEsc(escSerial, 0); + } + } + bitq++; + if (bitq>3) + { + escSerial->internalTxBuffer >>= 1; + escSerial->bitsLeftToTransmit--; + bitq=0; + if (escSerial->bitsLeftToTransmit==0) + { + goto reload; + } + } + return; + } + + if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { + escSerial->isTransmittingData = false; + escSerialInputPortConfig(escSerial->rxTimerHardware); + } +} + +static void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +{ + UNUSED(capture); + escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb); + + if (escSerial->isReceivingData) + { + escSerial->receiveTimeout++; + if (escSerial->receiveTimeout>8) + { + escSerial->isReceivingData=0; + escSerial->receiveTimeout=0; + timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_FALLING, 0); + } + } + + processTxStateEsc(escSerial); +} + +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; + static uint8_t bits=0; + static uint16_t bytes=0; + + escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb); + + //clear timer +#ifdef USE_HAL_DRIVER + __HAL_TIM_SetCounter(escSerial->rxTimerHandle, 0); +#else + TIM_SetCounter(escSerial->rxTimerHardware->tim,0); +#endif + + if (capture > 40 && capture < 90) + { + zerofirst++; + if (zerofirst>1) + { + zerofirst=0; + escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; + bits++; + } + } + else if (capture>90 && capture < 200) + { + zerofirst=0; + escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; + escSerial->internalRxBuffer |= 0x80; + bits++; + } + else + { + if (!escSerial->isReceivingData) + { + //start + //lets reset + + escSerial->isReceivingData = 1; + zerofirst=0; + bytes=0; + bits=1; + escSerial->internalRxBuffer = 0x80; + + timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_RISING, 0); + } + } + escSerial->receiveTimeout = 0; + + if (bits==8) + { + bits=0; + bytes++; + if (bytes>3) + { + extractAndStoreRxByteEsc(escSerial); + } + escSerial->internalRxBuffer=0; + } + +} + +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 = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]); + +#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; + } + + escSerial_t *s = (escSerial_t *)instance; + + return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1); +} + +static uint8_t escSerialReadByte(serialPort_t *instance) +{ + uint8_t ch; + + if ((instance->mode & MODE_RX) == 0) { + return 0; + } + + if (escSerialTotalBytesWaiting(instance) == 0) { + return 0; + } + + ch = instance->rxBuffer[instance->rxBufferTail]; + instance->rxBufferTail = (instance->rxBufferTail + 1) % instance->rxBufferSize; + return ch; +} + +static void escSerialWriteByte(serialPort_t *s, uint8_t ch) +{ + if ((s->mode & MODE_TX) == 0) { + return; + } + + s->txBuffer[s->txBufferHead] = ch; + s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize; +} + +static void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate) +{ + UNUSED(s); + UNUSED(baudRate); +} + +static void escSerialSetMode(serialPort_t *instance, portMode_t mode) +{ + instance->mode = mode; +} + +static uint32_t escSerialTxBytesFree(const serialPort_t *instance) +{ + if ((instance->mode & MODE_TX) == 0) { + return 0; + } + + escSerial_t *s = (escSerial_t *)instance; + + uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1); + + return (s->port.txBufferSize - 1) - bytesUsed; +} + +const struct serialPortVTable escSerialVTable[] = { + { + .serialWrite = escSerialWriteByte, + .serialTotalRxWaiting = escSerialTotalBytesWaiting, + .serialTotalTxFree = escSerialTxBytesFree, + .serialRead = escSerialReadByte, + .serialSetBaudRate = escSerialSetBaudRate, + .isSerialTransmitBufferEmpty = isEscSerialTransmitBufferEmpty, + .setMode = escSerialSetMode, + .writeBuf = NULL, + .beginWrite = NULL, + .endWrite = NULL + } +}; + +typedef enum { + IDLE, + HEADER_START, + HEADER_M, + HEADER_ARROW, + HEADER_SIZE, + HEADER_CMD, + COMMAND_RECEIVED +} mspState_e; + +typedef struct mspPort_s { + uint8_t offset; + uint8_t dataSize; + uint8_t checksum; + uint8_t indRX; + uint8_t inBuf[10]; + mspState_e c_state; + uint8_t cmdMSP; +} mspPort_t; + +static mspPort_t currentPort; + +static bool processExitCommand(uint8_t c) +{ + if (currentPort.c_state == IDLE) { + if (c == '$') { + currentPort.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) { + if (c > 10) { + currentPort.c_state = IDLE; + + } else { + currentPort.dataSize = c; + currentPort.offset = 0; + currentPort.checksum = 0; + currentPort.indRX = 0; + currentPort.checksum ^= c; + currentPort.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; + + if ((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0)) + { + currentPort.c_state = IDLE; + return true; + } + } else { + currentPort.c_state = IDLE; + } + } + return false; +} + + +void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode) +{ + bool exitEsc = false; + uint8_t motor_output = 0; + LED0_OFF; + LED1_OFF; + //StopPwmAllMotors(); + pwmDisableMotors(); + passPort = escPassthroughPort; + + uint32_t escBaudrate; + switch (mode) { + case PROTOCOL_KISS: + escBaudrate = BAUDRATE_KISS; + break; + case PROTOCOL_CASTLE: + escBaudrate = BAUDRATE_CASTLE; + break; + default: + escBaudrate = BAUDRATE_NORMAL; + break; + } + + if ((mode == PROTOCOL_KISS) && (output == 255)) { + motor_output = 255; + mode = PROTOCOL_KISSALL; + } + else { + uint8_t first_output = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) { + first_output = i; + break; + } + } + + //doesn't work with messy timertable + motor_output = first_output + output; + if (motor_output >= USABLE_TIMER_CHANNEL_COUNT) { + return; + } + } + + escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode); + + if (!escPort) { + return; + } + + uint8_t ch; + while (1) { + if (mode!=2) + { + if (serialRxBytesWaiting(escPort)) { + LED0_ON; + while (serialRxBytesWaiting(escPort)) + { + ch = serialRead(escPort); + serialWrite(escPassthroughPort, ch); + } + LED0_OFF; + } + } + if (serialRxBytesWaiting(escPassthroughPort)) { + LED1_ON; + while (serialRxBytesWaiting(escPassthroughPort)) + { + ch = serialRead(escPassthroughPort); + exitEsc = processExitCommand(ch); + if (exitEsc) + { + serialWrite(escPassthroughPort, 0x24); + serialWrite(escPassthroughPort, 0x4D); + serialWrite(escPassthroughPort, 0x3E); + serialWrite(escPassthroughPort, 0x00); + serialWrite(escPassthroughPort, 0xF4); + serialWrite(escPassthroughPort, 0xF4); + closeEscSerial(ESCSERIAL1, mode); + return; + } + if (mode==PROTOCOL_BLHELI) { + serialWrite(escPassthroughPort, ch); // blheli loopback + } + serialWrite(escPort, ch); + } + LED1_OFF; + } + if (mode != PROTOCOL_CASTLE) { + delay(5); + } + } +} + +#endif diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h index f2f430cb3a..bfcc50560c 100644 --- a/src/main/drivers/serial_escserial.h +++ b/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 { diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ad97c4ba2d..520d26ba1d 100755 --- a/src/main/fc/cli.c +++ b/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; - char out[9]; + uint32_t i; + char buf[RX_MAPPABLE_CHANNEL_COUNT + 1]; - len = strlen(cmdline); + uint32_t len = strlen(cmdline); + if (len == RX_MAPPABLE_CHANNEL_COUNT) { - if (len == 8) { - // uppercase it - for (uint32_t i = 0; i < 8; i++) - cmdline[i] = toupper((unsigned char)cmdline[i]); - 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]); + } + buf[i] = '\0'; + + 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 < 8; i++) - out[rxConfig()->rcmap[i]] = rcChannelLetters[i]; - out[i] = '\0'; - cliPrintLine(out); + + for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) { + buf[rxConfig()->rcmap[i]] = rcChannelLetters[i]; + } + + 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) - -#ifndef ALL_ESCS -#define ALL_ESCS 255 -#endif - -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"); +static int parseOutputIndex(char *pch, bool allowAllEscs) { + int outputIndex = atoi(pch); + if ((outputIndex >= 0) && (outputIndex < getMotorCount())) { + tfp_printf("Using output %d.\r\n", outputIndex); + } else if (allowAllEscs && outputIndex == ALL_MOTORS) { + tfp_printf("Using all outputs.\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); - if (escNumber == -1) { + escIndex = parseOutputIndex(pch, true); + 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); - if (escNumber == -1) { + escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS); + 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); - break; - case 1: - motor_value = atoi(pch); - break; + case 0: + motorIndex = parseOutputIndex(pch, true); + if (motorIndex == -1) { + return; + } + + break; + case 1: + 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 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 754abfca14..7123697b1f 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -220,9 +220,8 @@ void tryArm(void) return; } #ifdef USE_DSHOT - if (!feature(FEATURE_3D)) { - //TODO: Use BOXDSHOTREVERSE here - if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + if (isMotorProtocolDshot()) { + if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) { reverseMotors = false; for (unsigned index = 0; index < getMotorCount(); index++) { pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6e1af8e5dc..95cefe691b 100644 --- a/src/main/fc/fc_msp.c +++ b/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 - BME(BOX3DDISABLESWITCH); + if (feature(FEATURE_3D)) { + 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 diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index ee4da972d7..1957d3a542 100755 --- a/src/main/fc/fc_rc.c +++ b/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); } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 4cc6650340..8344326caa 100644 --- a/src/main/fc/rc_controls.c +++ b/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 { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 17c35f7e22..400f370a54 100644 --- a/src/main/fc/rc_modes.h +++ b/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; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 2480da3c6b..ad0ae8042d 100644 --- a/src/main/flight/mixer.h +++ b/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];