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;
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

6
shared/uavobjectdefinition/altholdsmoothed.xml
View File

@ -2,8 +2,10 @@
<object name="AltHoldSmoothed" singleinstance="true" settings="false" category="State">
<description>The output on the kalman filter on altitude.</description>
<field name="Altitude" units="m" 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="Velocity" units="m/s" 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"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>