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

Merge branch 'corvuscorax/Attitude-fixes' into next

Browse Source 
Conflicts:
	flight/libraries/insgps13state.c
	flight/modules/Attitude/revolution/attitude.c
This commit is contained in:
Corvus Corax 2013-05-05 23:09:07 +02:00
commit 39fe6beb4b
12 changed files with 545 additions and 235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/libraries/inc/insgps.h
View File

@ -59,11 +59,13 @@ void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed); void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]); void INSResetP(float PDiag[13]);
void INSGetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]); void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
void INSSetPosVelVar(float PosVar, float VelVar); void INSSetPosVelVar(float PosVar[3], float VelVar[3]);
void INSSetGyroBias(float gyro_bias[3]); void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]); void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]); void INSSetGyroVar(float gyro_var[3]);
void INSSetGyroBiasVar(float gyro_bias_var[3]);
void INSSetMagNorth(float B[3]); void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]); void INSSetMagVar(float scaled_mag_var[3]);
void INSSetBaroVar(float baro_var); void INSSetBaroVar(float baro_var);

33
flight/libraries/insgps13state.c
View File

@ -141,6 +141,18 @@ void INSResetP(float PDiag[NUMX])
} }
} }
void INSGetP(float PDiag[NUMX])
{
uint8_t i;
// retrieve diagonal elements (aka state variance)
for (i=0;i<NUMX;i++){
if (PDiag != 0){
PDiag[i] = P[i][i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3]) void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3])
{ {
/* Note: accel_bias not used in 13 state INS */ /* Note: accel_bias not used in 13 state INS */
@ -179,14 +191,14 @@ void INSPosVelReset(float pos[3], float vel[3])
X[5] = vel[2]; X[5] = vel[2];
} }
void INSSetPosVelVar(float PosVar, float VelVar) void INSSetPosVelVar(float PosVar[3], float VelVar[3])
{ {
R[0] = PosVar; R[0] = PosVar[0];
R[1] = PosVar; R[1] = PosVar[1];
R[2] = PosVar; R[2] = PosVar[2];
R[3] = VelVar; R[3] = VelVar[0];
R[4] = VelVar; R[4] = VelVar[1];
R[5] = VelVar; R[5] = VelVar[2];
} }
void INSSetGyroBias(float gyro_bias[3]) void INSSetGyroBias(float gyro_bias[3])
@ -210,6 +222,13 @@ void INSSetGyroVar(float gyro_var[3])
Q[2] = gyro_var[2]; Q[2] = gyro_var[2];
} }
void INSSetGyroBiasVar(float gyro_bias_var[3])
{
Q[6] = gyro_bias_var[0];
Q[7] = gyro_bias_var[1];
Q[8] = gyro_bias_var[2];
}
void INSSetMagVar(float scaled_mag_var[3]) void INSSetMagVar(float scaled_mag_var[3])
{ {
R[6] = scaled_mag_var[0]; R[6] = scaled_mag_var[0];

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

@ -66,6 +66,8 @@
#include "homelocation.h" #include "homelocation.h"
#include "magnetometer.h" #include "magnetometer.h"
#include "positionactual.h" #include "positionactual.h"
#include "ekfconfiguration.h"
#include "ekfstatevariance.h"
#include "revocalibration.h" #include "revocalibration.h"
#include "revosettings.h" #include "revosettings.h"
#include "velocityactual.h" #include "velocityactual.h"
@ -102,10 +104,15 @@ static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings; static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation; static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration; static RevoCalibrationData revoCalibration;
static EKFConfigurationData ekfConfiguration;
static RevoSettingsData revoSettings; static RevoSettingsData revoSettings;
static bool gyroBiasSettingsUpdated = false; 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 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);
@ -115,6 +122,25 @@ static void settingsUpdatedCb(UAVObjEvent * objEv);
static int32_t getNED(GPSPositionData * gpsPosition, float * NED); static int32_t getNED(GPSPositionData * gpsPosition, float * NED);
// check for invalid values
static inline bool invalid(float data) {
if ( isnan(data) || isinf(data) ){
return true;
}
return false;
}
// check for invalid variance values
static inline bool invalid_var(float data) {
if ( invalid(data) ) {
return true;
}
if ( data < 1e-15f ) { // var should not be close to zero. And not negative either.
return true;
}
return false;
}
/** /**
* API for sensor fusion algorithms: * API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro) * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
@ -138,6 +164,9 @@ int32_t AttitudeInitialize(void)
VelocityActualInitialize(); VelocityActualInitialize();
RevoSettingsInitialize(); RevoSettingsInitialize();
RevoCalibrationInitialize(); RevoCalibrationInitialize();
EKFConfigurationInitialize();
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();
@ -145,24 +174,26 @@ int32_t AttitudeInitialize(void)
// Initialize quaternion // Initialize quaternion
AttitudeActualData attitude; AttitudeActualData attitude;
AttitudeActualGet(&attitude); AttitudeActualGet(&attitude);
attitude.q1 = 1; attitude.q1 = 1.0f;
attitude.q2 = 0; attitude.q2 = 0.0f;
attitude.q3 = 0; attitude.q3 = 0.0f;
attitude.q4 = 0; attitude.q4 = 0.0f;
AttitudeActualSet(&attitude); AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs // Cannot trust the values to init right above if BL runs
GyrosBiasData gyrosBias; GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias); GyrosBiasGet(&gyrosBias);
gyrosBias.x = 0; gyrosBias.x = 0.0f;
gyrosBias.y = 0; gyrosBias.y = 0.0f;
gyrosBias.z = 0; gyrosBias.z = 0.0f;
GyrosBiasSet(&gyrosBias); GyrosBiasSet(&gyrosBias);
AttitudeSettingsConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb); HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0; return 0;
} }
@ -205,8 +236,7 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
*/ */
static void AttitudeTask(__attribute__((unused)) void *parameters) static void AttitudeTask(__attribute__((unused)) 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
@ -215,21 +245,19 @@ static void AttitudeTask(__attribute__((unused)) 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;
@ -244,8 +272,9 @@ static void AttitudeTask(__attribute__((unused)) 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);
} }
@ -282,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
@ -293,16 +320,36 @@ static int32_t updateAttitudeComplementary(bool first_run)
MagnetometerGet(&magData); MagnetometerGet(&magData);
#else #else
MagnetometerData magData; MagnetometerData magData;
magData.x = 100; magData.x = 100.0f;
magData.y = 0; magData.y = 0.0f;
magData.z = 0; magData.z = 0.0f;
#endif #endif
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
init = 0; init = 0;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z)); // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x)); // so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
@ -318,12 +365,12 @@ static int32_t updateAttitudeComplementary(bool first_run)
attitudeSettings.AccelKp = 1.0f; attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f; attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f; attitudeSettings.YawBiasRate = 0.23f;
magKp = 1; magKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f; attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f; attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f; attitudeSettings.YawBiasRate = 0.23f;
magKp = 1; magKp = 1.0f;
init = 0; init = 0;
} else if (init == 0) { } else if (init == 0) {
// Reload settings (all the rates) // Reload settings (all the rates)
@ -350,8 +397,8 @@ static int32_t updateAttitudeComplementary(bool first_run)
quat_copy(&attitudeActual.q1, q); quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels // Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err); CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
@ -387,13 +434,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
brot[2] /= bmag; brot[2] /= bmag;
// Only compute if neither vector is null // Only compute if neither vector is null
if (bmag < 1 || mag_len < 1) if (bmag < 1.0f || mag_len < 1.0f)
mag_err[0] = mag_err[1] = mag_err[2] = 0; mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
else else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err); CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
} }
} else { } else {
mag_err[0] = mag_err[1] = mag_err[2] = 0; mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
} }
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
@ -404,6 +451,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
gyrosBias.z -= mag_err[2] * magKi; gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias); GyrosBiasSet(&gyrosBias);
if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
// if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
// Correct rates based on error, integral component dealt with in updateSensors // Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
@ -412,10 +466,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s // Also accounts for the fact that gyros are in deg/s
float qdot[4]; float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * M_PI_F / 180 / 2; qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2;
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * M_PI_F / 180 / 2; qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2;
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * M_PI_F / 180 / 2; qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2;
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * M_PI_F / 180 / 2; qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2;
// Take a time step // Take a time step
q[0] = q[0] + qdot[0]; q[0] = q[0] + qdot[0];
@ -423,7 +477,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
q[2] = q[2] + qdot[2]; q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3]; q[3] = q[3] + qdot[3];
if(q[0] < 0) { if(q[0] < 0.0f) {
q[0] = -q[0]; q[0] = -q[0];
q[1] = -q[1]; q[1] = -q[1];
q[2] = -q[2]; q[2] = -q[2];
@ -440,10 +494,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
// If quaternion has become inappropriately short or is nan reinit. // If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN // THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { if((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1; q[0] = 1.0f;
q[1] = 0; q[1] = 0.0f;
q[2] = 0; q[2] = 0.0f;
q[3] = 0; q[3] = 0.0f;
} }
quat_copy(q, &attitudeActual.q1); quat_copy(q, &attitudeActual.q1);
@ -503,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;
} }
@ -537,7 +596,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
static bool gps_updated; static bool gps_updated;
static bool gps_vel_updated; static bool gps_vel_updated;
static float baroOffset = 0; static bool value_error = false;
static float baroOffset = 0.0f;
static uint32_t ins_last_time = 0; static uint32_t ins_last_time = 0;
static bool inited; static bool inited;
@ -611,119 +672,206 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
GPSVelocityGet(&gpsVelData); GPSVelocityGet(&gpsVelData);
GyrosBiasGet(&gyrosBias); GyrosBiasGet(&gyrosBias);
// Discard mag if it has NAN (normally from bad calibration) value_error = false;
mag_updated &= (!isnan(magData.x) && !isinf(magData.x) && !isnan(magData.y) && !isinf(magData.y) && !isnan(magData.z) && !isinf(magData.z)); // safety checks
if ( invalid(gyrosData.x) ||
invalid(gyrosData.y) ||
invalid(gyrosData.z) ||
invalid(accelsData.x) ||
invalid(accelsData.y) ||
invalid(accelsData.z) ) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
return 0;
}
if ( invalid(gyrosBias.x) ||
invalid(gyrosBias.y) ||
invalid(gyrosBias.z) ) {
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
}
if ( invalid(magData.x) ||
invalid(magData.y) ||
invalid(magData.z) ) {
// magnetometers can be ignored for a while
mag_updated = false;
value_error = true;
}
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false) // switching between indoor and outdoor mode with Set = false)
mag_updated &= (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] > 1e-5f); if ( (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f) ) {
mag_updated = false;
value_error = true;
}
if ( invalid(baroData.Altitude) ) {
baro_updated = false;
value_error = true;
}
if ( invalid(airspeedData.CalibratedAirspeed) ) {
airspeed_updated = false;
value_error = true;
}
if ( invalid(gpsData.Altitude) ) {
gps_updated = false;
value_error = true;
}
if ( invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN]) ) {
gps_updated = false;
value_error = true;
}
if ( invalid(gpsVelData.North) ||
invalid(gpsVelData.East) ||
invalid(gpsVelData.Down) ) {
gps_vel_updated = false;
value_error = true;
}
// Discard airspeed if sensor not connected // Discard airspeed if sensor not connected
airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ); if ( airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ) {
airspeed_updated = false;
}
// Have a minimum requirement for gps usage // Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE); if ( ( gpsData.Satellites < 7 ) ||
( gpsData.PDOP > 4.0f ) ||
( gpsData.Latitude==0 && gpsData.Longitude==0 ) ||
( homeLocation.Set != HOMELOCATION_SET_TRUE ) ) {
gps_updated = false;
gps_vel_updated = false;
}
if (!inited) if ( !inited ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else if (outdoor_mode && gpsData.Satellites < 7) } else if ( value_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else if ( variance_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else if (outdoor_mode && gpsData.Satellites < 7) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else } else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) { dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f) {
dT = 0.01f;
} else if(dT <= 0.001f) {
dT = 0.001f;
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) {
// Don't initialize until all sensors are read // Don't initialize until all sensors are read
if (init_stage == 0 && !outdoor_mode) { if (init_stage == 0) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4]; // Reset the INS algorithm
INSGPSInit();
INSSetMagVar( (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } );
INSSetAccelVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } );
INSSetGyroVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } );
INSSetGyroBiasVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } );
INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
// Initialize the gyro bias
float gyro_bias[3] = {0.0f, 0.0f, 0.0f};
INSSetGyroBias(gyro_bias);
float pos[3] = {0.0f, 0.0f, 0.0f}; float pos[3] = {0.0f, 0.0f, 0.0f};
// Initialize barometric offset to homelocation altitude if (outdoor_mode) {
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
// Reset the INS algorithm GPSPositionData gpsPosition;
INSGPSInit(); GPSPositionGet(&gpsPosition);
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
// Initialize the gyro bias from the settings // Transform the GPS position into NED coordinates
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f}; getNED(&gpsPosition, pos);
INSSetGyroBias(gyro_bias);
// Initialize barometric offset to current GPS NED coordinate
baroOffset = -pos[2] - baroData.Altitude;
} else {
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
}
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData); MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z)); AttitudeActualGet (&attitudeActual);
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x)); // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1; float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 };
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(pos, zeros, q, zeros, zeros); INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage == 0 && outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
float NED[3];
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
INSSetMagNorth(homeLocation.Be);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, NED);
// Initialize barometric offset to cirrent GPS NED coordinate INSResetP(ekfConfiguration.P);
baroOffset = -NED[2] - baroData.Altitude;
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); } else {
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(NED, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage > 0) {
// Run prediction a bit before any corrections // Run prediction a bit before any corrections
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
GyrosBiasGet(&gyrosBias); // Because the sensor module remove the bias we need to add it
float gyros[3] = {(gyrosData.x + gyrosBias.x) * M_PI_F / 180.0f, // back in here so that the INS algorithm can track it correctly
(gyrosData.y + gyrosBias.y) * M_PI_F / 180.0f, float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
(gyrosData.z + gyrosBias.z) * M_PI_F / 180.0f}; if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += DEG2RAD(gyrosBias.z);
}
INSStatePrediction(gyros, &accelsData.x, dT); INSStatePrediction(gyros, &accelsData.x, dT);
AttitudeActualData attitude; AttitudeActualData attitude;
AttitudeActualGet(&attitude); AttitudeActualGet(&attitude);
attitude.q1 = Nav.q[0]; attitude.q1 = Nav.q[0];
@ -738,38 +886,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if(init_stage > 10) if(init_stage > 10)
inited = true; inited = true;
ins_last_time = PIOS_DELAY_GetRaw();
return 0; return 0;
} }
if (!inited) if (!inited)
return 0; return 0;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f)
dT = 0.01f;
else if(dT <= 0.001f)
dT = 0.001f;
// If the gyro bias setting was updated we should reset
// the state estimate of the EKF
if(gyroBiasSettingsUpdated) {
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
gyroBiasSettingsUpdated = false;
}
// Because the sensor module remove the bias we need to add it // Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly // back in here so that the INS algorithm can track it correctly
float gyros[3] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f}; float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += gyrosBias.x * M_PI_F / 180.0f; gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += gyrosBias.y * M_PI_F / 180.0f; gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += gyrosBias.z * M_PI_F / 180.0f; gyros[2] += DEG2RAD(gyrosBias.z);
} }
// Advance the state estimate // Advance the state estimate
@ -798,14 +927,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (gps_updated && outdoor_mode) if (gps_updated && outdoor_mode)
{ {
INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]); INSSetPosVelVar((float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] });
sensors |= POS_SENSORS; sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS; sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0; vel[2] = 0.0f;
} }
// Transform the GPS position into NED coordinates // Transform the GPS position into NED coordinates
getNED(&gpsData, NED); getNED(&gpsData, NED);
@ -816,9 +950,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
* ( -NED[2] - baroData.Altitude ); * ( -NED[2] - baroData.Altitude );
} else if (!outdoor_mode) { } else if (!outdoor_mode) {
INSSetPosVelVar(1e6f, 1e5f); INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
vel[0] = vel[1] = vel[2] = 0; ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
NED[0] = NED[1] = 0; ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] });
vel[0] = vel[1] = vel[2] = 0.0f;
NED[0] = NED[1] = 0.0f;
NED[2] = -(baroData.Altitude + baroOffset); NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS |VERT_SENSORS; sensors |= POS_SENSORS |VERT_SENSORS;
@ -843,11 +982,16 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if ( !gps_vel_updated && !gps_updated) { if ( !gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance // feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS; sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar(1e6f, 1e2f); INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] });
// rotate airspeed vector into NED frame - airspeed is measured in X axis only // rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3]; float R[3][3];
Quaternion2R(Nav.q,R); Quaternion2R(Nav.q,R);
float vtas[3] = {airspeed.TrueAirspeed,0,0}; float vtas[3] = {airspeed.TrueAirspeed,0.0f,0.0f};
rot_mult(R,vtas,vel); rot_mult(R,vtas,vel);
} }
} }
@ -874,15 +1018,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
velocityActual.Down = Nav.Vel[2]; velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual); VelocityActualSet(&velocityActual);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) { gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]);
// Copy the gyro bias into the UAVO except when it was updated gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]);
// from the settings during the calculation, then consume it gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]);
// next cycle GyrosBiasSet(&gyrosBias);
gyrosBias.x = Nav.gyro_bias[0] * 180.0f / M_PI_F;
gyrosBias.y = Nav.gyro_bias[1] * 180.0f / M_PI_F; EKFStateVarianceData vardata;
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / M_PI_F; EKFStateVarianceGet(&vardata);
GyrosBiasSet(&gyrosBias); INSGetP(vardata.P);
} EKFStateVarianceSet(&vardata);
return 0; return 0;
} }
@ -912,24 +1056,58 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
static void settingsUpdatedCb(UAVObjEvent * ev) static void settingsUpdatedCb(UAVObjEvent * ev)
{ {
if (ev == NULL || ev->obj == FlightStatusHandle()) {
FlightStatusGet(&flightStatus);
}
if (ev == NULL || ev->obj == RevoCalibrationHandle()) { if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration); RevoCalibrationGet(&revoCalibration);
}
/* When the revo calibration is updated, update the GyroBias object */ // change of these settings require reinitialization of the EKF
GyrosBiasData gyrosBias; // when an error flag has been risen, we also listen to flightStatus updates,
GyrosBiasGet(&gyrosBias); // since we are waiting for the system to get disarmed so we can reinitialize safely.
gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; if (ev == NULL ||
gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; ev->obj == EKFConfigurationHandle() ||
gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; ev->obj == RevoSettingsHandle() ||
GyrosBiasSet(&gyrosBias); ( variance_error==true && ev->obj == FlightStatusHandle() )
) {
gyroBiasSettingsUpdated = true; bool error = false;
// In case INS currently running EKFConfigurationGet(&ekfConfiguration);
INSSetMagVar(revoCalibration.mag_var); int t;
INSSetAccelVar(revoCalibration.accel_var); for (t=0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
INSSetGyroVar(revoCalibration.gyro_var); if (invalid_var(ekfConfiguration.P[t])) {
INSSetBaroVar(revoCalibration.baro_var); error = true;
}
}
for (t=0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.Q[t])) {
error = true;
}
}
for (t=0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.R[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 {
// trigger reinitialization - possibly with new algorithm
running_algorithm = revoSettings.FusionAlgorithm;
variance_error = false;
initialization_required = true;
}
} }
if(ev == NULL || ev->obj == HomeLocationHandle()) { if(ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation); HomeLocationGet(&homeLocation);
@ -941,11 +1119,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);
} }
/** /**
* @} * @}

16
flight/modules/Sensors/sensors.c
View File

@ -87,6 +87,8 @@ static float mag_bias[3] = {0,0,0};
static float mag_scale[3] = {0,0,0}; static float mag_scale[3] = {0,0,0};
static float accel_bias[3] = {0,0,0}; static float accel_bias[3] = {0,0,0};
static float accel_scale[3] = {0,0,0}; static float accel_scale[3] = {0,0,0};
static float gyro_staticbias[3] = {0,0,0};
static float gyro_scale[3] = {0,0,0};
static float R[3][3] = {{0}}; static float R[3][3] = {{0}};
static int8_t rotate = 0; static int8_t rotate = 0;
@ -357,9 +359,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
float gyros[3] = {(float) gyro_accum[0] / gyro_samples, float gyros[3] = {(float) gyro_accum[0] / gyro_samples,
(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[2] / gyro_samples}; (float) gyro_accum[2] / gyro_samples};
float gyros_out[3] = {gyros[0] * gyro_scaling, float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
gyros[1] * gyro_scaling, gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
gyros[2] * gyro_scaling}; gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2]};
if (rotate) { if (rotate) {
rot_mult(R, gyros_out, gyros); rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0]; gyrosData.x = gyros[0];
@ -538,8 +540,12 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) {
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
// Do not store gyros_bias here as that comes from the state estimator and should be gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
// used from GyroBias directly gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
// Zero out any adaptive tracking // Zero out any adaptive tracking
MagBiasData magBias; MagBiasData magBias;

2
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -68,6 +68,8 @@ UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude UAVOBJSRCFILENAMES += sonaraltitude

2
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -41,7 +41,7 @@ MODULES += CameraStab
MODULES += FirmwareIAP MODULES += FirmwareIAP
MODULES += PathPlanner MODULES += PathPlanner
MODULES += FixedWingPathFollower MODULES += FixedWingPathFollower
MODULES += OveroSync #MODULES += OveroSync ## OveroSync disabled until OP292 is fixed
MODULES += Telemetry MODULES += Telemetry
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged #MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
#MODULES += Battery #MODULES += Battery

2
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -73,6 +73,8 @@ UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude UAVOBJSRCFILENAMES += sonaraltitude

62
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -41,6 +41,7 @@
#include <QDesktopServices> #include <QDesktopServices>
#include <QUrl> #include <QUrl>
#include <attitudesettings.h> #include <attitudesettings.h>
#include <ekfconfiguration.h>
#include <revocalibration.h> #include <revocalibration.h>
#include <homelocation.h> #include <homelocation.h>
#include <accels.h> #include <accels.h>
@ -212,6 +213,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
// Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
// will be dealing with some null pointers // will be dealing with some null pointers
addUAVObject("RevoCalibration"); addUAVObject("RevoCalibration");
addUAVObject("EKFConfiguration");
autoLoadWidgets(); autoLoadWidgets();
// Connect the signals // Connect the signals
@ -367,9 +369,9 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] += listMean(accel_accum_x); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] += listMean(accel_accum_x);
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += listMean(accel_accum_y); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += listMean(accel_accum_y);
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += ( listMean(accel_accum_z) + GRAVITY ); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += ( listMean(accel_accum_z) + GRAVITY );
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] = listMean(gyro_accum_x); revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] += listMean(gyro_accum_x);
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = listMean(gyro_accum_y); revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] += listMean(gyro_accum_y);
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = listMean(gyro_accum_z); revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] += listMean(gyro_accum_z);
revoCalibration->setData(revoCalibrationData); revoCalibration->setData(revoCalibrationData);
revoCalibration->updated(); revoCalibration->updated();
@ -965,20 +967,20 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
disconnect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*))); disconnect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
disconnect(mags, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*))); disconnect(mags, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(ekfConfiguration);
if(revoCalibration) { if(ekfConfiguration) {
RevoCalibration::DataFields revoCalData = revoCalibration->getData(); EKFConfiguration::DataFields revoCalData = ekfConfiguration->getData();
revoCalData.accel_var[RevoCalibration::ACCEL_VAR_X] = listVar(accel_accum_x); revoCalData.Q[EKFConfiguration::Q_ACCELX] = listVar(accel_accum_x);
revoCalData.accel_var[RevoCalibration::ACCEL_VAR_Y] = listVar(accel_accum_y); revoCalData.Q[EKFConfiguration::Q_ACCELY] = listVar(accel_accum_y);
revoCalData.accel_var[RevoCalibration::ACCEL_VAR_Z] = listVar(accel_accum_z); revoCalData.Q[EKFConfiguration::Q_ACCELZ] = listVar(accel_accum_z);
revoCalData.gyro_var[RevoCalibration::GYRO_VAR_X] = listVar(gyro_accum_x); revoCalData.Q[EKFConfiguration::Q_GYROX] = listVar(gyro_accum_x);
revoCalData.gyro_var[RevoCalibration::GYRO_VAR_Y] = listVar(gyro_accum_y); revoCalData.Q[EKFConfiguration::Q_GYROY] = listVar(gyro_accum_y);
revoCalData.gyro_var[RevoCalibration::GYRO_VAR_Z] = listVar(gyro_accum_z); revoCalData.Q[EKFConfiguration::Q_GYROZ] = listVar(gyro_accum_z);
revoCalData.mag_var[RevoCalibration::MAG_VAR_X] = listVar(mag_accum_x); revoCalData.R[EKFConfiguration::R_MAGX] = listVar(mag_accum_x);
revoCalData.mag_var[RevoCalibration::MAG_VAR_Y] = listVar(mag_accum_y); revoCalData.R[EKFConfiguration::R_MAGY] = listVar(mag_accum_y);
revoCalData.mag_var[RevoCalibration::MAG_VAR_Z] = listVar(mag_accum_z); revoCalData.R[EKFConfiguration::R_MAGZ] = listVar(mag_accum_z);
revoCalibration->setData(revoCalData); ekfConfiguration->setData(revoCalData);
} }
} }
} }
@ -989,42 +991,42 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
*/ */
void ConfigRevoWidget::drawVariancesGraph() void ConfigRevoWidget::drawVariancesGraph()
{ {
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); EKFConfiguration * ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(ekfConfiguration);
if(!revoCalibration) if(!ekfConfiguration)
return; return;
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); EKFConfiguration::DataFields ekfConfigurationData = ekfConfiguration->getData();
// The expected range is from 1E-6 to 1E-1 // The expected range is from 1E-6 to 1E-1
double steps = 6; // 6 bars on the graph double steps = 6; // 6 bars on the graph
float accel_x_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_X])); float accel_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELX]));
if(accel_x) if(accel_x)
accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false); accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false);
float accel_y_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Y])); float accel_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELY]));
if(accel_y) if(accel_y)
accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false); accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false);
float accel_z_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Z])); float accel_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELZ]));
if(accel_z) if(accel_z)
accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false); accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false);
float gyro_x_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_X])); float gyro_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROX]));
if(gyro_x) if(gyro_x)
gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false); gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false);
float gyro_y_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Y])); float gyro_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROY]));
if(gyro_y) if(gyro_y)
gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false); gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false);
float gyro_z_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Z])); float gyro_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROZ]));
if(gyro_z) if(gyro_z)
gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false); gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false);
// Scale by 1e-3 because mag vars are much higher. // Scale by 1e-3 because mag vars are much higher.
float mag_x_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_X])); float mag_x_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGX]));
if(mag_x) if(mag_x)
mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false); mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false);
float mag_y_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Y])); float mag_y_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGY]));
if(mag_y) if(mag_y)
mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false); mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false);
float mag_z_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Z])); float mag_z_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGZ]));
if(mag_z) if(mag_z)
mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false); mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false);
} }

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -33,6 +33,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
$$UAVOBJECT_SYNTHETICS/ekfconfiguration.h \
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.h \
$$UAVOBJECT_SYNTHETICS/revocalibration.h \ $$UAVOBJECT_SYNTHETICS/revocalibration.h \
$$UAVOBJECT_SYNTHETICS/revosettings.h \ $$UAVOBJECT_SYNTHETICS/revosettings.h \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \
@ -114,6 +116,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/ekfconfiguration.cpp \
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.cpp \
$$UAVOBJECT_SYNTHETICS/revocalibration.cpp \ $$UAVOBJECT_SYNTHETICS/revocalibration.cpp \
$$UAVOBJECT_SYNTHETICS/revosettings.cpp \ $$UAVOBJECT_SYNTHETICS/revosettings.cpp \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \

