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
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/Makefile b/Makefile
index db350ff96d..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 \
@@ -548,6 +549,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/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/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/config/config_master.h b/src/main/config/config_master.h
index 6070482cb3..60bdf4a800 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"
@@ -66,10 +67,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/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
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 59ddad65e9..46f082a1dc 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;
@@ -230,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);
}
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/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c
new file mode 100644
index 0000000000..577d305d52
--- /dev/null
+++ b/src/main/drivers/serial_escserial.c
@@ -0,0 +1,913 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+typedef enum {
+ BAUDRATE_NORMAL = 19200,
+ BAUDRATE_KISS = 38400
+} escBaudRate_e;
+
+#if defined(USE_ESCSERIAL)
+
+#include "build/build_config.h"
+#include "build/atomic.h"
+
+#include "common/utils.h"
+
+#include "nvic.h"
+#include "system.h"
+#include "io.h"
+#include "timer.h"
+
+#include "serial.h"
+#include "serial_escserial.h"
+#include "drivers/light_led.h"
+#include "drivers/pwm_output.h"
+#include "io/serial.h"
+#include "flight/mixer.h"
+
+#define RX_TOTAL_BITS 10
+#define TX_TOTAL_BITS 10
+
+#define MAX_ESCSERIAL_PORTS 1
+static serialPort_t *escPort = NULL;
+static serialPort_t *passPort = NULL;
+
+typedef struct escSerial_s {
+ serialPort_t port;
+
+ IO_t rxIO;
+ IO_t txIO;
+
+ const timerHardware_t *rxTimerHardware;
+ volatile uint8_t rxBuffer[ESCSERIAL_BUFFER_SIZE];
+ const timerHardware_t *txTimerHardware;
+ volatile uint8_t txBuffer[ESCSERIAL_BUFFER_SIZE];
+
+ uint8_t isSearchingForStartBit;
+ uint8_t rxBitIndex;
+ uint8_t rxLastLeadingEdgeAtBitIndex;
+ uint8_t rxEdge;
+
+ uint8_t isTransmittingData;
+ uint8_t isReceivingData;
+ int8_t bitsLeftToTransmit;
+
+ uint16_t internalTxBuffer; // includes start and stop bits
+ uint16_t internalRxBuffer; // includes start and stop bits
+
+ uint16_t receiveTimeout;
+ uint16_t transmissionErrors;
+ uint16_t receiveErrors;
+
+ uint8_t escSerialPortIndex;
+ uint8_t mode;
+
+ timerCCHandlerRec_t timerCb;
+ timerCCHandlerRec_t edgeCb;
+} escSerial_t;
+
+extern timerHardware_t* serialTimerHardware;
+extern escSerial_t escSerialPorts[];
+
+extern const struct serialPortVTable escSerialVTable[];
+
+
+escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS];
+
+void 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 setTxSignalEsc(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 serialInputPortConfigEsc(const timerHardware_t *timerHardwarePtr)
+{
+#ifdef STM32F10X
+ escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU);
+#else
+ escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_AF_PP_UP);
+#endif
+ timerChClearCCFlag(timerHardwarePtr);
+ timerChITConfig(timerHardwarePtr,ENABLE);
+}
+
+
+static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
+{
+ return timerPeriod > 0xFFFF;
+}
+
+static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud)
+{
+ uint32_t clock = SystemCoreClock/2;
+ uint32_t timerPeriod;
+ TIM_DeInit(timerHardwarePtr->tim);
+ do {
+ timerPeriod = clock / baud;
+ if (isTimerPeriodTooLarge(timerPeriod)) {
+ if (clock > 1) {
+ clock = clock / 2; // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200)
+ } else {
+ // TODO unable to continue, unable to determine clock and timerPeriods for the given baud
+ }
+
+ }
+ } while (isTimerPeriodTooLarge(timerPeriod));
+
+ uint8_t mhz = clock / 1000000;
+ timerConfigure(timerHardwarePtr, timerPeriod, mhz);
+ timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL);
+ timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
+}
+
+static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
+{
+ // start bit is usually a FALLING signal
+ uint8_t mhz = SystemCoreClock / 2000000;
+ TIM_DeInit(timerHardwarePtr->tim);
+ timerConfigure(timerHardwarePtr, 0xFFFF, mhz);
+ serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
+ timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
+ timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
+}
+
+static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
+{
+ uint32_t timerPeriod=34;
+ TIM_DeInit(timerHardwarePtr->tim);
+ timerConfigure(timerHardwarePtr, timerPeriod, 1);
+ timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
+ 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, onSerialRxPinChangeEsc);
+ 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);
+ serialInputPortConfigEsc(escSerial->rxTimerHardware);
+
+ setTxSignalEsc(escSerial, ENABLE);
+ delay(50);
+
+ if(mode==0){
+ serialTimerTxConfig(escSerial->txTimerHardware, portIndex);
+ serialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
+ }
+ else if(mode==1){
+ serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
+ serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
+ }
+ else if(mode==2) {
+ serialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used
+ serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
+ }
+ escSerial->mode = mode;
+ return &escSerial->port;
+}
+
+
+void serialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
+{
+ timerChClearCCFlag(timerHardwarePtr);
+ timerChITConfig(timerHardwarePtr,DISABLE);
+ escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU);
+}
+
+
+void closeEscSerial(escSerialPortIndex_e portIndex, uint16_t output)
+{
+ escSerial_t *escSerial = &(escSerialPorts[portIndex]);
+
+ escSerial->rxTimerHardware = &(timerHardware[output]);
+ escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]);
+ serialInputPortDeConfig(escSerial->rxTimerHardware);
+ timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
+ timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
+ TIM_DeInit(escSerial->txTimerHardware->tim);
+ TIM_DeInit(escSerial->rxTimerHardware->tim);
+}
+
+/*********************************************/
+
+void processTxStateEsc(escSerial_t *escSerial)
+{
+ uint8_t mask;
+ static uint8_t bitq=0, transmitStart=0;
+ if (escSerial->isReceivingData) {
+ return;
+ }
+
+ if(transmitStart==0)
+ {
+ setTxSignalEsc(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)
+ {
+ setTxSignalEsc(escSerial, 1);
+ }
+ if(bitq==2 || bitq==3)
+ {
+ setTxSignalEsc(escSerial, 0);
+ }
+ }
+ else
+ {
+ if(bitq==0 || bitq==2)
+ {
+ setTxSignalEsc(escSerial, 1);
+ }
+ if(bitq==1 ||bitq==3)
+ {
+ setTxSignalEsc(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;
+ serialInputPortConfigEsc(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;
+
+ setTxSignalEsc(escSerial, mask);
+ escSerial->bitsLeftToTransmit--;
+ return;
+ }
+
+ escSerial->isTransmittingData = false;
+ if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
+ if(escSerial->mode==1)
+ {
+ serialInputPortConfigEsc(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 extractAndStoreRxByteEsc(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 onSerialTimerEsc(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);
+ }
+ }
+
+
+ processTxStateEsc(escSerial);
+}
+
+void onSerialRxPinChangeEsc(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)
+ {
+ extractAndStoreRxByteEsc(escSerial);
+ }
+ escSerial->internalRxBuffer=0;
+ }
+
+}
+
+uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
+{
+ if ((instance->mode & MODE_RX) == 0) {
+ return 0;
+ }
+
+ escSerial_t *s = (escSerial_t *)instance;
+
+ return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1);
+}
+
+uint8_t escSerialReadByte(serialPort_t *instance)
+{
+ uint8_t ch;
+
+ if ((instance->mode & MODE_RX) == 0) {
+ return 0;
+ }
+
+ if (escSerialTotalBytesWaiting(instance) == 0) {
+ return 0;
+ }
+
+ ch = instance->rxBuffer[instance->rxBufferTail];
+ instance->rxBufferTail = (instance->rxBufferTail + 1) % instance->rxBufferSize;
+ return ch;
+}
+
+void escSerialWriteByte(serialPort_t *s, uint8_t ch)
+{
+ if ((s->mode & MODE_TX) == 0) {
+ return;
+ }
+
+ s->txBuffer[s->txBufferHead] = ch;
+ s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
+}
+
+void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
+{
+ UNUSED(s);
+ UNUSED(baudRate);
+}
+
+void escSerialSetMode(serialPort_t *instance, portMode_t mode)
+{
+ instance->mode = mode;
+}
+
+bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance)
+{
+ // start listening
+ return instance->txBufferHead == instance->txBufferTail;
+}
+
+uint32_t escSerialTxBytesFree(const serialPort_t *instance)
+{
+ if ((instance->mode & MODE_TX) == 0) {
+ return 0;
+ }
+
+ escSerial_t *s = (escSerial_t *)instance;
+
+ uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1);
+
+ return (s->port.txBufferSize - 1) - bytesUsed;
+}
+
+const struct serialPortVTable escSerialVTable[] = {
+ {
+ .serialWrite = escSerialWriteByte,
+ .serialTotalRxWaiting = escSerialTotalBytesWaiting,
+ .serialTotalTxFree = escSerialTxBytesFree,
+ .serialRead = escSerialReadByte,
+ .serialSetBaudRate = escSerialSetBaudRate,
+ .isSerialTransmitBufferEmpty = isEscSerialTransmitBufferEmpty,
+ .setMode = escSerialSetMode,
+ .writeBuf = NULL,
+ .beginWrite = NULL,
+ .endWrite = NULL
+ }
+};
+
+void escSerialInitialize()
+{
+ //StopPwmAllMotors();
+ pwmDisableMotors();
+
+ for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+ // set outputs to pullup
+ if(timerHardware[i].output==1)
+ {
+ escSerialGPIOConfig(timerHardware[i].tag, IOCFG_IPU); //GPIO_Mode_IPU
+ }
+ }
+}
+
+typedef enum {
+ IDLE,
+ HEADER_START,
+ HEADER_M,
+ HEADER_ARROW,
+ HEADER_SIZE,
+ HEADER_CMD,
+ COMMAND_RECEIVED
+} mspState_e;
+
+typedef struct mspPort_s {
+ uint8_t offset;
+ uint8_t dataSize;
+ uint8_t checksum;
+ uint8_t indRX;
+ uint8_t inBuf[10];
+ mspState_e c_state;
+ uint8_t cmdMSP;
+} mspPort_t;
+
+static mspPort_t currentPort;
+
+static bool ProcessExitCommand(uint8_t c)
+{
+ if (currentPort.c_state == IDLE) {
+ if (c == '$') {
+ currentPort.c_state = HEADER_START;
+ } else {
+ return false;
+ }
+ } else if (currentPort.c_state == HEADER_START) {
+ currentPort.c_state = (c == 'M') ? HEADER_M : IDLE;
+ } else if (currentPort.c_state == HEADER_M) {
+ currentPort.c_state = (c == '<') ? HEADER_ARROW : IDLE;
+ } else if (currentPort.c_state == HEADER_ARROW) {
+ if (c > 10) {
+ currentPort.c_state = IDLE;
+
+ } else {
+ currentPort.dataSize = c;
+ currentPort.offset = 0;
+ currentPort.checksum = 0;
+ currentPort.indRX = 0;
+ currentPort.checksum ^= c;
+ currentPort.c_state = HEADER_SIZE;
+ }
+ } else if (currentPort.c_state == HEADER_SIZE) {
+ currentPort.cmdMSP = c;
+ currentPort.checksum ^= c;
+ currentPort.c_state = HEADER_CMD;
+ } else if (currentPort.c_state == HEADER_CMD && currentPort.offset < currentPort.dataSize) {
+ currentPort.checksum ^= c;
+ currentPort.inBuf[currentPort.offset++] = c;
+ } else if (currentPort.c_state == HEADER_CMD && currentPort.offset >= currentPort.dataSize) {
+ if (currentPort.checksum == c) {
+ currentPort.c_state = COMMAND_RECEIVED;
+
+ if((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0))
+ {
+ currentPort.c_state = IDLE;
+ return true;
+ }
+ } else {
+ currentPort.c_state = IDLE;
+ }
+ }
+ return false;
+}
+
+
+// mode 0=sk, 1=bl, 2=ki output=timerHardware PWM channel.
+void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode)
+{
+ bool exitEsc = false;
+ LED0_OFF;
+ LED1_OFF;
+ //StopPwmAllMotors();
+ pwmDisableMotors();
+ passPort = escPassthroughPort;
+
+ uint8_t first_output = 0;
+ for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+ if(timerHardware[i].output==1)
+ {
+ first_output=i;
+ break;
+ }
+ }
+
+ //doesn't work with messy timertable
+ uint8_t motor_output=first_output+output-1;
+ if(motor_output >=USABLE_TIMER_CHANNEL_COUNT)
+ return;
+
+ uint32_t escBaudrate = (mode == 2) ? BAUDRATE_KISS : BAUDRATE_NORMAL;
+
+ escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode);
+ uint8_t ch;
+ while(1) {
+ if(mode!=2)
+ {
+ if (serialRxBytesWaiting(escPort)) {
+ LED0_ON;
+ while(serialRxBytesWaiting(escPort))
+ {
+ ch = serialRead(escPort);
+ serialWrite(escPassthroughPort, ch);
+ }
+ LED0_OFF;
+ }
+ }
+ if (serialRxBytesWaiting(escPassthroughPort)) {
+ LED1_ON;
+ while(serialRxBytesWaiting(escPassthroughPort))
+ {
+ ch = serialRead(escPassthroughPort);
+ exitEsc = ProcessExitCommand(ch);
+ if(exitEsc)
+ {
+ serialWrite(escPassthroughPort, 0x24);
+ serialWrite(escPassthroughPort, 0x4D);
+ serialWrite(escPassthroughPort, 0x3E);
+ serialWrite(escPassthroughPort, 0x00);
+ serialWrite(escPassthroughPort, 0xF4);
+ serialWrite(escPassthroughPort, 0xF4);
+ closeEscSerial(ESCSERIAL1, output);
+ return;
+ }
+ if(mode==1){
+ serialWrite(escPassthroughPort, ch); // blheli loopback
+ }
+ serialWrite(escPort, ch);
+ }
+ LED1_OFF;
+ }
+ delay(5);
+ }
+}
+
+
+#endif
diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h
new file mode 100644
index 0000000000..113ce1c2a1
--- /dev/null
+++ b/src/main/drivers/serial_escserial.h
@@ -0,0 +1,38 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#define ESCSERIAL_BUFFER_SIZE 1024
+
+typedef enum {
+ ESCSERIAL1 = 0,
+ ESCSERIAL2
+} escSerialPortIndex_e;
+
+serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode);
+
+// serialPort API
+void escSerialWriteByte(serialPort_t *instance, uint8_t ch);
+uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance);
+uint32_t escSerialTxBytesFree(const serialPort_t *instance);
+uint8_t escSerialReadByte(serialPort_t *instance);
+void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
+bool isEscSerialTransmitBufferEmpty(const serialPort_t *s);
+
+void escSerialInitialize();
+void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode);
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 6eb8c50092..cf88d51829 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -73,6 +73,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"
@@ -260,7 +261,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++) {
@@ -381,7 +382,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;
@@ -408,13 +413,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;
@@ -574,13 +583,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);
@@ -775,7 +784,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/fc_msp.c b/src/main/fc/fc_msp.c
index b367c7f645..4edd2c4a6e 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -660,7 +660,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]);
@@ -1789,10 +1789,9 @@ 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;
}
void mspServerPush(mspPacket_t *push, uint8_t *data, int len)
diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h
index be575e72e4..df317ab3a1 100644
--- a/src/main/fc/fc_msp.h
+++ b/src/main/fc/fc_msp.h
@@ -19,5 +19,7 @@
#include "msp/msp.h"
-mspProcessCommandFnPtr mspFcInit(void);
+void mspFcInit(void);
+mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
+
mspPushCommandFnPtr mspFcPushInit(void);
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index c8ab268d37..37e05b936b 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"
@@ -100,7 +101,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/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/fc/rc_controls.c b/src/main/fc/rc_controls.c
index c63c976aa0..f87f00b27a 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -81,6 +81,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/flight/mixer.c b/src/main/flight/mixer.c
index c31f958a85..7846b91714 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,100 +228,21 @@ 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;
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;
@@ -370,51 +274,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 +283,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 +309,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 +320,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 +362,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 +396,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
@@ -843,7 +479,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 +511,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)
@@ -885,94 +521,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..1a4949a3cc 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,22 +106,15 @@ 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);
void stopPwmAllMotors(void);
+bool isMotorProtocolDshot(void);
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/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 ed62918554..64c9fdc4c5 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"
@@ -154,6 +155,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);
@@ -305,6 +309,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
@@ -519,7 +526,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
};
@@ -817,9 +824,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
@@ -2945,6 +2952,60 @@ static void cliGpsPassthrough(char *cmdline)
}
#endif
+#ifdef USE_ESCSERIAL
+static void cliEscPassthrough(char *cmdline)
+{
+ uint8_t mode = 0;
+ int index = 0;
+ int i = 0;
+ char *pch = NULL;
+ char *saveptr;
+
+ if (isEmpty(cmdline)) {
+ cliShowParseError();
+ return;
+ }
+
+ pch = strtok_r(cmdline, " ", &saveptr);
+ while (pch != NULL) {
+ switch (i) {
+ case 0:
+ if(strncasecmp(pch, "sk", strlen(pch)) == 0)
+ {
+ mode = 0;
+ }
+ else if(strncasecmp(pch, "bl", strlen(pch)) == 0)
+ {
+ mode = 1;
+ }
+ else if(strncasecmp(pch, "ki", strlen(pch)) == 0)
+ {
+ mode = 2;
+ }
+ else
+ {
+ cliShowParseError();
+ return;
+ }
+ break;
+ case 1:
+ index = atoi(pch);
+ if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) {
+ printf("passthru at pwm output %d enabled\r\n", index);
+ }
+ else {
+ printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT);
+ return;
+ }
+ break;
+ }
+ i++;
+ pch = strtok_r(NULL, " ", &saveptr);
+ }
+ escEnablePassthrough(cliPort,index,mode);
+}
+#endif
+
static void cliHelp(char *cmdline)
{
UNUSED(cmdline);
diff --git a/src/main/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 08dfe07b1f..3b811a2976 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -291,7 +291,9 @@ void init(void)
#endif
mixerConfigureOutput();
- // pwmInit() needs to be called as soon as possible for ESC compatibility reasons
+#ifdef USE_SERVOS
+ servoConfigureOutput();
+#endif
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
@@ -456,7 +458,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 212890e977..f587f5914b 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -31,8 +31,6 @@
#include "msp/msp.h"
#include "msp/msp_serial.h"
-static mspProcessCommandFnPtr mspProcessCommandFn;
-static mspPushCommandFnPtr mspPushCommandFn;
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
@@ -146,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];
@@ -180,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];
@@ -198,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.
}
}
@@ -209,13 +207,14 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
}
}
-void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
+void mspSerialInit(void)
{
- mspProcessCommandFn = mspProcessCommandFnToUse;
memset(mspPorts, 0, sizeof(mspPorts));
mspSerialAllocatePorts();
}
+mspPushCommandFnPtr mspPushCommandFn;
+
void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
{
static uint8_t pushBuf[30];
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index 6624dcbd36..5d92114736 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -62,8 +62,8 @@ 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);
void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn);
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/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..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
@@ -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
@@ -75,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
@@ -93,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/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h
index 022b51da5e..50b0d87ddd 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/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/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/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
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
-
diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index d059b58d11..259fde884b 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -81,6 +81,9 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define USE_SPI
#define USE_SPI_DEVICE_1
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 1ef2f55c74..04b9a08eda 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/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
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 474dd9b87e..e4e23315b6 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 f49652428f..aa8f6fa676 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..28f480e4bd 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
@@ -112,6 +115,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
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index a00f3a84bc..6ab414085a 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
@@ -134,7 +137,7 @@
#define CMS
// Use external OSD to run CMS
-//#define CANVAS
+#define CANVAS
// USE I2C OLED display to run CMS
-#define OLEDCMS
+//#define OLEDCMS
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 6b5b5c23b6..0f82fef749 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -153,6 +153,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 13345dc8e2..55c60a4c3c 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
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));
+}