From ec923b618078face63c14e9dbb0fe66232f4d26f Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sat, 22 Oct 2016 16:50:35 +0300 Subject: [PATCH 1/6] Escserial test --- Makefile | 1 + src/main/drivers/serial_escserial.c | 898 ++++++++++++++++++++++++++++ src/main/drivers/serial_escserial.h | 36 ++ src/main/io/serial_cli.c | 61 ++ src/main/target/REVO/target.h | 3 + 5 files changed, 999 insertions(+) create mode 100644 src/main/drivers/serial_escserial.c create mode 100644 src/main/drivers/serial_escserial.h diff --git a/Makefile b/Makefile index eb3a7cc281..d0c9f31c77 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 0000000000..b93aaf7890 --- /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 0000000000..3aefc1660c --- /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 d98b5d0793..de02e782f2 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 97b66191df..4d73c99355 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 From a0b8faa7b8af86216beab13d1a1e8ec8a532a996 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 22 Oct 2016 17:05:09 +0200 Subject: [PATCH 2/6] Add dynamic Baudrates --- src/main/drivers/serial_escserial.c | 9 ++++++++- src/main/target/OMNIBUS/target.h | 3 +++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index b93aaf7890..660ed40912 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -21,6 +21,11 @@ #include "platform.h" +typedef enum { + BAUDRATE_NORMAL = 19200, + BAUDRATE_KISS = 38400 +} escBaudRate_e; + #if defined(USE_ESCSERIAL) #include "build/build_config.h" @@ -854,7 +859,9 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) return; - escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, 19200, 0, mode); + uint32_t escBaudrate = (mode == 2) ? BAUDRATE_KISS : BAUDRATE_NORMAL; + + escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode); uint8_t ch; while(1) { if (serialRxBytesWaiting(escPort)) { diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 4b392c32fd..05676f0046 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 From 7929e1b8f111c5c32511e969575b88a9afe5e7d7 Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sat, 22 Oct 2016 16:50:35 +0300 Subject: [PATCH 3/6] Escserial test --- src/main/drivers/serial_escserial.c | 7 +++---- src/main/drivers/serial_escserial.h | 6 ++++-- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 660ed40912..556efd4d8c 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" @@ -668,7 +667,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) } -uint32_t escSerialTotalBytesWaiting(serialPort_t *instance) +uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance) { if ((instance->mode & MODE_RX) == 0) { return 0; @@ -717,13 +716,13 @@ void escSerialSetMode(serialPort_t *instance, portMode_t mode) instance->mode = mode; } -bool isEscSerialTransmitBufferEmpty(serialPort_t *instance) +bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance) { // start listening return instance->txBufferHead == instance->txBufferTail; } -uint32_t escSerialTxBytesFree(serialPort_t *instance) +uint32_t escSerialTxBytesFree(const serialPort_t *instance) { if ((instance->mode & MODE_TX) == 0) { return 0; diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h index 3aefc1660c..113ce1c2a1 100644 --- a/src/main/drivers/serial_escserial.h +++ b/src/main/drivers/serial_escserial.h @@ -28,9 +28,11 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac // serialPort API void escSerialWriteByte(serialPort_t *instance, uint8_t ch); -uint32_t escSerialTotalBytesWaiting(serialPort_t *instance); +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(serialPort_t *s); +bool isEscSerialTransmitBufferEmpty(const serialPort_t *s); + void escSerialInitialize(); void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode); From 14d1e25280d16ea0fbcea10ea77a89ef9d4c2daa Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sun, 23 Oct 2016 10:18:19 +0300 Subject: [PATCH 4/6] only TX in kiss mode --- src/main/drivers/serial_escserial.c | 265 ++++++++++++++-------------- 1 file changed, 137 insertions(+), 128 deletions(-) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 556efd4d8c..5e49e1cf7f 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -79,6 +79,7 @@ typedef struct escSerial_s { uint16_t receiveErrors; uint8_t escSerialPortIndex; + uint8_t mode; timerCCHandlerRec_t timerCb; timerCCHandlerRec_t edgeCb; @@ -120,12 +121,10 @@ static void escSerialGPIOConfig(ioTag_t tag, ioConfig_t cfg) void serialInputPortConfig(const timerHardware_t *timerHardwarePtr) { #ifdef STM32F10X - escSerialGPIOConfig(timerHardwarePtr->tag, Mode_IPU); + escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU); #else escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_AF_PP_UP); #endif - - //escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode); timerChClearCCFlag(timerHardwarePtr); timerChITConfig(timerHardwarePtr,ENABLE); } @@ -165,7 +164,7 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 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); + 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); } @@ -198,7 +197,7 @@ static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, 1); - serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); + serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChange); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } @@ -254,15 +253,19 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac 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); - } - + 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; } @@ -295,38 +298,38 @@ void processTxState(escSerial_t *escSerial) uint8_t mask; static uint8_t bitq=0, transmitStart=0; if (escSerial->isReceivingData) { - return; + return; } if(transmitStart==0) { - setTxSignal(escSerial, 1); + setTxSignal(escSerial, 1); } if (!escSerial->isTransmittingData) { char byteToSend; reload: if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { - // canreceive - transmitStart=0; + // canreceive + transmitStart=0; return; } if(transmitStart<3) { - if(transmitStart==0) - byteToSend = 0xff; - if(transmitStart==1) - byteToSend = 0xff; - if(transmitStart==2) - byteToSend = 0x7f; - transmitStart++; + 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; - } + // data to send + byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++]; + if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) { + escSerial->port.txBufferTail = 0; + } } @@ -344,43 +347,43 @@ reload: mask = escSerial->internalTxBuffer & 1; if(mask) { - if(bitq==0 || bitq==1) - { + if(bitq==0 || bitq==1) + { setTxSignal(escSerial, 1); - } - if(bitq==2 || bitq==3) - { + } + if(bitq==2 || bitq==3) + { setTxSignal(escSerial, 0); - } + } } else { - if(bitq==0 || bitq==2) - { + if(bitq==0 || bitq==2) + { setTxSignal(escSerial, 1); - } - if(bitq==1 ||bitq==3) - { + } + 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; - } + 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); + escSerial->isTransmittingData = false; + serialInputPortConfig(escSerial->rxTimerHardware); } } @@ -391,13 +394,13 @@ void processTxStateBL(escSerial_t *escSerial) { uint8_t mask; if (escSerial->isReceivingData) { - return; + return; } if (!escSerial->isTransmittingData) { char byteToSend; if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { - // canreceive + // canreceive return; } @@ -429,7 +432,10 @@ void processTxStateBL(escSerial_t *escSerial) escSerial->isTransmittingData = false; if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { - serialInputPortConfig(escSerial->rxTimerHardware); + if(escSerial->mode==1) + { + serialInputPortConfig(escSerial->rxTimerHardware); + } } } @@ -594,13 +600,13 @@ void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture) if(escSerial->isReceivingData) { - escSerial->receiveTimeout++; - if(escSerial->receiveTimeout>8) - { - escSerial->isReceivingData=0; - escSerial->receiveTimeout=0; + escSerial->receiveTimeout++; + if(escSerial->receiveTimeout>8) + { + escSerial->isReceivingData=0; + escSerial->receiveTimeout=0; serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling); - } + } } @@ -621,48 +627,48 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) if(capture > 40 && capture < 90) { - zerofirst++; - if(zerofirst>1) - { - zerofirst=0; - escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; - bits++; - } + 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++; + zerofirst=0; + escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1; + escSerial->internalRxBuffer |= 0x80; + bits++; } else { - if(!escSerial->isReceivingData) - { - //start - //lets reset + if(!escSerial->isReceivingData) + { + //start + //lets reset - escSerial->isReceivingData = 1; - zerofirst=0; - bytes=0; - bits=1; - escSerial->internalRxBuffer = 0x80; + escSerial->isReceivingData = 1; + zerofirst=0; + bytes=0; + bits=1; + escSerial->internalRxBuffer = 0x80; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising); - } + 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; + bits=0; + bytes++; + if(bytes>3) + { + extractAndStoreRxByte(escSerial); + } + escSerial->internalRxBuffer=0; } } @@ -718,7 +724,7 @@ void escSerialSetMode(serialPort_t *instance, portMode_t mode) bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance) { - // start listening + // start listening return instance->txBufferHead == instance->txBufferTail; } @@ -756,11 +762,11 @@ void escSerialInitialize() 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 - } + // set outputs to pullup + if(timerHardware[i].output==1) + { + escSerialGPIOConfig(timerHardware[i].tag, IOCFG_IPU); //GPIO_Mode_IPU + } } } @@ -823,8 +829,8 @@ static bool ProcessExitCommand(uint8_t c) if((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0)) { - currentPort.c_state = IDLE; - return true; + currentPort.c_state = IDLE; + return true; } } else { currentPort.c_state = IDLE; @@ -837,7 +843,7 @@ static bool ProcessExitCommand(uint8_t c) // mode 0=sk, 1=bl, 2=ki output=timerHardware PWM channel. void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode) { - bool exitEsc = false; + bool exitEsc = false; LED0_OFF; LED1_OFF; //StopPwmAllMotors(); @@ -846,53 +852,56 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin 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; - } + 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; + 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 (serialRxBytesWaiting(escPort)) { - LED0_ON; - while(serialRxBytesWaiting(escPort)) - { - ch = serialRead(escPort); - serialWrite(escPassthroughPort, ch); + if(mode!=2) + { + if (serialRxBytesWaiting(escPort)) { + LED0_ON; + while(serialRxBytesWaiting(escPort)) + { + ch = serialRead(escPort); + serialWrite(escPassthroughPort, ch); + } + LED0_OFF; } - 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); + 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; } From 2478fb84f1d5446650ed330890e9d67a90942a5e Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sun, 23 Oct 2016 10:45:30 +0300 Subject: [PATCH 5/6] fix cli outputs --- src/main/io/serial_cli.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index de02e782f2..70ba4fbda0 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -310,7 +310,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), #endif #ifdef USE_ESCSERIAL - CLI_COMMAND_DEF("escprog", "passthrough esc to serial", " ", cliEscPassthrough), + CLI_COMMAND_DEF("escprog", "passthrough esc to serial", " ", cliEscPassthrough), #endif CLI_COMMAND_DEF("help", NULL, NULL, cliHelp), #ifdef LED_STRIP @@ -2993,7 +2993,7 @@ static void cliEscPassthrough(char *cmdline) 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); + printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); return; } break; From 0c142707e82a6cf5d8cda6eaea910cfe3bb89746 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 24 Oct 2016 00:03:08 +0200 Subject: [PATCH 6/6] Add escprog to other F3 / F4 targets --- src/main/target/AIORACERF3/target.h | 3 +++ src/main/target/AIR32/target.h | 3 +++ src/main/target/AIRHEROF3/target.h | 3 +++ src/main/target/ALIENFLIGHTF3/target.h | 3 +++ src/main/target/ALIENFLIGHTF4/target.h | 3 +++ src/main/target/BETAFLIGHTF3/target.h | 3 +++ src/main/target/BLUEJAYF4/target.h | 3 +++ src/main/target/CHEBUZZF3/target.h | 3 +++ src/main/target/COLIBRI/target.h | 3 +++ src/main/target/COLIBRI_RACE/target.h | 3 +++ src/main/target/DOGE/target.h | 3 +++ src/main/target/FURYF3/target.h | 3 +++ src/main/target/OMNIBUSF4/target.h | 3 +++ src/main/target/PIKOBLX/target.h | 3 +++ src/main/target/RACEBASE/target.h | 3 +++ src/main/target/RCEXPLORERF3/target.h | 3 +++ src/main/target/REVONANO/target.h | 3 +++ src/main/target/RMDO/target.h | 3 +++ src/main/target/SINGULARITY/target.h | 3 +++ src/main/target/SIRINFPV/target.h | 3 +++ src/main/target/SOULF4/target.h | 3 +++ src/main/target/SPARKY/target.h | 3 +++ src/main/target/SPARKY2/target.h | 3 +++ src/main/target/SPRACINGF3/target.h | 3 +++ src/main/target/SPRACINGF3EVO/target.h | 3 +++ src/main/target/SPRACINGF3MINI/target.h | 3 +++ src/main/target/STM32F3DISCOVERY/target.h | 3 +++ src/main/target/VRRACE/target.h | 5 +++-- src/main/target/X_RACERSPI/target.h | 3 +++ src/main/target/YUPIF4/target.h | 2 ++ src/main/target/ZCOREF3/target.h | 2 ++ 31 files changed, 91 insertions(+), 2 deletions(-) diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h index f340a8f0c9..2698fa6ec5 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 3895e6f447..1b8f6c2111 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 46da06935f..e77e042cad 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 590e0b7ee5..1ab6fdb685 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 a8b0638f19..bc899a217d 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 8b66d5eb8f..aac7f46bb0 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 a013e3fb2d..e72a552383 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 5623a8a87f..76c811fd30 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 478e9c672e..d30ab8e774 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 d14274e3f0..2c4fd4c334 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 298ddd9554..dc31a1640a 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 f8c74899ef..c7292349b6 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/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index d1e205b8d8..aaa19719ef 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 4cdb2399f3..2b08b62b2e 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 1752be155b..fd44bd0415 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 a330a7ee89..a1f5f8527d 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/REVONANO/target.h b/src/main/target/REVONANO/target.h index fa38860682..e4dd143fab 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 6c873ef3ce..e7c5744bd2 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 7c3f1b4ca6..75895fc12a 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 cab5ad2edd..8062644aa9 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 e8cbf5a077..d20949eddd 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 1a6d3a1633..e98ef0aea8 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 9ef673ff5c..dc584cd8ce 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 d7c7d42f44..1d114be50f 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 eb06d7eb56..6e3991b631 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 de6063eac0..bedeb474fc 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 687fa1924a..766447a09e 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 f3540b06a1..ec7a8925f2 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 65def32008..7e3bee0a54 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 f8361330d4..733eb312cf 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 5c07c78190..db38c0bb12 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