diff --git a/Makefile b/Makefile index db350ff96..fa0c53e5a 100644 --- a/Makefile +++ b/Makefile @@ -548,6 +548,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..5e49e1cf7 --- /dev/null +++ b/src/main/drivers/serial_escserial.c @@ -0,0 +1,913 @@ +/* + * 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 "platform.h" + +typedef enum { + BAUDRATE_NORMAL = 19200, + BAUDRATE_KISS = 38400 +} escBaudRate_e; + +#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; + uint8_t mode; + + 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, IOCFG_IPU); +#else + escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_AF_PP_UP); +#endif + 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); + } + else if(mode==1){ + serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); + serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); + } + else if(mode==2) { + serialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used + serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); + } + escSerial->mode = mode; + 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)) { + if(escSerial->mode==1) + { + 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(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); +} + +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(const serialPort_t *instance) +{ + // start listening + return instance->txBufferHead == instance->txBufferTail; +} + +uint32_t escSerialTxBytesFree(const serialPort_t *instance) +{ + if ((instance->mode & MODE_TX) == 0) { + return 0; + } + + 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; + + uint32_t escBaudrate = (mode == 2) ? BAUDRATE_KISS : BAUDRATE_NORMAL; + + escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode); + 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, 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..113ce1c2a --- /dev/null +++ b/src/main/drivers/serial_escserial.h @@ -0,0 +1,38 @@ +/* + * 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(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); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d98b5d079..70ba4fbda 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: 1 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/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h index f340a8f0c..2698fa6ec 100644 --- a/src/main/target/AIORACERF3/target.h +++ b/src/main/target/AIORACERF3/target.h @@ -67,6 +67,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 3895e6f44..1b8f6c211 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -62,6 +62,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index 46da06935..e77e042ca 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -60,6 +60,9 @@ #define USE_SOFTSERIAL2 #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 590e0b7ee..1ab6fdb68 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -70,6 +70,9 @@ #define SERIAL_PORT_COUNT 4 #define AVOID_UART2_FOR_PWM_PPM +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index a8b0638f1..bc899a217 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -118,6 +118,9 @@ #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 8b66d5eb8..aac7f46bb 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -62,6 +62,9 @@ #define USE_SOFTSERIAL2 #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 022b51da5..50b0d87dd 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -111,6 +111,9 @@ #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 5623a8a87..76c811fd3 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -93,6 +93,9 @@ #define USE_UART2 #define SERIAL_PORT_COUNT 3 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_I2C #define I2C_DEVICE (I2CDEV_1) diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 478e9c672..d30ab8e77 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -97,6 +97,9 @@ #define SERIAL_PORT_COUNT 4 //VCP, UART1, UART2, UART3 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index d14274e3f..2c4fd4c33 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -84,6 +84,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PC4 #define UART1_RX_PIN PC5 diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 298ddd955..dc31a1640 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -98,6 +98,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index f8c74899e..c7292349b 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -117,6 +117,9 @@ #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 4b392c32f..05676f004 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -80,6 +80,9 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 1ef2f55c7..04b9a08ed 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -96,6 +96,9 @@ #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 #define USE_SPI diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 4cdb2399f..2b08b62b2 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -52,6 +52,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 1752be155..fd44bd041 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -54,6 +54,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 3 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index a330a7ee8..a1f5f8527 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -69,6 +69,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 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 diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index fa3886068..e4dd143fa 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -71,6 +71,9 @@ #define SERIAL_PORT_COUNT 3 //VCP, USART1, USART2 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_SPI //#define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 6c873ef3c..e7c5744bd 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -58,6 +58,9 @@ #define USE_SOFTSERIAL2 #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 7c3f1b4ca..75895fc12 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -47,6 +47,9 @@ #define USE_SOFTSERIAL1 // Telemetry #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index cab5ad2ed..8062644aa 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -61,6 +61,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/SOULF4/target.h b/src/main/target/SOULF4/target.h index f49652428..aa8f6fa67 100644 --- a/src/main/target/SOULF4/target.h +++ b/src/main/target/SOULF4/target.h @@ -77,6 +77,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 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 1a6d3a163..e98ef0aea 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -58,6 +58,9 @@ #define SERIAL_PORT_COUNT 4 #define AVOID_UART2_FOR_PWM_PPM +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 9ef673ff5..dc584cd8c 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -93,6 +93,9 @@ #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_SPI #define USE_SPI_DEVICE_1 //MPU9250 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index d7c7d42f4..1d114be50 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -67,6 +67,9 @@ #define USE_SOFTSERIAL2 #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index eb06d7eb5..6e3991b63 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -64,6 +64,9 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index de6063eac..bedeb474f 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -73,6 +73,9 @@ #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 5 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 687fa1924..766447a09 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -151,6 +151,9 @@ #define USE_UART5 #define SERIAL_PORT_COUNT 6 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7) diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index f3540b06a..ec7a8925f 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -120,10 +120,11 @@ #define SOFTSERIAL_1_TIMER TIM1 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 2 // PWM 3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2 - - #define SERIAL_PORT_COUNT 6 //VCP, USART1, USART2, USART3, USART6, SOFTSERIAL1 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define USE_SPI diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 65def3200..7e3bee0a5 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -56,6 +56,9 @@ #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 4 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 13345dc8e..55c60a4c3 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -70,6 +70,8 @@ #define SERIAL_PORT_COUNT 4 // VCP, UART1, UART3, UART6 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 // SD Card #define USE_SDCARD diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index 5c07c7819..db38c0bb1 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -54,6 +54,8 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 3 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define UART2_TX_PIN PA14 // PA14 / SWCLK #define UART2_RX_PIN PA15