From 81fcfd45c4fca2aedd42a30e60b3c78b75e5c081 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 7 Apr 2012 00:37:42 -0500 Subject: [PATCH] Some work on the code to initialize the INSGPS and allow setting the variance of the baro --- flight/Libraries/inc/insgps.h | 1 + flight/Libraries/insgps13state.c | 5 +++++ flight/Modules/Attitude/revolution/attitude.c | 16 +++++++++++----- shared/uavobjectdefinition/revocalibration.xml | 7 ++++--- 4 files changed, 21 insertions(+), 8 deletions(-) diff --git a/flight/Libraries/inc/insgps.h b/flight/Libraries/inc/insgps.h index 41f64c816..1ff6c4de7 100644 --- a/flight/Libraries/inc/insgps.h +++ b/flight/Libraries/inc/insgps.h @@ -66,6 +66,7 @@ void INSSetAccelVar(float accel_var[3]); void INSSetGyroVar(float gyro_var[3]); void INSSetMagNorth(float B[3]); void INSSetMagVar(float scaled_mag_var[3]); +void INSSetBaroVar(float baro_var); void INSPosVelReset(float pos[3], float vel[3]); void MagCorrection(float mag_data[3]); diff --git a/flight/Libraries/insgps13state.c b/flight/Libraries/insgps13state.c index 89ce0df79..f48000972 100644 --- a/flight/Libraries/insgps13state.c +++ b/flight/Libraries/insgps13state.c @@ -214,6 +214,11 @@ void INSSetMagVar(float scaled_mag_var[3]) R[8] = scaled_mag_var[2]; } +void INSSetBaroVar(float baro_var) +{ + R[9] = baro_var; +} + void INSSetMagNorth(float B[3]) { float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c index b5e18d150..e743ffc7e 100644 --- a/flight/Modules/Attitude/revolution/attitude.c +++ b/flight/Modules/Attitude/revolution/attitude.c @@ -204,6 +204,8 @@ static void AttitudeTask(void *parameters) // Main task loop while (1) { + int32_t ret_val = -1; + if (last_algorithm != revoSettings.FusionAlgorithm) { last_algorithm = revoSettings.FusionAlgorithm; first_run = true; @@ -212,20 +214,21 @@ static void AttitudeTask(void *parameters) // This function blocks on data queue switch (revoSettings.FusionAlgorithm ) { case REVOSETTINGS_FUSIONALGORITHM_COMPLIMENTARY: - updateAttitudeComplimentary(first_run); + ret_val = updateAttitudeComplimentary(first_run); break; case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: - updateAttitudeINSGPS(first_run, true); + ret_val = updateAttitudeINSGPS(first_run, true); break; case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: - updateAttitudeINSGPS(first_run, false); + ret_val = updateAttitudeINSGPS(first_run, false); break; default: AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); break; } - first_run = false; + if(ret_val == 0) + first_run = false; PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); } @@ -537,6 +540,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); + INSSetBaroVar(revoCalibration.baro_var); // Set initial attitude float rpy[3]; @@ -562,6 +566,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); + INSSetBaroVar(revoCalibration.baro_var); INSSetMagNorth(home.Be); @@ -594,7 +599,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) ins_last_time = PIOS_DELAY_GetRaw(); - return -1; + return 0; } if (!inited) @@ -755,6 +760,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); + INSSetBaroVar(revoCalibration.baro_var); T[0] = alt+6.378137E6f; T[1] = cosf(lat)*(alt+6.378137E6f); diff --git a/shared/uavobjectdefinition/revocalibration.xml b/shared/uavobjectdefinition/revocalibration.xml index 0235910eb..37e6c02db 100644 --- a/shared/uavobjectdefinition/revocalibration.xml +++ b/shared/uavobjectdefinition/revocalibration.xml @@ -6,16 +6,17 @@ - + - + - + +