From cefcb9881ad7147b9653eb8dfbb1472942af81d2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 29 Dec 2013 19:09:54 +0100 Subject: [PATCH] Altitude Hold - compensation for tilt --- flight/modules/AltitudeHold/altitudehold.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index e3e5fcd2a..f177d9c8b 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -149,10 +149,25 @@ static void altitudeHoldTask(void) // altitude control loop altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + AltitudeHoldStatusSet(&altitudeHoldStatus); + // velocity control loop 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) { throttle = 1.0f;