diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 3d71ee2b2..bf996d0d1 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -48,7 +48,6 @@ #include #include #include -#include #include #include // object that will be updated by the module #include @@ -170,21 +169,6 @@ static void altitudeHoldTask(void) // velocity control loop throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); - // 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; }