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];