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

OP-1177 remove cc algorithm from altitude hold code

Browse Source
This commit is contained in:
Corvus Corax 2014-01-21 20:21:38 +01:00
parent 98d061d647
commit 6d855fbaf0
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 <math.h>
#include <pid.h> #include <pid.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <attitudestate.h>
#include <altitudeholdsettings.h> #include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module #include <altitudeholddesired.h> // object that will be updated by the module
#include <altitudeholdstatus.h> #include <altitudeholdstatus.h>
@ -170,21 +169,6 @@ static void altitudeHoldTask(void)
// velocity control loop // velocity control loop
throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); 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) { if (throttle >= 1.0f) {
throttle = 1.0f; throttle = 1.0f;
} }