rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +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 HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static EKFInitialVarianceData ekfInitialVariance;
static RevoSettingsData revoSettings;
static FlightStatusData flightStatus;
const uint32_t SENSOR_QUEUE_SIZE = 10;
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
static void AttitudeTask(void *parameters);
@ -162,6 +166,7 @@ int32_t AttitudeInitialize(void)
RevoCalibrationInitialize();
EKFInitialVarianceInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
@ -187,6 +192,8 @@ int32_t AttitudeInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFInitialVarianceConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -229,8 +236,7 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
*/
static void AttitudeTask(void *parameters)
{
bool first_run = true;
uint32_t last_algorithm;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// 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
vTaskDelay(100);
// Invalidate previous algorithm to trigger a first run
last_algorithm = 0xfffffff;
// Main task loop
// Main task loop - TODO: make it run as delayed callback
while (1) {
int32_t ret_val = -1;
if (last_algorithm != revoSettings.FusionAlgorithm) {
last_algorithm = revoSettings.FusionAlgorithm;
bool first_run = false;
if (initialization_required) {
initialization_required = false;
first_run = true;
}
// This function blocks on data queue
switch (revoSettings.FusionAlgorithm ) {
switch (running_algorithm ) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
@ -268,8 +272,9 @@ static void AttitudeTask(void *parameters)
break;
}
if(ret_val == 0)
first_run = false;
if(ret_val != 0) {
initialization_required = true;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
@ -306,8 +311,6 @@ static int32_t updateAttitudeComplementary(bool first_run)
AccelsGet(&accelsData);
// During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// 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;
}
@ -663,7 +671,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
invalid(accelsData.z) ) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
return -1;
return 0;
}
if ( invalid(gyrosBias.x) ||
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 };
INSSetState(pos, zeros, q, zeros, zeros);
EKFInitialVarianceData vardata;
EKFInitialVarianceGet(&vardata);
INSResetP(vardata.P);
INSResetP(ekfInitialVariance.P);
} else {
// Run prediction a bit before any corrections
@ -1012,7 +1018,18 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
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;
RevoCalibrationGet(&revoCalibration);
@ -1031,17 +1048,31 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
invalid_var( revoCalibration.baro_var ) ) {
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) {
variance_error = true;
} else {
// In case INS currently running
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetGyroBiasVar(revoCalibration.gyro_bias_var);
INSSetBaroVar(revoCalibration.baro_var);
// trigger reinitialization - possibly with new algorithm
running_algorithm = revoSettings.FusionAlgorithm;
variance_error = false;
initialization_required = true;
}
}
if(ev == NULL || ev->obj == HomeLocationHandle()) {
@ -1054,11 +1085,12 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f);
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())
AttitudeSettingsGet(&attitudeSettings);
if (ev == NULL || ev->obj == RevoSettingsHandle())
RevoSettingsGet(&revoSettings);
}
/**
* @}