From 133ad414f81e303cdd0ca9e6497456dee89cf889 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 14 Jul 2013 07:46:42 +0000 Subject: [PATCH] OP-1022: Added a velocity loop in AH module, used when throttle is not in central position. --- flight/modules/AltitudeHold/altitudehold.c | 24 ++++++++++++------- flight/modules/ManualControl/manualcontrol.c | 22 ++++++++--------- .../altitudeholddesired.xml | 1 + .../altitudeholdsettings.xml | 5 ++-- 4 files changed, 30 insertions(+), 22 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 91ff452bf..45c73c611 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -114,6 +114,8 @@ float decay; float velocity_decay; bool running = false; float error; +float velError; + float switchThrottle; float smoothed_altitude; float starting_altitude; @@ -364,9 +366,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Compute the altitude error error = altitudeHoldDesired.Altitude - altHold.Altitude; + velError = altitudeHoldDesired.Velocity - altHold.Velocity; - // Compute integral off altitude error - throttleIntegral += error * altitudeHoldSettings.Ki * dT; + if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { + // Compute integral off altitude error + throttleIntegral += error * altitudeHoldSettings.Ki * dT; + } // Only update stabilizationDesired less frequently if ((thisTime - lastUpdateTime) < 20) { @@ -378,12 +383,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; - // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling - float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; - stabilizationDesired.Throttle /= throttlescale; - + if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { + stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - + altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; + // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling + float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; + stabilizationDesired.Throttle /= throttlescale; + } else { + stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral; + } if (stabilizationDesired.Throttle > 1) { throttleIntegral -= (stabilizationDesired.Throttle - 1); stabilizationDesired.Throttle = 1; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 96d468b7f..4a1fc7127 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -827,10 +827,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) uint8_t throttleExp; static uint8_t flightMode; - static portTickType lastSysTimeAH; static bool zeroed = false; - portTickType thisSysTime; - float dT; FlightStatusFlightModeGet(&flightMode); @@ -848,29 +845,30 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); - lastSysTimeAH = thisSysTime; - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; if (changed) { // After not being in this mode for a while init at current height - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - altitudeHoldDesiredData.Altitude = altHold.Altitude; + + AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude); + altitudeHoldDesiredData.Velocity = 0.0f; + zeroed = false; } 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.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate * dT; + altitudeHoldDesiredData.Velocity = ((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate * dT; + 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); } 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) { + AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude); + altitudeHoldDesiredData.Velocity = 0.0f; + } zeroed = true; } diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index cf056cfa0..bae414384 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,6 +1,7 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through + diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 3a2cf6149..40fdf30be 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,9 +1,10 @@ Settings for the @ref AltitudeHold module - + + - +