74
shared/uavobjectdefinition/ekfconfiguration.xml Normal file
View File

@ -0,0 +1,74 @@
<xml>
<object name="EKFConfiguration" singleinstance="true" settings="true">
<description>Extended Kalman Filter initialisation</description>
<field name="P" units="1^2" type="float" defaultvalue="
25.0, 25.0, 25.0,
5.0, 5.0, 5.0,
0.007, 0.007, 0.007, 0.007,
0.0001, 0.0001, 0.0001">
<elementnames>
<elementname>PositionNorth</elementname>
<elementname>PositionEast</elementname>
<elementname>PositionDown</elementname>
<elementname>VelocityNorth</elementname>
<elementname>VelocityEast</elementname>
<elementname>VelocityDown</elementname>
<elementname>AttitudeQ1</elementname>
<elementname>AttitudeQ2</elementname>
<elementname>AttitudeQ3</elementname>
<elementname>AttitudeQ4</elementname>
<elementname>GyroDriftX</elementname>
<elementname>GyroDriftY</elementname>
<elementname>GyroDriftZ</elementname>
</elementnames>
</field>
<field name="Q" units="1^2" type="float" defaultvalue="
0.1, 0.1, 0.1,
10.0, 10.0, 10.0,
0.00000002, 0.00000002, 0.00000002">
<elementnames>
<elementname>GyroX</elementname>
<elementname>GyroY</elementname>
<elementname>GyroZ</elementname>
<elementname>AccelX</elementname>
<elementname>AccelY</elementname>
<elementname>AccelZ</elementname>
<elementname>GyroDriftX</elementname>
<elementname>GyroDriftY</elementname>
<elementname>GyroDriftZ</elementname>
</elementnames>
</field>
<field name="R" units="1^2" type="float" defaultvalue="
1.0, 1.0, 1.0,
0.1, 0.1, 0.1,
1.0, 1.0, 1.0,
0.1">
<elementnames>
<elementname>GPSPosNorth</elementname>
<elementname>GPSPosEast</elementname>
<elementname>GPSPosDown</elementname>
<elementname>GPSVelNorth</elementname>
<elementname>GPSVelEast</elementname>
<elementname>GPSVelDown</elementname>
<elementname>MagX</elementname>
<elementname>MagY</elementname>
<elementname>MagZ</elementname>
<elementname>BaroZ</elementname>
</elementnames>
</field>
<field name="FakeR" type="float" units="1^2" defaultvalue="
1000000,
100000,
100">
<elementnames>
<elementname>FakeGPSPosIndoor</elementname>
<elementname>FakeGPSVelIndoor</elementname>
<elementname>FakeGPSVelAirspeed</elementname>
</elementnames>
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

