rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

OP-1022: replace old KF in alitude hold with status from filters

Browse Source
This commit is contained in:
Alessio Morale 2013-07-15 20:33:08 +00:00
parent a57f8913ba
commit e975e4d9b7
3 changed files with 38 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
flight/modules/AltitudeHold/altitudehold.c
View File

@ -64,7 +64,7 @@
#define MAX_QUEUE_SIZE 2 #define MAX_QUEUE_SIZE 2
#define STACK_SIZE_BYTES 1024 #define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define ACCEL_DOWNSAMPLE 4 #define ACCEL_DOWNSAMPLE 10
#define TIMEOUT_TRESHOLD 200000 #define TIMEOUT_TRESHOLD 200000
// Private types // Private types
@ -112,11 +112,12 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
float throttleIntegral; float throttleIntegral;
float switchThrottle; float switchThrottle;
float velocity; float velocity;
float accelAlpha;
float velAlpha;
bool running = false; bool running = false;
float error; float error;
float velError; float velError;
uint32_t timeval; uint32_t timeval;
bool posUpdated; bool posUpdated;
/** /**
@ -128,14 +129,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
StabilizationDesiredData stabilizationDesired; StabilizationDesiredData stabilizationDesired;
AltHoldSmoothedData altHold; AltHoldSmoothedData altHold;
AttitudeStateData attitudeState; AttitudeStateData attitudeState;
AccelStateData accelState;
VelocityStateData velocityData; VelocityStateData velocityData;
float dT; float dT;
float q[4], Rbe[3][3]; float q[4], Rbe[3][3];
float lastVertVelocity;
portTickType thisTime, lastUpdateTime; portTickType thisTime, lastUpdateTime;
UAVObjEvent ev; UAVObjEvent ev;
lastVertVelocity = 0;
timeval = 0; timeval = 0;
lastUpdateTime = 0; lastUpdateTime = 0;
// Force update of the settings // Force update of the settings
@ -145,7 +145,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
bool enterFailSafe = false; bool enterFailSafe = false;
// Listen for updates. // Listen for updates.
AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldDesiredConnectQueue(queue);
AccelStateConnectQueue(queue);
PositionStateConnectQueue(queue); PositionStateConnectQueue(queue);
FlightStatusConnectQueue(queue); FlightStatusConnectQueue(queue);
VelocityStateConnectQueue(queue); VelocityStateConnectQueue(queue);
@ -176,6 +175,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
FlightStatusFlightModeGet(&flightMode); FlightStatusFlightModeGet(&flightMode);
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
if (altitudeHoldFlightMode && !running) { 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 // Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&throttleIntegral); StabilizationDesiredThrottleGet(&throttleIntegral);
switchThrottle = throttleIntegral; switchThrottle = throttleIntegral;
@ -193,28 +198,24 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
running = false; running = false;
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
} }
} else if (ev.obj == AccelStateHandle()) { } else if (ev.obj == VelocityStateHandle()) {
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
timeval = PIOS_DELAY_GetRaw(); timeval = PIOS_DELAY_GetRaw();
AltHoldSmoothedGet(&altHold); 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); 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); PositionStateDownGet(&altHold.Altitude);
altHold.Altitude = -altHold.Altitude;
AltHoldSmoothedSet(&altHold); AltHoldSmoothedSet(&altHold);
@ -250,13 +251,22 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
StabilizationDesiredGet(&stabilizationDesired); StabilizationDesiredGet(&stabilizationDesired);
if (!enterFailSafe) { if (!enterFailSafe) {
if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) {
stabilizationDesired.Throttle = - error * altitudeHoldSettings.Kp + throttleIntegral + stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp
altHold.Velocity * altitudeHoldSettings.Kd;// - altHold.Accel * altitudeHoldSettings.Ka; + 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 // 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]; float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
stabilizationDesired.Throttle /= throttlescale; stabilizationDesired.Throttle /= throttlescale;
} else { } else {
stabilizationDesired.Throttle = -velError * altitudeHoldSettings.Kv + throttleIntegral; stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral;
} }
if (stabilizationDesired.Throttle > 1) { if (stabilizationDesired.Throttle > 1) {
@ -289,4 +299,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
AltitudeHoldSettingsGet(&altitudeHoldSettings); AltitudeHoldSettingsGet(&altitudeHoldSettings);
accelAlpha = expf(-(1000.0f/666.0f * ACCEL_DOWNSAMPLE)/ altitudeHoldSettings.AccelTau);
velAlpha = expf(-(1000.0f/666.0f * ACCEL_DOWNSAMPLE)/ altitudeHoldSettings.VelocityTau);
} }

4
flight/modules/ManualControl/manualcontrol.c
View File

@ -861,9 +861,9 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { } 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 // 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 // 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) { } 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)) { } 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 // Require the stick to enter the dead band before they can move height
// Vario is not "engaged" when throttleRate == 0 // Vario is not "engaged" when throttleRate == 0

5
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -6,9 +6,8 @@
<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0"/> <field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.03"/> <field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.03"/>
<field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/> <field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/>
<field name="PressureNoise" units="m" type="float" elements="1" defaultvalue="0.4"/> <field name="VelocityTau" units="" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelNoise" units="m/s^2" type="float" elements="1" defaultvalue="5"/> <field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelDrift" units="m/s^2" type="float" elements="1" defaultvalue="0.001"/>
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/> <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/> <field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>