From d2ce7761c51fe73d2ddb3253e98756cc2b534e89 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 2 Jan 2012 17:19:14 -0600 Subject: [PATCH] Add derivative term --- flight/Modules/AltitudeHold/altitudehold.c | 26 +++++++++++++++++-- flight/Modules/Stabilization/stabilization.c | 2 +- .../altitudeholdsettings.xml | 5 ++-- 3 files changed, 28 insertions(+), 5 deletions(-) diff --git a/flight/Modules/AltitudeHold/altitudehold.c b/flight/Modules/AltitudeHold/altitudehold.c index 47eabe72a..9b1ae1798 100644 --- a/flight/Modules/AltitudeHold/altitudehold.c +++ b/flight/Modules/AltitudeHold/altitudehold.c @@ -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); + +} diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 261e02303..b07513c66 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -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); } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 93807be4e..42b13e5ab 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,9 +1,10 @@ Settings for the @ref AltitudeHold module - + - + +