diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index b7783e932..b529ffcf5 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -783,10 +783,18 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // Reset the INS algorithm INSGPSInit(); - INSSetMagVar(&ekfConfiguration.R[EKFCONFIGURATION_R_MAGX]); - INSSetAccelVar(&ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX]); - INSSetGyroVar(&ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX]); - INSSetGyroBiasVar(&ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX]); + 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 @@ -919,7 +927,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) if (gps_updated && outdoor_mode) { - INSSetPosVelVar(&ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], &ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]); + 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; if (0) { // Old code to take horizontal velocity from GPS Position update @@ -937,7 +950,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) * ( -NED[2] - baroData.Altitude ); } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]){1e6f,1e6f,1e6f}, (float[3]){1e5f,1e5f,1e5f}); + INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + 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); @@ -964,7 +982,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) if ( !gps_vel_updated && !gps_updated) { // feed airspeed into EKF, treat wind as 1e2 variance sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]){1e6f,1e6f,1e6f}, (float[3]){1e2f,1e2f,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 float R[3][3]; Quaternion2R(Nav.q,R); diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index 30b16c40f..73fafdfdc 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -56,6 +56,16 @@ BaroZ + + + FakeGPSPosIndoor + FakeGPSVelIndoor + FakeGPSVelAirspeed + +