From 4ab5db8e4532092e26e40b87b2f6a1ebf49db9d0 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 17 Oct 2016 16:28:33 +0200 Subject: [PATCH 01/20] Add second notch default --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0e0ee40f46..829a396827 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -492,7 +492,7 @@ void createDefaultConfig(master_t *config) config->gyro_soft_lpf_hz = 90; config->gyro_soft_notch_hz_1 = 400; config->gyro_soft_notch_cutoff_1 = 300; - config->gyro_soft_notch_hz_2 = 0; + config->gyro_soft_notch_hz_2 = 200; config->gyro_soft_notch_cutoff_2 = 100; config->debug_mode = DEBUG_NONE; From 48b7b4fc652e7dc92f45829bb928a1b211a04579 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 18 Oct 2016 00:10:05 +0200 Subject: [PATCH 02/20] 3.0.1 Defaults // Improve setpoint transition --- src/main/config/config.c | 8 ++++---- src/main/flight/pid.c | 15 ++++++++++++--- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 829a396827..752eb85fd1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -205,11 +205,11 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT; #endif - pidProfile->P8[ROLL] = 45; + pidProfile->P8[ROLL] = 43; pidProfile->I8[ROLL] = 40; pidProfile->D8[ROLL] = 20; - pidProfile->P8[PITCH] = 60; - pidProfile->I8[PITCH] = 65; + pidProfile->P8[PITCH] = 58; + pidProfile->I8[PITCH] = 50; pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 70; pidProfile->I8[YAW] = 45; @@ -248,7 +248,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->setpointRelaxRatio = 85; + pidProfile->setpointRelaxRatio = 30; pidProfile->dtermSetpointWeight = 200; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 02f7f4e0ae..dee35d6af7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -260,10 +260,19 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc //-----calculate D-term (Yaw D not yet supported) if (axis != YAW) { - if (pidProfile->setpointRelaxRatio < 100) - dynC = c[axis] * powerf(rcInput[axis], 2) * relaxFactor[axis] + c[axis] * (1-relaxFactor[axis]); - else + static float previousSetpoint[3]; + dynC = c[axis]; + if (pidProfile->setpointRelaxRatio < 100) { dynC = c[axis]; + if (setpointRate[axis] > 0) { + if ((setpointRate[axis] - previousSetpoint[axis]) < previousSetpoint[axis]) + dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + } else if (setpointRate[axis] < 0) { + if ((setpointRate[axis] - previousSetpoint[axis]) > previousSetpoint[axis]) + dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + } + } + previousSetpoint[axis] = setpointRate[axis]; rD = dynC * setpointRate[axis] - PVRate; // cr - y delta = rD - lastRateError[axis]; lastRateError[axis] = rD; From ec923b618078face63c14e9dbb0fe66232f4d26f Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sat, 22 Oct 2016 16:50:35 +0300 Subject: [PATCH 03/20] 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 04/20] 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 05/20] 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 06/20] 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 07/20] 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 01be3842c8390a92dba8711ac6e5d773ea35e0b0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 17 Oct 2016 14:15:02 +0100 Subject: [PATCH 08/20] Added filter unit test and tidied filter code --- src/main/common/filter.c | 71 +----------- src/main/common/filter.h | 31 ++--- src/main/flight/pid.c | 10 +- src/main/sensors/gyro.c | 6 +- src/test/Makefile | 22 ++++ src/test/unit/common_filter_unittest.cc | 147 ++++++++++++++++++++++++ 6 files changed, 190 insertions(+), 97 deletions(-) create mode 100644 src/test/unit/common_filter_unittest.cc diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 420418e0d3..98d82919e4 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -140,7 +140,7 @@ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const flo firFilterInit2(filter, buf, bufLength, coeffs, bufLength); } -void filterFirUpdate(firFilter_t *filter, float input) +void firFilterUpdate(firFilter_t *filter, float input) { filter->buf[filter->index++] = input; // index is at the first empty buffer positon if (filter->index >= filter->bufLength) { @@ -207,74 +207,14 @@ float firFilterLastInput(const firFilter_t *filter) return filter->buf[index]; } - -/* - * int16_t based FIR filter - * Can be directly updated from devices that produce 16-bit data, eg gyros and accelerometers - */ -void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength) +void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { - filter->buf = buf; - filter->bufLength = bufLength; - filter->coeffs = coeffs; - filter->coeffsLength = coeffsLength; - memset(filter->buf, 0, sizeof(int16_t) * filter->bufLength); + filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE); } -/* - * FIR filter initialisation - * If FIR filter is just used for averaging, coeffs can be set to NULL - */ -void firFilterInt16Init(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs) +// prototype function for denoising of signal by dynamic moving average. Mainly for test purposes +float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input) { - firFilterInt16Init2(filter, buf, bufLength, coeffs, bufLength); -} - -void firFilterInt16Update(firFilterInt16_t *filter, int16_t input) -{ - memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(input)); - filter->buf[0] = input; -} - -float firFilterInt16Apply(const firFilterInt16_t *filter) -{ - float ret = 0.0f; - for (int ii = 0; ii < filter->coeffsLength; ++ii) { - ret += filter->coeffs[ii] * filter->buf[ii]; - } - return ret; -} - -float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t count) -{ - float ret = 0; - for (int ii = 0; ii < count; ++ii) { - ret += filter->buf[ii]; - } - return ret / count; -} - -float firFilterInt16CalcAverage(const firFilterInt16_t *filter) -{ - return firFilterInt16CalcPartialAverage(filter, filter->coeffsLength); -} - -int16_t firFilterInt16LastInput(const firFilterInt16_t *filter) -{ - return filter->buf[0]; -} - -int16_t firFilterInt16Get(const firFilter_t *filter, int index) -{ - return filter->buf[index]; -} - -void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { - filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_WINDOW_SIZE); -} - -/* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */ -float firFilterUpdate(firFilterState_t *filter, float input) { filter->state[filter->index] = input; filter->movingSum += filter->state[filter->index++]; if (filter->index == filter->targetCount) @@ -287,4 +227,3 @@ float firFilterUpdate(firFilterState_t *filter, float input) { return filter->movingSum / ++filter->filledCount + 1; } - diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 4db6999e1c..2b0e75a431 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -16,9 +16,9 @@ */ #ifdef STM32F10X -#define MAX_FIR_WINDOW_SIZE 60 +#define MAX_FIR_DENOISE_WINDOW_SIZE 60 #else -#define MAX_FIR_WINDOW_SIZE 120 +#define MAX_FIR_DENOISE_WINDOW_SIZE 120 #endif typedef struct pt1Filter_s { @@ -33,13 +33,13 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; -typedef struct firFilterState_s { +typedef struct firFilterDenoise_s{ int filledCount; int targetCount; int index; float movingSum; - float state[MAX_FIR_WINDOW_SIZE]; -} firFilterState_t; + float state[MAX_FIR_DENOISE_WINDOW_SIZE]; +} firFilterDenoise_t; typedef enum { FILTER_PT1 = 0, @@ -62,13 +62,6 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; -typedef struct firFilterInt16_s { - int16_t *buf; - const float *coeffs; - uint8_t bufLength; - uint8_t coeffsLength; -} firFilterInt16_t; - void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); @@ -81,21 +74,13 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); -void filterFirUpdate(firFilter_t *filter, float input); +void firFilterUpdate(firFilter_t *filter, float input); void firFilterUpdateAverage(firFilter_t *filter, float input); float firFilterApply(const firFilter_t *filter); float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); float firFilterCalcMovingAverage(const firFilter_t *filter); float firFilterLastInput(const firFilter_t *filter); -void firFilterInt16Init(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs); -void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); -void firFilterInt16Update(firFilterInt16_t *filter, int16_t input); -float firFilterInt16Apply(const firFilterInt16_t *filter); -float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t count); -float firFilterInt16CalcAverage(const firFilterInt16_t *filter); -int16_t firFilterInt16LastInput(const firFilterInt16_t *filter); -int16_t firFilterInt16Get(const firFilter_t *filter, int index); -void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); -float firFilterUpdate(firFilterState_t *filter, float input); +void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); +float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c73e04ddaa..a2c5bb4e7c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -102,9 +102,9 @@ biquadFilter_t dtermFilterLpf[3]; biquadFilter_t dtermFilterNotch[3]; bool dtermNotchInitialised; bool dtermBiquadLpfInitialised; -firFilterState_t dtermDenoisingState[3]; +firFilterDenoise_t dtermDenoisingState[3]; -void initFilters(const pidProfile_t *pidProfile) { +static void pidInitFilters(const pidProfile_t *pidProfile) { int axis; static uint8_t lowpassFilterType; @@ -120,7 +120,7 @@ void initFilters(const pidProfile_t *pidProfile) { } if (pidProfile->dterm_filter_type == FILTER_FIR) { - for (axis = 0; axis < 3; axis++) initFirFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + for (axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } lowpassFilterType = pidProfile->dterm_filter_type; } @@ -141,7 +141,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - initFilters(pidProfile); + pidInitFilters(pidProfile); if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions @@ -284,7 +284,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio else if (pidProfile->dterm_filter_type == FILTER_PT1) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); else - delta = firFilterUpdate(&dtermDenoisingState[axis], delta); + delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta); } DTerm = Kd[axis] * delta * tpaFactor; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 56b778e7a0..364502adeb 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -49,7 +49,7 @@ static const gyroConfig_t *gyroConfig; static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; -static firFilterState_t gyroDenoiseState[XYZ_AXIS_COUNT]; +static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; static float gyroSoftNotchQ1, gyroSoftNotchQ2; @@ -83,7 +83,7 @@ void gyroInit(void) else if (gyroSoftLpfType == FILTER_PT1) gyroDt = (float) gyro.targetLooptime * 0.000001f; else - initFirFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); + firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); } } @@ -198,7 +198,7 @@ void gyroUpdate(void) else if (gyroSoftLpfType == FILTER_PT1) gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); else - gyroADCf[axis] = firFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); + gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); diff --git a/src/test/Makefile b/src/test/Makefile index bc98d05f2d..6581e7ae2b 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -110,6 +110,28 @@ $(OBJECT_DIR)/common/maths.o : \ @mkdir -p $(dir $@) $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ +$(OBJECT_DIR)/common/filter.o : \ + $(USER_DIR)/common/filter.c \ + $(USER_DIR)/common/filter.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/filter.c -o $@ + +$(OBJECT_DIR)/common_filter_unittest.o : \ + $(TEST_DIR)/common_filter_unittest.cc \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/common_filter_unittest.cc -o $@ + +$(OBJECT_DIR)/common_filter_unittest : \ + $(OBJECT_DIR)/common_filter_unittest.o \ + $(OBJECT_DIR)/common/filter.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + $(OBJECT_DIR)/drivers/io.o : \ $(USER_DIR)/drivers/io.c \ diff --git a/src/test/unit/common_filter_unittest.cc b/src/test/unit/common_filter_unittest.cc new file mode 100644 index 0000000000..88d0c40ef6 --- /dev/null +++ b/src/test/unit/common_filter_unittest.cc @@ -0,0 +1,147 @@ +/* + * 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 + +extern "C" { + #include "common/filter.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +TEST(FilterUnittest, TestFirFilterInit) +{ +#define BUFLEN 4 + float buf[BUFLEN]; + firFilter_t filter; + + + firFilterInit(&filter, buf, BUFLEN, NULL); + + EXPECT_EQ(buf, filter.buf); + EXPECT_EQ(0, filter.coeffs); + EXPECT_EQ(0, filter.movingSum); + EXPECT_EQ(0, filter.index); + EXPECT_EQ(0, filter.count); + EXPECT_EQ(BUFLEN, filter.bufLength); + EXPECT_EQ(BUFLEN, filter.coeffsLength); +} + +TEST(FilterUnittest, TestFirFilterUpdateAverage) +{ +#define BUFLEN 4 + float buf[BUFLEN]; + const float coeffs[BUFLEN] = {1.0f, 1.0f, 1.0f, 1.0f}; + firFilter_t filter; + + firFilterInit(&filter, buf, BUFLEN, coeffs); + + firFilterUpdateAverage(&filter, 2.0f); + EXPECT_FLOAT_EQ(2.0f, filter.buf[0]); + EXPECT_FLOAT_EQ(2.0f, filter.movingSum); + EXPECT_EQ(1, filter.index); + EXPECT_EQ(1, filter.count); + EXPECT_EQ(2.0f, firFilterCalcMovingAverage(&filter)); + EXPECT_FLOAT_EQ(2.0f, firFilterCalcPartialAverage(&filter, 1)); + EXPECT_FLOAT_EQ(2.0f, firFilterApply(&filter)); + + firFilterUpdateAverage(&filter, 3.0f); + EXPECT_FLOAT_EQ(3.0f, filter.buf[1]); + EXPECT_FLOAT_EQ(5.0f, filter.movingSum); + EXPECT_EQ(2, filter.index); + EXPECT_EQ(2, filter.count); + EXPECT_EQ(2.5f, firFilterCalcMovingAverage(&filter)); + EXPECT_FLOAT_EQ(2.5f, firFilterCalcPartialAverage(&filter, 2)); + EXPECT_FLOAT_EQ(5.0f, firFilterApply(&filter)); + + firFilterUpdateAverage(&filter, 4.0f); + EXPECT_FLOAT_EQ(4.0f, filter.buf[2]); + EXPECT_FLOAT_EQ(9.0f, filter.movingSum); + EXPECT_EQ(3, filter.index); + EXPECT_EQ(3, filter.count); + EXPECT_EQ(3.0f, firFilterCalcMovingAverage(&filter)); + EXPECT_FLOAT_EQ(3.0f, firFilterCalcPartialAverage(&filter, 3)); + EXPECT_FLOAT_EQ(9.0f, firFilterApply(&filter)); + + firFilterUpdateAverage(&filter, 5.0f); + EXPECT_FLOAT_EQ(5.0f, filter.buf[3]); + EXPECT_FLOAT_EQ(14.0f, filter.movingSum); + EXPECT_EQ(0, filter.index); + EXPECT_EQ(4, filter.count); + EXPECT_EQ(3.5f, firFilterCalcMovingAverage(&filter)); + EXPECT_FLOAT_EQ(3.5f, firFilterCalcPartialAverage(&filter, 4)); + EXPECT_FLOAT_EQ(14.0f, firFilterApply(&filter)); + + firFilterUpdateAverage(&filter, 6.0f); + EXPECT_FLOAT_EQ(6.0f, filter.buf[0]); + EXPECT_FLOAT_EQ(18.0f, filter.movingSum); + EXPECT_EQ(1, filter.index); + EXPECT_EQ(BUFLEN, filter.count); + EXPECT_EQ(4.5f, firFilterCalcMovingAverage(&filter)); + EXPECT_FLOAT_EQ(4.5f, firFilterCalcPartialAverage(&filter, BUFLEN)); + EXPECT_FLOAT_EQ(18.0f, firFilterApply(&filter)); + + firFilterUpdateAverage(&filter, 7.0f); + EXPECT_FLOAT_EQ(7.0f, filter.buf[1]); + EXPECT_FLOAT_EQ(22.0f, filter.movingSum); + EXPECT_EQ(2, filter.index); + EXPECT_EQ(BUFLEN, filter.count); + EXPECT_EQ(5.5f, firFilterCalcMovingAverage(&filter)); + EXPECT_FLOAT_EQ(5.5f, firFilterCalcPartialAverage(&filter, BUFLEN)); + EXPECT_FLOAT_EQ(22.0f, firFilterApply(&filter)); +} + +TEST(FilterUnittest, TestFirFilterApply) +{ +#define BUFLEN 4 + float buf[BUFLEN]; + firFilter_t filter; + const float coeffs[BUFLEN] = {26.0f, 27.0f, 28.0f, 29.0f}; + + float expected = 0.0f; + firFilterInit(&filter, buf, BUFLEN, coeffs); + + firFilterUpdate(&filter, 2.0f); + expected = 2.0f * 26.0f; + EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); + + firFilterUpdate(&filter, 3.0f); + expected = 3.0f * 26.0f + 2.0 * 27.0; + EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); + + firFilterUpdate(&filter, 4.0f); + expected = 4.0f * 26.0f + 3.0 * 27.0 + 2.0 * 28.0; + EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); + + firFilterUpdate(&filter, 5.0f); + expected = 5.0f * 26.0f + 4.0 * 27.0 + 3.0 * 28.0 + 2.0f * 29.0f; + EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); + + firFilterUpdate(&filter, 6.0f); + expected = 6.0f * 26.0f + 5.0 * 27.0 + 4.0 * 28.0 + 3.0f * 29.0f; + EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); + + firFilterUpdate(&filter, 7.0f); + expected = 7.0f * 26.0f + 6.0 * 27.0 + 5.0 * 28.0 + 4.0f * 29.0f; + EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); +} From dcd87aa01adccff8b3155ff0cefc888600ef498d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 23 Oct 2016 16:13:47 +0100 Subject: [PATCH 09/20] Fixed spurious CR/LF in gitignore --- .gitignore | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index cca56cd746..86b85dc447 100644 --- a/.gitignore +++ b/.gitignore @@ -24,6 +24,6 @@ README.pdf /build # local changes only make/local.mk - -mcu.mak -mcu.mak.old + +mcu.mak +mcu.mak.old From 0c142707e82a6cf5d8cda6eaea910cfe3bb89746 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 24 Oct 2016 00:03:08 +0200 Subject: [PATCH 10/20] 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 From 726a8d29e27e44ca7deff7fd1326722d3f12c0f0 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 24 Oct 2016 01:52:51 +0200 Subject: [PATCH 11/20] Refactor conflicting serial function names --- src/main/drivers/serial_escserial.c | 44 +++++++++++++------------- src/main/target/BETAFLIGHTF3/target.h | 14 +++++--- src/main/target/BETAFLIGHTF3/target.mk | 5 +-- stm32.mak | 33 +++++++++++++++++++ 4 files changed, 67 insertions(+), 29 deletions(-) create mode 100755 stm32.mak diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 5e49e1cf7f..577d305d52 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -93,13 +93,13 @@ 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 onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); -void setTxSignal(escSerial_t *escSerial, uint8_t state) +void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) { if (state) { IOHi(escSerial->txIO); @@ -118,7 +118,7 @@ static void escSerialGPIOConfig(ioTag_t tag, ioConfig_t cfg) IOConfigGPIO(IOGetByTag(tag), cfg); } -void serialInputPortConfig(const timerHardware_t *timerHardwarePtr) +void serialInputPortConfigEsc(const timerHardware_t *timerHardwarePtr) { #ifdef STM32F10X escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU); @@ -174,7 +174,7 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t uint32_t timerPeriod=34; TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, timerPeriod, 1); - timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimer); + timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); } @@ -198,7 +198,7 @@ static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, 1); serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); - timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChange); + timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } @@ -248,9 +248,9 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac escSerial->escSerialPortIndex = portIndex; escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag); - serialInputPortConfig(escSerial->rxTimerHardware); + serialInputPortConfigEsc(escSerial->rxTimerHardware); - setTxSignal(escSerial, ENABLE); + setTxSignalEsc(escSerial, ENABLE); delay(50); if(mode==0){ @@ -293,7 +293,7 @@ void closeEscSerial(escSerialPortIndex_e portIndex, uint16_t output) /*********************************************/ -void processTxState(escSerial_t *escSerial) +void processTxStateEsc(escSerial_t *escSerial) { uint8_t mask; static uint8_t bitq=0, transmitStart=0; @@ -303,7 +303,7 @@ void processTxState(escSerial_t *escSerial) if(transmitStart==0) { - setTxSignal(escSerial, 1); + setTxSignalEsc(escSerial, 1); } if (!escSerial->isTransmittingData) { char byteToSend; @@ -349,22 +349,22 @@ reload: { if(bitq==0 || bitq==1) { - setTxSignal(escSerial, 1); + setTxSignalEsc(escSerial, 1); } if(bitq==2 || bitq==3) { - setTxSignal(escSerial, 0); + setTxSignalEsc(escSerial, 0); } } else { if(bitq==0 || bitq==2) { - setTxSignal(escSerial, 1); + setTxSignalEsc(escSerial, 1); } if(bitq==1 ||bitq==3) { - setTxSignal(escSerial, 0); + setTxSignalEsc(escSerial, 0); } } bitq++; @@ -383,7 +383,7 @@ reload: if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { escSerial->isTransmittingData = false; - serialInputPortConfig(escSerial->rxTimerHardware); + serialInputPortConfigEsc(escSerial->rxTimerHardware); } } @@ -425,7 +425,7 @@ void processTxStateBL(escSerial_t *escSerial) mask = escSerial->internalTxBuffer & 1; escSerial->internalTxBuffer >>= 1; - setTxSignal(escSerial, mask); + setTxSignalEsc(escSerial, mask); escSerial->bitsLeftToTransmit--; return; } @@ -434,7 +434,7 @@ void processTxStateBL(escSerial_t *escSerial) if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { if(escSerial->mode==1) { - serialInputPortConfig(escSerial->rxTimerHardware); + serialInputPortConfigEsc(escSerial->rxTimerHardware); } } } @@ -577,7 +577,7 @@ void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) } /*-------------------------BL*/ -void extractAndStoreRxByte(escSerial_t *escSerial) +void extractAndStoreRxByteEsc(escSerial_t *escSerial) { if ((escSerial->port.mode & MODE_RX) == 0) { return; @@ -593,7 +593,7 @@ void extractAndStoreRxByte(escSerial_t *escSerial) } } -void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture) { UNUSED(capture); escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb); @@ -610,10 +610,10 @@ void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture) } - processTxState(escSerial); + processTxStateEsc(escSerial); } -void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture) { UNUSED(capture); static uint8_t zerofirst=0; @@ -666,7 +666,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) bytes++; if(bytes>3) { - extractAndStoreRxByte(escSerial); + extractAndStoreRxByteEsc(escSerial); } escSerial->internalRxBuffer=0; } diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index aac7f46bb0..9be40e8489 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -18,7 +18,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "BETAFC" +#define TARGET_BOARD_IDENTIFIER "BFFC" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE @@ -32,7 +32,7 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define MPU6000_CS_PIN PA15 +#define MPU6000_CS_PIN PC13 #define MPU6000_SPI_INSTANCE SPI1 @@ -78,7 +78,6 @@ #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 5 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 6 - #undef USE_I2C #define USE_SPI @@ -96,8 +95,13 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 - - +#define OSD +// include the max7456 driver +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN PA1 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) #define USE_SDCARD #define USE_SDCARD_SPI2 diff --git a/src/main/target/BETAFLIGHTF3/target.mk b/src/main/target/BETAFLIGHTF3/target.mk index 63f46f10be..53222223b6 100755 --- a/src/main/target/BETAFLIGHTF3/target.mk +++ b/src/main/target/BETAFLIGHTF3/target.mk @@ -9,5 +9,6 @@ TARGET_SRC = \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/display_ug2864hsweg01.h \ - drivers/flash_m25p16.c - + drivers/flash_m25p16.c \ + drivers/max7456.c \ + io/osd.c \ No newline at end of file diff --git a/stm32.mak b/stm32.mak new file mode 100755 index 0000000000..d60c48e612 --- /dev/null +++ b/stm32.mak @@ -0,0 +1,33 @@ +#This file is generated by VisualGDB. +#It contains GCC settings automatically derived from the board support package (BSP). +#DO NOT EDIT MANUALLY. THE FILE WILL BE OVERWRITTEN. +#Use VisualGDB Project Properties dialog or modify Makefile or per-configuration .mak files instead. + +#VisualGDB provides BSP_ROOT and TOOLCHAIN_ROOT via environment when running Make. The line below will only be active if GNU Make is started manually. +BSP_ROOT ?= $(LOCALAPPDATA)/VisualGDB/EmbeddedBSPs/arm-eabi/com.sysprogs.arm.stm32 +EFP_BASE ?= $(LOCALAPPDATA)/VisualGDB/EmbeddedEFPs +TOOLCHAIN_ROOT ?= C:/Program Files (x86)/GNU Tools ARM Embedded/5.4 2016q2 + +#Embedded toolchain +CC := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-gcc.exe +CXX := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-g++.exe +LD := $(CXX) +AR := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-ar.exe +OBJCOPY := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-objcopy.exe + +#Additional flags +PREPROCESSOR_MACROS += ARM_MATH_CM3 STM32F103CB +INCLUDE_DIRS += . +LIBRARY_DIRS += +LIBRARY_NAMES += +ADDITIONAL_LINKER_INPUTS += +MACOS_FRAMEWORKS += +LINUX_PACKAGES += + +CFLAGS += +CXXFLAGS += +ASFLAGS += +LDFLAGS += +COMMONFLAGS += -mcpu=cortex-m3 -mthumb +LINKER_SCRIPT := $(BSP_ROOT)/STM32F1xxxx/LinkerScripts/STM32F103CB_flash.lds + From 6aa40846fe57943c283e77e157136f2083d034a7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 24 Oct 2016 18:49:08 +0100 Subject: [PATCH 12/20] Improved MSP initialisation and independence from serial code --- src/main/fc/fc_msp.c | 3 +-- src/main/fc/fc_msp.h | 3 ++- src/main/fc/fc_tasks.c | 3 ++- src/main/main.c | 3 ++- src/main/msp/msp_serial.c | 10 ++++------ src/main/msp/msp_serial.h | 4 ++-- 6 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c469175535..1f5a5cd943 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1782,8 +1782,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro /* * Return a pointer to the process command function */ -mspProcessCommandFnPtr mspFcInit(void) +void mspFcInit(void) { initActiveBoxIds(); - return mspFcProcessCommand; } diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index 745fd551a9..53edbd8ed1 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -19,4 +19,5 @@ #include "msp/msp.h" -mspProcessCommandFnPtr mspFcInit(void); +void mspFcInit(void); +mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index d3d088f29e..b81d36ce01 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -31,6 +31,7 @@ #include "drivers/serial.h" #include "fc/config.h" +#include "fc/fc_msp.h" #include "fc/fc_tasks.h" #include "fc/mw.h" #include "fc/rc_controls.h" @@ -97,7 +98,7 @@ static void taskHandleSerial(uint32_t currentTime) return; } #endif - mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA); + mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); } #ifdef BEEPER diff --git a/src/main/main.c b/src/main/main.c index ae8e88d179..c4ca10b686 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -449,7 +449,8 @@ void init(void) imuInit(); - mspSerialInit(mspFcInit()); + mspFcInit(); + mspSerialInit(); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index b0c80d116f..8bfff6b37b 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -31,7 +31,6 @@ #include "msp/msp.h" #include "msp/msp_serial.h" -static mspProcessCommandFnPtr mspProcessCommandFn; static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; @@ -145,7 +144,7 @@ static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) serialEndWrite(msp->port); } -static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp) +static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) { static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE]; @@ -179,7 +178,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp) * * Called periodically by the scheduler. */ -void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData) +void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn) { for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { mspPort_t * const mspPort = &mspPorts[portIndex]; @@ -197,7 +196,7 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData) } if (mspPort->c_state == MSP_COMMAND_RECEIVED) { - mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort); + mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn); break; // process one command at a time so as not to block. } } @@ -208,9 +207,8 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData) } } -void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse) +void mspSerialInit(void) { - mspProcessCommandFn = mspProcessCommandFnToUse; memset(mspPorts, 0, sizeof(mspPorts)); mspSerialAllocatePorts(); } diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 57704f33a3..f2f071464b 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -62,7 +62,7 @@ typedef struct mspPort_s { } mspPort_t; -void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn); -void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData); +void mspSerialInit(void); +void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); From 31828873fa5ec10663d224f08d17dfaee6b365db Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 14 Oct 2016 10:47:04 +0100 Subject: [PATCH 13/20] Split mixer and servo code --- Makefile | 1 + src/main/config/config_master.h | 4 +- src/main/fc/config.c | 21 +- src/main/fc/mw.c | 5 +- src/main/flight/mixer.c | 471 +----------------------------- src/main/flight/mixer.h | 117 +------- src/main/flight/servos.c | 490 ++++++++++++++++++++++++++++++++ src/main/flight/servos.h | 129 +++++++++ src/main/io/motors.h | 2 +- src/main/io/serial_cli.c | 6 +- src/main/io/servos.h | 4 +- src/main/main.c | 2 +- 12 files changed, 654 insertions(+), 598 deletions(-) create mode 100755 src/main/flight/servos.c create mode 100644 src/main/flight/servos.h diff --git a/Makefile b/Makefile index fa0c53e5ad..d234823bce 100644 --- a/Makefile +++ b/Makefile @@ -508,6 +508,7 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/servos.c \ io/beeper.c \ io/serial.c \ io/serial_4way.c \ diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 64132e301e..2e74441969 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -29,6 +29,7 @@ #include "flight/failsafe.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/imu.h" #include "flight/navigation.h" @@ -65,10 +66,11 @@ typedef struct master_s { // motor/esc/servo related stuff motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS]; motorConfig_t motorConfig; - servoConfig_t servoConfig; flight3DConfig_t flight3DConfig; #ifdef USE_SERVOS + servoConfig_t servoConfig; + servoMixerConfig_t servoMixerConfig; servoMixer_t customServoMixer[MAX_SERVO_RULES]; // Servo-related stuff servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 92c9b27a5d..adf21ae917 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -72,6 +72,7 @@ #include "telemetry/telemetry.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" @@ -407,13 +408,17 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->yaw_motor_direction = 1; -#ifdef USE_SERVOS - mixerConfig->tri_unarmed_servo = 1; - mixerConfig->servo_lowpass_freq = 400; - mixerConfig->servo_lowpass_enable = 0; -#endif } +#ifdef USE_SERVOS +void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig) +{ + servoMixerConfig->tri_unarmed_servo = 1; + servoMixerConfig->servo_lowpass_freq = 400; + servoMixerConfig->servo_lowpass_enable = 0; +} +#endif + uint8_t getCurrentProfile(void) { return masterConfig.current_profile_index; @@ -573,13 +578,13 @@ void createDefaultConfig(master_t *config) config->auto_disarm_delay = 5; config->small_angle = 25; - resetMixerConfig(&config->mixerConfig); - config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo + resetMixerConfig(&config->mixerConfig); resetMotorConfig(&config->motorConfig); #ifdef USE_SERVOS + resetServoMixerConfig(&config->servoMixerConfig); resetServoConfig(&config->servoConfig); #endif resetFlight3DConfig(&config->flight3DConfig); @@ -774,7 +779,7 @@ void activateConfig(void) ); #ifdef USE_SERVOS - servoUseConfigs(masterConfig.servoConf, &masterConfig.gimbalConfig); + servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig); #endif imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f; diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 583b746192..48d234fe32 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -65,6 +65,7 @@ #include "scheduler/scheduler.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/failsafe.h" #include "flight/gtune.h" @@ -740,7 +741,7 @@ void subTaskMainSubprocesses(void) if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS - && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) + && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING @@ -794,6 +795,8 @@ void subTaskMotorUpdate(void) mixTable(¤tProfile->pidProfile); #ifdef USE_SERVOS + // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. + servoTable(); filterServos(); writeServos(); #endif diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c31f958a85..093cf1749a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -17,14 +17,12 @@ #include #include -#include #include #include #include "platform.h" #include "build/build_config.h" -#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -32,18 +30,11 @@ #include "drivers/system.h" #include "drivers/pwm_output.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/system.h" + +#include "io/motors.h" #include "rx/rx.h" -#include "io/gimbal.h" -#include "io/motors.h" -#include "io/servos.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" #include "sensors/battery.h" #include "flight/mixer.h" @@ -66,20 +57,12 @@ static mixerConfig_t *mixerConfig; static flight3DConfig_t *flight3DConfig; static motorConfig_t *motorConfig; static airplaneConfig_t *airplaneConfig; -static rxConfig_t *rxConfig; +rxConfig_t *rxConfig; static bool syncMotorOutputWithPidLoop = false; -static mixerMode_e currentMixerMode; +mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; -#ifdef USE_SERVOS -static uint8_t servoRuleCount = 0; -static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; -static gimbalConfig_t *gimbalConfig; -int16_t servo[MAX_SUPPORTED_SERVOS]; -static int useServo; -static servoParam_t *servoConf; -#endif static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -245,92 +228,6 @@ const mixer_t mixers[] = { }; #endif -#ifdef USE_SERVOS - -#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t)) -// mixer rule format servo, input, rate, speed, min, max, box -static const servoMixer_t servoMixerAirplane[] = { - { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, - { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerFlyingWing[] = { - { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerBI[] = { - { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerTri[] = { - { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerDual[] = { - { SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerSingle[] = { - { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, - { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerGimbal[] = { - { SERVO_GIMBAL_PITCH, INPUT_GIMBAL_PITCH, 125, 0, 0, 100, 0 }, - { SERVO_GIMBAL_ROLL, INPUT_GIMBAL_ROLL, 125, 0, 0, 100, 0 }, -}; - - -const mixerRules_t servoMixers[] = { - { 0, NULL }, // entry 0 - { COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI - { 0, NULL }, // MULTITYPE_QUADP - { 0, NULL }, // MULTITYPE_QUADX - { COUNT_SERVO_RULES(servoMixerBI), servoMixerBI }, // MULTITYPE_BI - { COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal }, // * MULTITYPE_GIMBAL - { 0, NULL }, // MULTITYPE_Y6 - { 0, NULL }, // MULTITYPE_HEX6 - { COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING - { 0, NULL }, // MULTITYPE_Y4 - { 0, NULL }, // MULTITYPE_HEX6X - { 0, NULL }, // MULTITYPE_OCTOX8 - { 0, NULL }, // MULTITYPE_OCTOFLATP - { 0, NULL }, // MULTITYPE_OCTOFLATX - { COUNT_SERVO_RULES(servoMixerAirplane), servoMixerAirplane }, // * MULTITYPE_AIRPLANE - { 0, NULL }, // * MULTITYPE_HELI_120_CCPM - { 0, NULL }, // * MULTITYPE_HELI_90_DEG - { 0, NULL }, // MULTITYPE_VTAIL4 - { 0, NULL }, // MULTITYPE_HEX6H - { 0, NULL }, // * MULTITYPE_PPM_TO_SERVO - { COUNT_SERVO_RULES(servoMixerDual), servoMixerDual }, // MULTITYPE_DUALCOPTER - { COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle }, // MULTITYPE_SINGLECOPTER - { 0, NULL }, // MULTITYPE_ATAIL4 - { 0, NULL }, // MULTITYPE_CUSTOM - { 0, NULL }, // MULTITYPE_CUSTOM_PLANE - { 0, NULL }, // MULTITYPE_CUSTOM_TRI - { 0, NULL }, -}; - -static servoMixer_t *customServoMixers; -#endif - static motorMixer_t *customMixers; static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow; @@ -370,51 +267,6 @@ void mixerUseConfigs( initEscEndpoints(); } -#ifdef USE_SERVOS -void servoUseConfigs(servoParam_t *servoConfToUse, gimbalConfig_t *gimbalConfigToUse) -{ - servoConf = servoConfToUse; - gimbalConfig = gimbalConfigToUse; -} - -int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) -{ - uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; - - if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { - return rcData[channelToForwardFrom]; - } - - return servoConf[servoIndex].middle; -} - - -int servoDirection(int servoIndex, int inputSource) -{ - // determine the direction (reversed or not) from the direction bitfield of the servo - if (servoConf[servoIndex].reversedSources & (1 << inputSource)) - return -1; - else - return 1; -} - -void servoMixerInit(servoMixer_t *initialCustomServoMixers) -{ - customServoMixers = initialCustomServoMixers; - - // enable servos for mixes that require them. note, this shifts motor counts. - useServo = mixers[currentMixerMode].useServo; - // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (feature(FEATURE_SERVO_TILT)) - useServo = 1; - - // give all servos a default command - for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = DEFAULT_SERVO_MIDDLE; - } -} -#endif - void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) { currentMixerMode = mixerMode; @@ -424,23 +276,6 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) #ifndef USE_QUAD_MIXER_ONLY -void loadCustomServoMixer(void) -{ - // reset settings - servoRuleCount = 0; - memset(currentServoMixer, 0, sizeof(currentServoMixer)); - - // load custom mixer into currentServoMixer - for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) { - // check if done - if (customServoMixers[i].rate == 0) - break; - - currentServoMixer[i] = customServoMixers[i]; - servoRuleCount++; - } -} - void mixerConfigureOutput(void) { int i; @@ -467,14 +302,6 @@ void mixerConfigureOutput(void) } } - if (useServo) { - servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; - if (servoMixers[currentMixerMode].rule) { - for (i = 0; i < servoRuleCount; i++) - currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; - } - } - // in 3D mode, mixer gain has to be halved if (feature(FEATURE_3D)) { if (motorCount > 1) { @@ -486,42 +313,9 @@ void mixerConfigureOutput(void) } } - // set flag that we're on something with wings - if (currentMixerMode == MIXER_FLYING_WING || - currentMixerMode == MIXER_AIRPLANE || - currentMixerMode == MIXER_CUSTOM_AIRPLANE - ) { - ENABLE_STATE(FIXED_WING); - - if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) { - loadCustomServoMixer(); - } - } else { - DISABLE_STATE(FIXED_WING); - - if (currentMixerMode == MIXER_CUSTOM_TRI) { - loadCustomServoMixer(); - } - } - mixerResetDisarmedMotors(); } - -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers) -{ - int i; - - // we're 1-based - index++; - // clear existing - for (i = 0; i < MAX_SERVO_RULES; i++) - customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0; - - for (i = 0; i < servoMixers[index].servoRuleCount; i++) - customServoMixers[i] = servoMixers[index].rule[i]; -} - void mixerLoadMix(int index, motorMixer_t *customMixers) { int i; @@ -561,90 +355,6 @@ void mixerResetDisarmedMotors(void) motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand; } -#ifdef USE_SERVOS - -STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) -{ - // start forwarding from this channel - uint8_t channelOffset = AUX1; - - uint8_t servoOffset; - for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { - pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); - } -} - -static void updateGimbalServos(uint8_t firstServoIndex) -{ - pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]); - pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]); -} - -void writeServos(void) -{ - uint8_t servoIndex = 0; - - switch (currentMixerMode) { - case MIXER_BICOPTER: - pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]); - pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]); - break; - - case MIXER_TRI: - case MIXER_CUSTOM_TRI: - if (mixerConfig->tri_unarmed_servo) { - // if unarmed flag set, we always move servo - pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); - } else { - // otherwise, only move servo when copter is armed - if (ARMING_FLAG(ARMED)) - pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); - else - pwmWriteServo(servoIndex++, 0); // kill servo signal completely. - } - break; - - case MIXER_FLYING_WING: - pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]); - pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]); - break; - - case MIXER_DUALCOPTER: - pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]); - pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); - break; - - case MIXER_CUSTOM_AIRPLANE: - case MIXER_AIRPLANE: - for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) { - pwmWriteServo(servoIndex++, servo[i]); - } - break; - - case MIXER_SINGLECOPTER: - for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) { - pwmWriteServo(servoIndex++, servo[i]); - } - break; - - default: - break; - } - - // Two servos for SERVO_TILT, if enabled - if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) { - updateGimbalServos(servoIndex); - servoIndex += 2; - } - - // forward AUX to remaining servo outputs (not constrained) - if (feature(FEATURE_CHANNEL_FORWARDING)) { - forwardAuxChannelsToServos(servoIndex); - servoIndex += MAX_AUX_CHANNEL_COUNT; - } -} -#endif - void writeMotors(void) { for (uint8_t i = 0; i < motorCount; i++) { @@ -679,91 +389,10 @@ void stopPwmAllMotors(void) delayMicroseconds(1500); } -#ifndef USE_QUAD_MIXER_ONLY -STATIC_UNIT_TESTED void servoMixer(void) -{ - int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] - static int16_t currentOutput[MAX_SERVO_RULES]; - uint8_t i; - - if (FLIGHT_MODE(PASSTHRU_MODE)) { - // Direct passthru from RX - input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; - input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; - input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; - } else { - // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui - input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL]; - input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH]; - input[INPUT_STABILIZED_YAW] = axisPIDf[YAW]; - - // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { - input[INPUT_STABILIZED_YAW] *= -1; - } - } - - input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); - input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); - - input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] - - // center the RC input value around the RC middle value - // by subtracting the RC middle value from the RC input value, we get: - // data - middle = input - // 2000 - 1500 = +500 - // 1500 - 1500 = 0 - // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; - input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; - input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; - input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; - input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; - input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; - input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) - servo[i] = 0; - - // mix servos according to rules - for (i = 0; i < servoRuleCount; i++) { - // consider rule if no box assigned or box is active - if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { - uint8_t target = currentServoMixer[i].targetChannel; - uint8_t from = currentServoMixer[i].inputSource; - uint16_t servo_width = servoConf[target].max - servoConf[target].min; - int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; - int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; - - if (currentServoMixer[i].speed == 0) - currentOutput[i] = input[from]; - else { - if (currentOutput[i] < input[from]) - currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]); - else if (currentOutput[i] > input[from]) - currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]); - } - - servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max); - } else { - currentOutput[i] = 0; - } - } - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; - servo[i] += determineServoMiddleOrForwardFromChannel(i); - } -} - -#endif - -void mixTable(void *pidProfilePtr) +void mixTable(pidProfile_t *pidProfile) { uint32_t i = 0; float vbatCompensationFactor = 1; - pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr; // Scale roll/pitch/yaw uniformly to fit within throttle range // Initial mixer concept by bdoiron74 reused and optimized for Air Mode @@ -885,94 +514,4 @@ void mixTable(void *pidProfilePtr) } } } - - // motor outputs are used as sources for servo mixing, so motors must be calculated before servos. - -#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) - - // airplane / servo mixes - switch (currentMixerMode) { - case MIXER_CUSTOM_AIRPLANE: - case MIXER_FLYING_WING: - case MIXER_AIRPLANE: - case MIXER_BICOPTER: - case MIXER_CUSTOM_TRI: - case MIXER_TRI: - case MIXER_DUALCOPTER: - case MIXER_SINGLECOPTER: - case MIXER_GIMBAL: - servoMixer(); - break; - - /* - case MIXER_GIMBAL: - servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); - servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); - break; - */ - - default: - break; - } - - // camera stabilization - if (feature(FEATURE_SERVO_TILT)) { - // center at fixed position, or vary either pitch or roll by RC channel - servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); - servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); - - if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { - if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { - servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; - servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; - } else { - servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; - servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; - } - } - } - - // constrain servos - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values - } -#endif } - -#ifdef USE_SERVOS -bool isMixerUsingServos(void) -{ - return useServo; -} -#endif - -void filterServos(void) -{ -#ifdef USE_SERVOS - static int16_t servoIdx; - static bool servoFilterIsSet; - static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; - -#if defined(MIXER_DEBUG) - uint32_t startTime = micros(); -#endif - - if (mixerConfig->servo_lowpass_enable) { - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - if (!servoFilterIsSet) { - biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); - servoFilterIsSet = true; - } - - servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); - // Sanity check - servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); - } - } -#if defined(MIXER_DEBUG) - debug[0] = (int16_t)(micros() - startTime); -#endif - -#endif -} - diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 57f2ce0fc7..cf9c37fd85 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -18,7 +18,6 @@ #pragma once #define MAX_SUPPORTED_MOTORS 12 -#define MAX_SUPPORTED_SERVOS 8 #define QUAD_MOTOR_COUNT 4 @@ -77,11 +76,6 @@ typedef struct mixer_s { typedef struct mixerConfig_s { int8_t yaw_motor_direction; -#ifdef USE_SERVOS - uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq - int8_t servo_lowpass_enable; // enable/disable lowpass filter -#endif } mixerConfig_t; typedef struct flight3DConfig_s { @@ -97,105 +91,6 @@ typedef struct airplaneConfig_s { #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF -#ifdef USE_SERVOS - -// These must be consecutive, see 'reversedSources' -enum { - INPUT_STABILIZED_ROLL = 0, - INPUT_STABILIZED_PITCH, - INPUT_STABILIZED_YAW, - INPUT_STABILIZED_THROTTLE, - INPUT_RC_ROLL, - INPUT_RC_PITCH, - INPUT_RC_YAW, - INPUT_RC_THROTTLE, - INPUT_RC_AUX1, - INPUT_RC_AUX2, - INPUT_RC_AUX3, - INPUT_RC_AUX4, - INPUT_GIMBAL_PITCH, - INPUT_GIMBAL_ROLL, - - INPUT_SOURCE_COUNT -} inputSource_e; - -// target servo channels -typedef enum { - SERVO_GIMBAL_PITCH = 0, - SERVO_GIMBAL_ROLL = 1, - SERVO_FLAPS = 2, - SERVO_FLAPPERON_1 = 3, - SERVO_FLAPPERON_2 = 4, - SERVO_RUDDER = 5, - SERVO_ELEVATOR = 6, - SERVO_THROTTLE = 7, // for internal combustion (IC) planes - - SERVO_BICOPTER_LEFT = 4, - SERVO_BICOPTER_RIGHT = 5, - - SERVO_DUALCOPTER_LEFT = 4, - SERVO_DUALCOPTER_RIGHT = 5, - - SERVO_SINGLECOPTER_1 = 3, - SERVO_SINGLECOPTER_2 = 4, - SERVO_SINGLECOPTER_3 = 5, - SERVO_SINGLECOPTER_4 = 6, - -} servoIndex_e; // FIXME rename to servoChannel_e - -#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS -#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE - -#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT -#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT - -#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1 -#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4 - -#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1 -#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2 - -typedef struct servoMixer_s { - uint8_t targetChannel; // servo that receives the output of the rule - uint8_t inputSource; // input channel for this rule - int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction - uint8_t speed; // reduces the speed of the rule, 0=unlimited speed - int8_t min; // lower bound of rule range [0;100]% of servo max-min - int8_t max; // lower bound of rule range [0;100]% of servo max-min - uint8_t box; // active rule if box is enabled, range [0;3], 0=no box, 1=BOXSERVO1, 2=BOXSERVO2, 3=BOXSERVO3 -} servoMixer_t; - -#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) -#define MAX_SERVO_SPEED UINT8_MAX -#define MAX_SERVO_BOXES 3 - -// Custom mixer configuration -typedef struct mixerRules_s { - uint8_t servoRuleCount; - const servoMixer_t *rule; -} mixerRules_t; - -typedef struct servoParam_s { - int16_t min; // servo min - int16_t max; // servo max - int16_t middle; // servo middle - int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction - uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. - uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. - int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED - uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted -} __attribute__ ((__packed__)) servoParam_t; - -struct gimbalConfig_s; -struct motorConfig_s; -struct rxConfig_s; - -extern int16_t servo[MAX_SUPPORTED_SERVOS]; -bool isMixerUsingServos(void); -void writeServos(void); -void filterServos(void); -#endif - extern const mixer_t mixers[]; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; @@ -211,21 +106,13 @@ void mixerUseConfigs( void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); -#ifdef USE_SERVOS -void servoMixerInit(servoMixer_t *customServoMixers); -struct servoParam_s; -struct gimbalConfig_s; -void servoUseConfigs(struct servoParam_s *servoConfToUse, struct gimbalConfig_s *gimbalConfigToUse); -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); -void loadCustomServoMixer(void); -int servoDirection(int servoIndex, int fromChannel); -#endif void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); void mixerConfigureOutput(void); void mixerResetDisarmedMotors(void); -void mixTable(void *pidProfilePtr); +struct pidProfile_s; +void mixTable(struct pidProfile_s *pidProfile); void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c new file mode 100755 index 0000000000..0469238cbb --- /dev/null +++ b/src/main/flight/servos.c @@ -0,0 +1,490 @@ +/* + * 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 + +#include "platform.h" + +#ifdef USE_SERVOS + +#include "build/build_config.h" + +#include "common/filter.h" + +#include "drivers/pwm_output.h" +#include "drivers/system.h" + +#include "rx/rx.h" + +#include "io/gimbal.h" +#include "io/servos.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "config/feature.h" + +extern mixerMode_e currentMixerMode; +extern rxConfig_t *rxConfig; + +static servoMixerConfig_t *servoMixerConfig; + +static uint8_t servoRuleCount = 0; +static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; +static gimbalConfig_t *gimbalConfig; +int16_t servo[MAX_SUPPORTED_SERVOS]; +static int useServo; +static servoParam_t *servoConf; + + +#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t)) +// mixer rule format servo, input, rate, speed, min, max, box +static const servoMixer_t servoMixerAirplane[] = { + { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, + { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, + { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, + { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, + { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, +}; + +static const servoMixer_t servoMixerFlyingWing[] = { + { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, + { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, + { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 }, + { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, + { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, +}; + +static const servoMixer_t servoMixerBI[] = { + { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, + { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, + { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, + { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, +}; + +static const servoMixer_t servoMixerTri[] = { + { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, +}; + +static const servoMixer_t servoMixerDual[] = { + { SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, + { SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, +}; + +static const servoMixer_t servoMixerSingle[] = { + { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, + { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, + { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, + { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, + { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, + { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, + { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, + { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, +}; + +static const servoMixer_t servoMixerGimbal[] = { + { SERVO_GIMBAL_PITCH, INPUT_GIMBAL_PITCH, 125, 0, 0, 100, 0 }, + { SERVO_GIMBAL_ROLL, INPUT_GIMBAL_ROLL, 125, 0, 0, 100, 0 }, +}; + + +const mixerRules_t servoMixers[] = { + { 0, NULL }, // entry 0 + { COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI + { 0, NULL }, // MULTITYPE_QUADP + { 0, NULL }, // MULTITYPE_QUADX + { COUNT_SERVO_RULES(servoMixerBI), servoMixerBI }, // MULTITYPE_BI + { COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal }, // * MULTITYPE_GIMBAL + { 0, NULL }, // MULTITYPE_Y6 + { 0, NULL }, // MULTITYPE_HEX6 + { COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING + { 0, NULL }, // MULTITYPE_Y4 + { 0, NULL }, // MULTITYPE_HEX6X + { 0, NULL }, // MULTITYPE_OCTOX8 + { 0, NULL }, // MULTITYPE_OCTOFLATP + { 0, NULL }, // MULTITYPE_OCTOFLATX + { COUNT_SERVO_RULES(servoMixerAirplane), servoMixerAirplane }, // * MULTITYPE_AIRPLANE + { 0, NULL }, // * MULTITYPE_HELI_120_CCPM + { 0, NULL }, // * MULTITYPE_HELI_90_DEG + { 0, NULL }, // MULTITYPE_VTAIL4 + { 0, NULL }, // MULTITYPE_HEX6H + { 0, NULL }, // * MULTITYPE_PPM_TO_SERVO + { COUNT_SERVO_RULES(servoMixerDual), servoMixerDual }, // MULTITYPE_DUALCOPTER + { COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle }, // MULTITYPE_SINGLECOPTER + { 0, NULL }, // MULTITYPE_ATAIL4 + { 0, NULL }, // MULTITYPE_CUSTOM + { 0, NULL }, // MULTITYPE_CUSTOM_PLANE + { 0, NULL }, // MULTITYPE_CUSTOM_TRI + { 0, NULL }, +}; + +static servoMixer_t *customServoMixers; + +void servoUseConfigs(servoMixerConfig_t *servoMixerConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse) +{ + servoMixerConfig = servoMixerConfigToUse; + servoConf = servoParamsToUse; + gimbalConfig = gimbalConfigToUse; +} + +int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) +{ + uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; + + if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { + return rcData[channelToForwardFrom]; + } + + return servoConf[servoIndex].middle; +} + + +int servoDirection(int servoIndex, int inputSource) +{ + // determine the direction (reversed or not) from the direction bitfield of the servo + if (servoConf[servoIndex].reversedSources & (1 << inputSource)) + return -1; + else + return 1; +} + +void servoMixerInit(servoMixer_t *initialCustomServoMixers) +{ + customServoMixers = initialCustomServoMixers; + + // enable servos for mixes that require them. note, this shifts motor counts. + useServo = mixers[currentMixerMode].useServo; + // if we want camstab/trig, that also enables servos, even if mixer doesn't + if (feature(FEATURE_SERVO_TILT)) + useServo = 1; + + // give all servos a default command + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = DEFAULT_SERVO_MIDDLE; + } +} + +void loadCustomServoMixer(void) +{ + // reset settings + servoRuleCount = 0; + memset(currentServoMixer, 0, sizeof(currentServoMixer)); + + // load custom mixer into currentServoMixer + for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (customServoMixers[i].rate == 0) + break; + + currentServoMixer[i] = customServoMixers[i]; + servoRuleCount++; + } +} + +void servoConfigureOutput(void) +{ + if (useServo) { + servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; + if (servoMixers[currentMixerMode].rule) { + for (int i = 0; i < servoRuleCount; i++) + currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; + } + } + + // set flag that we're on something with wings + if (currentMixerMode == MIXER_FLYING_WING || + currentMixerMode == MIXER_AIRPLANE || + currentMixerMode == MIXER_CUSTOM_AIRPLANE + ) { + ENABLE_STATE(FIXED_WING); + + if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) { + loadCustomServoMixer(); + } + } else { + DISABLE_STATE(FIXED_WING); + + if (currentMixerMode == MIXER_CUSTOM_TRI) { + loadCustomServoMixer(); + } + } +} + + +void servoMixerLoadMix(int index, servoMixer_t *customServoMixers) +{ + int i; + + // we're 1-based + index++; + // clear existing + for (i = 0; i < MAX_SERVO_RULES; i++) + customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0; + + for (i = 0; i < servoMixers[index].servoRuleCount; i++) + customServoMixers[i] = servoMixers[index].rule[i]; +} + +STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) +{ + // start forwarding from this channel + uint8_t channelOffset = AUX1; + + uint8_t servoOffset; + for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { + pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); + } +} + +static void updateGimbalServos(uint8_t firstServoIndex) +{ + pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]); + pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]); +} + +void writeServos(void) +{ + uint8_t servoIndex = 0; + + switch (currentMixerMode) { + case MIXER_BICOPTER: + pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]); + pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]); + break; + + case MIXER_TRI: + case MIXER_CUSTOM_TRI: + if (servoMixerConfig->tri_unarmed_servo) { + // if unarmed flag set, we always move servo + pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); + } else { + // otherwise, only move servo when copter is armed + if (ARMING_FLAG(ARMED)) + pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); + else + pwmWriteServo(servoIndex++, 0); // kill servo signal completely. + } + break; + + case MIXER_FLYING_WING: + pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]); + pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]); + break; + + case MIXER_DUALCOPTER: + pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]); + pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); + break; + + case MIXER_CUSTOM_AIRPLANE: + case MIXER_AIRPLANE: + for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) { + pwmWriteServo(servoIndex++, servo[i]); + } + break; + + case MIXER_SINGLECOPTER: + for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) { + pwmWriteServo(servoIndex++, servo[i]); + } + break; + + default: + break; + } + + // Two servos for SERVO_TILT, if enabled + if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) { + updateGimbalServos(servoIndex); + servoIndex += 2; + } + + // forward AUX to remaining servo outputs (not constrained) + if (feature(FEATURE_CHANNEL_FORWARDING)) { + forwardAuxChannelsToServos(servoIndex); + servoIndex += MAX_AUX_CHANNEL_COUNT; + } +} + +STATIC_UNIT_TESTED void servoMixer(void) +{ + int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] + static int16_t currentOutput[MAX_SERVO_RULES]; + uint8_t i; + + if (FLIGHT_MODE(PASSTHRU_MODE)) { + // Direct passthru from RX + input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; + input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; + input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; + } else { + // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui + input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL]; + input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH]; + input[INPUT_STABILIZED_YAW] = axisPIDf[YAW]; + + // Reverse yaw servo when inverted in 3D mode + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + input[INPUT_STABILIZED_YAW] *= -1; + } + } + + input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); + input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); + + input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] + + // center the RC input value around the RC middle value + // by subtracting the RC middle value from the RC input value, we get: + // data - middle = input + // 2000 - 1500 = +500 + // 1500 - 1500 = 0 + // 1000 - 1500 = -500 + input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; + input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; + input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; + input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; + input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; + input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; + input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; + input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; + + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) + servo[i] = 0; + + // mix servos according to rules + for (i = 0; i < servoRuleCount; i++) { + // consider rule if no box assigned or box is active + if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { + uint8_t target = currentServoMixer[i].targetChannel; + uint8_t from = currentServoMixer[i].inputSource; + uint16_t servo_width = servoConf[target].max - servoConf[target].min; + int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; + int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; + + if (currentServoMixer[i].speed == 0) + currentOutput[i] = input[from]; + else { + if (currentOutput[i] < input[from]) + currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]); + else if (currentOutput[i] > input[from]) + currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]); + } + + servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max); + } else { + currentOutput[i] = 0; + } + } + + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; + servo[i] += determineServoMiddleOrForwardFromChannel(i); + } +} + + +void servoTable(void) +{ + // airplane / servo mixes + switch (currentMixerMode) { + case MIXER_CUSTOM_AIRPLANE: + case MIXER_FLYING_WING: + case MIXER_AIRPLANE: + case MIXER_BICOPTER: + case MIXER_CUSTOM_TRI: + case MIXER_TRI: + case MIXER_DUALCOPTER: + case MIXER_SINGLECOPTER: + case MIXER_GIMBAL: + servoMixer(); + break; + + /* + case MIXER_GIMBAL: + servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); + servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); + break; + */ + + default: + break; + } + + // camera stabilization + if (feature(FEATURE_SERVO_TILT)) { + // center at fixed position, or vary either pitch or roll by RC channel + servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); + servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); + + if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { + if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { + servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + } else { + servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; + servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + } + } + } + + // constrain servos + for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values + } +} + +bool isMixerUsingServos(void) +{ + return useServo; +} + +void filterServos(void) +{ + static int16_t servoIdx; + static bool servoFilterIsSet; + static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; + +#if defined(MIXER_DEBUG) + uint32_t startTime = micros(); +#endif + + if (servoMixerConfig->servo_lowpass_enable) { + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + if (!servoFilterIsSet) { + biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime); + servoFilterIsSet = true; + } + + servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); + // Sanity check + servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); + } + } +#if defined(MIXER_DEBUG) + debug[0] = (int16_t)(micros() - startTime); +#endif +} +#endif diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h new file mode 100644 index 0000000000..66f86955b6 --- /dev/null +++ b/src/main/flight/servos.h @@ -0,0 +1,129 @@ +/* + * 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 MAX_SUPPORTED_SERVOS 8 + +// These must be consecutive, see 'reversedSources' +enum { + INPUT_STABILIZED_ROLL = 0, + INPUT_STABILIZED_PITCH, + INPUT_STABILIZED_YAW, + INPUT_STABILIZED_THROTTLE, + INPUT_RC_ROLL, + INPUT_RC_PITCH, + INPUT_RC_YAW, + INPUT_RC_THROTTLE, + INPUT_RC_AUX1, + INPUT_RC_AUX2, + INPUT_RC_AUX3, + INPUT_RC_AUX4, + INPUT_GIMBAL_PITCH, + INPUT_GIMBAL_ROLL, + + INPUT_SOURCE_COUNT +} inputSource_e; + +// target servo channels +typedef enum { + SERVO_GIMBAL_PITCH = 0, + SERVO_GIMBAL_ROLL = 1, + SERVO_FLAPS = 2, + SERVO_FLAPPERON_1 = 3, + SERVO_FLAPPERON_2 = 4, + SERVO_RUDDER = 5, + SERVO_ELEVATOR = 6, + SERVO_THROTTLE = 7, // for internal combustion (IC) planes + + SERVO_BICOPTER_LEFT = 4, + SERVO_BICOPTER_RIGHT = 5, + + SERVO_DUALCOPTER_LEFT = 4, + SERVO_DUALCOPTER_RIGHT = 5, + + SERVO_SINGLECOPTER_1 = 3, + SERVO_SINGLECOPTER_2 = 4, + SERVO_SINGLECOPTER_3 = 5, + SERVO_SINGLECOPTER_4 = 6, + +} servoIndex_e; // FIXME rename to servoChannel_e + +#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS +#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE + +#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT +#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT + +#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1 +#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4 + +#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1 +#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2 + +typedef struct servoMixer_s { + uint8_t targetChannel; // servo that receives the output of the rule + uint8_t inputSource; // input channel for this rule + int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction + uint8_t speed; // reduces the speed of the rule, 0=unlimited speed + int8_t min; // lower bound of rule range [0;100]% of servo max-min + int8_t max; // lower bound of rule range [0;100]% of servo max-min + uint8_t box; // active rule if box is enabled, range [0;3], 0=no box, 1=BOXSERVO1, 2=BOXSERVO2, 3=BOXSERVO3 +} servoMixer_t; + +#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) +#define MAX_SERVO_SPEED UINT8_MAX +#define MAX_SERVO_BOXES 3 + +// Custom mixer configuration +typedef struct mixerRules_s { + uint8_t servoRuleCount; + const servoMixer_t *rule; +} mixerRules_t; + +typedef struct servoParam_s { + int16_t min; // servo min + int16_t max; // servo max + int16_t middle; // servo middle + int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction + uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. + uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. + int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED + uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted +} __attribute__ ((__packed__)) servoParam_t; + +typedef struct servoMixerConfig_s{ + uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq + int8_t servo_lowpass_enable; // enable/disable lowpass filter +} servoMixerConfig_t; + +extern int16_t servo[MAX_SUPPORTED_SERVOS]; + +void servoTable(void); +bool isMixerUsingServos(void); +void writeServos(void); +void filterServos(void); + +void servoMixerInit(servoMixer_t *customServoMixers); +struct gimbalConfig_s; +void servoUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse); +void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); +void loadCustomServoMixer(void); +void servoConfigureOutput(void); +int servoDirection(int servoIndex, int fromChannel); + diff --git a/src/main/io/motors.h b/src/main/io/motors.h index d7fedf85c5..d90c0a9e1b 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "drivers/io_types.h" #include "flight/mixer.h" typedef struct motorConfig_s { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 70ba4fbda0..76c2a0164e 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -823,9 +823,9 @@ const clivalue_t valueTable[] = { { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, #ifdef USE_SERVOS { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, - { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, - { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, + { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, + { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoMixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, + { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoPwmRate, .config.minmax = { 50, 498 } }, { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } }, #endif diff --git a/src/main/io/servos.h b/src/main/io/servos.h index 311b0c5798..711ceec755 100644 --- a/src/main/io/servos.h +++ b/src/main/io/servos.h @@ -17,8 +17,8 @@ #pragma once -#include "drivers/io.h" -#include "flight/mixer.h" +#include "drivers/io_types.h" +#include "flight/servos.h" typedef struct servoConfig_s { // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms) diff --git a/src/main/main.c b/src/main/main.c index ae8e88d179..e84a14db5a 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -288,7 +288,7 @@ void init(void) #endif mixerConfigureOutput(); - // pwmInit() needs to be called as soon as possible for ESC compatibility reasons + servoConfigureOutput(); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER From 6b457e5da10abd339287641c30c624fd62454a13 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 25 Oct 2016 20:21:29 +1100 Subject: [PATCH 14/20] Fixes for NAZE (and potentially other target) uart issues --- src/main/config/config_eeprom.h | 2 +- src/main/fc/config.c | 4 ++++ src/main/target/NAZE/target.h | 6 +++--- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index b48961f91f..3202389bdd 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,7 +17,7 @@ #pragma once -#define EEPROM_CONF_VERSION 143 +#define EEPROM_CONF_VERSION 144 void initEEPROM(void); void writeEEPROM(); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 92c9b27a5d..74e5b2194e 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -380,7 +380,11 @@ void resetSerialConfig(serialConfig_t *serialConfig) for (index = 0; index < SERIAL_PORT_COUNT; index++) { serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index]; +#ifdef USE_VCP serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_500000; +#else + serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200; +#endif serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600; serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO; serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index ec9358222c..eb8ab5503a 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -122,10 +122,11 @@ #define USE_UART1 #define USE_UART2 -#define USE_UART3 +/* only 2 uarts available on the NAZE, add ifdef here if present on other boards */ +//#define USE_UART3 //#define USE_SOFTSERIAL1 //#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 3 +#define SERIAL_PORT_COUNT 2 //#define SOFTSERIAL_1_TIMER TIM3 //#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 @@ -134,7 +135,6 @@ //#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 //#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 -// USART3 only on NAZE32_SP - Flex Port #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 From 4a4daedb1a9654784ffd780b3953c52b2b953946 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 25 Oct 2016 21:03:40 +1100 Subject: [PATCH 15/20] dma_stm32f4xx warning fixed --- src/main/drivers/dma_stm32f4xx.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index fe3b7f4f66..4147f29c07 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -99,4 +99,5 @@ uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream) RETURN_TCIF_FLAG(stream, 5); RETURN_TCIF_FLAG(stream, 6); RETURN_TCIF_FLAG(stream, 7); + return 0; } \ No newline at end of file From f79896b1bd7de0c697100e914aa5488fc035b33a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 25 Oct 2016 01:09:06 +0200 Subject: [PATCH 16/20] Add DSHOT300 Bool check for dshot protocol Add dshot300 to init ident --- src/main/drivers/pwm_output.c | 1 + src/main/drivers/pwm_output.h | 1 + src/main/drivers/pwm_output_stm32f3xx.c | 17 +++++++++++++++-- src/main/drivers/pwm_output_stm32f4xx.c | 15 ++++++++++++++- src/main/drivers/pwm_output_stm32f7xx.c | 15 ++++++++++++++- src/main/fc/config.c | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/flight/mixer.c | 13 ++++++++++--- src/main/flight/mixer.h | 1 + src/main/io/serial_cli.c | 2 +- 10 files changed, 59 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 59ddad65e9..ae9aa3ede0 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -189,6 +189,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; #ifdef USE_DSHOT case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 0c1a61832a..53a0a7fc4b 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,6 +28,7 @@ typedef enum { PWM_TYPE_MULTISHOT, PWM_TYPE_BRUSHED, PWM_TYPE_DSHOT600, + PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT150, PWM_TYPE_MAX } motorPwmProtocolTypes_e; diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index f45b0483e7..80903ce19f 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -33,6 +33,7 @@ #define MAX_DMA_TIMERS 8 #define MOTOR_DSHOT600_MHZ 24 +#define MOTOR_DSHOT300_MHZ 12 #define MOTOR_DSHOT150_MHZ 6 #define MOTOR_BIT_0 14 @@ -121,8 +122,20 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - - uint32_t hz = (pwmProtocolType == PWM_TYPE_DSHOT600 ? MOTOR_DSHOT600_MHZ : MOTOR_DSHOT150_MHZ) * 1000000; + + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } + TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1); TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index f5a92fb371..7e60c62f1d 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -34,6 +34,7 @@ #define MAX_DMA_TIMERS 8 #define MOTOR_DSHOT600_MHZ 12 +#define MOTOR_DSHOT300_MHZ 6 #define MOTOR_DSHOT150_MHZ 3 #define MOTOR_BIT_0 7 @@ -123,7 +124,19 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz = (pwmProtocolType == PWM_TYPE_DSHOT600 ? MOTOR_DSHOT600_MHZ : MOTOR_DSHOT150_MHZ) * 1000000; + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } + TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1; TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 4d47bfc990..523b993df2 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -33,6 +33,7 @@ #define MAX_DMA_TIMERS 8 #define MOTOR_DSHOT600_MHZ 12 +#define MOTOR_DSHOT300_MHZ 6 #define MOTOR_DSHOT150_MHZ 3 #define MOTOR_BIT_0 7 @@ -128,7 +129,19 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t if (configureTimer) { RCC_ClockCmd(timerRCC(timer), ENABLE); - uint32_t hz = (pwmProtocolType == PWM_TYPE_DSHOT600 ? MOTOR_DSHOT600_MHZ : MOTOR_DSHOT150_MHZ) * 1000000; + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } + motor->TimHandle.Instance = timerHardware->tim; motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;; motor->TimHandle.Init.Period = MOTOR_BITLENGTH; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 92c9b27a5d..d983143ff2 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -259,7 +259,7 @@ void resetMotorConfig(motorConfig_t *motorConfig) #endif motorConfig->maxthrottle = 2000; motorConfig->mincommand = 1000; - motorConfig->digitalIdleOffset = 0; + motorConfig->digitalIdleOffset = 40; uint8_t motorIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c469175535..0c264ca6cf 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -653,7 +653,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, 0); continue; } - if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_DSHOT150 || masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_DSHOT600) + if (isMotorProtocolDshot()) sbufWriteU16(dst, constrain((motor[i] / 2) + 1000, 1000, 2000)); // This is to get it working in the configurator else sbufWriteU16(dst, motor[i]); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c31f958a85..3c5bf25f65 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -336,9 +336,16 @@ static motorMixer_t *customMixers; static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow; static float rcCommandThrottleRange; +bool isMotorProtocolDshot(void) { + if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) + return true; + else + return false; +} + // Add here scaled ESC outputs for digital protol void initEscEndpoints(void) { - if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) { + if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset; maxMotorOutputNormal = DSHOT_MAX_THROTTLE; @@ -843,7 +850,7 @@ void mixTable(void *pidProfilePtr) motor[i] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) ); if (failsafeIsActive()) { - if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) + if (isMotorProtocolDshot()) motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax); @@ -875,7 +882,7 @@ void mixTable(void *pidProfilePtr) // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { - if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) { + if (isMotorProtocolDshot()) { motor[i] = (motor_disarmed[i] < motorOutputMin) ? disarmMotorOutput : motor_disarmed[i]; // Prevent getting into special reserved range if (motor_disarmed[i] != disarmMotorOutput) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 57f2ce0fc7..43ef84185a 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -230,3 +230,4 @@ void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); void stopPwmAllMotors(void); +bool isMotorProtocolDshot(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 70ba4fbda0..88dcd5a0a3 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -525,7 +525,7 @@ static const char * const lookupTableSuperExpoYaw[] = { static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", #ifdef USE_DSHOT - "DSHOT600", "DSHOT150" + "DSHOT600", "DSHOT300", "DSHOT150" #endif }; From 50e9b19fab2b47e229fc9e498260971cabae1fd1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 25 Oct 2016 14:41:05 +0200 Subject: [PATCH 17/20] Remove OLEMEXINO // Fix CJMCU build --- .travis.yml | 3 - src/main/fc/rc_controls.c | 1 + src/main/main.c | 2 + src/main/target/CJMCU/target.h | 4 +- src/main/target/OLIMEXINO/target.c | 41 ------------- src/main/target/OLIMEXINO/target.h | 95 ----------------------------- src/main/target/OLIMEXINO/target.mk | 10 --- 7 files changed, 5 insertions(+), 151 deletions(-) delete mode 100644 src/main/target/OLIMEXINO/target.c delete mode 100644 src/main/target/OLIMEXINO/target.h delete mode 100644 src/main/target/OLIMEXINO/target.mk diff --git a/.travis.yml b/.travis.yml index 4e7a7b408b..8ba47b575c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,7 +21,6 @@ env: # - TARGET=COLIBRI_OPBL # - TARGET=COLIBRI_RACE # - TARGET=DOGE -# - TARGET=EUSTM32F103RC # - TARGET=F4BY # - TARGET=FURYF3 - TARGET=FURYF4 @@ -32,11 +31,9 @@ env: # - TARGET=MICROSCISKY # - TARGET=MOTOLAB - TARGET=NAZE -# - TARGET=OLIMEXINO # - TARGET=OMNIBUS # - TARGET=OMNIBUSF4 # - TARGET=PIKOBLX -# - TARGET=PORT103R # - TARGET=RACEBASE - TARGET=REVO # - TARGET=REVONANO diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 314ee07ecd..1dbf51b70a 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -79,6 +79,7 @@ bool isAirmodeActive(void) { void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { #ifndef BLACKBOX +#define UNUSED(x) (void)(x) UNUSED(adjustmentFunction); UNUSED(newValue); #else diff --git a/src/main/main.c b/src/main/main.c index e84a14db5a..49ecdba993 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -288,7 +288,9 @@ void init(void) #endif mixerConfigureOutput(); +#ifdef USE_SERVOS servoConfigureOutput(); +#endif systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 86c6b88f36..f47123b1b0 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -32,8 +32,8 @@ #define ACC #define USE_ACC_MPU6050 -#define MAG -#define USE_MAG_HMC5883 +//#define MAG +//#define USE_MAG_HMC5883 #define BRUSHED_MOTORS diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c deleted file mode 100644 index 51f6b87ef7..0000000000 --- a/src/main/target/OLIMEXINO/target.c +++ /dev/null @@ -1,41 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include - -#include -#include "drivers/io.h" - -#include "drivers/timer.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 -}; - diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h deleted file mode 100644 index ae8ac3b38e..0000000000 --- a/src/main/target/OLIMEXINO/target.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "OLI1" // Olimexino - -//#define OLIMEXINO_UNCUT_LED1_E_JUMPER -//#define OLIMEXINO_UNCUT_LED2_E_JUMPER - -#ifdef OLIMEXINO_UNCUT_LED1_E_JUMPER -#define LED0 PA5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green -#endif - -#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER -// "LED2" is using one of the PWM pins (CH2/PWM2), so we must not use PWM2 unless the jumper is cut. @See pwmInit() -#define LED1 PA1 // D3, PA1/UART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow -#endif - -#define GYRO -#define USE_FAKE_GYRO -//#define USE_GYRO_L3G4200D -//#define USE_GYRO_L3GD20 -//#define USE_GYRO_MPU3050 -#define USE_GYRO_MPU6050 -//#define USE_GYRO_SPI_MPU6000 -//#define USE_GYRO_SPI_MPU6500 - -#define ACC -#define USE_FAKE_ACC -//#define USE_ACC_ADXL345 -//#define USE_ACC_BMA280 -//#define USE_ACC_MMA8452 -//#define USE_ACC_LSM303DLHC -#define USE_ACC_MPU6050 -//#define USE_ACC_SPI_MPU6000 -//#define USE_ACC_SPI_MPU6500 - -#define BARO -//#define USE_BARO_MS5611 -#define USE_BARO_BMP085 -#define USE_BARO_BMP280 - -#define MAG -#define USE_MAG_HMC5883 - -#define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 - -#define USE_UART1 -#define USE_UART2 -#define USE_SOFTSERIAL1 -#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 - -#define SOFTSERIAL_1_TIMER TIM3 -#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 -#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 -#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 -#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 - -#define USE_I2C -#define I2C_DEVICE (I2CDEV_2) - -#define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 - - -// IO - assuming all IOs on smt32f103rb LQFP64 package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) - -#define USABLE_TIMER_CHANNEL_COUNT 14 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/OLIMEXINO/target.mk b/src/main/target/OLIMEXINO/target.mk deleted file mode 100644 index 512ba68ccb..0000000000 --- a/src/main/target/OLIMEXINO/target.mk +++ /dev/null @@ -1,10 +0,0 @@ -F1_TARGETS += $(TARGET) -FEATURES = HIGHEND - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/compass_hmc5883l.c - From b410200f0cb80670e2507a7207638684e423e209 Mon Sep 17 00:00:00 2001 From: Bas Huisman Date: Tue, 25 Oct 2016 14:58:47 +0200 Subject: [PATCH 18/20] fix battery and current monitoring for sparky2 --- src/main/target/SPARKY2/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 9ef673ff5c..041d1906b6 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -112,6 +112,8 @@ //#define I2C_DEVICE_EXT (I2CDEV_2) #define USE_ADC +#define VBAT_ADC_PIN PC3 +#define CURRENT_METER_ADC_PIN PC2 #define LED_STRIP #define LED_STRIP_TIMER TIM5 From 5fef78e6c786f5105f954ee18da0c579be7e3a9d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 25 Oct 2016 16:16:17 +0200 Subject: [PATCH 19/20] Fix unsynced PWM --- src/main/drivers/pwm_output.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ae9aa3ede0..46f082a1dc 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -231,7 +231,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot motors[motorIndex].pwmWritePtr = pwmWritePtr; if (useUnsyncedPwm) { const uint32_t hz = timerMhzCounter * 1000000; - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmProtocol, idlePulse); + pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse); } else { pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0); } From 846705afda584aa681de42011eb5fb41974ac9be Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 25 Oct 2016 16:30:59 +0200 Subject: [PATCH 20/20] Delete stm32.mak --- stm32.mak | 33 --------------------------------- 1 file changed, 33 deletions(-) delete mode 100755 stm32.mak diff --git a/stm32.mak b/stm32.mak deleted file mode 100755 index d60c48e612..0000000000 --- a/stm32.mak +++ /dev/null @@ -1,33 +0,0 @@ -#This file is generated by VisualGDB. -#It contains GCC settings automatically derived from the board support package (BSP). -#DO NOT EDIT MANUALLY. THE FILE WILL BE OVERWRITTEN. -#Use VisualGDB Project Properties dialog or modify Makefile or per-configuration .mak files instead. - -#VisualGDB provides BSP_ROOT and TOOLCHAIN_ROOT via environment when running Make. The line below will only be active if GNU Make is started manually. -BSP_ROOT ?= $(LOCALAPPDATA)/VisualGDB/EmbeddedBSPs/arm-eabi/com.sysprogs.arm.stm32 -EFP_BASE ?= $(LOCALAPPDATA)/VisualGDB/EmbeddedEFPs -TOOLCHAIN_ROOT ?= C:/Program Files (x86)/GNU Tools ARM Embedded/5.4 2016q2 - -#Embedded toolchain -CC := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-gcc.exe -CXX := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-g++.exe -LD := $(CXX) -AR := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-ar.exe -OBJCOPY := $(TOOLCHAIN_ROOT)/bin/arm-none-eabi-objcopy.exe - -#Additional flags -PREPROCESSOR_MACROS += ARM_MATH_CM3 STM32F103CB -INCLUDE_DIRS += . -LIBRARY_DIRS += -LIBRARY_NAMES += -ADDITIONAL_LINKER_INPUTS += -MACOS_FRAMEWORKS += -LINUX_PACKAGES += - -CFLAGS += -CXXFLAGS += -ASFLAGS += -LDFLAGS += -COMMONFLAGS += -mcpu=cortex-m3 -mthumb -LINKER_SCRIPT := $(BSP_ROOT)/STM32F1xxxx/LinkerScripts/STM32F103CB_flash.lds -