rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

Safety modification, defer any change to RevoSettings (fusionalgorithm), RevoCalibration, and EKFInitialVariance until the craft has been disarmed!

Browse Source
This commit is contained in:
Corvus Corax 2013-05-01 20:21:46 +02:00
parent e7dc665c14
commit ff5fd9c7c9
1 changed files with 59 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
flight/modules/Attitude/revolution/attitude.c
View File

@ -104,10 +104,14 @@ static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings; static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation; static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration; static RevoCalibrationData revoCalibration;
static EKFInitialVarianceData ekfInitialVariance;
static RevoSettingsData revoSettings; static RevoSettingsData revoSettings;
static FlightStatusData flightStatus;
const uint32_t SENSOR_QUEUE_SIZE = 10; const uint32_t SENSOR_QUEUE_SIZE = 10;
static bool volatile variance_error = true; static bool volatile variance_error = true;
static bool volatile initialization_required = true;
static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
// Private functions // Private functions
static void AttitudeTask(void *parameters); static void AttitudeTask(void *parameters);
@ -162,6 +166,7 @@ int32_t AttitudeInitialize(void)
RevoCalibrationInitialize(); RevoCalibrationInitialize();
EKFInitialVarianceInitialize(); EKFInitialVarianceInitialize();
EKFStateVarianceInitialize(); EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS // Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize(); HomeLocationInitialize();
@ -187,6 +192,8 @@ int32_t AttitudeInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb); HomeLocationConnectCallback(&settingsUpdatedCb);
EKFInitialVarianceConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0; return 0;
} }
@ -229,8 +236,7 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
*/ */
static void AttitudeTask(void *parameters) static void AttitudeTask(void *parameters)
{ {
bool first_run = true;
uint32_t last_algorithm;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded // Force settings update to make sure rotation loaded
@ -239,21 +245,19 @@ static void AttitudeTask(void *parameters)
// Wait for all the sensors be to read // Wait for all the sensors be to read
vTaskDelay(100); vTaskDelay(100);
// Invalidate previous algorithm to trigger a first run // Main task loop - TODO: make it run as delayed callback
last_algorithm = 0xfffffff;
// Main task loop
while (1) { while (1) {
int32_t ret_val = -1; int32_t ret_val = -1;
if (last_algorithm != revoSettings.FusionAlgorithm) { bool first_run = false;
last_algorithm = revoSettings.FusionAlgorithm; if (initialization_required) {
initialization_required = false;
first_run = true; first_run = true;
} }
// This function blocks on data queue // This function blocks on data queue
switch (revoSettings.FusionAlgorithm ) { switch (running_algorithm ) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run); ret_val = updateAttitudeComplementary(first_run);
break; break;
@ -268,8 +272,9 @@ static void AttitudeTask(void *parameters)
break; break;
} }
if(ret_val == 0) if(ret_val != 0) {
first_run = false; initialization_required = true;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
} }
@ -306,8 +311,6 @@ static int32_t updateAttitudeComplementary(bool first_run)
AccelsGet(&accelsData); AccelsGet(&accelsData);
// During initialization and // During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) { if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883) #if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading // To initialize we need a valid mag reading
@ -554,7 +557,12 @@ static int32_t updateAttitudeComplementary(bool first_run)
} }
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); if ( variance_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
return 0; return 0;
} }
@ -663,7 +671,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
invalid(accelsData.z) ) { invalid(accelsData.z) ) {
// cannot run process update, raise error! // cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
return -1; return 0;
} }
if ( invalid(gyrosBias.x) || if ( invalid(gyrosBias.x) ||
invalid(gyrosBias.y) || invalid(gyrosBias.y) ||
@ -826,9 +834,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 }; float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 };
INSSetState(pos, zeros, q, zeros, zeros); INSSetState(pos, zeros, q, zeros, zeros);
EKFInitialVarianceData vardata; INSResetP(ekfInitialVariance.P);
EKFInitialVarianceGet(&vardata);
INSResetP(vardata.P);
} else { } else {
// Run prediction a bit before any corrections // Run prediction a bit before any corrections
@ -1012,7 +1018,18 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
static void settingsUpdatedCb(UAVObjEvent * ev) static void settingsUpdatedCb(UAVObjEvent * ev)
{ {
if (ev == NULL || ev->obj == RevoCalibrationHandle()) { if (ev == NULL || ev->obj == FlightStatusHandle()) {
FlightStatusGet(&flightStatus);
}
// change of these settings require reinitialization of the EKF
// when an error flag has been risen, we also listen to flightStatus updates,
// since we are waiting for the system to get disarmed so we can reinitialize safely.
if (ev == NULL || ev->obj == RevoCalibrationHandle() ||
ev->obj == EKFInitialVarianceHandle() ||
ev->obj == RevoSettingsHandle() ||
( variance_error==true && ev->obj == FlightStatusHandle() )
) {
bool error = false; bool error = false;
RevoCalibrationGet(&revoCalibration); RevoCalibrationGet(&revoCalibration);
@ -1031,17 +1048,31 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
invalid_var( revoCalibration.baro_var ) ) { invalid_var( revoCalibration.baro_var ) ) {
error = true; error = true;
} }
EKFInitialVarianceGet(&ekfInitialVariance);
int t;
for (t=0; t < EKFINITIALVARIANCE_P_NUMELEM; t++) {
if (invalid_var(ekfInitialVariance.P[t])) {
error = true;
}
}
RevoSettingsGet(&revoSettings);
// Reinitialization of the EKF is not desired during flight.
// It will be delayed until the board is disarmed by raising the error flag.
// We will not prevent the initial initialization though, since the board could be in always armed mode.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required ) {
error = true;
}
if (error) { if (error) {
variance_error = true; variance_error = true;
} else { } else {
// In case INS currently running // trigger reinitialization - possibly with new algorithm
INSSetMagVar(revoCalibration.mag_var); running_algorithm = revoSettings.FusionAlgorithm;
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetGyroBiasVar(revoCalibration.gyro_bias_var);
INSSetBaroVar(revoCalibration.baro_var);
variance_error = false; variance_error = false;
initialization_required = true;
} }
} }
if(ev == NULL || ev->obj == HomeLocationHandle()) { if(ev == NULL || ev->obj == HomeLocationHandle()) {
@ -1054,11 +1085,12 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
T[0] = alt+6.378137E6f; T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f); T[1] = cosf(lat)*(alt+6.378137E6f);
T[2] = -1.0f; T[2] = -1.0f;
// TODO: convert positionActual to new reference frame and gracefully update EKF state!
// needed for long range flights where the reference coordinate is adjusted in flight
} }
if (ev == NULL || ev->obj == AttitudeSettingsHandle()) if (ev == NULL || ev->obj == AttitudeSettingsHandle())
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
if (ev == NULL || ev->obj == RevoSettingsHandle())
RevoSettingsGet(&revoSettings);
} }
/** /**
* @} * @}