From 6b27ff1e27e195af719805095078dd26e71fe482 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 19 Jul 2013 11:28:48 +0000 Subject: [PATCH] OP-1022 add position lowpass, rework setting uavo, cleanup --- flight/modules/AltitudeHold/altitudehold.c | 36 ++++++++----------- flight/modules/ManualControl/manualcontrol.c | 8 ++--- .../uavobjectdefinition/altholdsmoothed.xml | 3 -- .../altitudeholdsettings.xml | 10 ++---- 4 files changed, 21 insertions(+), 36 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 7e5618165..258daaca0 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -66,7 +66,7 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define ACCEL_DOWNSAMPLE 4 #define TIMEOUT_TRESHOLD 200000 -#define DESIRED_UPDATE_RATE_MS 20 // milliseconds +#define DESIRED_UPDATE_RATE_MS 100 // milliseconds // Private types // Private variables @@ -113,7 +113,7 @@ int32_t AltitudeHoldInitialize() MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); float tau; -float accelAlpha; +float positionAlpha; float velAlpha; bool running = false; @@ -137,14 +137,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) VelocityStateData velocityData; float dT; float fblimit = 0; - float lastVertVelocity; + portTickType thisTime, lastUpdateTime; UAVObjEvent ev; dT = 0; - lastVertVelocity = 0; timeval = 0; - lastUpdateTime = 0; + lastUpdateTime = 0; // Force update of the settings SettingsUpdatedCb(&ev); // Failsafe handling @@ -215,15 +214,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) VelocityStateGet(&velocityData); altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); - float vertAccel = (velocityData.Down - lastVertVelocity) / dT; - lastVertVelocity = velocityData.Down; - - altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); - altHold.StateUpdateInterval = dT; - - PositionStateDownGet(&altHold.Altitude); - altHold.Altitude = -altHold.Altitude; + float position; + PositionStateDownGet(&position); + altHold.Altitude = -(positionAlpha * position) + (1 - positionAlpha) * altHold.Altitude; AltHoldSmoothedSet(&altHold); @@ -246,8 +240,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) velError = altitudeHoldDesired.Velocity - altHold.Velocity; // Compute altitude and velocity integral - altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudeKi * dT; - velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityKi * dT; + altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KI] * dT; + velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KI] * dT; thisTime = xTaskGetTickCount(); @@ -255,17 +249,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { continue; } - altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; lastUpdateTime = thisTime; // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral - + error * fabsf(error) * altitudeHoldSettings.Altitude2Kp - + error * altitudeHoldSettings.AltitudeKp - + velError * altitudeHoldSettings.VelocityKp - + derivative * altitudeHoldSettings.AltitudeKd; + + error * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KP] + + velError * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KP] + + derivative * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KD]; // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling AttitudeStateData attitudeState; @@ -312,8 +304,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau); - velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); + positionAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.PositionTau); + velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); // don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency) if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) { diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index ef804277c..0c42f2bcc 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -861,16 +861,16 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude = altHold.Altitude; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude = altHold.Altitude; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height // Vario is not "engaged" when throttleRate == 0 - if(fabsf(altitudeHoldDesiredData.Velocity)> 1e-3f) - {; + if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { + ; altitudeHoldDesiredData.Velocity = 0; altitudeHoldDesiredData.Altitude = altHold.Altitude; } diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml index b990228b0..1339e1337 100644 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ b/shared/uavobjectdefinition/altholdsmoothed.xml @@ -3,9 +3,6 @@ The output on the kalman filter on altitude. - - - diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 45e7dd544..806d86714 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,14 +1,10 @@ Settings for the @ref AltitudeHold module - - - - - - + + - +