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

More readable horizonlevelStrength

Browse source
This commit is contained in:
borisbstyle 2016-12-31 01:58:51 +01:00
parent 79d76f6b61
commit b5d1ef779c
1 changed files with 5 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -157,15 +157,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
} }
float calcHorizonLevelStrength(const pidProfile_t *pidProfile) { float calcHorizonLevelStrength(void) {
float horizonLevelStrength; float horizonLevelStrength = 0.0f;
if(pidProfile->D8[PIDLEVEL] == 0){ if (horizonTransition > 0.0f) {
horizonLevelStrength = 0;
} else {
const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH)); const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
// Progressively turn off the horizon self level strength as the stick is banged over // Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (1.0f - mostDeflectedPos); // 1 at centre stick, 0 = max stick deflection horizonLevelStrength = constrainf(1 - mostDeflectedPos * horizonTransition, 0, 1);
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * horizonTransition) + 1, 0, 1);
} }
return horizonLevelStrength; return horizonLevelStrength;
} }
@ -184,7 +181,7 @@ float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims
} else { } else {
// HORIZON mode - direct sticks control is applied to rate PID // HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel // mix up angle error to desired AngleRate to add a little auto-level feel
const float horizonLevelStrength = calcHorizonLevelStrength(pidProfile); const float horizonLevelStrength = calcHorizonLevelStrength();
currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength); currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
} }
return currentPidSetpoint; return currentPidSetpoint;