From 5d8f4b6cdec1ddaf29a67612685ab26e2178dad5 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 18 Jul 2016 01:31:54 +0200 Subject: [PATCH] LP-443 - Fixes to make ins13 and 14 interchangeable --- flight/libraries/inc/insgps.h | 4 +-- flight/libraries/insgps13state.c | 59 +++++++++++++++++++------------- flight/libraries/insgps14state.c | 2 +- 3 files changed, 38 insertions(+), 27 deletions(-) diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index eb3994818..fabf77b48 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -58,11 +58,11 @@ void INSGPSInit(); void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT); void INSCovariancePrediction(float dT); void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], - float BaroAlt, uint16_t SensorsUsed); + float BaroAlt, uint16_t SensorsUsed); void INSResetP(const float PDiag[13]); void INSGetVariance(float PDiag[13]); void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]); -void INSSetPosVelVar(float PosVar[3], float VelVar[3]); +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]); void INSSetGyroBias(const float gyro_bias[3]); void INSSetAccelVar(const float accel_var[3]); void INSSetGyroVar(const float gyro_var[3]); diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index c311f05d3..a81802035 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -163,7 +163,22 @@ void INSGPSInit() // pretty much just a place holder for now ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) } -void INSResetP(float PDiag[NUMX]) +// ! Set the current flight state +void INSSetArmed(bool armed) +{ + return; + + // Speed up convergence of accel and gyro bias when not armed + if (armed) { + ekf.Q[9] = 1e-4f; + ekf.Q[8] = 2e-9f; + } else { + ekf.Q[9] = 1e-2f; + ekf.Q[8] = 2e-8f; + } +} + +void INSResetP(const float PDiag[NUMX]) { uint8_t i, j; @@ -178,7 +193,7 @@ void INSResetP(float PDiag[NUMX]) } } -void INSGetP(float PDiag[NUMX]) +void INSGetVariance(float PDiag[NUMX]) { uint8_t i; @@ -189,8 +204,7 @@ void INSGetP(float PDiag[NUMX]) } } } - -void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3]) +void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], __attribute__((unused)) const float accel_bias[3]) { /* Note: accel_bias not used in 13 state INS */ ekf.X[0] = pos[0]; @@ -208,7 +222,7 @@ void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __a ekf.X[12] = gyro_bias[2]; } -void INSPosVelReset(float pos[3], float vel[3]) +void INSPosVelReset(const float pos[3], const float vel[3]) { for (int i = 0; i < 6; i++) { for (int j = i; j < NUMX; j++) { @@ -228,7 +242,7 @@ void INSPosVelReset(float pos[3], float vel[3]) ekf.X[5] = vel[2]; } -void INSSetPosVelVar(float PosVar[3], float VelVar[3]) +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]) { ekf.R[0] = PosVar[0]; ekf.R[1] = PosVar[1]; @@ -238,35 +252,35 @@ void INSSetPosVelVar(float PosVar[3], float VelVar[3]) ekf.R[5] = VelVar[2]; } -void INSSetGyroBias(float gyro_bias[3]) +void INSSetGyroBias(const float gyro_bias[3]) { ekf.X[10] = gyro_bias[0]; ekf.X[11] = gyro_bias[1]; ekf.X[12] = gyro_bias[2]; } -void INSSetAccelVar(float accel_var[3]) +void INSSetAccelVar(const float accel_var[3]) { ekf.Q[3] = accel_var[0]; ekf.Q[4] = accel_var[1]; ekf.Q[5] = accel_var[2]; } -void INSSetGyroVar(float gyro_var[3]) +void INSSetGyroVar(const float gyro_var[3]) { ekf.Q[0] = gyro_var[0]; ekf.Q[1] = gyro_var[1]; ekf.Q[2] = gyro_var[2]; } -void INSSetGyroBiasVar(float gyro_bias_var[3]) +void INSSetGyroBiasVar(const float gyro_bias_var[3]) { ekf.Q[6] = gyro_bias_var[0]; ekf.Q[7] = gyro_bias_var[1]; ekf.Q[8] = gyro_bias_var[2]; } -void INSSetMagVar(float scaled_mag_var[3]) +void INSSetMagVar(const float scaled_mag_var[3]) { ekf.R[6] = scaled_mag_var[0]; ekf.R[7] = scaled_mag_var[1]; @@ -278,16 +292,14 @@ void INSSetBaroVar(float baro_var) ekf.R[9] = baro_var; } -void INSSetMagNorth(float B[3]) +void INSSetMagNorth(const float B[3]) { - float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); - - ekf.Be[0] = B[0] * invmag; - ekf.Be[1] = B[1] * invmag; - ekf.Be[2] = B[2] * invmag; + ekf.Be[0] = B[0]; + ekf.Be[1] = B[1]; + ekf.Be[2] = B[2]; } -void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) +void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT) { float U[6]; float invqmag; @@ -371,8 +383,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt) HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR); } -void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], - float BaroAlt, uint16_t SensorsUsed) +void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], + const float BaroAlt, uint16_t SensorsUsed) { float Z[10] = { 0 }; float Y[10] = { 0 }; @@ -390,10 +402,9 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], if (SensorsUsed & MAG_SENSORS) { - float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); - Z[6] = mag_data[0] * invBmag; - Z[7] = mag_data[1] * invBmag; - Z[8] = mag_data[2] * invBmag; + Z[6] = mag_data[0]; + Z[7] = mag_data[1]; + Z[8] = mag_data[2]; } // barometric altimeter in meters and in local NED frame diff --git a/flight/libraries/insgps14state.c b/flight/libraries/insgps14state.c index 02b9b8faa..5a22a85cf 100644 --- a/flight/libraries/insgps14state.c +++ b/flight/libraries/insgps14state.c @@ -265,7 +265,7 @@ void INSPosVelReset(const float pos[3], const float vel[3]) ekf.X[4] = vel[1]; ekf.X[5] = vel[2]; } -void INSSetPosVelVar(float PosVar[3], float VelVar[3]) +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]) // void INSSetPosVelVar(float PosVar, float VelVar, float VertPosVar) { ekf.R[0] = PosVar[0];