diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 2e76a47d55..ed66acec7a 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -778,6 +778,9 @@ static const clivalue_t valueTable[] = { { "level_sensitivity", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelSensitivity) }, { "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 120 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, + + { "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, + { "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) }, #ifdef GPS { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PIDPOS]) }, { "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PIDPOS]) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2b082e89cf..819d2ae6ae 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -97,7 +97,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .D8[PIDNAVR] = 83, // NAV_D * 1000, .P8[PIDLEVEL] = 50, .I8[PIDLEVEL] = 50, - .D8[PIDLEVEL] = 100, + .D8[PIDLEVEL] = 75, .P8[PIDMAG] = 40, .P8[PIDVEL] = 55, .I8[PIDVEL] = 55, @@ -126,7 +126,9 @@ void resetPidProfile(pidProfile_t *pidProfile) .crash_recovery_rate = 100, // 100 degrees/second .crash_dthreshold = 50, // 50 degrees/second/second .crash_gthreshold = 200, // 200 degrees/second - .crash_recovery = PID_CRASH_RECOVERY_OFF // off by default + .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default + .horizon_tilt_effect = 75, + .horizon_tilt_expert_mode = false ); } @@ -236,7 +238,9 @@ void pidInitFilters(const pidProfile_t *pidProfile) static float Kp[3], Ki[3], Kd[3], maxVelocity[3]; static float relaxFactor; static float dtermSetpointWeight; -static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv; +static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, + horizonFactorRatio, ITermWindupPoint, ITermWindupPointInv; +static uint8_t horizonTiltExpertMode; static timeDelta_t crashTimeLimitUs; static int32_t crashRecoveryAngleDeciDegrees; static float crashRecoveryRate; @@ -253,7 +257,10 @@ void pidInitConfig(const pidProfile_t *pidProfile) { relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f); levelGain = pidProfile->P8[PIDLEVEL] / 10.0f; horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f; - horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL]; + horizonTransition = (float)pidProfile->D8[PIDLEVEL]; + horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode; + horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f; + horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; @@ -272,14 +279,64 @@ void pidInit(const pidProfile_t *pidProfile) pidInitConfig(pidProfile); } -static float calcHorizonLevelStrength(void) { - float horizonLevelStrength = 0.0f; - if (horizonTransition > 0.0f) { - const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH)); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = constrainf(1 - mostDeflectedPos * horizonTransition, 0, 1); +// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling +static float calcHorizonLevelStrength() { + // start with 1.0 at center stick, 0.0 at max stick deflection: + float horizonLevelStrength = 1.0f - + MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH)); + + // 0 at level, 90 at vertical, 180 at inverted (degrees): + const float currentInclination = MAX(ABS(attitude.values.roll), + ABS(attitude.values.pitch)) / 10.0f; + + // horizonTiltExpertMode: 0 = leveling always active when sticks centered, + // 1 = leveling can be totally off when inverted + if (horizonTiltExpertMode) { + if (horizonTransition > 0 && horizonCutoffDegrees > 0) { + // if d_level > 0 and horizonTiltEffect < 175 + // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero) + // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling) + // for larger inclinations; 0.0 at horizonCutoffDegrees value: + const float inclinationLevelRatio = constrainf( + (horizonCutoffDegrees-currentInclination) / horizonCutoffDegrees, 0, 1); + // apply configured horizon sensitivity: + // when stick is near center (horizonLevelStrength ~= 1.0) + // H_sensitivity value has little effect, + // when stick is deflected (horizonLevelStrength near 0.0) + // H_sensitivity value has more effect: + horizonLevelStrength = (horizonLevelStrength - 1) * + 100 / horizonTransition + 1; + // apply inclination ratio, which may lower leveling + // to zero regardless of stick position: + horizonLevelStrength *= inclinationLevelRatio; + } + else // d_level=0 or horizon_tilt_effect>=175 means no leveling + horizonLevelStrength = 0; + } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered) + float sensitFact; + if (horizonFactorRatio < 1.01f) { // if horizonTiltEffect > 0 + // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling) + // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling) + // for larger inclinations, goes to 1.0 at inclination==level: + const float inclinationLevelRatio = (180-currentInclination)/180 * + (1.0f-horizonFactorRatio) + horizonFactorRatio; + // apply ratio to configured horizon sensitivity: + sensitFact = horizonTransition * inclinationLevelRatio; + } + else // horizonTiltEffect=0 for "old" functionality + sensitFact = horizonTransition; + + if (sensitFact <= 0) { // zero means no leveling + horizonLevelStrength = 0; + } else { + // when stick is near center (horizonLevelStrength ~= 1.0) + // sensitFact value has little effect, + // when stick is deflected (horizonLevelStrength near 0.0) + // sensitFact value has more effect: + horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1; + } } - return horizonLevelStrength; + return constrainf(horizonLevelStrength, 0, 1); } static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 5e65d13662..19678d47d1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -84,6 +84,9 @@ typedef struct pidProfile_s { uint8_t levelAngleLimit; // Max angle in degrees in level mode uint8_t levelSensitivity; // Angle mode sensitivity reflected in degrees assuming user using full stick + uint8_t horizon_tilt_effect; // inclination factor for Horizon mode + uint8_t horizon_tilt_expert_mode; // OFF or ON + // Betaflight PID controller parameters uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit