rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

proper uncrustification

Browse Source
This commit is contained in:
Corvus Corax 2013-12-07 23:31:26 +01:00
parent c423cba40f
commit 8832ea24f5
1 changed files with 74 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -116,11 +116,11 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
*/ */
static void altitudeHoldTask(void) static void altitudeHoldTask(void)
{ {
static float startThrottle = 0.5f;
static float startThrottle =0.5f;
// make sure we run only when we are supposed to run // make sure we run only when we are supposed to run
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
@ -131,6 +131,7 @@ static void altitudeHoldTask(void)
StabilizationDesiredThrottleGet(&startThrottle); StabilizationDesiredThrottleGet(&startThrottle);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return; return;
break; break;
} }
@ -146,7 +147,7 @@ static void altitudeHoldTask(void)
float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity;
// velocity control loop // velocity control loop
float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) -9.81f; float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) - 9.81f;
// compensate acceleration by rotation // compensate acceleration by rotation
@ -160,23 +161,23 @@ static void altitudeHoldTask(void)
AttitudeStateData attitudeState; AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState); AttitudeStateGet(&attitudeState);
float Rbe[3][3]; float Rbe[3][3];
Quaternion2R(&attitudeState.q1,Rbe); Quaternion2R(&attitudeState.q1, Rbe);
float rotatedAccelDesired = realAccelDesired; float rotatedAccelDesired = realAccelDesired;
if (fabsf(Rbe[2][2])>1e-3f) { if (fabsf(Rbe[2][2]) > 1e-3f) {
rotatedAccelDesired /= Rbe[2][2]; rotatedAccelDesired /= Rbe[2][2];
} else { } else {
rotatedAccelDesired = accelStateDown; rotatedAccelDesired = accelStateDown;
} }
// acceleration control loop // acceleration control loop
float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000/DESIRED_UPDATE_RATE_MS); float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS);
if (throttle>=1.0f) { if (throttle >= 1.0f) {
throttle=1.0f; throttle = 1.0f;
} }
if (throttle<=0.0f) { if (throttle <= 0.0f) {
throttle=0.0f; throttle = 0.0f;
} }
StabilizationDesiredData stab; StabilizationDesiredData stab;
StabilizationDesiredGet(&stab); StabilizationDesiredGet(&stab);
@ -196,6 +197,7 @@ static void altitudeHoldTask(void)
static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
float down; float down;
AccelStatezGet(&down); AccelStatezGet(&down);
accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha);
} }
@ -205,5 +207,5 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
AltitudeHoldSettingsGet(&altitudeHoldSettings); AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit); pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit);
pid_zero(&accelpid); pid_zero(&accelpid);
accelStateDown=0.0f; accelStateDown = 0.0f;
} }