rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'corvuscorax/OP-1177_althold_cruisecontrol_cleanups' into rel-14.01

Browse Source
This commit is contained in:
Corvus Corax 2014-01-22 18:28:09 +01:00
commit 6ce7907cd3
1 changed files with 0 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,7 +48,6 @@
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <attitudestate.h>
#include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module
#include <altitudeholdstatus.h>
@ -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;
}