From 7708aab3133e67e1e0a187d6ee0ece71bdfab1ea Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 11 Jul 2013 18:17:00 +0200 Subject: [PATCH] Added vertical velocity as a control loop input to improve dynamics during commanded ascend/decent. Conflicts: flight/modules/AltitudeHold/altitudehold.c flight/modules/ManualControl/manualcontrol.c shared/uavobjectdefinition/altitudeholdsettings.xml --- flight/modules/AltitudeHold/altitudehold.c | 78 +++++++++---------- flight/modules/ManualControl/manualcontrol.c | 27 ++++--- .../boards/revolution/firmware/pios_board.c | 2 +- .../altitudeholddesired.xml | 2 +- .../altitudeholdsettings.xml | 3 +- 5 files changed, 57 insertions(+), 55 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 1cb27fc82..fa65d8fa0 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -64,7 +64,7 @@ #define MAX_QUEUE_SIZE 2 #define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 10 +#define ACCEL_DOWNSAMPLE 4 #define TIMEOUT_TRESHOLD 200000 // Private types @@ -109,8 +109,11 @@ int32_t AltitudeHoldInitialize() } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); -float throttleIntegral; -float switchThrottle; +float tau; +float altitudeIntegral; +float velocityIntegral; +float decay; +float velocity_decay; float velocity; float accelAlpha; float velAlpha; @@ -131,7 +134,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) AttitudeStateData attitudeState; VelocityStateData velocityData; float dT; - float q[4], Rbe[3][3]; + float q[4], Rbe[3][3], fblimit = 0; float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; @@ -164,7 +167,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) { if (!running) { - throttleIntegral = 0; + altitudeIntegral = 0; } // Todo: Add alarm if it should be running @@ -180,17 +183,19 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) q[3] = attitudeState.q4; Quaternion2R(q, Rbe); // Copy the current throttle as a starting point for integral - StabilizationDesiredThrottleGet(&throttleIntegral); - switchThrottle = throttleIntegral; - throttleIntegral *= Rbe[2][2]; // rotate into earth frame - if (throttleIntegral > 1) { - throttleIntegral = 1; - } else if (throttleIntegral < 0) { - throttleIntegral = 0; + float initThrottle; + StabilizationDesiredThrottleGet(&initThrottle); + initThrottle *= Rbe[2][2]; // rotate into earth frame + if (initThrottle > 1) { + initThrottle = 1; + } else if (initThrottle < 0) { + initThrottle = 0; } error = 0; - velocity = 0; + altitudeHoldDesired.Velocity = 0; altitudeHoldDesired.Altitude = altHold.Altitude; + altitudeIntegral = altHold.Altitude * altitudeHoldSettings.Kp + initThrottle; + velocityIntegral = 0; running = true; } else if (!altitudeHoldFlightMode) { running = false; @@ -230,14 +235,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) continue; } - // Compute the altitude error - error = altitudeHoldDesired.Altitude - altHold.Altitude; - velError = altitudeHoldDesired.Velocity - altHold.Velocity; + // Compute altitude and velocity integral + altitudeIntegral += (altitudeHoldDesired.Altitude - altHold.Altitude - fblimit) * altitudeHoldSettings.AltitudeKi * dT; + velocityIntegral += (altitudeHoldDesired.Velocity - altHold.Velocity - fblimit) * altitudeHoldSettings.VelocityKi * dT; - if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { - // Compute integral off altitude error - throttleIntegral += error * altitudeHoldSettings.Ki * dT; - } thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently if ((thisTime - lastUpdateTime) < 20) { @@ -249,31 +250,28 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); 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; - // scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling - AttitudeStateGet(&attitudeState); - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; - stabilizationDesired.Throttle /= throttlescale; - } else { - stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral; - } + stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral + + error * fabsf(error) * altitudeHoldSettings.Kp2 + - altHold.Altitude * altitudeHoldSettings.Kp + - 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 + AttitudeStateGet(&attitudeState); + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; + Quaternion2R(q, Rbe); + float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; + stabilizationDesired.Throttle /= throttlescale; + fblimit = 0; if (stabilizationDesired.Throttle > 1) { - throttleIntegral -= (stabilizationDesired.Throttle - 1); + fblimit = stabilizationDesired.Throttle - 1; stabilizationDesired.Throttle = 1; } else if (stabilizationDesired.Throttle < 0) { - throttleIntegral -= stabilizationDesired.Throttle; + fblimit = stabilizationDesired.Throttle; stabilizationDesired.Throttle = 0; } } else { diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b402bde4f..58704e0fc 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -827,7 +827,10 @@ 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); @@ -844,8 +847,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - AltHoldSmoothedData altHoldSmoothed; - AltHoldSmoothedGet(&altHoldSmoothed); + + 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; @@ -853,24 +858,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height - - altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; - altitudeHoldDesiredData.Velocity = 0.0f; - + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Altitude = altHold.Altitude; 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.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 += altitudeHoldDesiredData.Velocity * dT; } 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 += altitudeHoldDesiredData.Velocity * dT; } 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) { - altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; - altitudeHoldDesiredData.Velocity = 0.0f; - } + altitudeHoldDesiredData.Velocity = 0; zeroed = true; } diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index ffc05ce50..f630892b5 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -133,7 +133,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_4096, }; #endif /* PIOS_INCLUDE_MS5611 */ diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index bae414384..e91f5a0c6 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,8 +1,8 @@ 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 f609dc81d..1b2b2c26e 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -4,7 +4,8 @@ - + +