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

simplified altitude hold control loop

Browse Source
This commit is contained in:
Corvus Corax 2013-12-27 18:37:27 +01:00
parent d60eda1a83
commit 505d334c4b
3 changed files with 11 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -70,14 +70,12 @@
// Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid accelpid;
static float accelStateDown;
static struct pid pid0, pid1;
// Private functions
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void AccelStateUpdatedCb(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
@ -106,7 +104,6 @@ int32_t AltitudeHoldInitialize()
altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
AccelStateConnectCallback(&AccelStateUpdatedCb);
return 0;
}
@ -129,7 +126,8 @@ static void altitudeHoldTask(void)
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
break;
default:
pid_zero(&accelpid);
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrottleGet(&startThrottle);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
@ -149,40 +147,13 @@ static void altitudeHoldTask(void)
VelocityStateDownGet(&velocityStateDown);
// altitude control loop
altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (altitudeHoldDesired.Altitude - positionStateDown) + altitudeHoldDesired.Velocity;
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
// velocity control loop
altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (altitudeHoldStatus.VelocityDesired - velocityStateDown) - 9.81f;
altitudeHoldStatus.AccelerationFiltered = accelStateDown;
float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
AltitudeHoldStatusSet(&altitudeHoldStatus);
// compensate acceleration by rotation
// 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
// 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.
// 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
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float Rbe[3][3];
Quaternion2R(&attitudeState.q1, Rbe);
float rotatedAccelDesired = altitudeHoldStatus.AccelerationDesired;
#if 0
if (fabsf(Rbe[2][2]) > 1e-3f) {
rotatedAccelDesired /= Rbe[2][2];
} else {
rotatedAccelDesired = accelStateDown;
}
#endif
// 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;
}
@ -204,18 +175,11 @@ static void altitudeHoldTask(void)
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
}
static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
float down;
AccelStatezGet(&down);
accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit);
pid_zero(&accelpid);
accelStateDown = 0.0f;
pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
pid_zero(&pid0);
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
}

6
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -1,10 +1,8 @@
<xml>
<object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref AltitudeHold module</description>
<field name="AltitudeP" units="(m/s)/m" type="float" elements="1" defaultvalue="2" />
<field name="VelocityP" units="(m/s^2)/(m/s)" type="float" elements="1" defaultvalue="0.5" />
<field name="AccelPI" units="throttle/(m/s^2)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.1,0.01,1.0" />
<field name="AccelAlpha" units="" type="float" elements="1" defaultvalue="0.1" />
<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="1,0,0" />
<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.02,0.0001,2.0" />
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5" />
<access gcs="readwrite" flight="readwrite"/>

2
shared/uavobjectdefinition/altitudeholdstatus.xml
View File

@ -2,8 +2,6 @@
<object name="AltitudeHoldStatus" singleinstance="true" settings="false" category="Control">
<description>Status Data from Altitude Hold Control Loops</description>
<field name="VelocityDesired" units="m/s" type="float" elements="1"/>
<field name="AccelerationDesired" units="m/s^2" type="float" elements="1"/>
<field name="AccelerationFiltered" units="m/s^2" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>