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

Altitude Hold - compensation for tilt

Browse Source
This commit is contained in:
Corvus Corax 2013-12-29 19:09:54 +01:00
parent 505d334c4b
commit cefcb9881a
1 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -149,10 +149,25 @@ static void altitudeHoldTask(void)
// altitude control loop // altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
AltitudeHoldStatusSet(&altitudeHoldStatus);
// velocity control loop // velocity control loop
float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
AltitudeHoldStatusSet(&altitudeHoldStatus); // compensate throttle for current attitude
// Rbe[2][2] in the rotation matrix is the rotated down component of a
// 0,0,1 vector
// it is used to scale the throttle, but only up to 4 times the regular
// throttle
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float Rbe[3][3];
Quaternion2R(&attitudeState.q1, Rbe);
if (fabsf(Rbe[2][2]) > 0.25f) {
throttle /= Rbe[2][2];
} else {
throttle /= 0.25f;
}
if (throttle >= 1.0f) { if (throttle >= 1.0f) {
throttle = 1.0f; throttle = 1.0f;