rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

Scale up desired throttle in AH mode to compensate for roll/pitch

Browse Source
This commit is contained in:
Werner Backes 2013-06-29 12:45:40 +02:00 committed by Alessio Morale
parent 73aa4df431
commit 1da68cda9a
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/modules/AltitudeHold/altitudehold.c
View File

@ -125,6 +125,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
{ {
AltitudeHoldDesiredData altitudeHoldDesired; AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired; StabilizationDesiredData stabilizationDesired;
float q[4], Rbe[3][3];
portTickType thisTime, lastUpdateTime; portTickType thisTime, lastUpdateTime;
UAVObjEvent ev; UAVObjEvent ev;
@ -238,7 +239,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
x[0] = baro.Altitude; x[0] = baro.Altitude;
// rotate avg accels into earth frame and store it // rotate avg accels into earth frame and store it
float q[4], Rbe[3][3];
q[0] = attitudeState.q1; q[0] = attitudeState.q1;
q[1] = attitudeState.q2; q[1] = attitudeState.q2;
q[2] = attitudeState.q3; q[2] = attitudeState.q3;
@ -373,6 +373,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
if (!enterFailSafe) { if (!enterFailSafe) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
// scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling
float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
stabilizationDesired.Throttle /= throttlescale;
if (stabilizationDesired.Throttle > 1) { if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1); throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1; stabilizationDesired.Throttle = 1;