rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Expose gyro bias process variance to settings UAVObject, code cleanup in revolution/attitude to make it more readable

Browse Source
This commit is contained in:
Corvus Corax 2013-04-28 14:46:27 +02:00
parent dff6c2cb98
commit 63c4dc1ff0
3 changed files with 48 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/libraries/insgps13state.c
View File

@ -210,6 +210,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];

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

@ -625,28 +625,59 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
else else
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
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)) { if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
// 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 Pdiag[13]={
25.0f, 25.0f, 25.0f, // initial position variance - 5 meters mean uncertainty (5²=25)
4.0f, 4.0f, 4.0f, // initial velocity variance - 2 m/s mean uncertainty
2e-6f, 2e-6f, 2e-6f, 2e-6f, // initial orientation variance - 5 deg (sin((5*(pi/180)))² ~ 2e-6)
8e-3f, 8e-3f, 8e-3f, // initial gyro drift variance - 5 deg/s ((5*(pi/180))² ~ 0.008 )
};
float q[4]; float q[4];
float pos[3] = {0.0f, 0.0f, 0.0f}; float pos[3] = {0.0f, 0.0f, 0.0f};
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
// Reset the INS algorithm // Reset the INS algorithm
INSGPSInit(); INSGPSInit();
INSSetMagVar(revoCalibration.mag_var); INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var); INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var); INSSetGyroVar(revoCalibration.gyro_var);
INSSetGyroBiasVar(revoCalibration.gyro_bias_var);
INSSetBaroVar(revoCalibration.baro_var); INSSetBaroVar(revoCalibration.baro_var);
// Initialize the gyro bias // Initialize the gyro bias
float gyro_bias[3] = {0,0,0}; float gyro_bias[3] = {0,0,0};
INSSetGyroBias(gyro_bias); INSSetGyroBias(gyro_bias);
if (outdoor_mode) {
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, pos);
// 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);
@ -664,54 +695,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
q[3] = attitudeActual.q4; q[3] = attitudeActual.q4;
INSSetState(pos, zeros, q, zeros, zeros); INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(Pdiag); INSResetP(Pdiag);
} else if (init_stage == 0 && outdoor_mode) { } else {
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] = {0,0,0};
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
baroOffset = -NED[2] - baroData.Altitude;
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
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;
// 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
@ -722,7 +707,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
gyros[2] += gyrosBias.z * M_PI_F / 180.0f; gyros[2] += gyrosBias.z * M_PI_F / 180.0f;
} }
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];
@ -737,23 +722,12 @@ 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;
// 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] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f};
@ -906,6 +880,7 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
INSSetMagVar(revoCalibration.mag_var); INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var); INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var); INSSetGyroVar(revoCalibration.gyro_var);
INSSetGyroBiasVar(revoCalibration.gyro_bias_var);
INSSetBaroVar(revoCalibration.baro_var); INSSetBaroVar(revoCalibration.baro_var);
} }
if(ev == NULL || ev->obj == HomeLocationHandle()) { if(ev == NULL || ev->obj == HomeLocationHandle()) {

2
shared/uavobjectdefinition/revocalibration.xml
View File

@ -10,7 +10,7 @@
<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.1,0.1,0.1"/> <field name="gyro_var" units="(deg/s)^2" type="float" elementnames="X,Y,Z" defaultvalue="0.1,0.1,0.1"/>
<field name="gyro_tempcoeff" units="(deg/s)/deg" type="float" elementnames="X,Y,Z" defaultvalue="1"/> <field name="gyro_bias_var" units="(deg/s)^2" type="float" elementnames="X,Y,Z" defaultvalue="0.00000001,0.00000001,0.00000001"/>
<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="1,1,1"/> <field name="mag_var" units="mGau^2" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>