diff --git a/conf/modules/ins_extended.xml b/conf/modules/ins_extended.xml
index 0c4c39d343..dadc6512ef 100644
--- a/conf/modules/ins_extended.xml
+++ b/conf/modules/ins_extended.xml
@@ -15,6 +15,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c
index e435aaa286..20e418108a 100644
--- a/sw/airborne/subsystems/ins/vf_extended_float.c
+++ b/sw/airborne/subsystems/ins/vf_extended_float.c
@@ -96,6 +96,11 @@ void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_b
vff.P[i][i] = VFF_EXTENDED_INIT_PXX;
}
+ vff.accel_noise = VFF_EXTENDED_ACCEL_NOISE;
+ vff.r_baro = R_BARO;
+ vff.r_alt = R_ALT;
+ vff.r_offset = R_OFFSET;
+
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VFF_EXTENDED, send_vffe);
#endif
@@ -142,11 +147,11 @@ void vff_propagate(float accel, float dt)
const float FPF22 = vff.P[2][2];
const float FPF33 = vff.P[3][3];
- vff.P[0][0] = FPF00 + VFF_EXTENDED_ACCEL_NOISE * dt * dt / 2.;
+ vff.P[0][0] = FPF00 + vff.accel_noise * dt * dt / 2.;
vff.P[0][1] = FPF01;
vff.P[0][2] = FPF02;
vff.P[1][0] = FPF10;
- vff.P[1][1] = FPF11 + VFF_EXTENDED_ACCEL_NOISE * dt;
+ vff.P[1][1] = FPF11 + vff.accel_noise * dt;
vff.P[1][2] = FPF12;
vff.P[2][0] = FPF20;
vff.P[2][1] = FPF21;
@@ -215,7 +220,7 @@ static void update_baro_conf(float z_meas, float conf)
void vff_update_baro(float z_meas)
{
- update_baro_conf(z_meas, R_BARO);
+ update_baro_conf(z_meas, vff.r_baro);
}
void vff_update_baro_conf(float z_meas, float conf)
@@ -278,7 +283,7 @@ static void update_alt_conf(float z_meas, float conf)
void vff_update_z(float z_meas)
{
- update_alt_conf(z_meas, R_ALT);
+ update_alt_conf(z_meas, vff.r_alt);
}
void vff_update_z_conf(float z_meas, float conf)
@@ -340,7 +345,7 @@ static void update_offset_conf(float offset, float conf)
void vff_update_offset(float offset)
{
- update_offset_conf(offset, R_OFFSET);
+ update_offset_conf(offset, vff.r_offset);
}
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.h b/sw/airborne/subsystems/ins/vf_extended_float.h
index fc50b9a33f..09d069fa40 100644
--- a/sw/airborne/subsystems/ins/vf_extended_float.h
+++ b/sw/airborne/subsystems/ins/vf_extended_float.h
@@ -44,6 +44,11 @@ struct VffExtended {
float z_meas_baro; ///< last z measurement from baro in m
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]; ///< covariance matrix
+
+ float accel_noise;
+ float r_baro;
+ float r_alt;
+ float r_offset;
};
extern struct VffExtended vff;