From f1aa9e6f4487838a6f3585d61ddeef97bd7f62c3 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Tue, 2 Feb 2016 17:39:00 +1000 Subject: [PATCH] PID: LuxFloat rewritten from scratch. New I-term back-tracking for anti-windup prevention AIRMODE: Air mode anti-windup improvements PID: Remove integer PID (leave only one PID controller), remove pid_controller setting, remove float-point PID parameters (scale integer parameters to use in float-point PIDC) PID: New set of PID tuning multipliers for better tuning range PID: Make max_angle_inclination a PID-profile variable PID: Reinstate yaw_p_limit PID: Switch to a noise-robust differentiator Remove GTUNE PID: Use same self-leveling strength for ANGLE and HORIZON PID: Self-leveling LPF to smooth out leveling response to rapid attitude or target angle change --- Makefile | 12 +- src/main/blackbox/blackbox.c | 11 +- src/main/blackbox/blackbox_fielddefs.h | 8 - src/main/common/filter.c | 2 +- src/main/common/filter.h | 2 +- src/main/config/config.c | 60 +--- src/main/config/config_master.h | 2 +- src/main/config/runtime_config.h | 13 +- src/main/flight/gtune.c | 211 ----------- src/main/flight/gtune.h | 21 -- src/main/flight/mixer.c | 2 + src/main/flight/mixer.h | 1 + src/main/flight/navigation_rewrite.c | 27 -- .../flight/navigation_rewrite_fixedwing.c | 4 +- .../flight/navigation_rewrite_multicopter.c | 4 +- src/main/flight/navigation_rewrite_private.h | 3 - src/main/flight/pid.c | 337 ++++++------------ src/main/flight/pid.h | 46 +-- src/main/io/rc_controls.c | 119 ++----- src/main/io/rc_controls.h | 10 +- src/main/io/serial_cli.c | 38 +- src/main/io/serial_msp.c | 66 +--- src/main/mw.c | 64 ++-- src/main/target/ALIENWIIF3/target.h | 1 - src/main/target/CHEBUZZF3/target.h | 1 - src/main/target/CJMCU/target.h | 1 - src/main/target/COLIBRI_RACE/target.h | 1 - src/main/target/EUSTM32F103RC/target.h | 1 - src/main/target/NAZE/target.h | 2 - src/main/target/NAZE32PRO/target.h | 1 - src/main/target/PORT103R/target.h | 1 - src/main/target/SPARKY/target.h | 2 - src/main/target/SPRACINGF3/target.h | 1 - src/main/target/STM32F3DISCOVERY/target.h | 1 - 34 files changed, 250 insertions(+), 826 deletions(-) delete mode 100644 src/main/flight/gtune.c delete mode 100644 src/main/flight/gtune.h diff --git a/Makefile b/Makefile index 88f600479d..d408ce3a31 100755 --- a/Makefile +++ b/Makefile @@ -266,12 +266,11 @@ COMMON_SRC = build_config.c \ $(DEVICE_STDPERIPH_SRC) HIGHEND_SRC = \ - flight/gtune.c \ - flight/navigation_rewrite.c \ - flight/navigation_rewrite_multicopter.c \ - flight/navigation_rewrite_fixedwing.c \ - flight/navigation_rewrite_pos_estimator.c \ - flight/navigation_rewrite_geo.c \ + flight/navigation_rewrite.c \ + flight/navigation_rewrite_multicopter.c \ + flight/navigation_rewrite_fixedwing.c \ + flight/navigation_rewrite_pos_estimator.c \ + flight/navigation_rewrite_geo.c \ flight/gps_conversion.c \ common/colorconversion.c \ io/gps.c \ @@ -440,7 +439,6 @@ CJMCU_SRC = \ drivers/timer.c \ drivers/timer_stm32f10x.c \ hardware_revision.c \ - flight/gtune.c \ blackbox/blackbox.c \ blackbox/blackbox_io.c \ $(COMMON_SRC) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9fc428d114..3bb36c39d7 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -425,11 +425,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { - return currentProfile->pidProfile.D_f[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; - } else { - return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; - } + return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0; case FLIGHT_LOG_FIELD_CONDITION_MAG: #ifdef MAG @@ -1279,11 +1275,6 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWrite(data->inflightAdjustment.adjustmentFunction); blackboxWriteSignedVB(data->inflightAdjustment.newValue); } - case FLIGHT_LOG_EVENT_GTUNE_RESULT: - blackboxWrite(data->gtuneCycleResult.gtuneAxis); - blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG); - blackboxWriteS16(data->gtuneCycleResult.gtuneNewP); - break; case FLIGHT_LOG_EVENT_LOGGING_RESUME: blackboxWriteUnsignedVB(data->loggingResume.logIteration); blackboxWriteUnsignedVB(data->loggingResume.currentTime); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 0bee570f20..270e78bd7e 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -105,7 +105,6 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13, FLIGHT_LOG_EVENT_LOGGING_RESUME = 14, - FLIGHT_LOG_EVENT_GTUNE_RESULT = 20, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -127,17 +126,10 @@ typedef struct flightLogEvent_loggingResume_s { #define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128 -typedef struct flightLogEvent_gtuneCycleResult_s { - uint8_t gtuneAxis; - int32_t gtuneGyroAVG; - int16_t gtuneNewP; -} flightLogEvent_gtuneCycleResult_t; - typedef union flightLogEventData_u { flightLogEvent_syncBeep_t syncBeep; flightLogEvent_inflightAdjustment_t inflightAdjustment; flightLogEvent_loggingResume_t loggingResume; - flightLogEvent_gtuneCycleResult_t gtuneCycleResult; } flightLogEventData_t; typedef struct flightLogEvent_s { diff --git a/src/main/common/filter.c b/src/main/common/filter.c index d5a7007a69..61c87ec2d8 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -85,7 +85,7 @@ float filterApplyBiQuad(float sample, biquad_t *state) } // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime) -float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) +float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) { // Pre calculate and store RC if (!filter->RC) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index dcfec491db..2ded2c6877 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -29,7 +29,7 @@ typedef struct biquad_s { float x1, x2, y1, y2; } biquad_t; -float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt); +float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt); void filterResetPt1(filterStatePt1_t *filter, float input); void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate); diff --git a/src/main/config/config.c b/src/main/config/config.c index 1dd60b6702..b16c0d6a2d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -149,17 +149,15 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynam void resetPidProfile(pidProfile_t *pidProfile) { - pidProfile->pidController = PID_CONTROLLER_MWREWRITE; - - pidProfile->P8[ROLL] = 40; - pidProfile->I8[ROLL] = 30; - pidProfile->D8[ROLL] = 23; - pidProfile->P8[PITCH] = 40; - pidProfile->I8[PITCH] = 30; - pidProfile->D8[PITCH] = 23; - pidProfile->P8[YAW] = 85; - pidProfile->I8[YAW] = 45; - pidProfile->D8[YAW] = 0; + pidProfile->P8[ROLL] = 56; + pidProfile->I8[ROLL] = 120; + pidProfile->D8[ROLL] = 80; + pidProfile->P8[PITCH] = 56; + pidProfile->I8[PITCH] = 120; + pidProfile->D8[PITCH] = 80; + pidProfile->P8[YAW] = 140; // 3.5 * 40 + pidProfile->I8[YAW] = 160; // 4.0 * 40 + pidProfile->D8[YAW] = 40; // 0.01 * 4000 pidProfile->P8[PIDALT] = 50; // NAV_POS_Z_P * 100 pidProfile->I8[PIDALT] = 0; // not used pidProfile->D8[PIDALT] = 0; // not used @@ -172,9 +170,9 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PIDNAVR] = 14; // FW_NAV_P * 100 pidProfile->I8[PIDNAVR] = 2; // FW_NAV_I * 100 pidProfile->D8[PIDNAVR] = 8; // FW_NAV_D * 100 - pidProfile->P8[PIDLEVEL] = 20; - pidProfile->I8[PIDLEVEL] = 20; - pidProfile->D8[PIDLEVEL] = 100; + pidProfile->P8[PIDLEVEL] = 160; // Self-level strength * 40 (4 * 40) + pidProfile->I8[PIDLEVEL] = 10; // Self-leveing low-pass frequency (0 - disabled) + pidProfile->D8[PIDLEVEL] = 75; // 75% horizon strength pidProfile->P8[PIDMAG] = 40; pidProfile->P8[PIDVEL] = 100; // NAV_VEL_Z_P * 100 pidProfile->I8[PIDVEL] = 50; // NAV_VEL_Z_I * 100 @@ -182,34 +180,11 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->acc_soft_lpf_hz = 15; pidProfile->gyro_soft_lpf_hz = 60; - - pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->dterm_lpf_hz = 30; - pidProfile->P_f[ROLL] = 1.4f; // new PID with preliminary defaults test carefully - pidProfile->I_f[ROLL] = 0.3f; - pidProfile->D_f[ROLL] = 0.02f; - pidProfile->P_f[PITCH] = 1.4f; - pidProfile->I_f[PITCH] = 0.3f; - pidProfile->D_f[PITCH] = 0.02f; - pidProfile->P_f[YAW] = 3.5f; - pidProfile->I_f[YAW] = 0.4f; - pidProfile->D_f[YAW] = 0.01f; - pidProfile->A_level = 4.0f; - pidProfile->H_level = 4.0f; - pidProfile->H_sensitivity = 75; + pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; -#ifdef GTUNE - pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. - pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune. - pidProfile->gtune_lolimP[YAW] = 10; // [0..200] Lower limit of YAW P during G tune. - pidProfile->gtune_hilimP[ROLL] = 100; // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis. - pidProfile->gtune_hilimP[PITCH] = 100; // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis. - pidProfile->gtune_hilimP[YAW] = 100; // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis. - pidProfile->gtune_pwr = 0; // [0..10] Strength of adjustment - pidProfile->gtune_settle_time = 450; // [200..1000] Settle time in ms - pidProfile->gtune_average_cycles = 16; // [8..128] Number of looptime cycles used for gyro average calculation -#endif + pidProfile->max_angle_inclination = 300; // 30 degrees } #ifdef NAV @@ -464,7 +439,6 @@ static void resetConf(void) masterConfig.boardAlignment.pitchDeciDegrees = 0; masterConfig.boardAlignment.yawDeciDegrees = 0; masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect - masterConfig.max_angle_inclination = 300; // 30 degrees masterConfig.yaw_control_direction = 1; masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; @@ -602,8 +576,6 @@ static void resetConf(void) #if defined(COLIBRI_RACE) masterConfig.looptime = 1000; - currentProfile->pidProfile.pidController = 1; - masterConfig.rxConfig.rcmap[0] = 1; masterConfig.rxConfig.rcmap[1] = 2; masterConfig.rxConfig.rcmap[2] = 3; @@ -635,7 +607,6 @@ static void resetConf(void) masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; masterConfig.looptime = 2000; - currentProfile->pidProfile.pidController = 3; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; masterConfig.failsafeConfig.failsafe_delay = 2; @@ -769,9 +740,6 @@ void activateConfig(void) telemetryUseConfig(&masterConfig.telemetryConfig); #endif - currentProfile->pidProfile.pidController = constrain(currentProfile->pidProfile.pidController, 1, 2); // This should limit to USED values only - pidSetController(currentProfile->pidProfile.pidController); - useFailsafeConfig(&masterConfig.failsafeConfig); setAccelerationZero(&masterConfig.accZero); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index d5b8fe80a0..3c9c461f1d 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -55,6 +55,7 @@ typedef struct master_t { uint16_t dcm_ki_acc; // DCM filter integral gain ( x 10000) for accelerometer uint16_t dcm_kp_mag; // DCM filter proportional gain ( x 10000) for magnetometer and GPS heading uint16_t dcm_ki_mag; // DCM filter integral gain ( x 10000) for magnetometer and GPS heading + uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. gyroConfig_t gyroConfig; @@ -64,7 +65,6 @@ typedef struct master_t { uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device uint8_t baro_hardware; // Barometer hardware to use - uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). flightDynamicsTrims_t accZero; // Accelerometer offset flightDynamicsTrims_t accGain; // Accelerometer gain to read exactly 1G flightDynamicsTrims_t magZero; // Compass offset diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 6362df169e..be8cdd0c07 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -42,7 +42,7 @@ typedef enum { UNUSED_MODE = (1 << 7), // old autotune PASSTHRU_MODE = (1 << 8), FAILSAFE_MODE = (1 << 10), - GTUNE_MODE = (1 << 11), + UNUSED_MODE2 = (1 << 11), // old G-Tune NAV_WP_MODE = (1 << 12), } flightModeFlags_e; @@ -53,11 +53,12 @@ extern uint16_t flightModeFlags; #define FLIGHT_MODE(mask) (flightModeFlags & (mask)) typedef enum { - GPS_FIX_HOME = (1 << 0), - GPS_FIX = (1 << 1), - CALIBRATE_MAG = (1 << 2), - SMALL_ANGLE = (1 << 3), - FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code + GPS_FIX_HOME = (1 << 0), + GPS_FIX = (1 << 1), + CALIBRATE_MAG = (1 << 2), + SMALL_ANGLE = (1 << 3), + FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code + ANTI_WINDUP = (1 << 5), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c deleted file mode 100644 index d32a93ad8d..0000000000 --- a/src/main/flight/gtune.c +++ /dev/null @@ -1,211 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include -#include - -#include "platform.h" - -#ifdef GTUNE - -#include "common/axis.h" -#include "common/maths.h" - -#include "drivers/system.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" - -#include "sensors/sensors.h" -#include "sensors/gyro.h" -#include "sensors/acceleration.h" - -#include "flight/pid.h" -#include "flight/imu.h" - -#include "config/config.h" -#include "blackbox/blackbox.h" - -#include "io/rc_controls.h" - -#include "config/runtime_config.h" - -extern uint16_t cycleTime; -extern uint8_t motorCount; - -/* - **************************************************************************** - *** G_Tune *** - **************************************************************************** - G_Tune Mode - This is the multiwii implementation of ZERO-PID Algorithm - http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html - The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com) - - You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution. - */ - -/* - version 1.0.0: MIN & Maxis & Tuned Band - version 1.0.1: - a. error is gyro reading not rc - gyro. - b. OldError = Error no averaging. - c. No Min Maxis BOUNDRY - version 1.0.2: - a. no boundaries - b. I - Factor tune. - c. time_skip - - Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore. - Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well. - See also: - http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2 - http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190 - Gyrosetting 2000DPS - GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s - - pidProfile->gtune_lolimP[ROLL] = 10; [0..200] Lower limit of ROLL P during G tune. - pidProfile->gtune_lolimP[PITCH] = 10; [0..200] Lower limit of PITCH P during G tune. - pidProfile->gtune_lolimP[YAW] = 10; [0..200] Lower limit of YAW P during G tune. - pidProfile->gtune_hilimP[ROLL] = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis. - pidProfile->gtune_hilimP[PITCH] = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis. - pidProfile->gtune_hilimP[YAW] = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis. - pidProfile->gtune_pwr = 0; [0..10] Strength of adjustment - pidProfile->gtune_settle_time = 450; [200..1000] Settle time in ms - pidProfile->gtune_average_cycles = 16; [8..128] Number of looptime cycles used for gyro average calculation -*/ - -static pidProfile_t *pidProfile; -static int16_t delay_cycles; -static int16_t time_skip[3]; -static int16_t OldError[3], result_P64[3]; -static int32_t AvgGyro[3]; -static bool floatPID; - -void updateDelayCycles(void) -{ - delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime); -} - -void init_Gtune(pidProfile_t *pidProfileToTune) -{ - uint8_t i; - - pidProfile = pidProfileToTune; - if (pidProfile->pidController == 2) { - floatPID = true; // LuxFloat is using float values for PID settings - } else { - floatPID = false; - } - updateDelayCycles(); - for (i = 0; i < 3; i++) { - if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning - pidProfile->gtune_hilimP[i] = 0; // Disable YAW tuning for everything below a quadcopter - } - if (floatPID) { - if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) { - pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f); - } - result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P. - } else { - if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) { - pidProfile->P8[i] = pidProfile->gtune_lolimP[i]; - } - result_P64[i] = (int16_t)pidProfile->P8[i] << 6; // 6 bit extra resolution for P. - } - OldError[i] = 0; - time_skip[i] = delay_cycles; - } -} - -void calculate_Gtune(uint8_t axis) -{ - int16_t error, diff_G, threshP; - - if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) { // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode - OldError[axis] = 0; - time_skip[axis] = delay_cycles; // Some settle time after stick center. default 450ms - } else { - if (!time_skip[axis]) AvgGyro[axis] = 0; - time_skip[axis]++; - if (time_skip[axis] > 0) { - if (axis == FD_YAW) { - AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32); // Chop some jitter and average - } else { - AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128); // Chop some jitter and average - } - } - if (time_skip[axis] == pidProfile->gtune_average_cycles) { // Looptime cycles for gyro average calculation. default 16. - AvgGyro[axis] /= time_skip[axis]; // AvgGyro[axis] has now very clean gyrodata - time_skip[axis] = 0; - if (axis == FD_YAW) { - threshP = 20; - error = -AvgGyro[axis]; - } else { - threshP = 10; - error = AvgGyro[axis]; - } - if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) { // Don't run when not needed or pointless to do so - diff_G = ABS(error) - ABS(OldError[axis]); - if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) { - if (diff_G > threshP) { - if (axis == FD_YAW) { - result_P64[axis] += 256 + pidProfile->gtune_pwr; // YAW ends up at low limit on float PID, give it some more to work with. - } else { - result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side. - } - } else { - if (diff_G < -threshP) { - if (axis == FD_YAW) { - result_P64[axis] -= 64 + pidProfile->gtune_pwr; - } else { - result_P64[axis] -= 32; - } - } - } - } else { - if (ABS(diff_G) > threshP && axis != FD_YAW) { - result_P64[axis] -= 32; // Don't use antiwobble for YAW - } - } - int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]); - -#ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_gtuneCycleResult_t eventData; - - eventData.gtuneAxis = axis; - eventData.gtuneGyroAVG = AvgGyro[axis]; - eventData.gtuneNewP = newP; // for float PID the logged P value is still mutiplyed by 10 - blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData); - } -#endif - - if (floatPID) { - pidProfile->P_f[axis] = (float)newP / 10.0f; // new P value for float PID - } else { - pidProfile->P8[axis] = newP; // new P value - } - } - OldError[axis] = error; - } - } -} - -#endif - diff --git a/src/main/flight/gtune.h b/src/main/flight/gtune.h deleted file mode 100644 index f580c7c125..0000000000 --- a/src/main/flight/gtune.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -void init_Gtune(pidProfile_t *pidProfileToTune); -void calculate_Gtune(uint8_t axis); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9304c06d1b..0a06cf958b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -61,6 +61,8 @@ uint8_t motorCount; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +bool motorLimitReached = false; + static mixerConfig_t *mixerConfig; static flight3DConfig_t *flight3DConfig; static escAndServoConfig_t *escAndServoConfig; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1a8a621681..14b547225e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -191,6 +191,7 @@ void filterServos(void); extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +extern bool motorLimitReached; struct escAndServoConfig_s; struct rxConfig_s; diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 9fc99ddf0e..f12ef0e72b 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -2240,33 +2240,6 @@ float getEstimatedActualPosition(int axis) return posControl.actualState.pos.A[axis]; } -/*----------------------------------------------------------- - * Interface with PIDs: Angle-Command transformation - *-----------------------------------------------------------*/ -int16_t rcCommandToLeanAngle(int16_t rcCommand) -{ - if (posControl.pidProfile->pidController == PID_CONTROLLER_LUX_FLOAT) { - // LuxFloat is the only PID controller that uses raw rcCommand as target angle - return rcCommand; - } - else { - // Most PID controllers use 2 * rcCommand as target angle for ANGLE mode - return rcCommand * 2; - } -} - -int16_t leanAngleToRcCommand(int16_t leanAngle) -{ - if (posControl.pidProfile->pidController == PID_CONTROLLER_LUX_FLOAT) { - // LuxFloat is the only PID controller that uses raw rcCommand as target angle - return leanAngle; - } - else { - // Most PID controllers use 2 * rcCommand as target angle for ANGLE mode - return leanAngle / 2; - } -} - /*----------------------------------------------------------- * Ability to execute RTH on external event *-----------------------------------------------------------*/ diff --git a/src/main/flight/navigation_rewrite_fixedwing.c b/src/main/flight/navigation_rewrite_fixedwing.c index c2ee447277..4a562488ea 100755 --- a/src/main/flight/navigation_rewrite_fixedwing.c +++ b/src/main/flight/navigation_rewrite_fixedwing.c @@ -340,13 +340,13 @@ void applyFixedWingPitchRollThrottleController(void) if (isPitchAndThrottleAdjustmentValid) { // PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb) pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_dive_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_climb_angle)); - rcCommand[PITCH] = -leanAngleToRcCommand(pitchCorrection); + rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection); rcCommand[THROTTLE] = constrain(throttleCorrection, posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); } if (isRollAdjustmentValid) { rollCorrection = constrain(rollCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle)); - rcCommand[ROLL] = leanAngleToRcCommand(rollCorrection); + rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection); } } diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index 4e2a1c304b..a7300d6589 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/src/main/flight/navigation_rewrite_multicopter.c @@ -440,8 +440,8 @@ static void applyMulticopterPositionController(uint32_t currentTime) } if (!bypassPositionController) { - rcCommand[PITCH] = leanAngleToRcCommand(posControl.rcAdjustment[PITCH]); - rcCommand[ROLL] = leanAngleToRcCommand(posControl.rcAdjustment[ROLL]); + rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH]); + rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL]); } } diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 95aa181110..7e7e24a1ac 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -292,9 +292,6 @@ bool isWaypointMissed(navWaypointPosition_t * waypoint); bool isApproachingLastWaypoint(void); float getActiveWaypointSpeed(void); -int16_t rcCommandToLeanAngle(int16_t rcCommand); -int16_t leanAngleToRcCommand(int16_t leanAngle); - void updateActualHeading(int32_t newHeading); void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY); void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 70c05838c9..75077dd9e0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -22,6 +22,7 @@ #include #include "build_config.h" +#include "debug.h" #include "common/axis.h" #include "common/maths.h" @@ -43,12 +44,12 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/navigation_rewrite.h" -#include "flight/gtune.h" #include "config/runtime_config.h" extern uint16_t cycleTime; extern uint8_t motorCount; +extern bool motorLimitReached; extern float dT; int16_t axisPID[3]; @@ -60,23 +61,11 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[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 dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; -static int32_t errorGyroI[3] = { 0, 0, 0 }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; - -static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rxConfig_t *rxConfig); - -typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rxConfig_t *rxConfig); // pid controller function prototype - -pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using +static float errorGyroIfLimit[3] = { 0.0f, 0.0f, 0.0f }; void pidResetErrorGyro(void) { - errorGyroI[ROLL] = 0; - errorGyroI[PITCH] = 0; - errorGyroI[YAW] = 0; - errorGyroIf[ROLL] = 0.0f; errorGyroIf[PITCH] = 0.0f; errorGyroIf[YAW] = 0.0f; @@ -85,245 +74,149 @@ void pidResetErrorGyro(void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static biquad_t deltaBiQuadState[3]; -static bool deltaStateIsSet = false; +static bool deltaFilterInit = false; -static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rxConfig_t *rxConfig) +float pidRcCommandToAngle(int16_t stick) { - float RateError, errorAngle, AngleRate, gyroRate; - float ITerm,PTerm,DTerm; - int32_t stickPosAil, stickPosEle, mostDeflectedPos; - static float lastError[3]; - float delta; - int axis; - float horizonLevelStrength = 1; - - if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) - filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0); - deltaStateIsSet = true; - } - - if (FLIGHT_MODE(HORIZON_MODE)) { - - // Figure out the raw stick positions - stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); - stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); - - if(ABS(stickPosAil) > ABS(stickPosEle)){ - mostDeflectedPos = ABS(stickPosAil); - } - else { - mostDeflectedPos = ABS(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->H_sensitivity == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - // -----Get the desired angle rate depending on flight mode - uint8_t rate = controlRateConfig->rates[axis]; - - if (axis == FD_YAW) { - // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate - AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; - } else { - // calculate error and limit the angle to the max inclination - errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis]) / 10.0f; // 16 bits is ok here - - if (FLIGHT_MODE(ANGLE_MODE)) { - // it's the ANGLE mode - control is angle based, so control loop is needed - AngleRate = errorAngle * pidProfile->A_level; - } else { - //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate - if (FLIGHT_MODE(HORIZON_MODE)) { - // mix up angle error to desired AngleRate to add a little auto-level feel - AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; - } - } - } - - gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps - - // --------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 - RateError = AngleRate - gyroRate; - - // -----calculate P component - PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; - - // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); - - // 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 - ITerm = errorGyroIf[axis]; - - //-----calculate D-term - delta = RateError - lastError[axis]; - lastError[axis] = RateError; - - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta *= (1.0f / dT); - - if (deltaStateIsSet) { - delta = filterApplyBiQuad(delta, &deltaBiQuadState[axis]); - } - - DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); - -#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 - } + return stick * 2.0f; } -static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - rxConfig_t *rxConfig) +int16_t pidAngleToRcCommand(float angleDeciDegrees) +{ + return angleDeciDegrees / 2.0f; +} + +float pidRcCommandToRate(int16_t stick, uint8_t rate) +{ + // Map stick position from 200dps to 1200dps + return (float)((rate + 20) * stick) / 50.0f; +} + +#define FP_PID_RATE_P_MULTIPLIER 40.0f +#define FP_PID_RATE_I_MULTIPLIER 40.0f +#define FP_PID_RATE_D_MULTIPLIER 4000.0f +#define FP_PID_LEVEL_P_MULTIPLIER 40.0f + +void pidController(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig) { UNUSED(rxConfig); - int axis; - int32_t delta; - int32_t PTerm, ITerm, DTerm; - static int32_t lastError[3] = { 0, 0, 0 }; - int32_t AngleRateTmp, RateError, gyroRate; - - int8_t horizonLevelStrength = 100; - - if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) - filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0); - deltaStateIsSet = true; - } + float horizonLevelStrength = 1; + static float rateErrorBuf[3][5] = { { 0 } }; + int axis, n; 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); + 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.0f / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } } - // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { - uint8_t rate = controlRateConfig->rates[axis]; + float kP = pidProfile->P8[axis] / FP_PID_RATE_P_MULTIPLIER; + float kI = pidProfile->I8[axis] / FP_PID_RATE_I_MULTIPLIER; + float kD = pidProfile->D8[axis] / FP_PID_RATE_D_MULTIPLIER; + bool useIntegralComponent = (pidProfile->P8[axis] != 0) && (pidProfile->I8[axis] != 0); - // -----Get the desired angle rate depending on flight mode - if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5); - } else { - AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4; + float rateTarget; // rotation rate target (dps) - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { - // calculate error and limit the angle to max configured inclination - int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here + // Rate setpoint for ACRO and HORIZON modes + rateTarget = pidRcCommandToRate(rcCommand[axis], controlRateConfig->rates[axis]); - if (FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - // it's the 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 += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4; - } + // Outer PID loop (ANGLE/HORIZON) + if ((axis != FD_YAW) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + static filterStatePt1_t angleFilterState[2]; // Only ROLL and PITCH + + float angleTarget = pidRcCommandToAngle(rcCommand[axis]); + float angleError = (constrain(angleTarget, -pidProfile->max_angle_inclination, +pidProfile->max_angle_inclination) - attitude.raw[axis]) / 10.0f; + + // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes) + if (FLIGHT_MODE(HORIZON_MODE)) { + rateTarget += angleError * (pidProfile->P8[PIDLEVEL] / FP_PID_LEVEL_P_MULTIPLIER) * horizonLevelStrength; + } + else { + rateTarget = angleError * (pidProfile->P8[PIDLEVEL] / FP_PID_LEVEL_P_MULTIPLIER); + } + + // Apply simple LPF to rateTarget to make response less jerky + if (pidProfile->I8[PIDLEVEL]) { + // I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz + rateTarget = filterApplyPt1(rateTarget, &angleFilterState[axis], pidProfile->I8[PIDLEVEL], dT); } } - // --------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; + // Limit desired rate to something gyro can measure reliably + rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + // Inner PID loop - gyro-based control to reach rateTarget + float gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps + float rateError = rateTarget - gyroRate; - // -----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. - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)cycleTime) >> 11) * pidProfile->I8[axis]; + // Calculate new P-term + float newPTerm = rateError * kP * (PIDweight[axis] / 100); - // 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 - delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastError[axis] = RateError; - - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)cycleTime >> 4))) >> 6; - - if (deltaStateIsSet) { - // Upscale x3 before filtering to avoid rounding errors - delta = lrintf(filterApplyBiQuad(3.0f * delta, &deltaBiQuadState[axis])); + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + newPTerm = constrain(newPTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); + // Calculate new D-term + // Shift old error values + for (n = 4; n > 0; n--) { + rateErrorBuf[axis][n] = rateErrorBuf[axis][n-1]; } -#endif + + // Store new error value + rateErrorBuf[axis][0] = rateError; + + // Calculate derivative using 5-point noise-robust differentiator by Pavel Holoborodko + float newDTerm = ((2 * (rateErrorBuf[axis][1] - rateErrorBuf[axis][3]) + (rateErrorBuf[axis][0] - rateErrorBuf[axis][4])) / (8 * dT)) * kD * (PIDweight[axis] / 100); + + // Apply additional lowpass + if (pidProfile->dterm_lpf_hz) { + if (!deltaFilterInit) { + for (axis = 0; axis < 3; axis++) + filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0); + deltaFilterInit = true; + } + + newDTerm = filterApplyBiQuad(newDTerm, &deltaBiQuadState[axis]); + } + + // TODO: Get feedback from mixer on available correction range for each axis + float newOutput = newPTerm + errorGyroIf[axis] + newDTerm; + float newOutputLimited = constrainf(newOutput, -PID_MAX_OUTPUT, +PID_MAX_OUTPUT); + + // Integrate only if we can do backtracking + if (useIntegralComponent) { + float kT = 2.0f / ((kP / kI) + (kD / kP)); + errorGyroIf[axis] += (rateError * kI * dT) + ((newOutputLimited - newOutput) * kT * dT); + + // Don't grow I-term if motors are at their limit + if (STATE(ANTI_WINDUP) || motorLimitReached) { + errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); + } else { + errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]); + } + } else { + errorGyroIf[axis] = 0; + } + + axisPID[axis] = newOutputLimited; + + debug[axis] = newOutputLimited; #ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; + axisPID_P[axis] = newPTerm; + axisPID_I[axis] = errorGyroIf[axis]; + axisPID_D[axis] = newDTerm; #endif } } - -void pidSetController(pidControllerType_e type) -{ - // Force filter re-init - deltaStateIsSet = false; - - switch (type) { - default: - case PID_CONTROLLER_MWREWRITE: - pid_controller = pidMultiWiiRewrite; - break; - case PID_CONTROLLER_LUX_FLOAT: - pid_controller = pidLuxFloat; - break; - } -} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d4ea811332..fe073ae78a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -17,9 +17,16 @@ #pragma once -#define GYRO_I_MAX 256 // Gyro I limiter +#include "rx/rx.h" + +#include "io/rc_controls.h" + +#include "config/runtime_config.h" + +#define GYRO_SATURATION_LIMIT 1800 // 1800dps +#define PID_MAX_OUTPUT 1000 #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter -#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter +#define YAW_P_LIMIT_MAX 300 // Maximum value for yaw P limiter typedef enum { PIDROLL, @@ -35,46 +42,27 @@ typedef enum { PID_ITEM_COUNT } pidIndex_e; -typedef enum { - PID_CONTROLLER_MWREWRITE = 1, - PID_CONTROLLER_LUX_FLOAT, - PID_COUNT -} pidControllerType_e; - -#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) - typedef struct pidProfile_s { - uint8_t pidController; // 1 = rewrite, 2 = luxfloat - uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; - float P_f[3]; // float p i and d factors for lux float pid controller - float I_f[3]; - float D_f[3]; - float A_level; - float H_level; - uint8_t H_sensitivity; - - uint16_t yaw_p_limit; // set P term limit (fixed value was 300) uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 - + uint8_t yaw_pterm_lpf_hz; // Used for filering Pterm noise on noisy frames uint8_t gyro_soft_lpf_hz; // Gyro FIR filtering uint8_t acc_soft_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter -#ifdef GTUNE - uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune - uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis. - uint8_t gtune_pwr; // [0..10] Strength of adjustment - uint16_t gtune_settle_time; // [200..1000] Settle time in ms - uint8_t gtune_average_cycles; // [8..128] Number of looptime cycles used for gyro average calculation -#endif + uint16_t yaw_p_limit; + + int16_t max_angle_inclination; // Max possible inclination } pidProfile_t; extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; -void pidSetController(pidControllerType_e type); void pidResetErrorGyro(void); +float pidRcCommandToAngle(int16_t stick); +int16_t pidAngleToRcCommand(float angleDeciDegrees); + +void pidController(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index edd8e62447..b3d38d2d4a 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -59,6 +59,7 @@ #include "mw.h" +#define AIRMODE_DEADBAND 10 static escAndServoConfig_t *escAndServoConfig; static pidProfile_t *pidProfile; @@ -133,6 +134,15 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband return THROTTLE_HIGH; } +rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig) +{ + if (((rcData[PITCH] < (rxConfig->midrc + AIRMODE_DEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODE_DEADBAND))) + && ((rcData[ROLL] < (rxConfig->midrc + AIRMODE_DEADBAND)) && (rcData[ROLL] > (rxConfig->midrc -AIRMODE_DEADBAND)))) + return CENTERED; + + return NOT_CENTERED; +} + void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors @@ -439,7 +449,6 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { int newValue; - float newFloatValue; if (delta > 0) { beeperConfirmationBeeps(2); @@ -486,114 +495,60 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_P: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P_f[PIDPITCH] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_P, newFloatValue); - } else { - newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); - } + newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_P) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_P case ADJUSTMENT_ROLL_P: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P_f[PIDROLL] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_P, newFloatValue); - } else { - newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); - } + newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); break; case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_I: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I_f[PIDPITCH] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_I, newFloatValue); - } else { - newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); - } + newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_I) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_I case ADJUSTMENT_ROLL_I: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I_f[PIDROLL] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_I, newFloatValue); - } else { - newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); - } + newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); break; case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_D: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D_f[PIDPITCH] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_D, newFloatValue); - } else { - newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); - } + newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_D) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_D case ADJUSTMENT_ROLL_D: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D_f[PIDROLL] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_D, newFloatValue); - } else { - newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); - } + newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); break; case ADJUSTMENT_YAW_P: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P_f[PIDYAW] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_P, newFloatValue); - } else { - newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->P8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); - } + newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); break; case ADJUSTMENT_YAW_I: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I_f[PIDYAW] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_I, newFloatValue); - } else { - newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->I8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); - } + newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); break; case ADJUSTMENT_YAW_D: - if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D_f[PIDYAW] = newFloatValue; - blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_D, newFloatValue); - } else { - newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidProfile->D8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); - } + newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); break; default: break; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 53339f1810..be6e393421 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -39,7 +39,7 @@ typedef enum { BOXGOV, BOXOSD, BOXTELEMETRY, - BOXGTUNE, + //BOXGTUNE, BOXSERVO1, BOXSERVO2, BOXSERVO3, @@ -77,6 +77,11 @@ typedef enum { THROTTLE_HIGH } throttleStatus_e; +typedef enum { + NOT_CENTERED = 0, + CENTERED +} rollPitchStatus_e; + #define ROL_LO (1 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL)) #define ROL_HI (2 << (2 * ROLL)) @@ -126,8 +131,6 @@ typedef struct modeActivationCondition_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) -#define SHOULD_RESET_ERRORS ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED)) || ((throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)))) - typedef struct controlRateConfig_s { uint8_t rcRate8; uint8_t rcExpo8; @@ -152,6 +155,7 @@ bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); +rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig); void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ab45230486..5d2eecc39a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -645,8 +645,6 @@ const clivalue_t valueTable[] = { { "align_board_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.pitchDeciDegrees, .config.minmax = { -1800, 3600 }, 0 }, { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDeciDegrees, .config.minmax = { -1800, 3600 }, 0 }, - { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 }, 0 }, - { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 }, 0 }, @@ -666,6 +664,7 @@ const clivalue_t valueTable[] = { { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 }, 0 }, { "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH }, 0 }, + { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, 0 }, #ifdef USE_SERVOS { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 }, @@ -707,8 +706,6 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, .config.minmax = { 0, MAG_MAX }, 0 }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000, 18000 }, 0 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER }, 0 }, - { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 }, 0 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 }, 0 }, { "d_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], .config.minmax = { 0, 200 }, 0 }, @@ -719,41 +716,16 @@ const clivalue_t valueTable[] = { { "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 }, 0 }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 }, 0 }, - { "p_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[PITCH], .config.minmax = { 0, 100 }, 0 }, - { "i_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[PITCH], .config.minmax = { 0, 100 }, 0 }, - { "d_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[PITCH], .config.minmax = { 0, 100 }, 0 }, - { "p_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[ROLL], .config.minmax = { 0, 100 }, 0 }, - { "i_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[ROLL], .config.minmax = { 0, 100 }, 0 }, - { "d_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[ROLL], .config.minmax = { 0, 100 }, 0 }, - { "p_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[YAW], .config.minmax = { 0, 100 }, 0 }, - { "i_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[YAW], .config.minmax = { 0, 100 }, 0 }, - { "d_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[YAW], .config.minmax = { 0, 100 }, 0 }, + { "p_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDLEVEL], .config.minmax = { 0, 255 }, 0 }, + { "i_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDLEVEL], .config.minmax = { 0, 100 }, 0 }, + { "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0, 100 }, 0 }, - { "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, .config.minmax = { 0, 10 }, 0 }, - { "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, .config.minmax = { 0, 10 }, 0 }, - { "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, .config.minmax = { 0, 250 }, 0 }, + { "max_angle_inclination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.max_angle_inclination, .config.minmax = { 100, 900 }, 0 }, - { "p_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDLEVEL], .config.minmax = { 0, 200 }, 0 }, - { "i_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDLEVEL], .config.minmax = { 0, 200 }, 0 }, - { "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0, 200 }, 0 }, - - { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, { "gyro_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_soft_lpf_hz, .config.minmax = {0, 200 } }, { "acc_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acc_soft_lpf_hz, .config.minmax = {0, 200 } }, { "dterm_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 200 } }, -#ifdef GTUNE - { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 }, 0 }, - { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 }, 0 }, - { "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10, 200 }, 0 }, - { "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0, 200 }, 0 }, - { "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0, 200 }, 0 }, - { "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0, 200 }, 0 }, - { "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0, 10 }, 0 }, - { "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200, 1000 }, 0 }, - { "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 }, 0 }, -#endif - #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 }, 0 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 }, 0 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9488723b7e..6d2758f9ef 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -358,7 +358,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXGOV, "GOVERNOR;", 18 }, { BOXOSD, "OSD SW;", 19 }, { BOXTELEMETRY, "TELEMETRY;", 20 }, - { BOXGTUNE, "GTUNE;", 21 }, + //{ BOXGTUNE, "GTUNE;", 21 }, { BOXSERVO1, "SERVO1;", 23 }, { BOXSERVO2, "SERVO2;", 24 }, { BOXSERVO3, "SERVO3;", 25 }, @@ -715,10 +715,6 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; } -#ifdef GTUNE - activeBoxIds[activeBoxIdCount++] = BOXGTUNE; -#endif - memset(mspPorts, 0x00, sizeof(mspPorts)); mspAllocateSerialPorts(serialConfig); } @@ -748,7 +744,6 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | @@ -994,29 +989,10 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid - for (i = 0; i < 3; i++) { - serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 255)); - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 255)); - serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 255)); - } else { - serialize8(currentProfile->pidProfile.P8[i]); - serialize8(currentProfile->pidProfile.I8[i]); - serialize8(currentProfile->pidProfile.D8[i]); - } - } - } else { - for (i = 0; i < PID_ITEM_COUNT; i++) { - serialize8(currentProfile->pidProfile.P8[i]); - serialize8(currentProfile->pidProfile.I8[i]); - serialize8(currentProfile->pidProfile.D8[i]); - } + for (i = 0; i < PID_ITEM_COUNT; i++) { + serialize8(currentProfile->pidProfile.P8[i]); + serialize8(currentProfile->pidProfile.I8[i]); + serialize8(currentProfile->pidProfile.D8[i]); } break; case MSP_PIDNAMES: @@ -1025,7 +1001,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID_CONTROLLER: headSerialReply(1); - serialize8(currentProfile->pidProfile.pidController); + serialize8(2); // FIXME: Report as LuxFloat break; case MSP_MODE_RANGES: headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); @@ -1430,33 +1406,13 @@ static bool processInCommand(void) masterConfig.looptime = read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility - pidSetController(currentProfile->pidProfile.pidController); + // FIXME: Do nothing break; case MSP_SET_PID: - if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { - for (i = 0; i < 3; i++) { - currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; - currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; - currentProfile->pidProfile.D_f[i] = (float)read8() / 1000.0f; - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - currentProfile->pidProfile.A_level = (float)read8() / 10.0f; - currentProfile->pidProfile.H_level = (float)read8() / 10.0f; - currentProfile->pidProfile.H_sensitivity = read8(); - } else { - currentProfile->pidProfile.P8[i] = read8(); - currentProfile->pidProfile.I8[i] = read8(); - currentProfile->pidProfile.D8[i] = read8(); - } - } - } else { - for (i = 0; i < PID_ITEM_COUNT; i++) { - currentProfile->pidProfile.P8[i] = read8(); - currentProfile->pidProfile.I8[i] = read8(); - currentProfile->pidProfile.D8[i] = read8(); - } + for (i = 0; i < PID_ITEM_COUNT; i++) { + currentProfile->pidProfile.P8[i] = read8(); + currentProfile->pidProfile.I8[i] = read8(); + currentProfile->pidProfile.D8[i] = read8(); } break; case MSP_SET_MODE_RANGE: diff --git a/src/main/mw.c b/src/main/mw.c index 0f0159553e..1a0f8cd2b0 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -76,7 +76,6 @@ #include "flight/imu.h" #include "flight/hil.h" #include "flight/failsafe.h" -#include "flight/gtune.h" #include "flight/navigation_rewrite.h" #include "config/runtime_config.h" @@ -115,35 +114,6 @@ extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; static bool isRXDataNew; -typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rxConfig_t *rxConfig); // pid controller function prototype - -extern pidControllerFuncPtr pid_controller; - -#ifdef GTUNE - -void updateGtuneState(void) -{ - static bool GTuneWasUsed = false; - - if (IS_RC_MODE_ACTIVE(BOXGTUNE)) { - if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - ENABLE_FLIGHT_MODE(GTUNE_MODE); - init_Gtune(¤tProfile->pidProfile); - GTuneWasUsed = true; - } - if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) { - saveConfigAndNotify(); - GTuneWasUsed = false; - } - } else { - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - DISABLE_FLIGHT_MODE(GTUNE_MODE); - } - } -} -#endif - bool isCalibrating() { #ifdef BARO @@ -401,8 +371,26 @@ void processRx(void) throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - if (SHOULD_RESET_ERRORS) { - pidResetErrorGyro(); + /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. + This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air + Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */ + if (throttleStatus == THROTTLE_LOW) { + if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { + rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig); + + if (rollPitchStatus == CENTERED) { + ENABLE_STATE(ANTI_WINDUP); + } + else { + DISABLE_STATE(ANTI_WINDUP); + } + } + else { + pidResetErrorGyro(); + } + } + else { + DISABLE_STATE(ANTI_WINDUP); } // When armed and motors aren't spinning, do beeps and then disarm @@ -604,10 +592,6 @@ void taskMainPidLoop(void) isRXDataNew = false; -#ifdef GTUNE - updateGtuneState(); -#endif - #if defined(NAV) updatePositionEstimator(); applyWaypointNavigationAndAltitudeHold(); @@ -655,13 +639,7 @@ void taskMainPidLoop(void) } } - // PID - note this is function pointer set by setPIDController() - pid_controller( - ¤tProfile->pidProfile, - currentControlRateProfile, - masterConfig.max_angle_inclination, - &masterConfig.rxConfig - ); + pidController(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig); #ifdef HIL if (hilActive) { diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h index a4f8f85ed7..1d06dad406 100644 --- a/src/main/target/ALIENWIIF3/target.h +++ b/src/main/target/ALIENWIIF3/target.h @@ -118,7 +118,6 @@ //#define GPS_PROTO_I2C_NAV //#define GPS_PROTO_NAZA -#define GTUNE //#define DISPLAY //#define DISPLAY_ARMED_BITMAP #define USE_SERVOS diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 7923317238..2df59c5f13 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -124,7 +124,6 @@ #endif #define BLACKBOX -#define GTUNE #define TELEMETRY #define TELEMETRY_FRSKY #define TELEMETRY_HOTT diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 352b1a8420..a14a3a35a8 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -78,7 +78,6 @@ #endif //#undef USE_CLI -#define GTUNE //#define BLACKBOX #define DISABLE_UNCOMMON_MIXERS diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 4a99f832c0..8f6e1473a6 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -150,7 +150,6 @@ #define GPS_PROTO_I2C_NAV #define GPS_PROTO_NAZA -#define GTUNE #define LED_STRIP #define LED_STRIP_TIMER TIM16 diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 75cb3a6b26..dba512cf10 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -128,7 +128,6 @@ #define LED_STRIP_TIMER TIM3 #define BLACKBOX -#define GTUNE #define TELEMETRY #define TELEMETRY_FRSKY #define TELEMETRY_HOTT diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 98060c668d..ad2d65d8aa 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -180,8 +180,6 @@ #define NAV -//#define GTUNE - //#define LED_STRIP //#define LED_STRIP_TIMER TIM3 diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index 9a8484ca8b..97fe22b374 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -48,7 +48,6 @@ #define GPS_PROTO_I2C_NAV #define GPS_PROTO_NAZA -#define GTUNE #define SERIAL_RX #define TELEMETRY #define TELEMETRY_FRSKY diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 5b0d3ce776..f976afbe7f 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -157,7 +157,6 @@ #define GPS_PROTO_I2C_NAV #define GPS_PROTO_NAZA -#define GTUNE #define SERIAL_RX #define TELEMETRY #define TELEMETRY_FRSKY diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 6154a2bc04..201bdb45d0 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -118,8 +118,6 @@ #define GPS_PROTO_I2C_NAV #define GPS_PROTO_NAZA -#define GTUNE - #define DISPLAY #define DISPLAY_ARMED_BITMAP diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 287f60a4b1..1f5faec6d1 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -174,7 +174,6 @@ #define NAV -#define GTUNE #define SERIAL_RX #define TELEMETRY #define TELEMETRY_FRSKY diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index dbb46eeb7f..7fe4c8ddf2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -97,7 +97,6 @@ #define GPS_PROTO_I2C_NAV #define GPS_PROTO_NAZA -#define GTUNE #define LED_STRIP #define LED_STRIP_TIMER TIM16 #define TELEMETRY