rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +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) int32_t StateEstimationInitialize(void)
{ {
RevoSettingsInitialize(); RevoSettingsInitialize();
HomeLocationInitialize();
GyroSensorInitialize();
MagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
GPSPositionInitialize();
GPSVelocityInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
BaroStateInitialize();
AirspeedStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
RevoSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb); HomeLocationConnectCallback(&settingsUpdatedCb);
@ -248,7 +264,6 @@ static void StateEstimationCb(void)
// set alarm to warning if called through timeout // set alarm to warning if called through timeout
if (updatedSensors == 0) { if (updatedSensors == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
alarm = 1; alarm = 1;
} }
@ -355,22 +370,13 @@ static void StateEstimationCb(void)
// apply all filters in the current filter chain // apply all filters in the current filter chain
filterQueue *current = (filterQueue *)filterChain; filterQueue *current = (filterQueue *)filterChain;
uint8_t error = 0;
while (current != NULL) { while (current != NULL) {
int32_t result = current->filter->filter(&states); int32_t result = current->filter->filter(&states);
if (result > error) { if (result > alarm) {
error = result; alarm = result;
alarm = 1;
} }
current = current->next; 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 // the final output of filters is saved in state variables
#define STORE3(statename, shortname, a1, a2, a3) \ #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 // 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); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
} }
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);