rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1022 Fix problem with EventSystem due to too high update rate

Browse Source
This commit is contained in:
Alessio Morale 2013-07-16 11:58:20 +00:00
parent e975e4d9b7
commit 5b4d46819e
2 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -135,6 +135,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
float lastVertVelocity; float lastVertVelocity;
portTickType thisTime, lastUpdateTime; portTickType thisTime, lastUpdateTime;
UAVObjEvent ev; UAVObjEvent ev;
dT = 0;
lastVertVelocity = 0; lastVertVelocity = 0;
timeval = 0; timeval = 0;
lastUpdateTime = 0; lastUpdateTime = 0;
@ -145,7 +146,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
bool enterFailSafe = false; bool enterFailSafe = false;
// Listen for updates. // Listen for updates.
AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldDesiredConnectQueue(queue);
PositionStateConnectQueue(queue); //PositionStateConnectQueue(queue);
FlightStatusConnectQueue(queue); FlightStatusConnectQueue(queue);
VelocityStateConnectQueue(queue); VelocityStateConnectQueue(queue);
bool altitudeHoldFlightMode = false; bool altitudeHoldFlightMode = false;
@ -167,10 +168,6 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Todo: Add alarm if it should be running // Todo: Add alarm if it should be running
continue; continue;
} else if (ev.obj == PositionStateHandle()) {
posUpdated = true;
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
} else if (ev.obj == FlightStatusHandle()) { } else if (ev.obj == FlightStatusHandle()) {
FlightStatusFlightModeGet(&flightMode); FlightStatusFlightModeGet(&flightMode);
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO; altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
@ -199,8 +196,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw(); lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
} }
} else if (ev.obj == VelocityStateHandle()) { } else if (ev.obj == VelocityStateHandle()) {
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; dT = 0.1f * PIOS_DELAY_DiffuS(timeval) / 1.0e6f + 0.9f * dT;
timeval = PIOS_DELAY_GetRaw(); timeval = PIOS_DELAY_GetRaw();
AltHoldSmoothedGet(&altHold); AltHoldSmoothedGet(&altHold);
@ -213,6 +210,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
lastVertVelocity = velocityData.Down; 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); PositionStateDownGet(&altHold.Altitude);
altHold.Altitude = -altHold.Altitude; altHold.Altitude = -altHold.Altitude;
@ -244,7 +242,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
if ((thisTime - lastUpdateTime) < 20) { if ((thisTime - lastUpdateTime) < 20) {
continue; continue;
} }
altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime;
lastUpdateTime = thisTime; lastUpdateTime = thisTime;
// Instead of explicit limit on integral you output limit feedback // Instead of explicit limit on integral you output limit feedback

6
shared/uavobjectdefinition/altholdsmoothed.xml
View File

@ -2,8 +2,10 @@
<object name="AltHoldSmoothed" singleinstance="true" settings="false" category="State"> <object name="AltHoldSmoothed" singleinstance="true" settings="false" category="State">
<description>The output on the kalman filter on altitude.</description> <description>The output on the kalman filter on altitude.</description>
<field name="Altitude" units="m" type="float" elements="1"/> <field name="Altitude" units="m" type="float" elements="1"/>
<field name="Velocity" units="m/s" type="float" elements="1"/> <field name="Velocity" units="m/s" type="float" elements="1"/>
<field name="Accel" units="m/s^2" type="float" elements="1"/> <field name="Accel" units="m/s^2" type="float" elements="1"/>
<field name="StateUpdateInterval" units="ms" type="float" elements="1"/>
<field name="ThrottleUpdateInterval" units="ms" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>