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

Add derivative term

Browse Source
This commit is contained in:
James Cotton 2012-01-02 17:19:14 -06:00
parent ef05cafaca
commit d2ce7761c5
3 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flight/Modules/AltitudeHold/altitudehold.c
View File

@ -61,6 +61,7 @@ static xQueueHandle queue;
// Private functions
static void altitudeHoldTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent * ev);
/**
* Initialise the module, called on startup
@ -90,6 +91,8 @@ int32_t AltitudeHoldInitialize()
// Listen for updates.
AltitudeHoldDesiredConnectQueue(queue);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart)
@ -110,6 +113,9 @@ static void altitudeHoldTask(void *parameters)
portTickType lastSysTime;
UAVObjEvent ev;
// Force update of the settings
SettingsUpdatedCb(&ev);
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
@ -121,8 +127,6 @@ static void altitudeHoldTask(void *parameters)
throttleIntegral = 0;
continue;
} else {
AltitudeHoldDesiredGet(&altitudeHoldDesired);
AltitudeHoldSettingsGet(&altitudeHoldSettings);
PositionActualGet(&positionActual);
StabilizationDesiredGet(&stabilizationDesired);
float dT;
@ -131,9 +135,14 @@ static void altitudeHoldTask(void *parameters)
if(thisTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisTime;
static float altitude;
const float altitudeTau = 0.1;
// Flipping sign on error since altitude is "down"
float error = - (altitudeHoldDesired.Down - positionActual.Down);
static float
throttleIntegral += error * altitudeHoldSettings.Ki * dT * 1000;
if(throttleIntegral > altitudeHoldSettings.ILimit)
throttleIntegral = altitudeHoldSettings.ILimit;
@ -156,3 +165,16 @@ static void altitudeHoldTask(void *parameters)
}
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
AltitudeHoldDesiredGet(&altitudeHoldDesired);
AltitudeHoldSettingsGet(&altitudeHoldSettings);
const float fakeDt = 0.0025;
if(settings.GyroTau < 0.0001)
gyro_alpha = 0; // not trusting this to resolve to 0
else
gyro_alpha = expf(-fakeDt / settings.GyroTau);
}

2
flight/Modules/Stabilization/stabilization.c
View File

@ -466,7 +466,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
if(settings.GyroTau < 0.0001)
gyro_alpha = 0; // not trusting this to resolve to 0
else
gyro_alpha = exp(-fakeDt / settings.GyroTau);
gyro_alpha = expf(-fakeDt / settings.GyroTau);
}

5
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -1,9 +1,10 @@
<xml>
<object name="AltitudeHoldSettings" singleinstance="true" settings="true">
<description>Settings for the @ref AltitudeHold module</description>
<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.025"/>
<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0.025"/>
<field name="ILimit" units="throttle" type="float" elements="1" defaultvalue ="0.5"/>
<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="AltitudeTau" units="s" type="float" elements="1" defaultvalue="0.1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>