diff --git a/conf/messages.xml b/conf/messages.xml
index 072f47a999..b08ab3fe65 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1158,7 +1158,7 @@
-
+
diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index 473c8cb80d..32e52365db 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -75,7 +75,7 @@
-
+
diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml
index 221d34bd00..0f5b813376 100644
--- a/conf/telemetry/telemetry_booz2_flixr.xml
+++ b/conf/telemetry/telemetry_booz2_flixr.xml
@@ -74,7 +74,7 @@
-
+
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index 14a590e9d7..60e8e69e85 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -473,18 +473,18 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_VFF
#include "ins/vf_float.h"
-#define PERIODIC_SEND_BOOZ2_VFF(_chan) { \
- DOWNLINK_SEND_BOOZ2_VFF(_chan, \
- &b2_vff_z_meas, \
- &b2_vff_z, \
- &b2_vff_zdot, \
- &b2_vff_bias, \
- & b2_vff_P[0][0], \
- & b2_vff_P[1][1], \
- & b2_vff_P[2][2]); \
+#define PERIODIC_SEND_VFF(_chan) { \
+ DOWNLINK_SEND_VFF(_chan, \
+ &vff_z_meas, \
+ &vff_z, \
+ &vff_zdot, \
+ &vff_bias, \
+ & vff_P[0][0], \
+ & vff_P[1][1], \
+ & vff_P[2][2]); \
}
#else
-#define PERIODIC_SEND_BOOZ2_VFF(_chan) {}
+#define PERIODIC_SEND_VFF(_chan) {}
#endif
#ifdef USE_HFF
diff --git a/sw/airborne/firmwares/rotorcraft/ins.c b/sw/airborne/firmwares/rotorcraft/ins.c
index 80859e9547..04762da494 100644
--- a/sw/airborne/firmwares/rotorcraft/ins.c
+++ b/sw/airborne/firmwares/rotorcraft/ins.c
@@ -113,7 +113,7 @@ void ins_init() {
#ifdef BOOZ2_SONAR
ins_update_on_agl = FALSE;
#endif
- b2_vff_init(0., 0., 0.);
+ vff_init(0., 0., 0.);
#endif
ins_vf_realign = FALSE;
ins_hf_realign = FALSE;
@@ -142,7 +142,7 @@ void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatV
void ins_realign_v(float z) {
#ifdef USE_VFF
- b2_vff_realign(z);
+ vff_realign(z);
#endif
}
@@ -156,10 +156,10 @@ void ins_propagate() {
#ifdef USE_VFF
if (baro.status == BS_RUNNING && ins_baro_initialised) {
- b2_vff_propagate(z_accel_float);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ vff_propagate(z_accel_float);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
@@ -196,15 +196,15 @@ void ins_update_baro() {
#ifdef BOOZ2_SONAR
ins_sonar_offset = sonar_meas;
#endif
- b2_vff_realign(0.);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ vff_realign(0.);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
ins_enu_pos.z = -ins_ltp_pos.z;
ins_enu_speed.z = -ins_ltp_speed.z;
ins_enu_accel.z = -ins_ltp_accel.z;
}
- b2_vff_update(alt_float);
+ vff_update(alt_float);
}
#endif
}
diff --git a/sw/airborne/firmwares/rotorcraft/ins/vf_float.c b/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
index 1acd5242a7..2654a7dd1c 100644
--- a/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
+++ b/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
@@ -41,24 +41,24 @@ temps :
#define Qbiasbias 1e-7
#define R 1.
-float b2_vff_z;
-float b2_vff_bias;
-float b2_vff_zdot;
-float b2_vff_zdotdot;
+float vff_z;
+float vff_bias;
+float vff_zdot;
+float vff_zdotdot;
-float b2_vff_P[B2_VFF_STATE_SIZE][B2_VFF_STATE_SIZE];
+float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
-float b2_vff_z_meas;
+float vff_z_meas;
-void b2_vff_init(float init_z, float init_zdot, float init_bias) {
- b2_vff_z = init_z;
- b2_vff_zdot = init_zdot;
- b2_vff_bias = init_bias;
+void vff_init(float init_z, float init_zdot, float init_bias) {
+ vff_z = init_z;
+ vff_zdot = init_zdot;
+ vff_bias = init_bias;
int i, j;
- for (i=0; i> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZD_FRAC - B2_VFI_Z_FRAC);
- b2_vfi_z += dz;
- const int32_t dzd = b2_vfi_zdd >> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZDD_FRAC - B2_VFI_ZD_FRAC);
- b2_vfi_zd += dzd;
+ const int32_t dz = vfi_zd >> ( VFI_F_UPDATE_FRAC + VFI_ZD_FRAC - VFI_Z_FRAC);
+ vfi_z += dz;
+ const int32_t dzd = vfi_zdd >> ( VFI_F_UPDATE_FRAC + VFI_ZDD_FRAC - VFI_ZD_FRAC);
+ vfi_zd += dzd;
// propagate covariance
- const int32_t tmp1 = b2_vfi_P[1][0] + b2_vfi_P[0][1] + (b2_vfi_P[1][1]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF00 = b2_vfi_P[0][0] + (tmp1>>B2_VFI_F_UPDATE_FRAC);
- const int32_t tmp2 = b2_vfi_P[1][1] - b2_vfi_P[0][2] - (b2_vfi_P[1][2]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF01 = b2_vfi_P[0][1] + (tmp2>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF02 = b2_vfi_P[0][2] + (b2_vfi_P[1][2] >> B2_VFI_F_UPDATE_FRAC);;
- const int32_t tmp3 = -b2_vfi_P[2][0] + b2_vfi_P[1][1] - (b2_vfi_P[2][1]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF10 = b2_vfi_P[1][0] + (tmp3>>B2_VFI_F_UPDATE_FRAC);
- const int32_t tmp4 = -b2_vfi_P[2][1] - b2_vfi_P[1][2] + (b2_vfi_P[2][2]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF11 = b2_vfi_P[1][1] + (tmp4>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF12 = b2_vfi_P[1][2] - (b2_vfi_P[2][2] >> B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF20 = b2_vfi_P[2][0] + (b2_vfi_P[2][1] >> B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF21 = b2_vfi_P[2][1] - (b2_vfi_P[2][2] >> B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF22 = b2_vfi_P[2][2];
+ const int32_t tmp1 = vfi_P[1][0] + vfi_P[0][1] + (vfi_P[1][1]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF00 = vfi_P[0][0] + (tmp1>>VFI_F_UPDATE_FRAC);
+ const int32_t tmp2 = vfi_P[1][1] - vfi_P[0][2] - (vfi_P[1][2]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF01 = vfi_P[0][1] + (tmp2>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF02 = vfi_P[0][2] + (vfi_P[1][2] >> VFI_F_UPDATE_FRAC);;
+ const int32_t tmp3 = -vfi_P[2][0] + vfi_P[1][1] - (vfi_P[2][1]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF10 = vfi_P[1][0] + (tmp3>>VFI_F_UPDATE_FRAC);
+ const int32_t tmp4 = -vfi_P[2][1] - vfi_P[1][2] + (vfi_P[2][2]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF11 = vfi_P[1][1] + (tmp4>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF12 = vfi_P[1][2] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF20 = vfi_P[2][0] + (vfi_P[2][1] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF21 = vfi_P[2][1] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF22 = vfi_P[2][2];
- b2_vfi_P[0][0] = FPF00 + VFI_QZZ;
- b2_vfi_P[0][1] = FPF01;
- b2_vfi_P[0][2] = FPF02;
- b2_vfi_P[1][0] = FPF10;
- b2_vfi_P[1][1] = FPF11 + VFI_QZDZD;
- b2_vfi_P[1][2] = FPF12;
- b2_vfi_P[2][0] = FPF20;
- b2_vfi_P[2][1] = FPF21;
- b2_vfi_P[2][2] = FPF22 + VFI_QABAB;
+ vfi_P[0][0] = FPF00 + VFI_QZZ;
+ vfi_P[0][1] = FPF01;
+ vfi_P[0][2] = FPF02;
+ vfi_P[1][0] = FPF10;
+ vfi_P[1][1] = FPF11 + VFI_QZDZD;
+ vfi_P[1][2] = FPF12;
+ vfi_P[2][0] = FPF20;
+ vfi_P[2][1] = FPF21;
+ vfi_P[2][2] = FPF22 + VFI_QABAB;
}
void vfi_update( int32_t z_meas ) {
- const int64_t y = (z_meas<<(B2_VFI_Z_FRAC-B2_VFI_MEAS_Z_FRAC)) - b2_vfi_z;
- const int32_t S = b2_vfi_P[0][0] + VFI_R;
+ const int64_t y = (z_meas<<(VFI_Z_FRAC-VFI_MEAS_Z_FRAC)) - vfi_z;
+ const int32_t S = vfi_P[0][0] + VFI_R;
- const int32_t K1 = b2_vfi_P[0][0] / S;
- const int32_t K2 = b2_vfi_P[1][0] / S;
- const int32_t K3 = b2_vfi_P[2][0] / S;
+ const int32_t K1 = vfi_P[0][0] / S;
+ const int32_t K2 = vfi_P[1][0] / S;
+ const int32_t K3 = vfi_P[2][0] / S;
- b2_vfi_z = b2_vfi_z + ((K1 * y)>>B2_VFI_P_FRAC);
- b2_vfi_zd = b2_vfi_zd + ((K2 * y)>>B2_VFI_P_FRAC);
- b2_vfi_abias = b2_vfi_abias + ((K3 * y)>>B2_VFI_P_FRAC);
+ vfi_z = vfi_z + ((K1 * y)>>VFI_P_FRAC);
+ vfi_zd = vfi_zd + ((K2 * y)>>VFI_P_FRAC);
+ vfi_abias = vfi_abias + ((K3 * y)>>VFI_P_FRAC);
#if 0
- const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][0])>>B2_VFI_P_RES;
- const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][1];
- const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][2];
- const int32_t P21 = -K2 * b2_vfi_P[0][0] + b2_vfi_P[1][0];
- const int32_t P22 = -K2 * b2_vfi_P[0][1] + b2_vfi_P[1][1];
- const int32_t P23 = -K2 * b2_vfi_P[0][2] + b2_vfi_P[1][2];
- const int32_t P31 = -K3 * b2_vfi_P[0][0] + b2_vfi_P[2][0];
- const int32_t P32 = -K3 * b2_vfi_P[0][1] + b2_vfi_P[2][1];
- const int32_t P33 = -K3 * b2_vfi_P[0][2] + b2_vfi_P[2][2];
+ const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][0])>>VFI_P_RES;
+ const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][1];
+ const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][2];
+ const int32_t P21 = -K2 * vfi_P[0][0] + vfi_P[1][0];
+ const int32_t P22 = -K2 * vfi_P[0][1] + vfi_P[1][1];
+ const int32_t P23 = -K2 * vfi_P[0][2] + vfi_P[1][2];
+ const int32_t P31 = -K3 * vfi_P[0][0] + vfi_P[2][0];
+ const int32_t P32 = -K3 * vfi_P[0][1] + vfi_P[2][1];
+ const int32_t P33 = -K3 * vfi_P[0][2] + vfi_P[2][2];
tl_vf_P[0][0] = P11;
tl_vf_P[0][1] = P12;
diff --git a/sw/airborne/firmwares/rotorcraft/ins/vf_int.h b/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
index a9faa485d9..6b34b32c2c 100644
--- a/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
+++ b/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_VF_INT_H
-#define BOOZ2_VF_INT_H
+#ifndef VF_INT_H
+#define VF_INT_H
#include "std.h"
#include "booz_geometry_int.h"
@@ -33,43 +33,43 @@ extern void vfi_propagate( int32_t accel_reading );
/* z_meas : altitude measurement in meter */
/* Q23.8 : accuracy 0.004m range 8388km */
extern void vfi_update( int32_t z_meas );
-#define B2_VFI_Z_MEAS_FRAC IPOS_FRAC
+#define VFI_Z_MEAS_FRAC IPOS_FRAC
/* propagate frequency : 512 Hz */
-#define B2_VFI_F_UPDATE_FRAC 9
-#define B2_VFI_F_UPDATE (1<