rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1750 - don't waste cycles by rescheduling each StateEstimation sub-step

Browse Source
This commit is contained in:
Alessio Morale 2015-02-26 06:58:57 +01:00
parent 362c0c67b7
commit 86111ddefd
1 changed files with 149 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
flight/modules/StateEstimation/stateestimation.c
View File

@ -345,7 +345,6 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
*/
static void StateEstimationCb(void)
{
static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD;
static filterResult alarm = FILTERRESULT_OK;
static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
static uint16_t alarmcounter = 0;
@ -361,9 +360,6 @@ static void StateEstimationCb(void)
return;
}
switch (runState) {
case RUNSTATE_LOAD:
alarm = FILTERRESULT_OK;
// set alarm to warning if called through timeout
@ -448,13 +444,8 @@ static void StateEstimationCb(void)
current = filterChain;
// we are not done, re-dispatch self execution
runState = RUNSTATE_FILTER;
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_FILTER:
if (current != NULL) {
while (current) {
filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) {
alarm = result;
@ -462,15 +453,6 @@ static void StateEstimationCb(void)
current = current->next;
}
// we are not done, re-dispatch self execution
if (!current) {
runState = RUNSTATE_SAVE;
}
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_SAVE:
// the final output of filters is saved in state variables
// EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
@ -513,7 +495,6 @@ static void StateEstimationCb(void)
Quaternion2RPY(&s.q1, &s.Roll);
AttitudeStateSet(&s);
}
// throttle alarms, raise alarm flags immediately
// but require system to run for a while before decreasing
// to prevent alarm flapping
@ -540,15 +521,11 @@ static void StateEstimationCb(void)
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
// we are done, re-schedule next self execution
runState = RUNSTATE_LOAD;
if (updatedSensors) {
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
} else {
PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
}
break;
}
}