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

Some work on the code to initialize the INSGPS and allow setting the variance

Browse Source 
of the baro
This commit is contained in:
James Cotton 2012-04-07 00:37:42 -05:00
parent 345b6578b1
commit 81fcfd45c4
4 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/Libraries/inc/insgps.h
View File

@ -66,6 +66,7 @@ void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]); void INSSetGyroVar(float gyro_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 INSPosVelReset(float pos[3], float vel[3]); void INSPosVelReset(float pos[3], float vel[3]);
void MagCorrection(float mag_data[3]); void MagCorrection(float mag_data[3]);

5
flight/Libraries/insgps13state.c
View File

@ -214,6 +214,11 @@ void INSSetMagVar(float scaled_mag_var[3])
R[8] = scaled_mag_var[2]; R[8] = scaled_mag_var[2];
} }
void INSSetBaroVar(float baro_var)
{
R[9] = baro_var;
}
void INSSetMagNorth(float B[3]) void INSSetMagNorth(float B[3])
{ {
float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);

16
flight/Modules/Attitude/revolution/attitude.c
View File

@ -204,6 +204,8 @@ static void AttitudeTask(void *parameters)
// Main task loop // Main task loop
while (1) { while (1) {
int32_t ret_val = -1;
if (last_algorithm != revoSettings.FusionAlgorithm) { if (last_algorithm != revoSettings.FusionAlgorithm) {
last_algorithm = revoSettings.FusionAlgorithm; last_algorithm = revoSettings.FusionAlgorithm;
first_run = true; first_run = true;
@ -212,20 +214,21 @@ static void AttitudeTask(void *parameters)
// This function blocks on data queue // This function blocks on data queue
switch (revoSettings.FusionAlgorithm ) { switch (revoSettings.FusionAlgorithm ) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLIMENTARY: case REVOSETTINGS_FUSIONALGORITHM_COMPLIMENTARY:
updateAttitudeComplimentary(first_run); ret_val = updateAttitudeComplimentary(first_run);
break; break;
case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR:
updateAttitudeINSGPS(first_run, true); ret_val = updateAttitudeINSGPS(first_run, true);
break; break;
case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR:
updateAttitudeINSGPS(first_run, false); ret_val = updateAttitudeINSGPS(first_run, false);
break; break;
default: default:
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
break; break;
} }
first_run = false; if(ret_val == 0)
first_run = false;
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
} }
@ -537,6 +540,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetMagVar(revoCalibration.mag_var); INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var); INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var); INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
// Set initial attitude // Set initial attitude
float rpy[3]; float rpy[3];
@ -562,6 +566,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetMagVar(revoCalibration.mag_var); INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var); INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var); INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
INSSetMagNorth(home.Be); INSSetMagNorth(home.Be);
@ -594,7 +599,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
ins_last_time = PIOS_DELAY_GetRaw(); ins_last_time = PIOS_DELAY_GetRaw();
return -1; return 0;
} }
if (!inited) if (!inited)
@ -755,6 +760,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv)
INSSetMagVar(revoCalibration.mag_var); INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var); INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var); INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
T[0] = alt+6.378137E6f; T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f); T[1] = cosf(lat)*(alt+6.378137E6f);

7
shared/uavobjectdefinition/revocalibration.xml
View File

@ -6,16 +6,17 @@
<!-- Sensor noises --> <!-- Sensor noises -->
<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="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="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="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="50"/> <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="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"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>