rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-03 11: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 // Private functions
static void altitudeHoldTask(void *parameters); static void altitudeHoldTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent * ev);
/** /**
* Initialise the module, called on startup * Initialise the module, called on startup
@ -90,6 +91,8 @@ int32_t AltitudeHoldInitialize()
// Listen for updates. // Listen for updates.
AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldDesiredConnectQueue(queue);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
return 0; return 0;
} }
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart) MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart)
@ -110,6 +113,9 @@ static void altitudeHoldTask(void *parameters)
portTickType lastSysTime; portTickType lastSysTime;
UAVObjEvent ev; UAVObjEvent ev;
// Force update of the settings
SettingsUpdatedCb(&ev);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
while (1) { while (1) {
@ -121,8 +127,6 @@ static void altitudeHoldTask(void *parameters)
throttleIntegral = 0; throttleIntegral = 0;
continue; continue;
} else { } else {
AltitudeHoldDesiredGet(&altitudeHoldDesired);
AltitudeHoldSettingsGet(&altitudeHoldSettings);
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
StabilizationDesiredGet(&stabilizationDesired); StabilizationDesiredGet(&stabilizationDesired);
float dT; float dT;
@ -132,8 +136,13 @@ static void altitudeHoldTask(void *parameters)
dT = (thisTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; dT = (thisTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisTime; lastSysTime = thisTime;
static float altitude;
const float altitudeTau = 0.1;
// Flipping sign on error since altitude is "down" // Flipping sign on error since altitude is "down"
float error = - (altitudeHoldDesired.Down - positionActual.Down); float error = - (altitudeHoldDesired.Down - positionActual.Down);
static float
throttleIntegral += error * altitudeHoldSettings.Ki * dT * 1000; throttleIntegral += error * altitudeHoldSettings.Ki * dT * 1000;
if(throttleIntegral > altitudeHoldSettings.ILimit) if(throttleIntegral > altitudeHoldSettings.ILimit)
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) if(settings.GyroTau < 0.0001)
gyro_alpha = 0; // not trusting this to resolve to 0 gyro_alpha = 0; // not trusting this to resolve to 0
else 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> <xml>
<object name="AltitudeHoldSettings" singleinstance="true" settings="true"> <object name="AltitudeHoldSettings" singleinstance="true" settings="true">
<description>Settings for the @ref AltitudeHold module</description> <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="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"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>