26
shared/uavobjectdefinition/ekfstatevariance.xml Normal file
View File

@ -0,0 +1,26 @@
<xml>
<object name="EKFStateVariance" singleinstance="true" settings="false">
<description>Extended Kalman Filter state covariance</description>
<field name="P" units="1^2" type="float">
<elementnames>
<elementname>PositionNorth</elementname>
<elementname>PositionEast</elementname>
<elementname>PositionDown</elementname>
<elementname>VelocityNorth</elementname>
<elementname>VelocityEast</elementname>
<elementname>VelocityDown</elementname>
<elementname>AttitudeQ1</elementname>
<elementname>AttitudeQ2</elementname>
<elementname>AttitudeQ3</elementname>
<elementname>AttitudeQ4</elementname>
<elementname>GyroDriftX</elementname>
<elementname>GyroDriftY</elementname>
<elementname>GyroDriftZ</elementname>
</elementnames>
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

10
shared/uavobjectdefinition/revocalibration.xml
View File

@ -3,22 +3,16 @@
<description>Settings for the INS to control the algorithm and what is updated</description> <description>Settings for the INS to control the algorithm and what is updated</description>
<!-- Sensor noises --> <!-- Sensor calibration -->
<field name="accel_bias" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/> <field name="accel_bias" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="accel_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/> <field name="accel_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>
<field name="accel_var" units="(m/s)^2" type="float" elementnames="X,Y,Z" defaultvalue="0.01"/>
<field name="gyro_bias" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/> <field name="gyro_bias" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="gyro_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/> <field name="gyro_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>
<field name="gyro_var" units="(deg/s)^2" type="float" elementnames="X,Y,Z" defaultvalue="0.01"/>
<field name="gyro_tempcoeff" units="(deg/s)/deg" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/> <field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="mag_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1"/> <field name="mag_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="mag_var" units="mGau^2" type="float" elementnames="X,Y,Z" defaultvalue="0.01,0.01,10"/>
<field name="gps_var" units="m^2" type="float" elementnames="Pos,Vel" defaultvalue="1,1"/>
<field name="baro_var" units="m^2" type="float" elements="1" defaultvalue="1"/>
<!-- These settings are related to how the sensors are post processed --> <!-- These settings are related to how the sensors are post processed -->
<!-- TODO: reimplement, put elsewhere (later) -->
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/> <field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/> <field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>