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

Code tidy and comments update

Browse source
This commit is contained in:
Martin Budden 2017-08-30 08:50:40 +01:00
parent 524afb2a47
commit eadaff0bdd
3 changed files with 40 additions and 40 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -353,6 +353,7 @@ static float calcHorizonLevelStrength(void)
static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
// calculate error angle and limit the angle to the max inclination
// rcDeflection is in range [-1.0, 1.0]
float angle = pidProfile->levelSensitivity * getRcDeflection(axis);
#ifdef GPS
angle += GPS_angle[axis];
@ -360,11 +361,11 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
// ANGLE mode - control is angle based
currentPidSetpoint = errorAngle * levelGain;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
// HORIZON mode - mix of ANGLE and ACRO modes
// mix in errorAngle to currentPidSetpoint to add a little auto-level feel
const float horizonLevelStrength = calcHorizonLevelStrength();
currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
}