diff --git a/Makefile b/Makefile
index 93b38754ae..e4dc3b51a3 100644
--- a/Makefile
+++ b/Makefile
@@ -608,7 +608,6 @@ COLIBRI_RACE_SRC = \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stm32f30x.c \
- drivers/serial_escserial.c \
drivers/serial_usb_vcp.c \
$(HIGHEND_SRC) \
$(COMMON_SRC) \
diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c
deleted file mode 100755
index fe260bae5a..0000000000
--- a/src/main/drivers/serial_escserial.c
+++ /dev/null
@@ -1,888 +0,0 @@
-/*
- * 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_config.h"
-
-#include "common/utils.h"
-#include "common/atomic.h"
-#include "common/printf.h"
-
-#include "nvic.h"
-#include "system.h"
-#include "gpio.h"
-#include "timer.h"
-
-#include "serial.h"
-#include "serial_escserial.h"
-#include "drivers/light_led.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;
-
- 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 onEscSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
-void onEscSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
-void onEscSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
-void onEscSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
-static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
-
-void setEscTxSignal(escSerial_t *escSerial, uint8_t state)
-{
- if (state) {
- digitalHi(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin);
- } else {
- digitalLo(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin);
- }
-}
-
-static void escSerialGPIOConfig(GPIO_TypeDef *gpio, uint16_t pin, GPIO_Mode mode)
-{
- gpio_config_t cfg;
-
- cfg.pin = pin;
- cfg.mode = mode;
- cfg.speed = Speed_2MHz;
- gpioInit(gpio, &cfg);
-}
-
-void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
-{
-#ifdef STM32F10X
- escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
-#else
- escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_AF_PP_PU);
-#endif
- //escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
- timerChClearCCFlag(timerHardwarePtr);
- timerChITConfig(timerHardwarePtr,ENABLE);
-}
-
-
-static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
-{
- return timerPeriod > 0xFFFF;
-}
-
-static void escSerialTimerTxConfigBL(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, onEscSerialTimerBL);
- timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
-}
-
-static void escSerialTimerRxConfigBL(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);
- escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
- timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onEscSerialRxPinChangeBL);
- 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, 1);
- timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onEscSerialTimer);
- timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
-}
-
-static void escSerialICConfig(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 escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
-{
- // start bit is usually a FALLING signal
- TIM_DeInit(timerHardwarePtr->tim);
- timerConfigure(timerHardwarePtr, 0xFFFF, 1);
- escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
- timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onEscSerialRxPinChange);
- timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
-}
-
-static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
-{
- escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_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.callback = callback;
-
- resetBuffers(escSerial);
-
- escSerial->isTransmittingData = false;
-
- escSerial->isSearchingForStartBit = true;
- escSerial->rxBitIndex = 0;
-
- escSerial->transmissionErrors = 0;
- escSerial->receiveErrors = 0;
- escSerial->receiveTimeout = 0;
-
- escSerial->escSerialPortIndex = portIndex;
-
- escSerialInputPortConfig(escSerial->rxTimerHardware);
-
- setEscTxSignal(escSerial, ENABLE);
- delay(50);
-
- if(mode==0){
- escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
- escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
- }
- if(mode==1){
- escSerialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
- escSerialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
- }
-
- return &escSerial->port;
-}
-
-
-void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
-{
- timerChClearCCFlag(timerHardwarePtr);
- timerChITConfig(timerHardwarePtr,DISABLE);
- escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_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]);
- escSerialInputPortDeConfig(escSerial->rxTimerHardware);
- timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
- timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
- TIM_DeInit(escSerial->txTimerHardware->tim);
- TIM_DeInit(escSerial->rxTimerHardware->tim);
-}
-
-/*********************************************/
-
-void processEscTxState(escSerial_t *escSerial)
-{
- uint8_t mask;
- static uint8_t bitq=0, transmitStart=0;
- if (escSerial->isReceivingData) {
- return;
- }
-
- if(transmitStart==0)
- {
- setEscTxSignal(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)
- {
- setEscTxSignal(escSerial, 1);
- }
- if(bitq==2 || bitq==3)
- {
- setEscTxSignal(escSerial, 0);
- }
- }
- else
- {
- if(bitq==0 || bitq==2)
- {
- setEscTxSignal(escSerial, 1);
- }
- if(bitq==1 ||bitq==3)
- {
- setEscTxSignal(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);
- }
-}
-
-/*-----------------------BL*/
-/*********************************************/
-
-void processEscTxStateBL(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
- escSerialOutputPortConfig(escSerial->rxTimerHardware);
- return;
- }
-
- if (escSerial->bitsLeftToTransmit) {
- mask = escSerial->internalTxBuffer & 1;
- escSerial->internalTxBuffer >>= 1;
-
- setEscTxSignal(escSerial, mask);
- escSerial->bitsLeftToTransmit--;
- return;
- }
-
- escSerial->isTransmittingData = false;
- if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
- escSerialInputPortConfig(escSerial->rxTimerHardware);
- }
-}
-
-
-
-enum {
- TRAILING,
- LEADING
-};
-
-void applyChangedBitsEscBL(escSerial_t *escSerial)
-{
- if (escSerial->rxEdge == TRAILING) {
- uint8_t bitToSet;
- for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) {
- escSerial->internalRxBuffer |= 1 << bitToSet;
- }
- }
-}
-
-void prepareForNextEscRxByteBL(escSerial_t *escSerial)
-{
- // prepare for next byte
- escSerial->rxBitIndex = 0;
- escSerial->isSearchingForStartBit = true;
- if (escSerial->rxEdge == LEADING) {
- escSerial->rxEdge = TRAILING;
- escSerialICConfig(
- 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 extractAndStoreEscRxByteBL(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.callback) {
- escSerial->port.callback(rxByte);
- } else {
- escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
- escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
- }
-}
-
-void processEscRxStateBL(escSerial_t *escSerial)
-{
- if (escSerial->isSearchingForStartBit) {
- return;
- }
-
- escSerial->rxBitIndex++;
-
- if (escSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
- applyChangedBitsEscBL(escSerial);
- return;
- }
-
- if (escSerial->rxBitIndex == RX_TOTAL_BITS) {
-
- if (escSerial->rxEdge == TRAILING) {
- escSerial->internalRxBuffer |= STOP_BIT_MASK;
- }
-
- extractAndStoreEscRxByteBL(escSerial);
- prepareForNextEscRxByteBL(escSerial);
- }
-}
-
-void onEscSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
-{
- UNUSED(capture);
- escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
-
- processEscTxStateBL(escSerial);
- processEscRxStateBL(escSerial);
-}
-
-void onEscSerialRxPinChangeBL(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++;
- }
-
- escSerialICConfig(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;
- }
-
- applyChangedBitsEscBL(escSerial);
-
- if (escSerial->rxEdge == TRAILING) {
- escSerial->rxEdge = LEADING;
- escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
- } else {
- escSerial->rxEdge = TRAILING;
- escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
- }
-}
-/*-------------------------BL*/
-
-void extractAndStoreEscRxByte(escSerial_t *escSerial)
-{
- if ((escSerial->port.mode & MODE_RX) == 0) {
- return;
- }
-
- uint8_t rxByte = (escSerial->internalRxBuffer) & 0xFF;
-
- if (escSerial->port.callback) {
- escSerial->port.callback(rxByte);
- } else {
- escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
- escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
- }
-}
-
-void onEscSerialTimer(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;
- escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling);
- }
- }
-
-
- processEscTxState(escSerial);
-}
-
-void onEscSerialRxPinChange(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;
-
- escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising);
- }
- }
- escSerial->receiveTimeout = 0;
-
- if(bits==8)
- {
- bits=0;
- bytes++;
- if(bytes>3)
- {
- extractAndStoreEscRxByte(escSerial);
- }
- escSerial->internalRxBuffer=0;
- }
-
-}
-
-uint8_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;
-}
-
-uint8_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[] = {
- {
- escSerialWriteByte,
- escSerialTotalBytesWaiting,
- escSerialTxBytesFree,
- escSerialReadByte,
- escSerialSetBaudRate,
- isEscSerialTransmitBufferEmpty,
- escSerialSetMode,
- .writeBuf = NULL,
- }
-};
-
-void escSerialInitialize()
-{
- StopPwmAllMotors();
- for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
- // set outputs to pullup
- if(timerHardware[i].outputEnable==1)
- {
- escSerialGPIOConfig(timerHardware[i].gpio,timerHardware[i].pin, Mode_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, mode 1=bl output=timerHardware PWM channel.
-void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode)
-{
- bool exitEsc = false;
- LED0_OFF;
- LED1_OFF;
- StopPwmAllMotors();
- passPort = escPassthroughPort;
-
- uint8_t first_output = 0;
- for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
- if(timerHardware[i].outputEnable==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){
- 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
deleted file mode 100755
index ab4a1abd3d..0000000000
--- a/src/main/drivers/serial_escserial.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * 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);
-uint8_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 089d1e89b7..1976423ccb 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -141,9 +141,6 @@ static void cliRxRange(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);
@@ -274,9 +271,6 @@ 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
@@ -2201,56 +2195,6 @@ 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
- {
- 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)
{
uint32_t i = 0;
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index cbd629fbf8..e3ec3579ef 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -95,6 +95,7 @@
#ifdef USE_SERIAL_1WIRE
#include "io/serial_1wire.h"
#endif
+
#ifdef USE_SERIAL_1WIRE_VCP
#include "io/serial_1wire_vcp.h"
#endif
@@ -102,9 +103,6 @@
#include "io/serial_4way.h"
#endif
-#ifdef USE_ESCSERIAL
-#include "drivers/serial_escserial.h"
-#endif
static serialPort_t *mspSerialPort;
extern uint16_t cycleTime; // FIXME dependency on mw.c
@@ -1879,50 +1877,6 @@ static bool processInCommand(void)
// proceed as usual with MSP commands
break;
#endif
-
-#ifdef USE_ESCSERIAL
- case MSP_SET_ESCSERIAL:
- // get channel number
- i = read8();
- // we do not give any data back, assume channel number is transmitted OK
- if (i == 0xFF) {
- // 0xFF -> preinitialize the Passthrough
- // switch all motor lines HI
- escSerialInitialize();
-
- // and come back right afterwards
- // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
- // bootloader mode before try to connect any ESC
- }
- else {
- // Check for channel number 1..USABLE_TIMER_CHANNEL_COUNT-1
- if ((i > 0) && (i < USABLE_TIMER_CHANNEL_COUNT)) {
- // because we do not come back after calling escEnablePassthrough
- // proceed with a success reply first
- headSerialReply(0);
- tailSerialReply();
- // flush the transmit buffer
- bufWriterFlush(writer);
- // wait for all data to send
- while (!isSerialTransmitBufferEmpty(mspSerialPort)) {
- delay(50);
- }
- // Start to activate here
- // motor 1 => index 0
- escEnablePassthrough(mspSerialPort,i,0); //sk blmode
- // MPS uart is active again
- } else {
- // ESC channel higher than max. allowed
- // rem: BLHeliSuite will not support more than 8
- headSerialError(0);
- }
- // proceed as usual with MSP commands
- // and wait to switch to next channel
- // rem: App needs to call MSP_BOOT to deinitialize Passthrough
- }
- break;
-#endif
-
default:
// we do not know how to handle the (valid) message, indicate error MSP $M!
return false;
diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h
index fe9d9b9e1b..c493e37f11 100644
--- a/src/main/io/serial_msp.h
+++ b/src/main/io/serial_msp.h
@@ -255,7 +255,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough
-#define MSP_SET_ESCSERIAL 244 //in message Sets escserial passthrough
#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 87758ab4ad..c618dd14a8 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -110,9 +110,6 @@
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
-#define USE_ESCSERIAL
-#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
-
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2)