Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Added horizon_tilt_effect command

Browse source 
and horizon_tilt_expert_mode command.
Modified 'calcHorizonLevelStrength()' function.
Changed 'd_level' (D8[PIDLEVEL]) default from 100 to 75
Added horizon static float vars
This commit is contained in:
ethomas999 2017-03-04 00:13:38 -05:00
parent 2224347cbe
commit 8167892604
3 changed files with 74 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

79
src/main/flight/pid.c
View file

@ -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) {