From 45f2ce04d54f32257bd4a50481bfadc415df726e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 10:12:31 +0200 Subject: [PATCH 01/10] Just the defaults --- src/main/config/config.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index c1ddd2d28..bb177b2f4 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -203,10 +203,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; - pidProfile->D8[ROLL] = 18; - pidProfile->P8[PITCH] = 50; - pidProfile->I8[PITCH] = 55; - pidProfile->D8[PITCH] = 22; + pidProfile->D8[ROLL] = 25; + pidProfile->P8[PITCH] = 60; + pidProfile->I8[PITCH] = 60; + pidProfile->D8[PITCH] = 30; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -234,7 +234,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_lpf_hz = 80; pidProfile->rollPitchItermIgnoreRate = 200; pidProfile->yawItermIgnoreRate = 50; - pidProfile->dterm_filter_type = FILTER_PT1; + pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_cutoff = 150; @@ -479,7 +479,7 @@ static void resetConf(void) masterConfig.pid_process_denom = 2; #endif masterConfig.gyro_soft_type = FILTER_PT1; - masterConfig.gyro_soft_lpf_hz = 80; + masterConfig.gyro_soft_lpf_hz = 100; masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_cutoff = 150; From 7d119ae669caceecdf9375b97946c2cc9e991adf Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 11:46:55 +0200 Subject: [PATCH 02/10] Cleanup ONESHOT125 feature // Default denom --- src/main/config/config.c | 3 + src/main/config/config.h | 9 ++- src/main/io/serial_cli.c | 5 +- src/main/main.c | 6 -- src/main/target/ALIENFLIGHTF1/config.c | 1 - src/main/target/ALIENFLIGHTF3/config.c | 1 - src/main/target/ALIENFLIGHTF4/config.c | 1 - src/main/target/F4BY/target.h | 2 +- src/main/target/FURYF4/target.h | 2 +- src/main/target/MOTOLAB/config.c | 82 ++++++++++++++++++++++++++ src/main/target/OMNIBUS/target.h | 2 +- src/main/target/OMNIBUSF4/target.h | 2 +- src/main/target/REVO/target.h | 2 +- src/main/target/VRRACE/target.h | 2 +- 14 files changed, 97 insertions(+), 23 deletions(-) create mode 100755 src/main/target/MOTOLAB/config.c diff --git a/src/main/config/config.c b/src/main/config/config.c index bb177b2f4..3b03fb23b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -474,6 +474,9 @@ static void resetConf(void) #ifdef STM32F10X masterConfig.gyro_sync_denom = 8; masterConfig.pid_process_denom = 1; +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) + masterConfig.gyro_sync_denom = 8; + masterConfig.pid_process_denom = 4; #else masterConfig.gyro_sync_denom = 4; masterConfig.pid_process_denom = 2; diff --git a/src/main/config/config.h b/src/main/config/config.h index b97ac6b45..cc239da92 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -46,16 +46,15 @@ typedef enum { FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, FEATURE_DISPLAY = 1 << 17, - FEATURE_ONESHOT125 = 1 << 18, + FEATURE_OSD = 1 << 18, FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, - FEATURE_OSD = 1 << 24, - FEATURE_VTX = 1 << 25, - FEATURE_RX_NRF24 = 1 << 26, - FEATURE_SOFTSPI = 1 << 27, + FEATURE_VTX = 1 << 24, + FEATURE_RX_NRF24 = 1 << 25, + FEATURE_SOFTSPI = 1 << 26, } features_e; void latchActiveFeatures(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index dcf882845..5aef0f539 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -206,9 +206,8 @@ static const char * const featureNames[] = { "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", - "OSD", NULL + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index 7ec478fea..24b983308 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -288,12 +288,6 @@ void init(void) pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif - if (masterConfig.motor_pwm_protocol == PWM_TYPE_ONESHOT125) { - featureSet(FEATURE_ONESHOT125); - } else { - featureClear(FEATURE_ONESHOT125); - } - bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED; // Configurator feature abused for enabling Fast PWM diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 3369b1763..b16a16444 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -72,7 +72,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; masterConfig.motor_pwm_rate = 32000; diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index be0a9bfea..3aed5a3ce 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -77,7 +77,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.mag_hardware = MAG_NONE; // disabled by default masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index ecce870a4..5264a1673 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -77,7 +77,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.mag_hardware = MAG_NONE; // disabled by default masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 8b4b846c4..7c9e89b48 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -138,7 +138,7 @@ #define CURRENT_METER_ADC_PIN PC2 #define RSSI_ADC_PIN PC1 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 073e2fb91..8f6937859 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -150,7 +150,7 @@ #define RSSI_ADC_GPIO_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c new file mode 100755 index 000000000..490236ce0 --- /dev/null +++ b/src/main/target/MOTOLAB/config.c @@ -0,0 +1,82 @@ +/* + * 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 "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// Motolab target supports 2 different type of boards Tornado / Cyclone. +void targetConfiguration(void) { + masterConfig.gyro_sync_denom = 4; + masterConfig.pid_process_denom = 1; +} diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 0a0ec2593..ad84573b5 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -167,7 +167,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX) #define BUTTONS #define BUTTON_A_PORT GPIOB diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 5250a336b..4bd45103a 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -121,7 +121,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 9aab5b4b1..76738118d 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -110,7 +110,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index da1581e9f..cda9b1446 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -156,7 +156,7 @@ #define RSSI_ADC_GPIO_PIN PB1 #define CURRENT_METER_ADC_PIN PA5 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_ONESHOT125 | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS From 06be182e5066c9f8cd7661dc22551f5bd6d0fbf2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 00:02:38 +0200 Subject: [PATCH 03/10] Add yaw smoothing back // adjust defaults // KISS fc remove some shared timers --- src/main/config/config.c | 6 +++--- src/main/flight/pid.c | 2 +- src/main/mw.c | 4 +--- src/main/target/KISSFC/target.c | 5 ----- 4 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 3b03fb23b..2f047d64e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -203,10 +203,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; - pidProfile->D8[ROLL] = 25; + pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 60; - pidProfile->D8[PITCH] = 30; + pidProfile->D8[PITCH] = 25; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -244,7 +244,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; - pidProfile->dtermSetpointWeight = 200; + pidProfile->dtermSetpointWeight = 120; pidProfile->pidMaxVelocityYaw = 200; pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ae735be92..b405a9ab4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -194,7 +194,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc configD[axis] = pidProfile->D8[axis]; } - // Limit abrupt yaw inputs + // Limit abrupt yaw inputs / stops if (axis == YAW && pidProfile->pidMaxVelocityYaw) { float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; if (ABS(yawCurrentVelocity) > yawMaxVelocity) { diff --git a/src/main/mw.c b/src/main/mw.c index c3ebdf829..eded99556 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -263,9 +263,7 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - // Don't smooth yaw axis - int axisToCalculate = (isRXDataNew) ? 3 : 2; - for (int axis = 0; axis < axisToCalculate; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); isRXDataNew = false; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 167265ed7..cbb9d7481 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -29,11 +29,6 @@ const uint16_t multiPPM[] = { PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; From c33e14db4b2a273a07224842e295b443a0441f94 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:37:59 +0200 Subject: [PATCH 04/10] Start on version 3.1.0 --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index 0f770674c..c3c6529ed 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x From 2676408071a8013e654e8821201cb8b4d1a25e48 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 05/10] Optimisation of driver header files --- src/main/config/config.c | 1 + src/main/drivers/accgyro_mma845x.c | 2 +- src/main/drivers/adc.c | 2 + src/main/drivers/adc.h | 2 +- src/main/drivers/adc_impl.h | 6 +-- src/main/drivers/barometer_spi_bmp280.c | 2 + src/main/drivers/bus_i2c.h | 4 +- src/main/drivers/bus_i2c_soft.c | 2 +- src/main/drivers/bus_i2c_stm32f30x.c | 5 +- src/main/drivers/bus_spi.h | 5 +- src/main/drivers/compass_hmc5883l.h | 2 +- src/main/drivers/dma.c | 4 +- src/main/drivers/dma_stm32f4xx.c | 4 +- src/main/drivers/flash_m25p16.c | 7 +-- src/main/drivers/flash_m25p16.h | 2 +- src/main/drivers/gyro_sync.c | 6 +-- src/main/drivers/io.h | 24 +-------- src/main/drivers/io_types.h | 28 +++++++++++ src/main/drivers/light_ws2811strip.c | 4 +- .../drivers/light_ws2811strip_stm32f10x.c | 3 +- .../drivers/light_ws2811strip_stm32f30x.c | 2 +- src/main/drivers/max7456.c | 15 +++--- src/main/drivers/pwm_mapping.h | 5 +- src/main/drivers/pwm_rx.c | 2 +- src/main/drivers/rcc.h | 4 +- src/main/drivers/rcc_types.h | 4 ++ src/main/drivers/sdcard.c | 4 +- src/main/drivers/serial_usb_vcp.c | 4 +- src/main/drivers/sonar_hcsr04.h | 3 +- src/main/drivers/sound_beeper.h | 2 +- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 9 ++-- src/main/drivers/timer_stm32f10x.c | 5 ++ src/main/drivers/timer_stm32f30x.c | 5 ++ src/main/drivers/timer_stm32f4xx.c | 7 ++- src/main/drivers/transponder_ir.c | 6 +-- src/main/drivers/transponder_ir_stm32f30x.c | 6 +-- src/main/drivers/vtx_rtc6705.c | 6 +-- src/main/drivers/vtx_soft_spi_rtc6705.c | 7 +-- src/main/io/serial_4way_avrootloader.c | 2 + src/main/main.c | 1 + src/main/sensors/initialisation.c | 2 +- src/main/target/AIORACERF3/target.c | 1 + src/main/target/AIR32/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.h | 1 + src/main/target/ALIENFLIGHTF3/target.c | 1 + src/main/target/ALIENFLIGHTF3/target.h | 3 +- src/main/target/ALIENFLIGHTF4/target.c | 1 + src/main/target/BLUEJAYF4/target.c | 1 + src/main/target/CC3D/target.c | 1 + src/main/target/CHEBUZZF3/target.c | 1 + src/main/target/CJMCU/target.c | 1 + src/main/target/CJMCU/target.h | 1 + src/main/target/COLIBRI_RACE/target.c | 1 + src/main/target/DOGE/target.c | 1 + src/main/target/DOGE/target.h | 9 ++-- src/main/target/EUSTM32F103RC/target.c | 1 + src/main/target/EUSTM32F103RC/target.h | 1 + src/main/target/F4BY/target.c | 49 ++++++++++--------- src/main/target/F4BY/target.h | 3 +- src/main/target/FURYF3/target.c | 1 + src/main/target/FURYF4/target.c | 1 + src/main/target/FURYF4/target.h | 3 +- src/main/target/IRCFUSIONF3/target.c | 1 + src/main/target/IRCFUSIONF3/target.h | 3 +- src/main/target/KISSFC/target.c | 1 + src/main/target/KISSFC/target.h | 3 +- src/main/target/LUX_RACE/target.c | 1 + src/main/target/MICROSCISKY/target.c | 1 + src/main/target/MICROSCISKY/target.h | 1 + src/main/target/MOTOLAB/target.c | 1 + src/main/target/NAZE/target.c | 1 + src/main/target/NAZE/target.h | 1 + src/main/target/OLIMEXINO/target.c | 1 + src/main/target/OLIMEXINO/target.h | 1 + src/main/target/OMNIBUS/target.c | 1 + src/main/target/OMNIBUSF4/target.c | 1 + src/main/target/PIKOBLX/target.c | 1 + src/main/target/PIKOBLX/target.h | 3 +- src/main/target/PORT103R/target.c | 1 + src/main/target/PORT103R/target.h | 1 + src/main/target/REVO/target.c | 1 + src/main/target/REVONANO/target.c | 1 + src/main/target/RMDO/target.c | 1 + src/main/target/SINGULARITY/target.c | 1 + src/main/target/SINGULARITY/target.h | 3 +- src/main/target/SIRINFPV/target.c | 1 + src/main/target/SPARKY/target.c | 1 + src/main/target/SPRACINGF3/target.c | 1 + src/main/target/SPRACINGF3EVO/target.c | 1 + src/main/target/SPRACINGF3MINI/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.h | 1 + src/main/target/X_RACERSPI/target.c | 1 + src/main/target/X_RACERSPI/target.h | 3 +- src/main/target/ZCOREF3/target.c | 1 + src/main/target/ZCOREF3/target.h | 1 - src/main/telemetry/frsky.c | 2 + src/main/telemetry/telemetry.c | 2 +- 100 files changed, 212 insertions(+), 138 deletions(-) create mode 100644 src/main/drivers/io_types.h create mode 100644 src/main/drivers/rcc_types.h diff --git a/src/main/config/config.c b/src/main/config/config.c index 2f047d64e..a1927e274 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,6 +23,7 @@ #include "debug.h" #include "build_config.h" +#include "debug.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 202b90e80..69a82a0ad 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -21,7 +21,7 @@ #include "platform.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "bus_i2c.h" #include "sensor.h" diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index a65abf912..6bba10939 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -27,6 +27,8 @@ #include "adc.h" #include "adc_impl.h" +#include "common/utils.h" + //#define DEBUG_ADC_CHANNELS #ifdef USE_ADC diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 55914a336..47a4b52a0 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef enum { ADC_BATTERY = 0, diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 146ada62b..c89dc1bf5 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -17,8 +17,8 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) #define ADC_TAG_MAP_COUNT 16 @@ -65,4 +65,4 @@ extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; -uint8_t adcChannelByTag(ioTag_t ioTag); \ No newline at end of file +uint8_t adcChannelByTag(ioTag_t ioTag); diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c index 9d97343e1..0c9768497 100644 --- a/src/main/drivers/barometer_spi_bmp280.c +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -15,6 +15,7 @@ * along with Betaflight. If not, see . */ +#include #include #include @@ -24,6 +25,7 @@ #include "barometer.h" #include "barometer_bmp280.h" +#include "io.h" #ifdef USE_BARO_SPI_BMP280 #define DISABLE_BMP280 IOHi(bmp280CsPin) diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 86f7a6618..26cce3792 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -21,8 +21,8 @@ #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT -#include "drivers/io.h" -#include "drivers/rcc.h" +#include "io_types.h" +#include "rcc_types.h" #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 104c77c37..42ea8cadc 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "bus_i2c.h" -#include "drivers/io.h" +#include "io.h" // Software I2C driver, using same pins as hardware I2C, with hw i2c module disabled. // Can be configured for I2C2 pinout (SCL: PB10, SDA: PB11) or I2C1 pinout (SCL: PB6, SDA: PB7) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 88b267f11..f8c5b2369 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -20,9 +20,10 @@ #include -#include "gpio.h" #include "system.h" -#include "drivers/io_impl.h" +#include "io.h" +#include "io_impl.h" +#include "rcc.h" #include "bus_i2c.h" diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 5a6ea7dd3..58c76c9d7 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,9 +17,8 @@ #pragma once -#include -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 035a2c936..e2f71fd28 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef struct hmc5883Config_s { ioTag_t intTag; diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 0ad411312..7e4942fbe 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ec6e7908d..8372ad0bc 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 0ab3d6122..bf7cf6f0c 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -22,9 +22,10 @@ #ifdef USE_FLASH_M25P16 -#include "drivers/flash_m25p16.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "flash_m25p16.h" +#include "io.h" +#include "bus_spi.h" +#include "system.h" #define M25P16_INSTRUCTION_RDID 0x9F #define M25P16_INSTRUCTION_READ_BYTES 0x03 diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 223efa180..6cad8a8ea 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,7 +19,7 @@ #include #include "flash.h" -#include "io.h" +#include "io_types.h" #define M25P16_PAGESIZE 256 diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index cd95e186e..dbdc2244c 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -10,9 +10,9 @@ #include "platform.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" +#include "sensor.h" +#include "accgyro.h" +#include "gyro_sync.h" static uint8_t mpuDividerDrops; diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index d65564628..5afa4abb7 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -6,16 +6,7 @@ #include #include "resource.h" -// IO pin identification -// make sure that ioTag_t can't be assigned into IO_t without warning -typedef uint8_t ioTag_t; // packet tag to specify IO pin -typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change - -// NONE initializer for ioTag_t variables -#define IOTAG_NONE ((ioTag_t)0) - -// NONE initializer for IO_t variable -#define IO_NONE ((IO_t)0) +#include "io_types.h" // preprocessor is used to convert pinid to requested C data value // compile-time error is generated if requested pin is not available (not set in TARGET_IO_PORTx) @@ -24,19 +15,6 @@ typedef void* IO_t; // type specifying IO pin. Currently ioRec_t poin // expand pinid to to ioTag_t #define IO_TAG(pinid) DEFIO_TAG(pinid) -// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) -// this simplifies initialization (globals are zeroed on start) and allows -// omitting unused fields in structure initializers. -// it is also possible to use IO_t and ioTag_t as boolean value -// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment -// IO_t being pointer is only possibility I know of .. - -// pin config handling -// pin config is packed into ioConfig_t to decrease memory requirements -// IOCFG_x macros are defined for common combinations for all CPUs; this -// helps masking CPU differences - -typedef uint8_t ioConfig_t; // packed IO configuration #if defined(STM32F1) // mode is using only bits 6-2 diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h new file mode 100644 index 000000000..ba80cd3bc --- /dev/null +++ b/src/main/drivers/io_types.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +// IO pin identification +// make sure that ioTag_t can't be assigned into IO_t without warning +typedef uint8_t ioTag_t; // packet tag to specify IO pin +typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change + +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + +// NONE initializer for IO_t variable +#define IO_NONE ((IO_t)0) + +// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) +// this simplifies initialization (globals are zeroed on start) and allows +// omitting unused fields in structure initializers. +// it is also possible to use IO_t and ioTag_t as boolean value +// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment +// IO_t being pointer is only possibility I know of .. + +// pin config handling +// pin config is packed into ioConfig_t to decrease memory requirements +// IOCFG_x macros are defined for common combinations for all CPUs; this +// helps masking CPU differences + +typedef uint8_t ioConfig_t; // packed IO configuration diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 6c8f2bb18..d9c29af37 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -36,8 +36,8 @@ #include "common/color.h" #include "common/colorconversion.h" -#include "drivers/dma.h" -#include "drivers/light_ws2811strip.h" +#include "dma.h" +#include "light_ws2811strip.h" uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; volatile uint8_t ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 4b588a0c9..911b7cdc6 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -23,10 +23,11 @@ #ifdef LED_STRIP #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "nvic.h" #include "io.h" #include "dma.h" +#include "rcc.h" #include "timer.h" static IO_t ws2811IO = IO_NONE; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8379188f9..1fadfb1f0 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -26,7 +26,7 @@ #include "nvic.h" #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "dma.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 82f3f29af..7ddc2a058 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -19,16 +19,19 @@ #include #include "platform.h" -#include "version.h" #ifdef USE_MAX7456 +#include "version.h" + #include "common/printf.h" -#include "drivers/bus_spi.h" -#include "drivers/light_led.h" -#include "drivers/system.h" -#include "drivers/nvic.h" -#include "drivers/dma.h" + +#include "bus_spi.h" +#include "light_led.h" +#include "io.h" +#include "system.h" +#include "nvic.h" +#include "dma.h" #include "max7456.h" #include "max7456_symbols.h" diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 29a5d02f1..c01706183 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -17,7 +17,7 @@ #pragma once -#include "timer.h" +#include "io_types.h" #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 @@ -88,10 +88,11 @@ typedef enum { PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; +struct timerHardware_s; typedef struct pwmPortConfiguration_s { uint8_t index; pwmPortFlags_e flags; - const timerHardware_t *timerHardware; + const struct timerHardware_s *timerHardware; } pwmPortConfiguration_t; typedef struct pwmOutputConfiguration_s { diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 3c72d104d..60af85e97 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -30,7 +30,7 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "timer.h" #include "pwm_output.h" diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index fb04ec7e2..69dce965f 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -2,6 +2,7 @@ #include "platform.h" #include "common/utils.h" +#include "rcc_types.h" enum rcc_reg { RCC_EMPTY = 0, // make sure that default value (0) does not enable anything @@ -17,9 +18,6 @@ enum rcc_reg { #define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, RCC_APB1ENR_ ## periph ## EN) #define RCC_AHB1(periph) RCC_ENCODE(RCC_AHB1, RCC_AHB1ENR_ ## periph ## EN) -typedef uint8_t rccPeriphTag_t; - void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState); void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState); - diff --git a/src/main/drivers/rcc_types.h b/src/main/drivers/rcc_types.h new file mode 100644 index 000000000..1b7d7b9f2 --- /dev/null +++ b/src/main/drivers/rcc_types.h @@ -0,0 +1,4 @@ +#pragma once + +typedef uint8_t rccPeriphTag_t; + diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 878d5eacf..a86fc7457 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -25,8 +25,8 @@ #include "nvic.h" #include "io.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "bus_spi.h" +#include "system.h" #include "sdcard.h" #include "sdcard_standard.h" diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 2c2cebbac..50c6055dd 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "common/utils.h" -#include "drivers/io.h" +#include "io.h" #include "usb_core.h" #ifdef STM32F4 @@ -32,7 +32,7 @@ #include "hw_config.h" #endif -#include "drivers/system.h" +#include "system.h" #include "serial.h" #include "serial_usb_vcp.h" diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index d734f62f4..cb3f38c98 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -17,8 +17,7 @@ #pragma once -#include "platform.h" -#include "io.h" +#include "io_types.h" typedef struct sonarHardware_s { ioTag_t triggerTag; diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index ab7a7c3df..9caad4f55 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "io_types.h" #ifdef BEEPER #define BEEP_TOGGLE systemBeepToggle() diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 15e36cce3..5051588c9 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -25,7 +25,7 @@ #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "rcc.h" #include "system.h" diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 7050bbabd..00410a9c6 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,12 +17,11 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include +#include -#if !defined(USABLE_TIMER_CHANNEL_COUNT) -#define USABLE_TIMER_CHANNEL_COUNT 14 -#endif +#include "io_types.h" +#include "rcc_types.h" typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 1a5085e63..49106818f 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f10x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 04069b20c..99111bad7 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f30x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index af09f85e1..ccce10350 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -5,9 +5,14 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f4xx.h" -#include "timer.h" #include "rcc.h" +#include "timer.h" /** * @brief Selects the TIM Output Compare Mode. diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 626c60097..dbc6749af 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -21,9 +21,9 @@ #include -#include "drivers/dma.h" -#include "drivers/nvic.h" -#include "drivers/transponder_ir.h" +#include "dma.h" +#include "nvic.h" +#include "transponder_ir.h" /* * Implementation note: diff --git a/src/main/drivers/transponder_ir_stm32f30x.c b/src/main/drivers/transponder_ir_stm32f30x.c index 3962e1c99..0bff2e112 100644 --- a/src/main/drivers/transponder_ir_stm32f30x.c +++ b/src/main/drivers/transponder_ir_stm32f30x.c @@ -20,9 +20,9 @@ #include -#include "drivers/gpio.h" -#include "drivers/transponder_ir.h" -#include "drivers/nvic.h" +#include "gpio.h" +#include "transponder_ir.h" +#include "nvic.h" #ifndef TRANSPONDER_GPIO #define USE_TRANSPONDER_ON_DMA1_CHANNEL3 diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 93ffa83ec..08ec758e8 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -31,9 +31,9 @@ #include "common/maths.h" -#include "drivers/vtx_rtc6705.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "vtx_rtc6705.h" +#include "bus_spi.h" +#include "system.h" #define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400 #define RTC6705_SET_A1 0x8F3031 //5865 diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index 4ceba50df..e6340680f 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -22,9 +22,10 @@ #ifdef USE_RTC6705 -#include "drivers/bus_spi.h" -#include "drivers/system.h" -#include "drivers/light_led.h" +#include "bus_spi.h" +#include "io.h" +#include "system.h" +#include "light_led.h" #include "vtx_soft_spi_rtc6705.h" diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index f0f552581..223d3e10c 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,8 +25,10 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "drivers/io_types.h" #include "drivers/system.h" #include "drivers/serial.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "io/serial.h" #include "io/serial_msp.h" diff --git a/src/main/main.c b/src/main/main.c index 24b983308..436f5b579 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -33,6 +33,7 @@ #include "drivers/system.h" #include "drivers/dma.h" #include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" #include "drivers/timer.h" diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bb46059ac..c3465b393 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,7 +24,7 @@ #include "common/axis.h" -#include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index e780364ec..f950c6784 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 378627daa..b5b988937 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index 7116b5355..dec8739ba 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 6d945008f..04984912c 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -90,4 +90,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#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/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index c1946aa17..8169e9e91 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 28a896ff0..d2e9e92ba 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -35,8 +35,6 @@ #define BEEPER PA5 -#define USABLE_TIMER_CHANNEL_COUNT 11 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_EXTI @@ -131,5 +129,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index efedb5668..dafcdf86d 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index b85e50f53..83e0700cd 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index a870de9e2..589c6af30 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 631d83e6a..c2aca4671 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 0f8228559..3562e3234 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 973e0984f..beb4be965 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -71,4 +71,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#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/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 91b21c8b3..099f9b62e 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 05b642f08..ca13ef7d0 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 2399ede8d..3f40c4ab4 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -67,11 +67,6 @@ #define M25P16_CS_PIN PC15 #define M25P16_SPI_INSTANCE SPI2 -// timer definitions in drivers/timer.c -// channel mapping in drivers/pwm_mapping.c -// only 6 outputs available on hardware -#define USABLE_TIMER_CHANNEL_COUNT 9 - #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 @@ -147,5 +142,9 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +// timer definitions in drivers/timer.c +// channel mapping in drivers/pwm_mapping.c +// only 6 outputs available on hardware +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index b2a0db9a1..0dc2d6604 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 12ec85f37..d883641d4 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -113,5 +113,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|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/F4BY/target.c b/src/main/target/F4BY/target.c index a8d80e823..e9befde0a 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -3,17 +3,18 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -47,13 +48,13 @@ const uint16_t multiPWM[] = { const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), PWM14 | (MAP_TO_SERVO_OUTPUT << 8), - PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -84,23 +85,23 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 7c9e89b48..d8f815403 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -74,8 +74,6 @@ #define SDCARD_DMA_CHANNEL DMA_Channel_0 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -156,5 +154,6 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index bf4ff7b20..db28609d9 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index ac600ea43..a53aa4d44 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 8f6937859..05ff28628 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -101,8 +101,6 @@ #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 -#define USABLE_TIMER_CHANNEL_COUNT 5 - #define USE_VCP #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED @@ -167,5 +165,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 5 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index dd87a8641..2e4e12fb0 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8dbd6e55b..9ceabb35d 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -23,8 +23,6 @@ #define LED0 PB3 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define USE_MPU_DATA_READY_SIGNAL @@ -105,5 +103,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index cbb9d7481..a443fe862 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 05346145d..e532b9d78 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -28,8 +28,6 @@ #define BEEPER PB13 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_EXTI #define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL @@ -84,4 +82,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 91b21c8b3..099f9b62e 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index 7116b5355..dec8739ba 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 80d4f0485..9138f74e8 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -97,4 +97,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#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/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index a89063cc9..cc898ba83 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 7116b5355..dec8739ba 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0e8ba62be..7e3705b47 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -163,4 +163,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#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.c b/src/main/target/OLIMEXINO/target.c index ea20267b3..5d0a271c5 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index bb270cb4f..ae8ac3b38 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -91,4 +91,5 @@ #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/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 977d623f8..a3cd3e54d 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 7dd021055..2ed3eab8f 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index a89063cc9..cc898ba83 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 111f6fc47..7a104b746 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -28,8 +28,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -136,5 +134,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 8f21b7cbb..093fa6ee2 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 66e83fa3d..9f0239d57 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -123,5 +123,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|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/REVO/target.c b/src/main/target/REVO/target.c index 67f732728..a253a0b31 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9a579f2bc..bf5023fc2 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index ee96272c1..d344cda35 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 813157951..e48d0c33b 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 09a567817..ed23489a3 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -25,8 +25,6 @@ #define BEEPER PC15 -#define USABLE_TIMER_CHANNEL_COUNT 10 - #define USE_EXTI #define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL @@ -115,5 +113,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index b52fbd00e..7d602c74b 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 389617278..20b1f185d 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ec9176a27..99f79ec9b 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9a..87eb5db35 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index d1e3a0d8b..bf8776aa9 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 1712bebae..d2c17db5a 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 592833bc4..a75e95217 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -165,5 +165,6 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0x00ff +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 85db79093..a63258100 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 7838b1f46..9cc822ebe 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -26,8 +26,6 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH @@ -128,5 +126,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index b72400e6f..48a6ce60d 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c65abb7d9..5c07c7819 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -101,6 +101,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip - #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 33bd13193..d698b958a 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -19,6 +19,8 @@ * Initial FrSky Telemetry implementation by silpstream @ rcgroups. * Addition protocol work by airmamaf @ github. */ + +#include #include #include diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 96e2193e9..6373d166e 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include @@ -22,7 +23,6 @@ #ifdef TELEMETRY -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" From c1c94d67bdf35e91123ae2934eea680323dd98d5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 01:10:46 +0100 Subject: [PATCH 06/10] Added #pragma once to debug.h --- src/main/debug.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/debug.h b/src/main/debug.h index 2e2a90e89..6ebbba735 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#pragma once + #define DEBUG16_VALUE_COUNT 4 extern int16_t debug[DEBUG16_VALUE_COUNT]; extern uint8_t debugMode; From eb3da3ae2c425f6614d2a12179e861274e4f8b38 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 07/10] Removed dependencies on sensors/ --- src/main/drivers/compass_ak8963.c | 3 --- src/main/drivers/compass_ak8975.c | 2 -- src/main/drivers/compass_hmc5883l.c | 2 -- 3 files changed, 7 deletions(-) diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 0668fcdd7..d5b9a59de 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -33,9 +33,6 @@ #include "bus_i2c.h" #include "bus_spi.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 0d648ecc5..0e4d3677c 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -33,8 +33,6 @@ #include "gpio.h" #include "bus_i2c.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 3709cc510..10fa9b3c6 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -39,8 +39,6 @@ #include "sensor.h" #include "compass.h" -#include "sensors/sensors.h" - #include "compass_hmc5883l.h" //#define DEBUG_MAG_DATA_READY_INTERRUPT From ec558e8339b73fd6d93ff40286a09c8f9e120441 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 08/10] Put each PID in its own .c file --- Makefile | 2 + src/main/flight/pid.c | 387 ++----------------------------- src/main/flight/pid.h | 5 + src/main/flight/pid_betaflight.c | 277 ++++++++++++++++++++++ src/main/flight/pid_legacy.c | 210 +++++++++++++++++ 5 files changed, 509 insertions(+), 372 deletions(-) create mode 100644 src/main/flight/pid_betaflight.c create mode 100644 src/main/flight/pid_legacy.c diff --git a/Makefile b/Makefile index 281b169c6..68364d7b7 100644 --- a/Makefile +++ b/Makefile @@ -392,6 +392,8 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/pid_legacy.c \ + flight/pid_betaflight.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b405a9ab4..03c86f53a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,32 +47,25 @@ #include "config/runtime_config.h" -extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; -extern float rcInput[3]; -static bool pidStabilisationEnabled; +bool pidStabilisationEnabled; int16_t axisPID[3]; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + #ifdef BLACKBOX int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; +int32_t errorGyroI[3]; +float errorGyroIf[3]; -static int32_t errorGyroI[3]; -static float errorGyroIf[3]; - -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); #ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif @@ -94,7 +87,7 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; } -float getdT (void) +float getdT(void) { static float dT; if (!dT) dT = (float)targetPidLooptime * 0.000001f; @@ -104,13 +97,15 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static pt1Filter_t deltaFilter[3]; -static pt1Filter_t yawFilter; -static biquadFilter_t dtermFilterLpf[3]; -static biquadFilter_t dtermFilterNotch[3]; -static bool dtermNotchInitialised, dtermBiquadLpfInitialised; +pt1Filter_t deltaFilter[3]; +pt1Filter_t yawFilter; +biquadFilter_t dtermFilterLpf[3]; +biquadFilter_t dtermFilterNotch[3]; +bool dtermNotchInitialised; +bool dtermBiquadLpfInitialised; -void initFilters(const pidProfile_t *pidProfile) { +void initFilters(const pidProfile_t *pidProfile) +{ int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { @@ -125,358 +120,6 @@ void initFilters(const pidProfile_t *pidProfile) { } } -#ifndef SKIP_PID_FLOAT -// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - float errorRate = 0, rP = 0, rD = 0, PVRate = 0; - float ITerm,PTerm,DTerm; - static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->D8[PIDLEVEL] == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); - } - } - - // Yet Highly experimental and under test and development - // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) - static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { - const uint16_t maxLoopCount = 20000 / targetPidLooptime; - const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; - static int16_t previousThrottle; - static uint16_t loopIncrement; - - if (loopIncrement >= maxLoopCount) { - kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 - previousThrottle = rcCommand[THROTTLE]; - loopIncrement = 0; - } else { - loopIncrement++; - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - - // Limit abrupt yaw inputs / stops - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; - } - yawPreviousRate = setpointRate[axis]; - } - - // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to the max inclination -#ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); - } - } - - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - - // --------low-level gyro-based PID based on 2DOF PID controller. ---------- - // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; - - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; - - // -----calculate I component. - // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - - // Handle All windup Scenarios - // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; - - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { - rD = c[axis] * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; - lastRateError[axis] = rD; - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - } - - DTerm = Kd[axis] * delta * dynReduction; - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); - } - - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} -#endif - -// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - int axis; - int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; - - int8_t horizonLevelStrength = 100; - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection - // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. - // For more rate mode increase D and slower flips and rolls will be possible - horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)setpointRate[axis]; - - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to max configured inclination -#ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); - } - } - - // --------low-level gyro-based PID. ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - gyroRate = gyroADC[axis] / 4; - - RateError = AngleRateTmp - gyroRate; - - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - // -----calculate I component - // there should be no division before accumulating the error to integrator, because the precision would be reduced. - // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. - // Time correction (to avoid different I scaling for different builds based on average cycle time) - // is normalized to cycle time = 2048. - // Prevent Accumulation - uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); - uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; - - // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - - ITerm = errorGyroI[axis] >> 13; - - //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastRateError[axis]; - lastRateError[axis] = RateError; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Filter delta - if (pidProfile->dterm_lpf_hz) { - float deltaf = delta; // single conversion - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - delta = lrintf(deltaf); - } - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} void pidSetController(pidControllerType_e type) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4f3623c31..79e83a2c0 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -117,6 +117,11 @@ struct rxConfig_s; typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c new file mode 100644 index 000000000..72cdd4ea4 --- /dev/null +++ b/src/main/flight/pid_betaflight.c @@ -0,0 +1,277 @@ +/* + * 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 + +#ifndef SKIP_PID_FLOAT + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern float rcInput[3]; +extern float setpointRate[3]; + +extern float errorGyroIf[3]; +extern bool pidStabilisationEnabled; + +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern biquadFilter_t dtermFilterNotch[3]; +extern bool dtermNotchInitialised; +extern bool dtermBiquadLpfInitialised; + +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. +// Based on 2DOF reference design (matlab) +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + float errorRate = 0, rP = 0, rD = 0, PVRate = 0; + float ITerm,PTerm,DTerm; + static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; + float delta; + int axis; + float horizonLevelStrength = 1; + + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } + } + + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now + // Prepare all parameters for PID controller + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs + if (axis == YAW && pidProfile->pidMaxVelocityYaw) { + float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; + if (ABS(yawCurrentVelocity) > yawMaxVelocity) { + setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + } + yawPreviousRate = setpointRate[axis]; + } + + // Yaw control is GYRO based, direct sticks control is applied to rate PID + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to the max inclination +#ifdef GPS + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#else + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + } + } + + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y + + // Slowly restore original setpoint with more stick input + float diffRate = errorRate - rP; + rP += diffRate * rcInput[axis]; + + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + float dynReduction = tpaFactor; + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(errorRate) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (errorRate < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (errorRate > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + + // -----calculate P component + PTerm = Kp[axis] * rP * dynReduction; + + // -----calculate I component. + // Reduce strong Iterm accumulation during higher stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + + // I coefficient (I8) moved before integration to make limiting independent from PID settings + ITerm = errorGyroIf[axis]; + + //-----calculate D-term (Yaw D not yet supported) + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox + } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + + // Filter delta + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + } + + DTerm = Kd[axis] * delta * dynReduction; + + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } + + // Disable PID control at zero throttle + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} +#endif + diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c new file mode 100644 index 000000000..0424f5461 --- /dev/null +++ b/src/main/flight/pid_legacy.c @@ -0,0 +1,210 @@ +/* + * 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 "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern uint8_t motorCount; +extern uint8_t PIDweight[3]; +extern bool pidStabilisationEnabled; +extern float setpointRate[3]; +extern int32_t errorGyroI[3]; +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern bool dtermBiquadLpfInitialised; + + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + + +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. +// Don't expect much development in the future +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + int axis; + int32_t PTerm, ITerm, DTerm, delta; + static int32_t lastRateError[3]; + int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; + + int8_t horizonLevelStrength = 100; + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection + // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. + // For more rate mode increase D and slower flips and rolls will be possible + horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + // -----Get the desired angle rate depending on flight mode + AngleRateTmp = (int32_t)setpointRate[axis]; + + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to max configured inclination +#ifdef GPS + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#else + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; + } else { + // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, + // horizonLevelStrength is scaled to the stick input + AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); + } + } + + // --------low-level gyro-based PID. ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // -----calculate scaled error.AngleRates + // multiplication of rcCommand corresponds to changing the sticks scaling here + gyroRate = gyroADC[axis] / 4; + + RateError = AngleRateTmp - gyroRate; + + // -----calculate P component + PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + + // -----calculate I component + // there should be no division before accumulating the error to integrator, because the precision would be reduced. + // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. + // Time correction (to avoid different I scaling for different builds based on average cycle time) + // is normalized to cycle time = 2048. + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; + + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; + + // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. + // I coefficient (I8) moved before integration to make limiting independent from PID settings + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + + ITerm = errorGyroI[axis] >> 13; + + //-----calculate D-term + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } + + DTerm = 0; // needed for blackbox + } else { + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastRateError[axis]; + lastRateError[axis] = RateError; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // Filter delta + if (pidProfile->dterm_lpf_hz) { + float deltaf = delta; // single conversion + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + delta = lrintf(deltaf); + } + + DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } + + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} + From 64dc6b6b1a3def12c1705ce250c5f518af5ea7b3 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 09/10] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid.h | 3 +++ src/main/flight/pid_betaflight.c | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 79e83a2c0..04e129b9c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -127,6 +127,9 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; extern uint32_t targetPidLooptime; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +extern uint8_t PIDweight[3]; + void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 72cdd4ea4..eae0f4c17 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From d2600ed95b6632198be512bdf5dcb5665669a7f5 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 15:16:30 +0200 Subject: [PATCH 10/10] Added missing include. Target VRRACE build failed. --- src/main/target/VRRACE/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index 094ade421..90270b1fa 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input