rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

some bugfixes to make it boot on revo

Browse Source
This commit is contained in:
Corvus Corax 2013-05-20 20:18:38 +02:00
parent 0183996702
commit a04b76f836
1 changed files with 25 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -193,6 +193,22 @@ static bool sane(float value);
int32_t StateEstimationInitialize(void)
{
RevoSettingsInitialize();
HomeLocationInitialize();
GyroSensorInitialize();
MagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
GPSPositionInitialize();
GPSVelocityInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
BaroStateInitialize();
AirspeedStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
@ -248,7 +264,6 @@ static void StateEstimationCb(void)
// set alarm to warning if called through timeout
if (updatedSensors == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
alarm = 1;
}
@ -355,22 +370,13 @@ static void StateEstimationCb(void)
// apply all filters in the current filter chain
filterQueue *current = (filterQueue *)filterChain;
uint8_t error = 0;
while (current != NULL) {
int32_t result = current->filter->filter(&states);
if (result > error) {
error = result;
alarm = 1;
if (result > alarm) {
alarm = result;
}
current = current->next;
}
if (error == 1) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (error == 2) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (error == 3) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
}
// the final output of filters is saved in state variables
#define STORE3(statename, shortname, a1, a2, a3) \
@ -418,7 +424,13 @@ static void StateEstimationCb(void)
// clear alarms if everything is alright, then schedule callback execution after timeout
if (!alarm) {
if (alarm == 1) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (alarm == 2) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (alarm >= 3) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);