diff --git a/Makefile b/Makefile
index eb3a7cc28..d0c9f31c7 100644
--- a/Makefile
+++ b/Makefile
@@ -547,6 +547,7 @@ HIGHEND_SRC = \
common/colorconversion.c \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
+ drivers/serial_escserial.c \
drivers/serial_softserial.c \
drivers/sonar_hcsr04.c \
flight/gtune.c \
diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c
new file mode 100644
index 000000000..b93aaf789
--- /dev/null
+++ b/src/main/drivers/serial_escserial.c
@@ -0,0 +1,898 @@
+/*
+ * 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 "nvic.h"
+#include "system.h"
+#include "io.h"
+#include "timer.h"
+
+#include "serial.h"
+#include "serial_escserial.h"
+#include "drivers/light_led.h"
+#include "drivers/pwm_output.h"
+#include "io/serial.h"
+#include "flight/mixer.h"
+
+#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;
+
+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];
+
+ 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;
+
+ timerCCHandlerRec_t timerCb;
+ timerCCHandlerRec_t edgeCb;
+} escSerial_t;
+
+extern timerHardware_t* serialTimerHardware;
+extern escSerial_t escSerialPorts[];
+
+extern const struct serialPortVTable escSerialVTable[];
+
+
+escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS];
+
+void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
+static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
+
+void setTxSignal(escSerial_t *escSerial, uint8_t state)
+{
+ if (state) {
+ IOHi(escSerial->txIO);
+ } else {
+ IOLo(escSerial->txIO);
+ }
+}
+
+static void escSerialGPIOConfig(ioTag_t tag, ioConfig_t cfg)
+{
+ if (!tag) {
+ return;
+ }
+
+ IOInit(IOGetByTag(tag), OWNER_MOTOR, RESOURCE_OUTPUT, 0);
+ IOConfigGPIO(IOGetByTag(tag), cfg);
+}
+
+void serialInputPortConfig(const timerHardware_t *timerHardwarePtr)
+{
+#ifdef STM32F10X
+ escSerialGPIOConfig(timerHardwarePtr->tag, Mode_IPU);
+#else
+ escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_AF_PP_UP);
+#endif
+
+ //escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
+ timerChClearCCFlag(timerHardwarePtr);
+ timerChITConfig(timerHardwarePtr,ENABLE);
+}
+
+
+static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
+{
+ return timerPeriod > 0xFFFF;
+}
+
+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));
+
+ uint8_t mhz = clock / 1000000;
+ timerConfigure(timerHardwarePtr, timerPeriod, mhz);
+ timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL);
+ timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
+}
+
+static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
+{
+ // start bit is usually a FALLING signal
+ uint8_t mhz = SystemCoreClock / 2000000;
+ TIM_DeInit(timerHardwarePtr->tim);
+ timerConfigure(timerHardwarePtr, 0xFFFF, mhz);
+ serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
+ timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
+ timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
+}
+
+static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
+{
+ uint32_t timerPeriod=34;
+ TIM_DeInit(timerHardwarePtr->tim);
+ timerConfigure(timerHardwarePtr, timerPeriod, 1);
+ timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimer);
+ timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
+}
+
+static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
+{
+ TIM_ICInitTypeDef TIM_ICInitStructure;
+
+ TIM_ICStructInit(&TIM_ICInitStructure);
+ TIM_ICInitStructure.TIM_Channel = channel;
+ TIM_ICInitStructure.TIM_ICPolarity = polarity;
+ TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+ TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+ TIM_ICInitStructure.TIM_ICFilter = 0x0;
+
+ TIM_ICInit(tim, &TIM_ICInitStructure);
+}
+
+static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
+{
+ // start bit is usually a FALLING signal
+ TIM_DeInit(timerHardwarePtr->tim);
+ timerConfigure(timerHardwarePtr, 0xFFFF, 1);
+ serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
+ timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChange);
+ timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
+}
+
+static void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
+{
+ escSerialGPIOConfig(timerHardwarePtr->tag, 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]);
+
+ escSerial->rxTimerHardware = &(timerHardware[output]);
+ escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]);
+
+ 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;
+
+ escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag);
+ serialInputPortConfig(escSerial->rxTimerHardware);
+
+ setTxSignal(escSerial, ENABLE);
+ delay(50);
+
+ if(mode==0){
+ serialTimerTxConfig(escSerial->txTimerHardware, portIndex);
+ serialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
+ }
+ if(mode==1 || mode==2){
+ serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
+ serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
+ }
+
+ return &escSerial->port;
+}
+
+
+void serialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
+{
+ timerChClearCCFlag(timerHardwarePtr);
+ timerChITConfig(timerHardwarePtr,DISABLE);
+ escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU);
+}
+
+
+void closeEscSerial(escSerialPortIndex_e portIndex, uint16_t output)
+{
+ escSerial_t *escSerial = &(escSerialPorts[portIndex]);
+
+ escSerial->rxTimerHardware = &(timerHardware[output]);
+ escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]);
+ serialInputPortDeConfig(escSerial->rxTimerHardware);
+ timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
+ timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
+ TIM_DeInit(escSerial->txTimerHardware->tim);
+ TIM_DeInit(escSerial->rxTimerHardware->tim);
+}
+
+/*********************************************/
+
+void processTxState(escSerial_t *escSerial)
+{
+ uint8_t mask;
+ static uint8_t bitq=0, transmitStart=0;
+ if (escSerial->isReceivingData) {
+ return;
+ }
+
+ if(transmitStart==0)
+ {
+ setTxSignal(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
+ serialOutputPortConfig(escSerial->rxTimerHardware);
+ return;
+ }
+
+ if (escSerial->bitsLeftToTransmit) {
+ mask = escSerial->internalTxBuffer & 1;
+ if(mask)
+ {
+ if(bitq==0 || bitq==1)
+ {
+ setTxSignal(escSerial, 1);
+ }
+ if(bitq==2 || bitq==3)
+ {
+ setTxSignal(escSerial, 0);
+ }
+ }
+ else
+ {
+ if(bitq==0 || bitq==2)
+ {
+ setTxSignal(escSerial, 1);
+ }
+ if(bitq==1 ||bitq==3)
+ {
+ setTxSignal(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;
+ serialInputPortConfig(escSerial->rxTimerHardware);
+ }
+}
+
+/*-----------------------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
+ serialOutputPortConfig(escSerial->rxTimerHardware);
+ return;
+ }
+
+ if (escSerial->bitsLeftToTransmit) {
+ mask = escSerial->internalTxBuffer & 1;
+ escSerial->internalTxBuffer >>= 1;
+
+ setTxSignal(escSerial, mask);
+ escSerial->bitsLeftToTransmit--;
+ return;
+ }
+
+ escSerial->isTransmittingData = false;
+ if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
+ serialInputPortConfig(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;
+ serialICConfig(
+ escSerial->rxTimerHardware->tim,
+ escSerial->rxTimerHardware->channel,
+ (escSerial->port.options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling
+ );
+ }
+}
+
+#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) {
+ // synchronise bit counter
+ // FIXME this reduces functionality somewhat as receiving breaks concurrent transmission on all ports because
+ // the next callback to the onSerialTimer will happen too early causing transmission errors.
+ TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2);
+ if (escSerial->isTransmittingData) {
+ escSerial->transmissionErrors++;
+ }
+
+ serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
+ 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;
+ serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
+ } else {
+ escSerial->rxEdge = TRAILING;
+ serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
+ }
+}
+/*-------------------------BL*/
+
+void extractAndStoreRxByte(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 onSerialTimer(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;
+ serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling);
+ }
+ }
+
+
+ processTxState(escSerial);
+}
+
+void onSerialRxPinChange(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
+ TIM_SetCounter(escSerial->rxTimerHardware->tim,0);
+
+ 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;
+
+ serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising);
+ }
+ }
+ escSerial->receiveTimeout = 0;
+
+ if(bits==8)
+ {
+ bits=0;
+ bytes++;
+ if(bytes>3)
+ {
+ extractAndStoreRxByte(escSerial);
+ }
+ escSerial->internalRxBuffer=0;
+ }
+
+}
+
+uint32_t escSerialTotalBytesWaiting(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);
+}
+
+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;
+}
+
+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;
+}
+
+void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
+{
+ UNUSED(s);
+ UNUSED(baudRate);
+}
+
+void escSerialSetMode(serialPort_t *instance, portMode_t mode)
+{
+ instance->mode = mode;
+}
+
+bool isEscSerialTransmitBufferEmpty(serialPort_t *instance)
+{
+ // start listening
+ return instance->txBufferHead == instance->txBufferTail;
+}
+
+uint32_t escSerialTxBytesFree(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
+ }
+};
+
+void escSerialInitialize()
+{
+ //StopPwmAllMotors();
+ pwmDisableMotors();
+
+ for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+ // set outputs to pullup
+ if(timerHardware[i].output==1)
+ {
+ escSerialGPIOConfig(timerHardware[i].tag, IOCFG_IPU); //GPIO_Mode_IPU
+ }
+ }
+}
+
+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;
+}
+
+
+// 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;
+ LED0_OFF;
+ LED1_OFF;
+ //StopPwmAllMotors();
+ pwmDisableMotors();
+ passPort = escPassthroughPort;
+
+ uint8_t first_output = 0;
+ for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+ if(timerHardware[i].output==1)
+ {
+ first_output=i;
+ break;
+ }
+ }
+
+ //doesn't work with messy timertable
+ uint8_t motor_output=first_output+output-1;
+ if(motor_output >=USABLE_TIMER_CHANNEL_COUNT)
+ return;
+
+ escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, 19200, 0, mode);
+ uint8_t ch;
+ while(1) {
+ 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, output);
+ return;
+ }
+ if(mode==1){
+ serialWrite(escPassthroughPort, ch); // blheli loopback
+ }
+ serialWrite(escPort, ch);
+ }
+ LED1_OFF;
+ }
+ delay(5);
+ }
+}
+
+
+#endif
diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h
new file mode 100644
index 000000000..3aefc1660
--- /dev/null
+++ b/src/main/drivers/serial_escserial.h
@@ -0,0 +1,36 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#define ESCSERIAL_BUFFER_SIZE 1024
+
+typedef enum {
+ ESCSERIAL1 = 0,
+ ESCSERIAL2
+} escSerialPortIndex_e;
+
+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(serialPort_t *instance);
+uint8_t escSerialReadByte(serialPort_t *instance);
+void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
+bool isEscSerialTransmitBufferEmpty(serialPort_t *s);
+void escSerialInitialize();
+void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode);
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index d98b5d079..de02e782f 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -55,6 +55,7 @@ uint8_t cliMode = 0;
#include "drivers/pwm_rx.h"
#include "drivers/sdcard.h"
#include "drivers/buf_writer.h"
+#include "drivers/serial_escserial.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@@ -153,6 +154,9 @@ static void cliResource(char *cmdline);
#ifdef GPS
static void cliGpsPassthrough(char *cmdline);
#endif
+#ifdef USE_ESCSERIAL
+static void cliEscPassthrough(char *cmdline);
+#endif
static void cliHelp(char *cmdline);
static void cliMap(char *cmdline);
@@ -304,6 +308,9 @@ const clicmd_t cmdTable[] = {
"[name]", cliGet),
#ifdef GPS
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
+#endif
+#ifdef USE_ESCSERIAL
+ CLI_COMMAND_DEF("escprog", "passthrough esc to serial", " ", cliEscPassthrough),
#endif
CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
#ifdef LED_STRIP
@@ -2944,6 +2951,60 @@ static void cliGpsPassthrough(char *cmdline)
}
#endif
+#ifdef USE_ESCSERIAL
+static void cliEscPassthrough(char *cmdline)
+{
+ uint8_t mode = 0;
+ int index = 0;
+ int i = 0;
+ char *pch = NULL;
+ char *saveptr;
+
+ if (isEmpty(cmdline)) {
+ cliShowParseError();
+ return;
+ }
+
+ pch = strtok_r(cmdline, " ", &saveptr);
+ while (pch != NULL) {
+ switch (i) {
+ case 0:
+ if(strncasecmp(pch, "sk", strlen(pch)) == 0)
+ {
+ mode = 0;
+ }
+ else if(strncasecmp(pch, "bl", strlen(pch)) == 0)
+ {
+ mode = 1;
+ }
+ else if(strncasecmp(pch, "ki", strlen(pch)) == 0)
+ {
+ mode = 2;
+ }
+ else
+ {
+ cliShowParseError();
+ return;
+ }
+ break;
+ case 1:
+ index = atoi(pch);
+ if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) {
+ printf("passthru at pwm output %d enabled\r\n", index);
+ }
+ else {
+ printf("invalid pwm output, valid range: 0 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT);
+ return;
+ }
+ break;
+ }
+ i++;
+ pch = strtok_r(NULL, " ", &saveptr);
+ }
+ escEnablePassthrough(cliPort,index,mode);
+}
+#endif
+
static void cliHelp(char *cmdline)
{
UNUSED(cmdline);
diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h
index 97b66191d..4d73c9935 100644
--- a/src/main/target/REVO/target.h
+++ b/src/main/target/REVO/target.h
@@ -92,6 +92,9 @@
#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define USE_SPI
#define USE_SPI_DEVICE_1