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