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

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

@ -59,8 +59,8 @@
#include <positionstate.h> #include <positionstate.h>
// Private constants // Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 1024 #define STACK_SIZE_BYTES 1024
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds #define DESIRED_UPDATE_RATE_MS 100 // milliseconds
@ -116,88 +116,90 @@ 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
FlightStatusData flightStatus;
// make sure we run only when we are supposed to run FlightStatusGet(&flightStatus);
FlightStatusData flightStatus; switch (flightStatus.FlightMode) {
FlightStatusGet(&flightStatus); case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: default:
break; pid_zero(&accelpid);
default: StabilizationDesiredThrottleGet(&startThrottle);
pid_zero(&accelpid); DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
StabilizationDesiredThrottleGet(&startThrottle); return;
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
break;
}
// do the actual control loop(s) break;
AltitudeHoldDesiredData altitudeHoldDesired; }
AltitudeHoldDesiredGet(&altitudeHoldDesired);
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
// altitude control loop // do the actual control loop(s)
float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
// velocity control loop // altitude control loop
float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) -9.81f; float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity;
// velocity control loop
float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) - 9.81f;
// compensate acceleration by rotation // compensate acceleration by rotation
// explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q
// It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation
// multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical
// acceleration at the current tilt angle. // acceleration at the current tilt angle.
// around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep
// integrals from winding in any direction // integrals from winding in any direction
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float Rbe[3][3];
Quaternion2R(&attitudeState.q1,Rbe);
float rotatedAccelDesired = realAccelDesired;
if (fabsf(Rbe[2][2])>1e-3f) {
rotatedAccelDesired /= Rbe[2][2];
} else {
rotatedAccelDesired = accelStateDown;
}
// acceleration control loop
float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000/DESIRED_UPDATE_RATE_MS);
if (throttle>=1.0f) { AttitudeStateData attitudeState;
throttle=1.0f; AttitudeStateGet(&attitudeState);
} float Rbe[3][3];
if (throttle<=0.0f) { Quaternion2R(&attitudeState.q1, Rbe);
throttle=0.0f;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Throttle = throttle;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); float rotatedAccelDesired = realAccelDesired;
if (fabsf(Rbe[2][2]) > 1e-3f) {
rotatedAccelDesired /= Rbe[2][2];
} else {
rotatedAccelDesired = accelStateDown;
}
// acceleration control loop
float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS);
if (throttle >= 1.0f) {
throttle = 1.0f;
}
if (throttle <= 0.0f) {
throttle = 0.0f;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Throttle = throttle;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
} }
static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
float down; float down;
AccelStatezGet(&down);
accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); AccelStatezGet(&down);
accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha);
} }
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
@ -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;
} }