From aecc8543a53950657fcd9986481557218b111600 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:09:17 +0200 Subject: [PATCH 01/50] Corrected throttle stick exponentional function in AH mode --- flight/modules/ManualControl/manualcontrol.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 495f1d4a1..17109f170 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -863,10 +863,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) 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 + x*(256−k)) / 256 - altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + // 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; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + 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; } 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 From 45ed66414f09efbe02c06268417a22588602a92c Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:18:29 +0200 Subject: [PATCH 02/50] Changed AH to work with absolute instead of relative altitude --- flight/modules/AltitudeHold/altitudehold.c | 6 +----- flight/modules/ManualControl/manualcontrol.c | 5 ++++- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index a886de64c..768d9afd0 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -174,10 +174,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) error = 0; velocity = 0; running = true; - - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - starting_altitude = altHold.Altitude; } else if (!altitudeHoldFlightMode) { running = false; lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); @@ -364,7 +360,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } // Compute the altitude error - error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude; + error = altitudeHoldDesired.Altitude - altHold.Altitude; // Compute integral off altitude error throttleIntegral += error * altitudeHoldSettings.Ki * dT; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 17109f170..c1efb4ebd 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -38,6 +38,7 @@ #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" +#include "altholdsmoothed.h #include "flighttelemetrystats.h" #include "flightstatus.h" #include "sanitycheck.h" @@ -859,7 +860,9 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) PositionStateDownGet(¤tDown); if (changed) { // After not being in this mode for a while init at current height - altitudeHoldDesiredData.Altitude = 0; + 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 From 723f308ac72ee0473e78e4894803b2ed01d562d2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:26:28 +0200 Subject: [PATCH 03/50] Removed obsolete (unused) code --- flight/modules/AltitudeHold/altitudehold.c | 18 +++++++----------- flight/modules/ManualControl/manualcontrol.c | 2 -- 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 768d9afd0..e3d32ce92 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -238,17 +238,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) x[0] = baro.Altitude; // rotate avg accels into earth frame and store it - if (1) { - float q[4], Rbe[3][3]; - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - } else { - x[1] = -accelState.z + 9.81f; - } + float q[4], Rbe[3][3]; + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; + Quaternion2R(q, Rbe); + x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; timeval = PIOS_DELAY_GetRaw(); diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index c1efb4ebd..8e7e99aa4 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -856,8 +856,6 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - float currentDown; - PositionStateDownGet(¤tDown); if (changed) { // After not being in this mode for a while init at current height AltHoldSmoothedData altHold; From 73aa4df431f2aee303ae5c2477ecc1cd84fdb79a Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:44:48 +0200 Subject: [PATCH 04/50] Missing " --- flight/modules/ManualControl/manualcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 8e7e99aa4..96d468b7f 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -38,7 +38,7 @@ #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" -#include "altholdsmoothed.h +#include "altholdsmoothed.h" #include "flighttelemetrystats.h" #include "flightstatus.h" #include "sanitycheck.h" From 1da68cda9af3f8b36a2b9fe67a55c2bd55e58b16 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 12:45:40 +0200 Subject: [PATCH 05/50] Scale up desired throttle in AH mode to compensate for roll/pitch --- flight/modules/AltitudeHold/altitudehold.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index e3d32ce92..08aa13116 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -125,6 +125,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) { AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; + float q[4], Rbe[3][3]; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; @@ -238,7 +239,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) x[0] = baro.Altitude; // rotate avg accels into earth frame and store it - float q[4], Rbe[3][3]; q[0] = attitudeState.q1; q[1] = attitudeState.q2; q[2] = attitudeState.q3; @@ -373,6 +373,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) 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 (stabilizationDesired.Throttle > 1) { throttleIntegral -= (stabilizationDesired.Throttle - 1); stabilizationDesired.Throttle = 1; From 513d509865d33268ac7f1378c591923f28468b8f Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 29 Jun 2013 15:34:29 +0200 Subject: [PATCH 06/50] Initialize altitude before switching to "running". Not doing this could cause glitches during the very first switch to AH. --- flight/modules/AltitudeHold/altitudehold.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 08aa13116..31bbd4044 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -125,6 +125,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) { AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; + AltHoldSmoothedData altHold; float q[4], Rbe[3][3]; portTickType thisTime, lastUpdateTime; @@ -174,6 +175,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) switchThrottle = throttleIntegral; error = 0; velocity = 0; + altitudeHoldDesired.Altitude = altHold.Altitude; running = true; } else if (!altitudeHoldFlightMode) { running = false; @@ -334,7 +336,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f); } - AltHoldSmoothedData altHold; AltHoldSmoothedGet(&altHold); altHold.Altitude = z[0]; altHold.Velocity = z[1]; From 23b5b374803fbd5031fec967f9eb5a30bcc43582 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Mon, 1 Jul 2013 10:13:27 +0200 Subject: [PATCH 07/50] Scale down initial throttle when AH is engaged based on roll/pitch angle. This improves AH behaviour when switching to AH while being in fast forward flight. --- flight/modules/AltitudeHold/altitudehold.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 31bbd4044..91ff452bf 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -173,6 +173,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // 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; + } error = 0; velocity = 0; altitudeHoldDesired.Altitude = altHold.Altitude; From 133ad414f81e303cdd0ca9e6497456dee89cf889 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 14 Jul 2013 07:46:42 +0000 Subject: [PATCH 08/50] 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 - + + - + From a57f8913ba3cb9f9586bb3dd707bbe80a4be6c8d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 14 Jul 2013 20:20:22 +0000 Subject: [PATCH 09/50] OP-1022: replace old KF in alitude hold with status from filters --- flight/modules/AltitudeHold/altitudehold.c | 191 +++---------------- flight/modules/ManualControl/manualcontrol.c | 10 +- 2 files changed, 35 insertions(+), 166 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 45c73c611..59e5670bd 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -58,6 +58,8 @@ #include #include #include +#include +#include // Private constants #define MAX_QUEUE_SIZE 2 #define STACK_SIZE_BYTES 1024 @@ -107,18 +109,15 @@ int32_t AltitudeHoldInitialize() } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); -float tau; float throttleIntegral; +float switchThrottle; float velocity; -float decay; -float velocity_decay; bool running = false; float error; float velError; +uint32_t timeval; -float switchThrottle; -float smoothed_altitude; -float starting_altitude; +bool posUpdated; /** * Module thread, should not return. @@ -128,11 +127,17 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltHoldSmoothedData altHold; + AttitudeStateData attitudeState; + AccelStateData accelState; + VelocityStateData velocityData; + float dT; float q[4], Rbe[3][3]; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; + timeval = 0; + lastUpdateTime = 0; // Force update of the settings SettingsUpdatedCb(&ev); // Failsafe handling @@ -140,11 +145,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - BaroSensorConnectQueue(queue); - FlightStatusConnectQueue(queue); AccelStateConnectQueue(queue); + PositionStateConnectQueue(queue); + FlightStatusConnectQueue(queue); + VelocityStateConnectQueue(queue); bool altitudeHoldFlightMode = false; - BaroSensorAltitudeGet(&smoothed_altitude); running = false; enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; @@ -153,7 +158,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // initialize enable flag altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; // Main task loop - bool baro_updated = false; while (1) { enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe @@ -164,8 +168,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Todo: Add alarm if it should be running continue; - } else if (ev.obj == BaroSensorHandle()) { - baro_updated = true; + } else if (ev.obj == PositionStateHandle()) { + posUpdated = true; init = (init == WAITING_BARO) ? WAITIING_INIT : init; } else if (ev.obj == FlightStatusHandle()) { @@ -190,168 +194,30 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } } else if (ev.obj == AccelStateHandle()) { - static uint32_t timeval; + dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; + timeval = PIOS_DELAY_GetRaw(); - static float z[4] = { 0, 0, 0, 0 }; - float z_new[4]; - float P[4][4], K[4][2], x[2]; - float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f }; - static float V[4][4] = { - { 10.0f, 0.0f, 0.0f, 0.0f }, - { 0.0f, 100.0f, 0.0f, 0.0f }, - { 0.0f, 0.0f, 100.0f, 0.0f }, - { 0.0f, 0.0f, 0.0f, 1000.0f } - }; - static uint32_t accel_downsample_count = 0; - static float accels_accum[3] = { 0.0f, 0.0f, 0.0f }; - float dT; - static float S[2] = { 1.0f, 10.0f }; + AltHoldSmoothedGet(&altHold); - /* Somehow this always assigns to zero. Compiler bug? Race condition? */ - S[0] = altitudeHoldSettings.PressureNoise; - S[1] = altitudeHoldSettings.AccelNoise; - G[2] = altitudeHoldSettings.AccelDrift; - - AccelStateData accelState; - AccelStateGet(&accelState); - AttitudeStateData attitudeState; AttitudeStateGet(&attitudeState); - BaroSensorData baro; - BaroSensorGet(&baro); - /* Downsample accels to stop this calculation consuming too much CPU */ - accels_accum[0] += accelState.x; - accels_accum[1] += accelState.y; - accels_accum[2] += accelState.z; - accel_downsample_count++; - - if (accel_downsample_count < ACCEL_DOWNSAMPLE) { - continue; - } - - accel_downsample_count = 0; - accelState.x = accels_accum[0] / ACCEL_DOWNSAMPLE; - accelState.y = accels_accum[1] / ACCEL_DOWNSAMPLE; - accelState.z = accels_accum[2] / ACCEL_DOWNSAMPLE; - accels_accum[0] = accels_accum[1] = accels_accum[2] = 0; - - thisTime = xTaskGetTickCount(); - - if (init == WAITIING_INIT) { - z[0] = baro.Altitude; - z[1] = 0; - z[2] = accelState.z; - z[3] = 0; - init = INITED; - } else if (init == WAITING_BARO) { - continue; - } - - x[0] = baro.Altitude; - // rotate avg accels into earth frame and store it q[0] = attitudeState.q1; q[1] = attitudeState.q2; q[2] = attitudeState.q3; q[3] = attitudeState.q4; Quaternion2R(q, Rbe); - x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; - timeval = PIOS_DELAY_GetRaw(); + AccelStateGet(&accelState); + altHold.Accel = 0.95f * altHold.Accel + 0.05f * (Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0]; - P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1]; - P[0][2] = V[0][2] + dT * V[1][2]; - P[0][3] = V[0][3] + dT * V[1][3]; - P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0]; - P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1]; - P[1][2] = V[1][2] + dT * V[2][2]; - P[1][3] = V[1][3] + dT * V[2][3]; - P[2][0] = V[2][0] + dT * V[2][1]; - P[2][1] = V[2][1] + dT * V[2][2]; - P[2][2] = V[2][2] + G[2]; - P[2][3] = V[2][3]; - P[3][0] = V[3][0] + dT * V[3][1]; - P[3][1] = V[3][1] + dT * V[3][2]; - P[3][2] = V[3][2]; - P[3][3] = V[3][3] + G[3]; + VelocityStateGet(&velocityData); - if (baro_updated) { - K[0][0] = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f; - K[0][1] = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[1][0] = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[1][1] = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[2][0] = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[2][1] = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[3][0] = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - K[3][1] = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + altHold.Velocity = 0.95f * altHold.Velocity + 0.05f * (Rbe[0][2] * velocityData.East + Rbe[1][2] * velocityData.North + Rbe[2][2] * velocityData.Down); - z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0]; - z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1]; - z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2]; - z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3]; + PositionStateDownGet(&altHold.Altitude); - memcpy(z, z_new, sizeof(z_new)); - - V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f); - V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f); - V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f); - V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f); - V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0]; - V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2]; - V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2]; - V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3]; - V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f); - V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f); - V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f); - V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f); - V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f); - V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f); - V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f); - V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f); - - - baro_updated = false; - } else { - K[0][0] = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - K[1][0] = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - K[2][0] = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - K[3][0] = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - - - z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0]; - z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1]; - z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2]; - z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3]; - - memcpy(z, z_new, sizeof(z_new)); - - V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0]; - V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2]; - V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2]; - V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3]; - V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0]; - V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2]; - V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2]; - V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3]; - V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f); - V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f); - V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f); - V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f); - V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f); - V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f); - V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f); - V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f); - } - - AltHoldSmoothedGet(&altHold); - altHold.Altitude = z[0]; - altHold.Velocity = z[1]; - altHold.Accel = z[2]; AltHoldSmoothedSet(&altHold); - AltHoldSmoothedGet(&altHold); - // Verify that we are in altitude hold mode uint8_t armed; FlightStatusArmedGet(&armed); @@ -372,7 +238,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Compute integral off altitude error throttleIntegral += error * altitudeHoldSettings.Ki * dT; } - + thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently if ((thisTime - lastUpdateTime) < 20) { continue; @@ -384,14 +250,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; + 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; + 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 4a1fc7127..2f505bb16 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -844,6 +844,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); + AltHoldSmoothedData altHoldSmoothed; + AltHoldSmoothedGet(&altHoldSmoothed); altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; @@ -852,21 +854,21 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height - AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude); + altitudeHoldDesiredData.Altitude = altHoldSmoothed.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.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); } 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); } 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.Altitude = altHoldSmoothed.Altitude; altitudeHoldDesiredData.Velocity = 0.0f; } zeroed = true; From e975e4d9b789015e3933509a08f6debe9cb70244 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 15 Jul 2013 20:33:08 +0000 Subject: [PATCH 10/50] OP-1022: replace old KF in alitude hold with status from filters --- flight/modules/AltitudeHold/altitudehold.c | 56 +++++++++++-------- flight/modules/ManualControl/manualcontrol.c | 4 +- .../altitudeholdsettings.xml | 5 +- 3 files changed, 38 insertions(+), 27 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 59e5670bd..c96797129 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 4 +#define ACCEL_DOWNSAMPLE 10 #define TIMEOUT_TRESHOLD 200000 // Private types @@ -112,11 +112,12 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); float throttleIntegral; float switchThrottle; float velocity; +float accelAlpha; +float velAlpha; bool running = false; float error; float velError; uint32_t timeval; - bool posUpdated; /** @@ -128,14 +129,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) StabilizationDesiredData stabilizationDesired; AltHoldSmoothedData altHold; AttitudeStateData attitudeState; - AccelStateData accelState; VelocityStateData velocityData; float dT; float q[4], Rbe[3][3]; - + float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; - + lastVertVelocity = 0; timeval = 0; lastUpdateTime = 0; // Force update of the settings @@ -145,7 +145,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - AccelStateConnectQueue(queue); PositionStateConnectQueue(queue); FlightStatusConnectQueue(queue); VelocityStateConnectQueue(queue); @@ -176,6 +175,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) FlightStatusFlightModeGet(&flightMode); altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; if (altitudeHoldFlightMode && !running) { + AttitudeStateGet(&attitudeState); + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; + Quaternion2R(q, Rbe); // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&throttleIntegral); switchThrottle = throttleIntegral; @@ -193,28 +198,24 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) running = false; lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } - } else if (ev.obj == AccelStateHandle()) { + } else if (ev.obj == VelocityStateHandle()) { + dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; timeval = PIOS_DELAY_GetRaw(); AltHoldSmoothedGet(&altHold); - AttitudeStateGet(&attitudeState); - - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - - AccelStateGet(&accelState); - altHold.Accel = 0.95f * altHold.Accel + 0.05f * (Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); - VelocityStateGet(&velocityData); - altHold.Velocity = 0.95f * altHold.Velocity + 0.05f * (Rbe[0][2] * velocityData.East + Rbe[1][2] * velocityData.North + Rbe[2][2] * velocityData.Down); + 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); PositionStateDownGet(&altHold.Altitude); + altHold.Altitude = -altHold.Altitude; AltHoldSmoothedSet(&altHold); @@ -250,13 +251,22 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { - stabilizationDesired.Throttle = - error * altitudeHoldSettings.Kp + throttleIntegral + - altHold.Velocity * altitudeHoldSettings.Kd;// - altHold.Accel * altitudeHoldSettings.Ka; + 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 + 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 = velError * altitudeHoldSettings.Kv + throttleIntegral; } if (stabilizationDesired.Throttle > 1) { @@ -289,4 +299,6 @@ 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); } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 2f505bb16..3f6107f7b 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -861,9 +861,9 @@ 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); } 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); } 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 diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 40fdf30be..ca190ba56 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -6,9 +6,8 @@ - - - + + From 5b4d46819e7590af402d7a7245cdda01b60d8d32 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 16 Jul 2013 11:58:20 +0000 Subject: [PATCH 11/50] OP-1022 Fix problem with EventSystem due to too high update rate --- flight/modules/AltitudeHold/altitudehold.c | 14 ++++++-------- shared/uavobjectdefinition/altholdsmoothed.xml | 6 ++++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index c96797129..fb86e5443 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -135,6 +135,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; + dT = 0; lastVertVelocity = 0; timeval = 0; lastUpdateTime = 0; @@ -145,7 +146,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - PositionStateConnectQueue(queue); + //PositionStateConnectQueue(queue); FlightStatusConnectQueue(queue); VelocityStateConnectQueue(queue); bool altitudeHoldFlightMode = false; @@ -167,10 +168,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Todo: Add alarm if it should be running continue; - } else if (ev.obj == PositionStateHandle()) { - posUpdated = true; - - init = (init == WAITING_BARO) ? WAITIING_INIT : init; } else if (ev.obj == FlightStatusHandle()) { FlightStatusFlightModeGet(&flightMode); altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; @@ -199,8 +196,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } } else if (ev.obj == VelocityStateHandle()) { - - dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; + init = (init == WAITING_BARO) ? WAITIING_INIT : init; + dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; timeval = PIOS_DELAY_GetRaw(); AltHoldSmoothedGet(&altHold); @@ -213,6 +210,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastVertVelocity = velocityData.Down; altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); + altHold.StateUpdateInterval = dT; PositionStateDownGet(&altHold.Altitude); altHold.Altitude = -altHold.Altitude; @@ -244,7 +242,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if ((thisTime - lastUpdateTime) < 20) { continue; } - + altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; lastUpdateTime = thisTime; // Instead of explicit limit on integral you output limit feedback diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml index b32e977c2..b990228b0 100644 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ b/shared/uavobjectdefinition/altholdsmoothed.xml @@ -2,8 +2,10 @@ The output on the kalman filter on altitude. - - + + + + From 0203e2c6e290c5b3f1625fb9ced75bd6dad0c133 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 16 Jul 2013 20:01:16 +0000 Subject: [PATCH 12/50] 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 - - - - - - - - - + + + + + + + + + + From ed233efde20554bccfd105ee38b9d423b42d6184 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 16 Jul 2013 20:02:03 +0000 Subject: [PATCH 13/50] OP-1022 Uncrustify --- flight/modules/AltitudeHold/altitudehold.c | 25 ++++++++++---------- flight/modules/ManualControl/manualcontrol.c | 2 +- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 85cfaff6b..1cb27fc82 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -135,10 +135,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) 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 @@ -146,7 +147,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) bool enterFailSafe = false; // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - //PositionStateConnectQueue(queue); + // PositionStateConnectQueue(queue); FlightStatusConnectQueue(queue); VelocityStateConnectQueue(queue); bool altitudeHoldFlightMode = false; @@ -180,7 +181,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) Quaternion2R(q, Rbe); // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&throttleIntegral); - switchThrottle = throttleIntegral; + switchThrottle = throttleIntegral; throttleIntegral *= Rbe[2][2]; // rotate into earth frame if (throttleIntegral > 1) { throttleIntegral = 1; @@ -196,8 +197,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); } } else if (ev.obj == VelocityStateHandle()) { - init = (init == WAITING_BARO) ? WAITIING_INIT : init; - dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; + init = (init == WAITING_BARO) ? WAITIING_INIT : init; + dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; timeval = PIOS_DELAY_GetRaw(); AltHoldSmoothedGet(&altHold); @@ -205,11 +206,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) VelocityStateGet(&velocityData); altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); - float vertAccel = (velocityData.Down - lastVertVelocity)/dT; + float vertAccel = (velocityData.Down - lastVertVelocity) / dT; lastVertVelocity = velocityData.Down; - altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); + altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); altHold.StateUpdateInterval = dT; PositionStateDownGet(&altHold.Altitude); @@ -230,10 +231,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } // Compute the altitude error - error = altitudeHoldDesired.Altitude - altHold.Altitude; + error = altitudeHoldDesired.Altitude - altHold.Altitude; velError = altitudeHoldDesired.Velocity - altHold.Velocity; - if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { + if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { // Compute integral off altitude error throttleIntegral += error * altitudeHoldSettings.Ki * dT; } @@ -248,7 +249,7 @@ 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) { + if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + error * fabsf(error) * altitudeHoldSettings.Kp2 + throttleIntegral @@ -298,6 +299,6 @@ 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); + accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau); + velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau); } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 70730c599..b402bde4f 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -867,7 +867,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) } 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.Altitude = altHoldSmoothed.Altitude; altitudeHoldDesiredData.Velocity = 0.0f; } From 7708aab3133e67e1e0a187d6ee0ece71bdfab1ea Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 11 Jul 2013 18:17:00 +0200 Subject: [PATCH 14/50] 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 @@ - + + From 45cebda628ae1f334769f68763dd43d54d70805f Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 16 Jul 2013 14:08:59 +0200 Subject: [PATCH 15/50] Add a configurable lowpass filter to smooth throttle commands in AltitudeHold mode. Conflicts: flight/modules/AltitudeHold/altitudehold.c shared/uavobjectdefinition/altitudeholdsettings.xml --- flight/modules/AltitudeHold/altitudehold.c | 20 ++++++++++++++++++- .../altitudeholdsettings.xml | 1 + 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index fa65d8fa0..fd6f5ae68 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -66,12 +66,15 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define ACCEL_DOWNSAMPLE 4 #define TIMEOUT_TRESHOLD 200000 +#define DESIRED_UPDATE_RATE_MS 20 // milliseconds // Private types // Private variables static xTaskHandle altitudeHoldTaskHandle; static xQueueHandle queue; static AltitudeHoldSettingsData altitudeHoldSettings; +static float throttleAlpha = 1.0f; +static float throttle_old = 0.0f; // Private functions static void altitudeHoldTask(void *parameters); @@ -119,6 +122,9 @@ float accelAlpha; float velAlpha; bool running = false; float error; +float switchThrottle; +float smoothed_altitude; +float starting_altitude; float velError; uint32_t timeval; bool posUpdated; @@ -241,7 +247,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently - if ((thisTime - lastUpdateTime) < 20) { + if ((thisTime - lastUpdateTime)*1000/configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { continue; } altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; @@ -265,6 +271,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) Quaternion2R(q, Rbe); float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; stabilizationDesired.Throttle /= throttlescale; + stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); + throttle_old = stabilizationDesired.Throttle; fblimit = 0; if (stabilizationDesired.Throttle > 1) { @@ -299,4 +307,14 @@ 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); + + // 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) + { + throttleAlpha = (float)DESIRED_UPDATE_RATE_MS/((float)DESIRED_UPDATE_RATE_MS + 1000.0f/(2.0f*M_PI_F*altitudeHoldSettings.ThrottleFilterCutoff)); + } + else + { + throttleAlpha = 1.0f; + } } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 1b2b2c26e..76536c0c8 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -10,6 +10,7 @@ + From 7138faf16759772ab08524ff3e67809ab8d3cff2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 16 Jul 2013 14:38:20 +0200 Subject: [PATCH 16/50] Adjusted default settings for altitude hold. Conflicts: shared/uavobjectdefinition/altitudeholdsettings.xml --- .../altitudeholdsettings.xml | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 76536c0c8..bc234931c 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,18 +1,17 @@ Settings for the @ref AltitudeHold module - - - - - - - - - - - - + + + + + + + + + + + From 8c70e64544cdc7279befda24d64b464026eceab9 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 17 Jul 2013 08:37:56 +0000 Subject: [PATCH 17/50] OP-1022 reset velocity when changing flight mode --- flight/modules/ManualControl/manualcontrol.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 58704e0fc..b888c0476 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -860,15 +860,16 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) // After not being in this mode for a while init at current height AltHoldSmoothedData altHold; AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Velocity = 0; 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 From 005a68826d78fca58ed926dd67112e9a254153c0 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 17 Jul 2013 08:38:44 +0000 Subject: [PATCH 18/50] OP-1022 Converted to a plain Altitude PID + Velocity PI implementation --- flight/modules/AltitudeHold/altitudehold.c | 72 ++++++++++--------- .../altitudeholdsettings.xml | 8 +-- 2 files changed, 42 insertions(+), 38 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index fd6f5ae68..7e5618165 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -61,11 +61,11 @@ #include #include // Private constants -#define MAX_QUEUE_SIZE 2 -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 4 -#define TIMEOUT_TRESHOLD 200000 +#define MAX_QUEUE_SIZE 2 +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define ACCEL_DOWNSAMPLE 4 +#define TIMEOUT_TRESHOLD 200000 #define DESIRED_UPDATE_RATE_MS 20 // milliseconds // Private types @@ -74,7 +74,7 @@ static xTaskHandle altitudeHoldTaskHandle; static xQueueHandle queue; static AltitudeHoldSettingsData altitudeHoldSettings; static float throttleAlpha = 1.0f; -static float throttle_old = 0.0f; +static float throttle_old = 0.0f; // Private functions static void altitudeHoldTask(void *parameters); @@ -113,19 +113,16 @@ int32_t AltitudeHoldInitialize() MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); float tau; -float altitudeIntegral; -float velocityIntegral; -float decay; -float velocity_decay; -float velocity; float accelAlpha; float velAlpha; bool running = false; + +float velocity; +float velocityIntegral; +float altitudeIntegral; float error; -float switchThrottle; -float smoothed_altitude; -float starting_altitude; float velError; +float derivative; uint32_t timeval; bool posUpdated; @@ -137,10 +134,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltHoldSmoothedData altHold; - AttitudeStateData attitudeState; VelocityStateData velocityData; float dT; - float q[4], Rbe[3][3], fblimit = 0; + float fblimit = 0; float lastVertVelocity; portTickType thisTime, lastUpdateTime; UAVObjEvent ev; @@ -182,6 +178,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) FlightStatusFlightModeGet(&flightMode); altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; if (altitudeHoldFlightMode && !running) { + AttitudeStateData attitudeState; + float q[4], Rbe[3][3]; AttitudeStateGet(&attitudeState); q[0] = attitudeState.q1; q[1] = attitudeState.q2; @@ -197,12 +195,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } else if (initThrottle < 0) { initThrottle = 0; } - error = 0; + error = 0; altitudeHoldDesired.Velocity = 0; altitudeHoldDesired.Altitude = altHold.Altitude; - altitudeIntegral = altHold.Altitude * altitudeHoldSettings.Kp + initThrottle; + altitudeIntegral = initThrottle; velocityIntegral = 0; - running = true; + running = true; } else if (!altitudeHoldFlightMode) { running = false; lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); @@ -241,13 +239,20 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) continue; } + float lastError = error; + error = altitudeHoldDesired.Altitude - altHold.Altitude; + derivative = (error - lastError) / dT; + + 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; + altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudeKi * dT; + velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityKi * dT; + thisTime = xTaskGetTickCount(); // Only update stabilizationDesired less frequently - if ((thisTime - lastUpdateTime)*1000/configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { + if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { continue; } altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; @@ -256,13 +261,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Instead of explicit limit on integral you output limit feedback StabilizationDesiredGet(&stabilizationDesired); if (!enterFailSafe) { - stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral - + error * fabsf(error) * altitudeHoldSettings.Kp2 - - altHold.Altitude * altitudeHoldSettings.Kp - - altHold.Velocity * altitudeHoldSettings.Kd - - altHold.Accel * altitudeHoldSettings.Ka; + + error * fabsf(error) * altitudeHoldSettings.Altitude2Kp + + error * altitudeHoldSettings.AltitudeKp + + velError * altitudeHoldSettings.VelocityKp + + derivative * altitudeHoldSettings.AltitudeKd; + // 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; + float q[4], Rbe[3][3]; AttitudeStateGet(&attitudeState); q[0] = attitudeState.q1; q[1] = attitudeState.q2; @@ -271,7 +278,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) Quaternion2R(q, Rbe); float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; stabilizationDesired.Throttle /= throttlescale; - stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); + stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); throttle_old = stabilizationDesired.Throttle; fblimit = 0; @@ -309,12 +316,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) 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) - { - throttleAlpha = (float)DESIRED_UPDATE_RATE_MS/((float)DESIRED_UPDATE_RATE_MS + 1000.0f/(2.0f*M_PI_F*altitudeHoldSettings.ThrottleFilterCutoff)); - } - else - { + if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) { + throttleAlpha = (float)DESIRED_UPDATE_RATE_MS / ((float)DESIRED_UPDATE_RATE_MS + 1000.0f / (2.0f * M_PI_F * altitudeHoldSettings.ThrottleFilterCutoff)); + } else { throttleAlpha = 1.0f; } } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index bc234931c..45e7dd544 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,12 +1,12 @@ Settings for the @ref AltitudeHold module - - + + + - - + From e2a7c6cb25441ce76677dbc9543a5887c01ad784 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 19 Jul 2013 07:57:24 +0000 Subject: [PATCH 19/50] OP-1022 Reset altitude and velocity when stick goes to deadband --- flight/modules/ManualControl/manualcontrol.c | 21 ++++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b888c0476..ef804277c 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,9 +845,9 @@ 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; + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; @@ -858,8 +855,6 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); altitudeHoldDesiredData.Velocity = 0; altitudeHoldDesiredData.Altitude = altHold.Altitude; zeroed = false; @@ -867,14 +862,18 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) // 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.Altitude += altitudeHoldDesiredData.Velocity * dT; + 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.Altitude += altitudeHoldDesiredData.Velocity * dT; + 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 - altitudeHoldDesiredData.Velocity = 0; + if(fabsf(altitudeHoldDesiredData.Velocity)> 1e-3f) + {; + altitudeHoldDesiredData.Velocity = 0; + altitudeHoldDesiredData.Altitude = altHold.Altitude; + } zeroed = true; } From 6b27ff1e27e195af719805095078dd26e71fe482 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 19 Jul 2013 11:28:48 +0000 Subject: [PATCH 20/50] 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 - - - - - - + + - + From ff5927bc43e8f160a5ce08009110b9fe899c8f9c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 17:28:01 +0100 Subject: [PATCH 21/50] some changes to altitudehold, hope i get that done today... --- flight/modules/ManualControl/manualcontrol.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b1ab39276..8a7d0a7fb 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -38,7 +38,6 @@ #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" -#include "altholdsmoothed.h" #include "flighttelemetrystats.h" #include "flightstatus.h" #include "sanitycheck.h" @@ -872,8 +871,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); + PositionStateData posState; + PositionStateGet(&posState); altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; @@ -883,23 +882,23 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) if (changed) { // After not being in this mode for a while init at current height altitudeHoldDesiredData.Velocity = 0; - altitudeHoldDesiredData.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; 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.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; } 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.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; } 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.Velocity = 0; - altitudeHoldDesiredData.Altitude = altHold.Altitude; + altitudeHoldDesiredData.Altitude = posState.Down; } zeroed = true; } From a4d53c18aceb7d04bfce5a476430c904a32fc71e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 20:14:02 +0100 Subject: [PATCH 22/50] got rid of altholdsmoothed uavobject --- .../targets/boards/revolution/firmware/UAVObjects.inc | 1 - .../targets/boards/revoproto/firmware/UAVObjects.inc | 1 - .../targets/boards/simposix/firmware/UAVObjects.inc | 1 - .../src/plugins/uavobjects/uavobjects.pro | 2 -- shared/uavobjectdefinition/altholdsmoothed.xml | 11 ----------- 5 files changed, 16 deletions(-) delete mode 100644 shared/uavobjectdefinition/altholdsmoothed.xml diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 7a63dc9f2..8d7c97181 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -23,7 +23,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += gyrostate diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 80dab9e61..56eff01b1 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += gyrostate diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index be99363d3..0f8adf8f8 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings -UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate UAVOBJSRCFILENAMES += attitudesimulated diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d1b002994..0ca81220d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -32,7 +32,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ $$UAVOBJECT_SYNTHETICS/attitudestate.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ - $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ @@ -123,7 +122,6 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ - $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \ diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml deleted file mode 100644 index 1339e1337..000000000 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - The output on the kalman filter on altitude. - - - - - - - - From 623c25aa9918baf485eb00b39b1a0b87e272c1a0 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 23:16:22 +0100 Subject: [PATCH 23/50] new design of altitude hold - warning not tested yet! --- flight/modules/AltitudeHold/altitudehold.c | 289 ++++++------------ flight/modules/ManualControl/manualcontrol.c | 4 +- .../altitudeholdsettings.xml | 13 +- 3 files changed, 99 insertions(+), 207 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 5bf40a086..180cb9e00 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -46,39 +46,37 @@ #include #include +#include #include -#include #include #include #include // object that will be updated by the module -#include -#include #include #include #include -#include #include #include #include // Private constants -#define MAX_QUEUE_SIZE 2 + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + #define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define ACCEL_DOWNSAMPLE 4 -#define TIMEOUT_TRESHOLD 200000 #define DESIRED_UPDATE_RATE_MS 100 // milliseconds // Private types // Private variables -static xTaskHandle altitudeHoldTaskHandle; -static xQueueHandle queue; +static DelayedCallbackInfo *altitudeHoldCBInfo; static AltitudeHoldSettingsData altitudeHoldSettings; -static float throttleAlpha = 1.0f; -static float throttle_old = 0.0f; +static struct pid accelpid; +static float accelStateDown; + // Private functions -static void altitudeHoldTask(void *parameters); +static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); +static void AccelStateUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -87,8 +85,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); int32_t AltitudeHoldStart() { // Start main task - xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); + SettingsUpdatedCb(NULL); + DelayedCallbackDispatch(altitudeHoldCBInfo); return 0; } @@ -101,216 +99,111 @@ int32_t AltitudeHoldInitialize() { AltitudeHoldSettingsInitialize(); AltitudeHoldDesiredInitialize(); - AltHoldSmoothedInitialize(); // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); + AccelStateConnectCallback(&AccelStateUpdatedCb); return 0; } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); -float tau; -float positionAlpha; -float velAlpha; -bool running = false; - -float velocity; -float velocityIntegral; -float altitudeIntegral; -float error; -float velError; -float derivative; -uint32_t timeval; -bool posUpdated; /** * Module thread, should not return. */ -static void altitudeHoldTask(__attribute__((unused)) void *parameters) +static void altitudeHoldTask(void) { - AltitudeHoldDesiredData altitudeHoldDesired; - StabilizationDesiredData stabilizationDesired; - AltHoldSmoothedData altHold; - VelocityStateData velocityData; - float dT; - float fblimit = 0; - portTickType thisTime, lastUpdateTime; - UAVObjEvent ev; + static float startThrottle =0.5f; - dT = 0; - timeval = 0; - lastUpdateTime = 0; - // Force update of the settings - SettingsUpdatedCb(&ev); - // Failsafe handling - uint32_t lastAltitudeHoldDesiredUpdate = 0; - bool enterFailSafe = false; - // Listen for updates. - AltitudeHoldDesiredConnectQueue(queue); - // PositionStateConnectQueue(queue); - FlightStatusConnectQueue(queue); - VelocityStateConnectQueue(queue); - bool altitudeHoldFlightMode = false; - running = false; - enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; + // make sure we run only when we are supposed to run + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + break; + default: + pid_zero(&accelpid); + StabilizationDesiredThrottleGet(&startThrottle); + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; + break; + } - uint8_t flightMode; - FlightStatusFlightModeGet(&flightMode); - // initialize enable flag - altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; - // Main task loop - while (1) { - enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD; - // 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) { - altitudeIntegral = 0; - } + // do the actual control loop(s) + AltitudeHoldDesiredData altitudeHoldDesired; + AltitudeHoldDesiredGet(&altitudeHoldDesired); + float positionStateDown; + PositionStateDownGet(&positionStateDown); + float velocityStateDown; + VelocityStateDownGet(&velocityStateDown); - // Todo: Add alarm if it should be running - continue; - } else if (ev.obj == FlightStatusHandle()) { - FlightStatusFlightModeGet(&flightMode); - altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; - if (altitudeHoldFlightMode && !running) { - AttitudeStateData attitudeState; - float q[4], Rbe[3][3]; - AttitudeStateGet(&attitudeState); - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - // Copy the current throttle as a starting point for integral - 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; - altitudeHoldDesired.Velocity = 0; - altitudeHoldDesired.Altitude = altHold.Altitude; - altitudeIntegral = initThrottle; - velocityIntegral = 0; - running = true; - } else if (!altitudeHoldFlightMode) { - running = false; - lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); - } - } else if (ev.obj == VelocityStateHandle()) { - init = (init == WAITING_BARO) ? WAITIING_INIT : init; - dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT; - timeval = PIOS_DELAY_GetRaw(); + // altitude control loop + float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; - AltHoldSmoothedGet(&altHold); - - VelocityStateGet(&velocityData); - - altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); - - float position; - PositionStateDownGet(&position); - altHold.Altitude = -(positionAlpha * position) + (1 - positionAlpha) * altHold.Altitude; - - AltHoldSmoothedSet(&altHold); - - // Verify that we are in altitude hold mode - uint8_t armed; - FlightStatusArmedGet(&armed); - if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) { - running = false; - } - - if (!running) { - lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); - continue; - } - - float lastError = error; - error = altitudeHoldDesired.Altitude - altHold.Altitude; - derivative = (error - lastError) / dT; - - velError = altitudeHoldDesired.Velocity - altHold.Velocity; - - // Compute altitude and velocity integral - altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KI] * dT; - velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KI] * dT; + // velocity control loop + float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) -9.81f; - thisTime = xTaskGetTickCount(); - // Only update stabilizationDesired less frequently - if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { - continue; - } - lastUpdateTime = thisTime; + // compensate acceleration by rotation + // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q + // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation + // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical + // acceleration at the current tilt angle. + // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep + // integrals from winding in any direction + + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1,Rbe); + + float rotatedAccelDesired = realAccelDesired; + if (fabsf(Rbe[2][2])>1e-3f) { + rotatedAccelDesired /= Rbe[2][2]; + } else { + rotatedAccelDesired = accelStateDown; + } + + // acceleration control loop + float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000/DESIRED_UPDATE_RATE_MS); - // Instead of explicit limit on integral you output limit feedback - StabilizationDesiredGet(&stabilizationDesired); - if (!enterFailSafe) { - stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral - + error * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KP] - + velError * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KP] - + derivative * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KD]; + if (throttle>=1.0f) { + throttle=1.0f; + } + if (throttle<=0.0f) { + throttle=0.0f; + } + StabilizationDesiredData stab; + StabilizationDesiredGet(&stab); + stab.Roll = altitudeHoldDesired.Roll; + stab.Pitch = altitudeHoldDesired.Pitch; + stab.Yaw = altitudeHoldDesired.Yaw; + stab.Throttle = throttle; + stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + + StabilizationDesiredSet(&stab); - // 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; - float q[4], Rbe[3][3]; - 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; - stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha); - throttle_old = stabilizationDesired.Throttle; - fblimit = 0; + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); +} - if (stabilizationDesired.Throttle > 1) { - fblimit = stabilizationDesired.Throttle - 1; - stabilizationDesired.Throttle = 1; - } else if (stabilizationDesired.Throttle < 0) { - fblimit = stabilizationDesired.Throttle; - stabilizationDesired.Throttle = 0; - } - } else { - // shutdown motors - stabilizationDesired.Throttle = -1; - } - stabilizationDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabilizationDesired.Roll = altitudeHoldDesired.Roll; - stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; - stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; - - StabilizationDesiredSet(&stabilizationDesired); - } else if (ev.obj == AltitudeHoldDesiredHandle()) { - // reset the failsafe timer - lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); - AltitudeHoldDesiredGet(&altitudeHoldDesired); - } - } +static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + float down; + AccelStatezGet(&down); + accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - 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) { - throttleAlpha = (float)DESIRED_UPDATE_RATE_MS / ((float)DESIRED_UPDATE_RATE_MS + 1000.0f / (2.0f * M_PI_F * altitudeHoldSettings.ThrottleFilterCutoff)); - } else { - throttleAlpha = 1.0f; - } + pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit); + pid_zero(&accelpid); + accelStateDown=0.0f; } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 8a7d0a7fb..5913878ea 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -887,10 +887,10 @@ 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 = posState.Down; } 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 = posState.Down; } 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 diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 806d86714..26491eb2f 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,13 +1,12 @@ Settings for the @ref AltitudeHold module - - - - - - - + + + + + + From 8832ea24f5b886f34ead71ed4099cd16cc230096 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 7 Dec 2013 23:31:26 +0100 Subject: [PATCH 24/50] proper uncrustification --- flight/modules/AltitudeHold/altitudehold.c | 146 +++++++++++---------- 1 file changed, 74 insertions(+), 72 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 180cb9e00..84abe76f4 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -59,8 +59,8 @@ #include // Private constants -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define STACK_SIZE_BYTES 1024 #define DESIRED_UPDATE_RATE_MS 100 // milliseconds @@ -116,88 +116,90 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); */ static void altitudeHoldTask(void) { + static float startThrottle = 0.5f; - static float startThrottle =0.5f; + // make sure we run only when we are supposed to run + FlightStatusData flightStatus; - // make sure we run only when we are supposed to run - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: - break; - default: - pid_zero(&accelpid); - StabilizationDesiredThrottleGet(&startThrottle); - DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); - return; - break; - } + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + break; + default: + pid_zero(&accelpid); + StabilizationDesiredThrottleGet(&startThrottle); + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; - // do the actual control loop(s) - AltitudeHoldDesiredData altitudeHoldDesired; - AltitudeHoldDesiredGet(&altitudeHoldDesired); - float positionStateDown; - PositionStateDownGet(&positionStateDown); - float velocityStateDown; - VelocityStateDownGet(&velocityStateDown); + break; + } - // altitude control loop - float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + // do the actual control loop(s) + AltitudeHoldDesiredData altitudeHoldDesired; + AltitudeHoldDesiredGet(&altitudeHoldDesired); + float positionStateDown; + PositionStateDownGet(&positionStateDown); + float velocityStateDown; + VelocityStateDownGet(&velocityStateDown); - // velocity control loop - float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) -9.81f; + // altitude control loop + float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + + // velocity control loop + float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) - 9.81f; - // compensate acceleration by rotation - // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q - // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation - // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical - // acceleration at the current tilt angle. - // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep - // integrals from winding in any direction - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - float Rbe[3][3]; - Quaternion2R(&attitudeState.q1,Rbe); - - float rotatedAccelDesired = realAccelDesired; - if (fabsf(Rbe[2][2])>1e-3f) { - rotatedAccelDesired /= Rbe[2][2]; - } else { - rotatedAccelDesired = accelStateDown; - } - - // acceleration control loop - float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000/DESIRED_UPDATE_RATE_MS); + // compensate acceleration by rotation + // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q + // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation + // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical + // acceleration at the current tilt angle. + // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep + // integrals from winding in any direction - if (throttle>=1.0f) { - throttle=1.0f; - } - if (throttle<=0.0f) { - throttle=0.0f; - } - StabilizationDesiredData stab; - StabilizationDesiredGet(&stab); - stab.Roll = altitudeHoldDesired.Roll; - stab.Pitch = altitudeHoldDesired.Pitch; - stab.Yaw = altitudeHoldDesired.Yaw; - stab.Throttle = throttle; - stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - - StabilizationDesiredSet(&stab); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1, Rbe); - DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + float rotatedAccelDesired = realAccelDesired; + if (fabsf(Rbe[2][2]) > 1e-3f) { + rotatedAccelDesired /= Rbe[2][2]; + } else { + rotatedAccelDesired = accelStateDown; + } + + // acceleration control loop + float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); + + if (throttle >= 1.0f) { + throttle = 1.0f; + } + if (throttle <= 0.0f) { + throttle = 0.0f; + } + StabilizationDesiredData stab; + StabilizationDesiredGet(&stab); + stab.Roll = altitudeHoldDesired.Roll; + stab.Pitch = altitudeHoldDesired.Pitch; + stab.Yaw = altitudeHoldDesired.Yaw; + stab.Throttle = throttle; + stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + + StabilizationDesiredSet(&stab); + + DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); } static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - float down; - AccelStatezGet(&down); - accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); + float down; + + AccelStatezGet(&down); + accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) @@ -205,5 +207,5 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit); pid_zero(&accelpid); - accelStateDown=0.0f; + accelStateDown = 0.0f; } From 981fc4321d65bcf744bdb1d96d08c4d7d2c2734f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 10:32:24 +0100 Subject: [PATCH 25/50] removed requirement for non existing task --- flight/libraries/sanitycheck.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 51442f48e..ab53e2302 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -113,8 +113,6 @@ int32_t configuration_check() case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: From 90f689cfa13329615b249d2d25907510e44c8940 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 10:44:17 +0100 Subject: [PATCH 26/50] throttle does not accelerate downwards but upwards - switched sign --- flight/modules/AltitudeHold/altitudehold.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 84abe76f4..ffe5b5cc3 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -171,7 +171,7 @@ static void altitudeHoldTask(void) } // acceleration control loop - float throttle = startThrottle + pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); + float throttle = startThrottle - pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); if (throttle >= 1.0f) { throttle = 1.0f; From 4ad0e730dad417f03f52f537d98171f2cc56f74e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 12:23:16 +0100 Subject: [PATCH 27/50] added status uavobject for easier debugging --- flight/modules/AltitudeHold/altitudehold.c | 14 +++++++++++--- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 ++ shared/uavobjectdefinition/altitudeholddesired.xml | 2 +- 6 files changed, 17 insertions(+), 4 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index ffe5b5cc3..78ec27332 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -51,6 +51,7 @@ #include #include #include // object that will be updated by the module +#include #include #include #include @@ -99,6 +100,7 @@ int32_t AltitudeHoldInitialize() { AltitudeHoldSettingsInitialize(); AltitudeHoldDesiredInitialize(); + AltitudeHoldStatusInitialize(); // Create object queue @@ -135,6 +137,9 @@ static void altitudeHoldTask(void) break; } + AltitudeHoldStatusData altitudeHoldStatus; + AltitudeHoldStatusGet(&altitudeHoldStatus); + // do the actual control loop(s) AltitudeHoldDesiredData altitudeHoldDesired; AltitudeHoldDesiredGet(&altitudeHoldDesired); @@ -144,11 +149,12 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; // velocity control loop - float realAccelDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) - 9.81f; + altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + AltitudeHoldStatusSet(&altitudeHoldStatus); // compensate acceleration by rotation // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q @@ -163,12 +169,14 @@ static void altitudeHoldTask(void) float Rbe[3][3]; Quaternion2R(&attitudeState.q1, Rbe); - float rotatedAccelDesired = realAccelDesired; + float rotatedAccelDesired = altitudeHoldStatus.AccelerationDesired; +#if 0 if (fabsf(Rbe[2][2]) > 1e-3f) { rotatedAccelDesired /= Rbe[2][2]; } else { rotatedAccelDesired = accelStateDown; } +#endif // acceleration control loop float throttle = startThrottle - pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 8d7c97181..33a47e757 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -98,6 +98,7 @@ UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += poilocation diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 56eff01b1..ee6c02bbd 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -96,6 +96,7 @@ UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += poilocation diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 0f8adf8f8..e9fc43178 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -101,6 +101,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 0ca81220d..8fb4d384e 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -34,6 +34,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ + $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \ $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.h \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.h \ @@ -124,6 +125,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogcontrol.cpp \ $$UAVOBJECT_SYNTHETICS/debuglogstatus.cpp \ diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 116e82306..7a69227fd 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -8,7 +8,7 @@ - + From 3cea14a809ced88e7164b03098974c9166e40e85 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 13:06:28 +0100 Subject: [PATCH 28/50] added missing object, added more debug output --- flight/modules/AltitudeHold/altitudehold.c | 2 ++ shared/uavobjectdefinition/altitudeholdstatus.xml | 12 ++++++++++++ 2 files changed, 14 insertions(+) create mode 100644 shared/uavobjectdefinition/altitudeholdstatus.xml diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 78ec27332..771aca1e6 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -154,6 +154,8 @@ static void altitudeHoldTask(void) // velocity control loop altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + altitudeHoldStatus.AccelerationFiltered = accelStateDown; + AltitudeHoldStatusSet(&altitudeHoldStatus); // compensate acceleration by rotation diff --git a/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml new file mode 100644 index 000000000..928496f51 --- /dev/null +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -0,0 +1,12 @@ + + + Status Data from Altitude Hold Control Loops + + + + + + + + + From 5cc8dedadf09ae68cf037fdd4f3b372d0b13ac37 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 13:07:26 +0100 Subject: [PATCH 29/50] uncrustify changed things --- flight/modules/AltitudeHold/altitudehold.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 771aca1e6..c92014b97 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -149,10 +149,10 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; // velocity control loop - altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; altitudeHoldStatus.AccelerationFiltered = accelStateDown; From 66b0ffd682371f7262a3514cd709e036dfac2900 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 13:35:00 +0100 Subject: [PATCH 30/50] small fix to baro state filter - more init cycles --- flight/modules/StateEstimation/filterbaro.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 739fa5d72..18078efd1 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -37,6 +37,7 @@ // Private constants #define STACK_REQUIRED 64 +#define INIT_CYCLES 1000 // Private types struct data { @@ -67,7 +68,7 @@ static int32_t init(stateFilter *self) struct data *this = (struct data *)self->localdata; this->baroOffset = 0.0f; - this->first_run = 100; + this->first_run = INIT_CYCLES; RevoSettingsInitialize(); RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); @@ -82,7 +83,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0]; + this->baroOffset = (INIT_CYCLES - this->first_run) / INIT_CYCLES * this->baroOffset + (this->first_run / INIT_CYCLES) * state->baro[0]; this->baroAlt = this->baroOffset; this->first_run--; UNSET_MASK(state->updated, SENSORUPDATES_baro); From 84af4b7651683cc972e51bb30e1a82751577f11d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 14:00:09 +0100 Subject: [PATCH 31/50] fixes to barofilter --- flight/modules/StateEstimation/filterbaro.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 18078efd1..a44927392 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -37,7 +37,7 @@ // Private constants #define STACK_REQUIRED 64 -#define INIT_CYCLES 1000 +#define INIT_CYCLES 100 // Private types struct data { @@ -83,8 +83,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->baroOffset = (INIT_CYCLES - this->first_run) / INIT_CYCLES * this->baroOffset + (this->first_run / INIT_CYCLES) * state->baro[0]; - this->baroAlt = this->baroOffset; + this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0]; + this->baroAlt = 0; this->first_run--; UNSET_MASK(state->updated, SENSORUPDATES_baro); } From 95d52c6b08b3f580e9ca8e2f83c4b2fa09504fd7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 14:00:18 +0100 Subject: [PATCH 32/50] fixed signs in control loop --- flight/modules/AltitudeHold/altitudehold.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index c92014b97..153ffcf24 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -149,10 +149,10 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (altitudeHoldDesired.Altitude - positionStateDown) + altitudeHoldDesired.Velocity; // velocity control loop - altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f; + altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (altitudeHoldStatus.VelocityDesired - velocityStateDown) - 9.81f; altitudeHoldStatus.AccelerationFiltered = accelStateDown; From 75842cb64840396b42acf3b88ba0734888f83899 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:21:05 +0100 Subject: [PATCH 33/50] =?UTF-8?q?OP-1139:=20perform=20second=20order=20low?= =?UTF-8?q?=20temperature=20compensation.=20It=20follows=20the=20procedure?= =?UTF-8?q?=20described=20in=20MS5611=20datasheet(http://www.meas-spec.com?= =?UTF-8?q?/downloads/MS5611-01BA03.pdf,=20page=208)=20to=20perform=20low(?= =?UTF-8?q?20=C2=B0)=20and=20very=20low(-15=C2=B0C)=20temperature=20compen?= =?UTF-8?q?sation.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- flight/pios/common/pios_ms5611.c | 32 ++++++++++++++++++++++++++------ 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index 1d1a289e7..aeb99de45 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -51,7 +51,7 @@ static int64_t Temperature; static int32_t lastConversionStart; static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); - +static int64_t t2; // Move into proper driver structure with cfg stored static uint32_t oversampling; static const struct pios_ms5611_cfg *dev_cfg; @@ -74,6 +74,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) uint8_t data[2]; + // reset calibration values + t2 = 0; + /* Calibration parameters */ for (int i = 0; i < 6; i++) { PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); @@ -190,17 +193,34 @@ int32_t PIOS_MS5611_ReadADC(void) } else { int64_t Offset; int64_t Sens; + // used for second order temperature compensation + int64_t Offset2 = 0; + int64_t Sens2 = 0; /* Read the pressure conversion */ if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { return -1; } + // check if temperature is less than 20°C + if(Temperature < 2000){ + Offset2 = 5 * (Temperature - 2000) >> 1; + Sens2 = Offset2 >> 1; + t2 = (deltaTemp*deltaTemp) >> 31; + // Apply the "Very low temperature compensation" when temp is less than -15°C + if(Temperature < -1500){ + int64_t tcorr = (Temperature + 1500)*(Temperature + 1500); + Offset2 += 7 * tcorr; + Sens2 += (11 * tcorr) >> 1; + } + } else { + t2 = 0; + Offset2 = 0; + Sens2 = 0; + } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); - - Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7); + Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; Sens = ((int64_t)CalibData.C[0]) << 15; - Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8); - + Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2; Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15; } return 0; @@ -211,7 +231,7 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float)Temperature) / 100.0f; + return ((float)(Temperature - t2)) / 100.0f; } /** From 13b45b2b7846055ed55266c0bdec21690ff76e81 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:44:13 +0100 Subject: [PATCH 34/50] OP.1139: uncrustify --- flight/pios/common/pios_ms5611.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index aeb99de45..f21fff0fc 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -195,27 +195,27 @@ int32_t PIOS_MS5611_ReadADC(void) int64_t Sens; // used for second order temperature compensation int64_t Offset2 = 0; - int64_t Sens2 = 0; + int64_t Sens2 = 0; /* Read the pressure conversion */ if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { return -1; } // check if temperature is less than 20°C - if(Temperature < 2000){ + if (Temperature < 2000) { Offset2 = 5 * (Temperature - 2000) >> 1; - Sens2 = Offset2 >> 1; - t2 = (deltaTemp*deltaTemp) >> 31; + Sens2 = Offset2 >> 1; + t2 = (deltaTemp * deltaTemp) >> 31; // Apply the "Very low temperature compensation" when temp is less than -15°C - if(Temperature < -1500){ - int64_t tcorr = (Temperature + 1500)*(Temperature + 1500); + if (Temperature < -1500) { + int64_t tcorr = (Temperature + 1500) * (Temperature + 1500); Offset2 += 7 * tcorr; - Sens2 += (11 * tcorr) >> 1; + Sens2 += (11 * tcorr) >> 1; } } else { - t2 = 0; + t2 = 0; Offset2 = 0; - Sens2 = 0; + Sens2 = 0; } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; From 634ba79dcb4ee858524d34c64d386a9eb48818cf Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:46:28 +0100 Subject: [PATCH 35/50] OP-1141: allow to input a barometer bias model. It uses a 3rd degree polynomial to model pressure bias as a function of temperature. --- flight/modules/Altitude/revolution/altitude.c | 21 ++++++++++++++----- shared/uavobjectdefinition/revosettings.xml | 4 +++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index c66f52ae8..27a26278d 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -40,22 +40,24 @@ #include "altitude.h" #include "barosensor.h" // object that will be updated by the module +#include "revosettings.h" #if defined(PIOS_INCLUDE_HCSR04) #include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants -#define STACK_SIZE_BYTES 500 +#define STACK_SIZE_BYTES 550 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types // Private variables static xTaskHandle taskHandle; - +static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection; // Private functions static void altitudeTask(void *parameters); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -77,6 +79,9 @@ int32_t AltitudeStart() int32_t AltitudeInitialize() { BaroSensorInitialize(); + RevoSettingsInitialize(); + RevoSettingsConnectCallback(&SettingsUpdatedCb); + #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeInitialize(); #endif @@ -103,6 +108,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters) // Undef for normal operation // #define PIOS_MS5611_SLOW_TEMP_RATE 20 + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); + #ifdef PIOS_MS5611_SLOW_TEMP_RATE uint8_t temp_press_interleave_count = 1; #endif @@ -154,12 +161,12 @@ static void altitudeTask(__attribute__((unused)) void *parameters) vTaskDelay(PIOS_MS5611_GetDelay()); PIOS_MS5611_ReadADC(); - temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); + float temp2 = temp * temp; + float correction = baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - - float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press - correction) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; @@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters) } } +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); +} /** * @} * @} diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 0cf9a4561..8d634d1cd 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -7,7 +7,9 @@ Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062 Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. --> - + + From 44269b67623069b576294eb8c545227bd56f7e09 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 02:02:30 +0100 Subject: [PATCH 36/50] OP-1139: export corrected pressure value to uavobject --- flight/modules/Altitude/revolution/altitude.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 27a26278d..d46c7c5bb 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -164,9 +164,9 @@ static void altitudeTask(__attribute__((unused)) void *parameters) temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); float temp2 = temp * temp; - float correction = baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; + press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - float altitude = 44330.0f * (1.0f - powf((press - correction) / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; From 29df9d6dad71b0b2f7fa641ea8e6c569ec0af59f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 13 Dec 2013 17:10:11 +0100 Subject: [PATCH 37/50] OP-1139: Add some more descriptive names and comments for compensation variables --- flight/pios/common/pios_ms5611.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index f21fff0fc..db8d16f1b 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -51,7 +51,10 @@ static int64_t Temperature; static int32_t lastConversionStart; static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); -static int64_t t2; + +// Second order temperature compensation. Temperature offset +static int64_t compensation_t2; + // Move into proper driver structure with cfg stored static uint32_t oversampling; static const struct pios_ms5611_cfg *dev_cfg; @@ -74,8 +77,8 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) uint8_t data[2]; - // reset calibration values - t2 = 0; + // reset temperature compensation values + compensation_t2 = 0; /* Calibration parameters */ for (int i = 0; i < 6; i++) { @@ -205,7 +208,7 @@ int32_t PIOS_MS5611_ReadADC(void) if (Temperature < 2000) { Offset2 = 5 * (Temperature - 2000) >> 1; Sens2 = Offset2 >> 1; - t2 = (deltaTemp * deltaTemp) >> 31; + compensation_t2 = (deltaTemp * deltaTemp) >> 31; // Apply the "Very low temperature compensation" when temp is less than -15°C if (Temperature < -1500) { int64_t tcorr = (Temperature + 1500) * (Temperature + 1500); @@ -213,9 +216,9 @@ int32_t PIOS_MS5611_ReadADC(void) Sens2 += (11 * tcorr) >> 1; } } else { - t2 = 0; + compensation_t2 = 0; Offset2 = 0; - Sens2 = 0; + Sens2 = 0; } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; @@ -231,7 +234,8 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float)(Temperature - t2)) / 100.0f; + // Apply the second order low and very low temperature compensation offset + return ((float)(Temperature - compensation_t2)) / 100.0f; } /** From f95a86eaed8971761bd78209c7c75166cf037255 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 13 Dec 2013 17:10:39 +0100 Subject: [PATCH 38/50] OP-1139: missing uncrustification --- flight/modules/Altitude/revolution/altitude.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index d46c7c5bb..0c6dbd2ed 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -163,10 +163,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters) temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); - float temp2 = temp * temp; + float temp2 = temp * temp; press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; From 36788a60f42a9bd0611d4006f8f41140ddc98164 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 16:26:59 +0100 Subject: [PATCH 39/50] bugfix for xplane hitl, convert gyroscope sensor from rad/s to deg/s --- ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index 53609951e..20beab45d 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -318,10 +318,10 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) out.velEast = velX; out.velDown = -velZ; - // Update gyroscope sensor data - out.rollRate = rollRate_rad; - out.pitchRate = pitchRate_rad; - out.yawRate = yawRate_rad; + // Update gyroscope sensor data - convert from rad/s to deg/s + out.rollRate = rollRate_rad * (180.0/M_PI); + out.pitchRate = pitchRate_rad * (180.0/M_PI); + out.yawRate = yawRate_rad * (180.0/M_PI); // Update accelerometer sensor data out.accX = accX; From 1cccd152bd7adb68c95d1467ceb6efc3ccc42591 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 16:50:30 +0100 Subject: [PATCH 40/50] added AltitudeHold to simposix --- flight/targets/boards/simposix/firmware/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index c9d719334..c64974d0a 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -42,6 +42,7 @@ MODULES += FirmwareIAP MODULES += StateEstimation #MODULES += Sensors/simulated/Sensors MODULES += Airspeed +MODULES += AltitudeHold #MODULES += OveroSync # Paths From d60eda1a83449accfc956ab835bc95060d5d1109 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 18:37:14 +0100 Subject: [PATCH 41/50] forgot uncrustification --- ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index 20beab45d..e7b409815 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -319,9 +319,9 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) out.velDown = -velZ; // Update gyroscope sensor data - convert from rad/s to deg/s - out.rollRate = rollRate_rad * (180.0/M_PI); - out.pitchRate = pitchRate_rad * (180.0/M_PI); - out.yawRate = yawRate_rad * (180.0/M_PI); + out.rollRate = rollRate_rad * (180.0 / M_PI); + out.pitchRate = pitchRate_rad * (180.0 / M_PI); + out.yawRate = yawRate_rad * (180.0 / M_PI); // Update accelerometer sensor data out.accX = accX; From 505d334c4beee15f837e27c1b0ba216543771f3f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 27 Dec 2013 18:37:27 +0100 Subject: [PATCH 42/50] simplified altitude hold control loop --- flight/modules/AltitudeHold/altitudehold.c | 54 ++++--------------- .../altitudeholdsettings.xml | 6 +-- .../altitudeholdstatus.xml | 2 - 3 files changed, 11 insertions(+), 51 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 153ffcf24..e3e5fcd2a 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -70,14 +70,12 @@ // Private variables static DelayedCallbackInfo *altitudeHoldCBInfo; static AltitudeHoldSettingsData altitudeHoldSettings; -static struct pid accelpid; -static float accelStateDown; +static struct pid pid0, pid1; // Private functions static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); -static void AccelStateUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -106,7 +104,6 @@ int32_t AltitudeHoldInitialize() altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); - AccelStateConnectCallback(&AccelStateUpdatedCb); return 0; } @@ -129,7 +126,8 @@ static void altitudeHoldTask(void) case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: break; default: - pid_zero(&accelpid); + pid_zero(&pid0); + pid_zero(&pid1); StabilizationDesiredThrottleGet(&startThrottle); DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; @@ -149,40 +147,13 @@ static void altitudeHoldTask(void) VelocityStateDownGet(&velocityStateDown); // altitude control loop - altitudeHoldStatus.VelocityDesired = altitudeHoldSettings.AltitudeP * (altitudeHoldDesired.Altitude - positionStateDown) + altitudeHoldDesired.Velocity; + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); // velocity control loop - altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (altitudeHoldStatus.VelocityDesired - velocityStateDown) - 9.81f; - - altitudeHoldStatus.AccelerationFiltered = accelStateDown; + float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); AltitudeHoldStatusSet(&altitudeHoldStatus); - // compensate acceleration by rotation - // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q - // It is 1.0 for no rotation, 0.0 for a 90 degrees roll or pitch and -1.0 for a 180 degrees flipped rotation - // multiplying with 1/Rbe[2][2] therefore is the acceleration/thrust required to overcome gravity and achieve the wanted vertical - // acceleration at the current tilt angle. - // around 90 degrees rotation this is infinite (since no possible acceleration would get us up or down) so we set the error to zero to keep - // integrals from winding in any direction - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - float Rbe[3][3]; - Quaternion2R(&attitudeState.q1, Rbe); - - float rotatedAccelDesired = altitudeHoldStatus.AccelerationDesired; -#if 0 - if (fabsf(Rbe[2][2]) > 1e-3f) { - rotatedAccelDesired /= Rbe[2][2]; - } else { - rotatedAccelDesired = accelStateDown; - } -#endif - - // acceleration control loop - float throttle = startThrottle - pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS); - if (throttle >= 1.0f) { throttle = 1.0f; } @@ -204,18 +175,11 @@ static void altitudeHoldTask(void) DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); } -static void AccelStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - float down; - - AccelStatezGet(&down); - accelStateDown = down * altitudeHoldSettings.AccelAlpha + accelStateDown * (1.0f - altitudeHoldSettings.AccelAlpha); -} - static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - pid_configure(&accelpid, altitudeHoldSettings.AccelPI.Kp, altitudeHoldSettings.AccelPI.Kp, 0, altitudeHoldSettings.AccelPI.Ilimit); - pid_zero(&accelpid); - accelStateDown = 0.0f; + pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit); + pid_zero(&pid0); + pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit); + pid_zero(&pid1); } diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 26491eb2f..f956cca30 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,10 +1,8 @@ Settings for the @ref AltitudeHold module - - - - + + diff --git a/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml index 928496f51..f298a45ab 100644 --- a/shared/uavobjectdefinition/altitudeholdstatus.xml +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -2,8 +2,6 @@ Status Data from Altitude Hold Control Loops - - From cefcb9881ad7147b9653eb8dfbb1472942af81d2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 29 Dec 2013 19:09:54 +0100 Subject: [PATCH 43/50] Altitude Hold - compensation for tilt --- flight/modules/AltitudeHold/altitudehold.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index e3e5fcd2a..f177d9c8b 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -149,10 +149,25 @@ static void altitudeHoldTask(void) // altitude control loop altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + AltitudeHoldStatusSet(&altitudeHoldStatus); + // velocity control loop float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); - AltitudeHoldStatusSet(&altitudeHoldStatus); + // compensate throttle for current attitude + // Rbe[2][2] in the rotation matrix is the rotated down component of a + // 0,0,1 vector + // it is used to scale the throttle, but only up to 4 times the regular + // throttle + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1, Rbe); + if (fabsf(Rbe[2][2]) > 0.25f) { + throttle /= Rbe[2][2]; + } else { + throttle /= 0.25f; + } if (throttle >= 1.0f) { throttle = 1.0f; From b786cec832d7fb5a8b7ecc57ae99ae996d8ba8c4 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 30 Dec 2013 01:40:27 +0100 Subject: [PATCH 44/50] bugfix to eventdispatcher to fix uavtalk issue on coptercontrol --- flight/uavobjects/eventdispatcher.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index 9fc78a21c..d9518d620 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -104,6 +104,7 @@ int32_t EventDispatcherInitialize() // Create callback eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4); + DelayedCallbackDispatch(eventSchedulerCallback); // Done return 0; From a322c14d35d17113aa0c42eca1da5dd1766291e8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 2 Jan 2014 17:17:19 +0100 Subject: [PATCH 45/50] changed defaults for altitude hold control loop coefficients --- shared/uavobjectdefinition/altitudeholdsettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index f956cca30..61724dc35 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,8 +1,8 @@ Settings for the @ref AltitudeHold module - - + + From ca607ad924151be2019b9c4715aa4f024a8c32b7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 11 Jan 2014 10:51:47 +0100 Subject: [PATCH 46/50] OP-1022 overhaul of altitudehold and manualcontrol - cleanup, reimplemented safe throttle cutoff and altitudevario features --- flight/modules/AltitudeHold/altitudehold.c | 65 +++++++---- flight/modules/ManualControl/manualcontrol.c | 109 ++++++++---------- .../altitudeholddesired.xml | 6 +- .../altitudeholdsettings.xml | 1 + 4 files changed, 96 insertions(+), 85 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index f177d9c8b..6a81518d6 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -146,35 +146,54 @@ static void altitudeHoldTask(void) float velocityStateDown; VelocityStateDownGet(&velocityStateDown); - // altitude control loop - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + switch (altitudeHoldDesired.ThrottleMode) { + case ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE: + // altitude control loop + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.ThrottleCommand, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + break; + case ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY: + altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.ThrottleCommand; + break; + default: + altitudeHoldStatus.VelocityDesired = 0; + break; + } AltitudeHoldStatusSet(&altitudeHoldStatus); - // velocity control loop - float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + float throttle; + switch (altitudeHoldDesired.ThrottleMode) { + case ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE: + throttle = altitudeHoldDesired.ThrottleCommand; + break; + default: + // velocity control loop + throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); - // compensate throttle for current attitude - // Rbe[2][2] in the rotation matrix is the rotated down component of a - // 0,0,1 vector - // it is used to scale the throttle, but only up to 4 times the regular - // throttle - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - float Rbe[3][3]; - Quaternion2R(&attitudeState.q1, Rbe); - if (fabsf(Rbe[2][2]) > 0.25f) { - throttle /= Rbe[2][2]; - } else { - throttle /= 0.25f; + // compensate throttle for current attitude + // Rbe[2][2] in the rotation matrix is the rotated down component of a + // 0,0,1 vector + // it is used to scale the throttle, but only up to 4 times the regular + // throttle + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rbe[3][3]; + Quaternion2R(&attitudeState.q1, Rbe); + if (fabsf(Rbe[2][2]) > 0.25f) { + throttle /= Rbe[2][2]; + } else { + throttle /= 0.25f; + } + + if (throttle >= 1.0f) { + throttle = 1.0f; + } + if (throttle <= 0.0f) { + throttle = 0.0f; + } + break; } - if (throttle >= 1.0f) { - throttle = 1.0f; - } - if (throttle <= 0.0f) { - throttle = 0.0f; - } StabilizationDesiredData stab; StabilizationDesiredGet(&stab); stab.Roll = altitudeHoldDesired.Roll; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index a9882966e..dbc7ecc03 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -691,26 +691,26 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont } stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode stabilization.Pitch = - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode @@ -722,18 +722,17 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; - } - else { + } else { stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode } @@ -850,30 +849,20 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - // Stop updating AltitudeHoldDesired triggering a failsafe condition. - if (cmd->Throttle < 0) { - return; - } - // this is the max speed in m/s at the extents of throttle uint8_t throttleRate; uint8_t throttleExp; static uint8_t flightMode; - static bool zeroed = false; + static bool newaltitude = true; FlightStatusFlightModeGet(&flightMode); AltitudeHoldDesiredData altitudeHoldDesiredData; AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { - throttleExp = 128; - throttleRate = 0; - } else { - AltitudeHoldSettingsThrottleExpGet(&throttleExp); - AltitudeHoldSettingsThrottleRateGet(&throttleRate); - } + AltitudeHoldSettingsThrottleExpGet(&throttleExp); + AltitudeHoldSettingsThrottleRateGet(&throttleRate); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); @@ -881,33 +870,35 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) PositionStateData posState; PositionStateGet(&posState); - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; if (changed) { - // After not being in this mode for a while init at current height - altitudeHoldDesiredData.Velocity = 0; - altitudeHoldDesiredData.Altitude = posState.Down; - zeroed = false; - } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { + newaltitude = true; + } + + uint8_t cutOff; + AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff); + if (cutOff && cmd->Throttle < 0) { + // Cut throttle if desired + altitudeHoldDesiredData.ThrottleCommand = cmd->Throttle; + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) { // 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.Altitude = posState.Down; - } 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.Altitude = posState.Down; - } 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.Velocity = 0; - altitudeHoldDesiredData.Altitude = posState.Down; - } - zeroed = true; + altitudeHoldDesiredData.ThrottleCommand = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) { + altitudeHoldDesiredData.ThrottleCommand = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + newaltitude = true; + } else if (newaltitude == true) { + altitudeHoldDesiredData.ThrottleCommand = posState.Down; + altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE; + newaltitude = false; } AltitudeHoldDesiredSet(&altitudeHoldDesiredData); diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 7a69227fd..0dfe36ceb 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,9 +1,9 @@ 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 61724dc35..8e88100f1 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -3,6 +3,7 @@ Settings for the @ref AltitudeHold module + From 879e29b6cd4a2fb9b47bab9c65f37e80175f0450 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 11 Jan 2014 14:12:02 +0100 Subject: [PATCH 47/50] OP-1022 Changed altitude (vario) complementary filter to pay more attention to barometer and apply more filtering to accelerometer (needed to compensate vibration noise and errors on flying quads) --- shared/uavobjectdefinition/altitudefiltersettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/altitudefiltersettings.xml b/shared/uavobjectdefinition/altitudefiltersettings.xml index bc6222ad1..9b3eb0838 100644 --- a/shared/uavobjectdefinition/altitudefiltersettings.xml +++ b/shared/uavobjectdefinition/altitudefiltersettings.xml @@ -1,9 +1,9 @@ Settings for the @ref State Estimator module plugin altitudeFilter - + - + From 07d5c8e4d28dd801209994a719915c5aa2a3ee94 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 14 Jan 2014 00:26:27 +0100 Subject: [PATCH 48/50] OP-1022 fixed the config plugin widget to represent new simpler settings layout --- flight/modules/ManualControl/manualcontrol.c | 2 +- .../src/plugins/config/stabilization.ui | 42 +++++++++++-------- .../altitudeholdsettings.xml | 2 +- 3 files changed, 26 insertions(+), 20 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index dbc7ecc03..7dcfb32ea 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -850,7 +850,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; // this is the max speed in m/s at the extents of throttle - uint8_t throttleRate; + float throttleRate; uint8_t throttleExp; static uint8_t flightMode; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 61352bd2f..e4a520311 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -28291,7 +28291,7 @@ border-radius: 5; - Integral + Velocity Proportional Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28316,7 +28316,7 @@ border-radius: 5; - Proportional + Altitude Proportional Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28347,7 +28347,7 @@ border-radius: 5; - Derivative + Velocity Integral Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -28371,8 +28371,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kp - scale:0.001 + fieldname:AltitudePI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -28396,8 +28397,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Ki - scale:0.001 + fieldname:VelocityPI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -28407,7 +28409,7 @@ border-radius: 5; - 100 + 1000 50 @@ -28421,8 +28423,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kd - scale:0.001 + fieldname:VelocityPI + element:Ki + scale:0.00001 haslimits:yes buttongroup:99 @@ -28988,7 +28991,7 @@ color: rgb(255, 255, 255); border-radius: 5; - Altitude + Control Coefficients Qt::AlignCenter @@ -29033,8 +29036,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kp - scale:0.001 + fieldname:AltitudePI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -29079,8 +29083,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Ki - scale:0.001 + fieldname:VelocityPI + element:Kp + scale:0.01 haslimits:yes buttongroup:99 @@ -29117,7 +29122,7 @@ border-radius: 5; Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - 100 + 1000 51 @@ -29125,8 +29130,9 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:Kd - scale:0.001 + fieldname:VelocityPI + element:Ki + scale:0.00001 haslimits:yes buttongroup:99 diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 8e88100f1..174468344 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -5,7 +5,7 @@ - + From 92beb54e2dff6c922eb68246cb68d333b8b4fd05 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 15 Jan 2014 18:43:47 +0100 Subject: [PATCH 49/50] OP-1022 some small cosmetic fixes for AltitudeHold as suggested by Alessio --- flight/libraries/sanitycheck.c | 2 ++ flight/modules/AltitudeHold/altitudehold.c | 16 ++++++++-------- flight/modules/ManualControl/manualcontrol.c | 16 ++++++++-------- .../boards/revoproto/firmware/pios_board.c | 2 +- .../uavobjectdefinition/altitudeholddesired.xml | 4 ++-- 5 files changed, 21 insertions(+), 19 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index ab53e2302..8d876a258 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -114,6 +114,8 @@ int32_t configuration_check() if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } + // TODO: put check equivalent to TASK_MONITOR_IsRunning + // here as soon as available for delayed callbacks break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: if (coptercontrol) { diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 6a81518d6..3d71ee2b2 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -146,13 +146,13 @@ static void altitudeHoldTask(void) float velocityStateDown; VelocityStateDownGet(&velocityStateDown); - switch (altitudeHoldDesired.ThrottleMode) { - case ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE: + switch (altitudeHoldDesired.ControlMode) { + case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE: // altitude control loop - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.ThrottleCommand, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); break; - case ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY: - altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.ThrottleCommand; + case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY: + altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint; break; default: altitudeHoldStatus.VelocityDesired = 0; @@ -162,9 +162,9 @@ static void altitudeHoldTask(void) AltitudeHoldStatusSet(&altitudeHoldStatus); float throttle; - switch (altitudeHoldDesired.ThrottleMode) { - case ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE: - throttle = altitudeHoldDesired.ThrottleCommand; + switch (altitudeHoldDesired.ControlMode) { + case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE: + throttle = altitudeHoldDesired.SetPoint; break; default: // velocity control loop diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 7dcfb32ea..35cfc525c 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -882,22 +882,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff); if (cutOff && cmd->Throttle < 0) { // Cut throttle if desired - altitudeHoldDesiredData.ThrottleCommand = cmd->Throttle; - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE; + altitudeHoldDesiredData.SetPoint = cmd->Throttle; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE; newaltitude = true; } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) { // 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.ThrottleCommand = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; newaltitude = true; } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) { - altitudeHoldDesiredData.ThrottleCommand = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; + altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; newaltitude = true; } else if (newaltitude == true) { - altitudeHoldDesiredData.ThrottleCommand = posState.Down; - altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE; + altitudeHoldDesiredData.SetPoint = posState.Down; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; newaltitude = false; } diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index b3a76c386..ed142f951 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -129,7 +129,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 0dfe36ceb..1b349766f 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 - - + + From eb8d57cf3159b16311145430453cf8d0e5d7b627 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 15 Jan 2014 18:47:47 +0100 Subject: [PATCH 50/50] uncrustification --- .../src/libs/utils/mytabbedstackwidget.cpp | 1 + .../src/libs/utils/mytabbedstackwidget.h | 2 +- .../uavobjectwidgetutils/configtaskwidget.cpp | 12 ++++++------ 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index e465b50f9..2626ca47b 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -137,6 +137,7 @@ void MyTabbedStackWidget::showWidget(int index) void MyTabbedStackWidget::resizeEvent(QResizeEvent *event) { QWidget::resizeEvent(event); + m_listWidget->setFixedWidth(m_listWidget->verticalScrollBar()->isVisible() ? LIST_VIEW_WIDTH + 20 : LIST_VIEW_WIDTH); } diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h index 4b0777e4e..661200383 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -79,7 +79,7 @@ private: static const int LIST_VIEW_WIDTH = 80; protected: - void resizeEvent(QResizeEvent * event); + void resizeEvent(QResizeEvent *event); }; #endif // MYTABBEDSTACKWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index 12f58a50e..5c2817dcc 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -667,7 +667,7 @@ void ConfigTaskWidget::autoLoadWidgets() forceShadowUpdates(); /* - foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { + foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { if (binding->widget()) { qDebug() << "Binding :" << binding->widget()->objectName(); qDebug() << " Object :" << binding->object()->getName(); @@ -681,8 +681,8 @@ void ConfigTaskWidget::autoLoadWidgets() qDebug() << " Scale :" << shadow->scale(); } } - } - */ + } + */ } void ConfigTaskWidget::addWidgetToReloadGroups(QWidget *widget, QList *reloadGroupIDs) @@ -1157,10 +1157,10 @@ void WidgetBinding::setValue(const QVariant &value) { m_value = value; /* - if (m_object && m_field) { + if (m_object && m_field) { qDebug() << "WidgetBinding" << m_object->getName() << ":" << m_field->getName() << "value =" << value.toString(); - } - */ + } + */ } void WidgetBinding::updateObjectFieldFromValue()