From 0203e2c6e290c5b3f1625fb9ced75bd6dad0c133 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 16 Jul 2013 20:01:16 +0000 Subject: [PATCH] OP-1022 Adding a proportional term on the square of the altitude error. --- flight/modules/AltitudeHold/altitudehold.c | 1 + flight/modules/ManualControl/manualcontrol.c | 2 +- .../altitudeholdsettings.xml | 19 ++++++++++--------- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index fb86e5443..85cfaff6b 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -250,6 +250,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if (!enterFailSafe) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + + error * fabsf(error) * altitudeHoldSettings.Kp2 + throttleIntegral - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 3f6107f7b..70730c599 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -813,7 +813,7 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * */ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) { - const float DEADBAND = 0.25f; + const float DEADBAND = 0.20f; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index ca190ba56..f609dc81d 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,15 +1,16 @@ Settings for the @ref AltitudeHold module - - - - - - - - - + + + + + + + + + +