diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index 68780181d8..d07be55acd 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -204,7 +204,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
#enable horizontal filter
- ap.CFLAGS += -DUSE_HFF -DHFF_PRESCALER=16
+ ap.CFLAGS += -DUSE_HFF -DHFF_PRESCALER=16 -DGPS_LAG=0.2
diff --git a/conf/messages.xml b/conf/messages.xml
index fdbff85508..16fe614c0d 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1086,6 +1086,12 @@
+
+
+
+
+
+
diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index 5b73971998..81a9c57048 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -89,6 +89,7 @@
+
diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml
index b225747b84..edea79fea9 100644
--- a/conf/telemetry/telemetry_booz2_flixr.xml
+++ b/conf/telemetry/telemetry_booz2_flixr.xml
@@ -89,6 +89,7 @@
+
diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index c9c9c6ff37..2f527c6fda 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -52,6 +52,9 @@ struct LtpDef_i booz_ins_ltp_def;
bool_t booz_ins_ltp_initialised;
struct NedCoor_i booz_ins_gps_pos_cm_ned;
struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
+/* horizontal gps transformed to NED in meters as float */
+struct FloatVect2 booz_ins_gps_pos_m_ned;
+struct FloatVect2 booz_ins_gps_speed_m_s_ned;
/* barometer */
#ifdef USE_VFF
@@ -143,6 +146,9 @@ void booz_ins_propagate() {
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body);
float x_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
float y_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
+#ifdef GPS_LAG
+ b2_hff_store_accel(x_accel_mean_f, y_accel_mean_f);
+#endif
if ( booz_ins_ltp_initialised ) {
/* propagate horizontal filter */
b2_hff_propagate(x_accel_mean_f, y_accel_mean_f);
@@ -202,27 +208,20 @@ void booz_ins_update_gps(void) {
booz_ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
+ VECT2_ASSIGN(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x, booz_ins_gps_pos_cm_ned.y);
+ VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
+ VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x, booz_ins_gps_speed_cm_s_ned.y);
+ VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
#ifdef USE_HFF
- struct FloatVect2 gps_float;
-#ifdef B2_HFF_UPDATE_POS
- VECT2_ASSIGN(gps_float, booz_ins_gps_pos_cm_ned.x, booz_ins_gps_pos_cm_ned.y);
- VECT2_SDIV(gps_float, gps_float, 100.);
- b2_hff_update_pos(gps_float.x, gps_float.y);
-#endif
-#ifdef B2_HFF_UPDATE_SPEED
- VECT2_ASSIGN(gps_float, booz_ins_gps_speed_cm_s_ned.x, booz_ins_gps_speed_cm_s_ned.y);
- VECT2_SDIV(gps_float, gps_float, 100.);
- b2_hff_update_v(gps_float.x, gps_float.y);
-#endif
+ b2_hff_update_gps();
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y);
-
#ifndef USE_VFF /* only hf */
booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
diff --git a/sw/airborne/booz/booz2_ins.h b/sw/airborne/booz/booz2_ins.h
index b49c7f719f..28a71ba913 100644
--- a/sw/airborne/booz/booz2_ins.h
+++ b/sw/airborne/booz/booz2_ins.h
@@ -26,6 +26,7 @@
#include "std.h"
#include "math/pprz_geodetic_int.h"
+#include "math/pprz_algebra_float.h"
/* gps transformed to LTP-NED */
extern struct LtpDef_i booz_ins_ltp_def;
@@ -49,6 +50,9 @@ extern struct NedCoor_i booz_ins_ltp_accel;
extern struct EnuCoor_i booz_ins_enu_pos;
extern struct EnuCoor_i booz_ins_enu_speed;
extern struct EnuCoor_i booz_ins_enu_accel;
+/* horizontal gps transformed to NED in meters as float */
+extern struct FloatVect2 booz_ins_gps_pos_m_ned;
+extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
extern void booz_ins_init( void );
extern void booz_ins_periodic( void );
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index 7506fca5d5..21e87d7005 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -476,9 +476,16 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
& b2_hff_yP[1][1], \
& b2_hff_yP[2][2]); \
}
+#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
+ DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
+ &lag_counter, \
+ &lag_counter_err, \
+ &save_counter); \
+ }
#else
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) {}
+#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
#endif
#define PERIODIC_SEND_BOOZ2_GUIDANCE(_chan) { \
diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c
index b6f5e98ece..962bbfa1d4 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.c
+++ b/sw/airborne/booz/ins/booz2_hf_float.c
@@ -23,6 +23,8 @@
*/
#include "booz2_hf_float.h"
+#include "booz2_ins.h"
+#include
/*
@@ -63,6 +65,52 @@ float b2_hff_ydotdot;
float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+#ifdef GPS_LAG
+/* GPS_LAG is defined in seconds
+ * GPS_LAG_PS in multiple of prescaler
+ */
+/* number of propagaton steps to redo according to GPS_LAG */
+#define GPS_LAG_N (int) (GPS_LAG * 512. / HFF_PRESCALER + 0.5)
+
+/* state and covariance when GPS was valid */
+float b2_hff_x_sav;
+float b2_hff_xbias_sav;
+float b2_hff_xdot_sav;
+float b2_hff_xdotdot_sav;
+float b2_hff_y_sav;
+float b2_hff_ybias_sav;
+float b2_hff_ydot_sav;
+float b2_hff_ydotdot_sav;
+float b2_hff_xP_sav[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+float b2_hff_yP_sav[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+
+#define BUF_MAXN GPS_LAG_N+2
+/* buffer with past mean accel values for redoing the propagation */
+struct FloatVect2 past_accel[BUF_MAXN];
+
+/* variables for accel buffer */
+uint8_t buf_r; /* pos to read from, oldest measurement */
+uint8_t buf_w; /* pos to write to */
+uint8_t buf_n; /* number of elements in buffer */
+
+/* number of propagation steps since state was saved */
+uint8_t lag_counter;
+/* by how many steps the estimated GPS validity point in time differed from GPS_LAG_N */
+int8_t lag_counter_err;
+
+/* counts down the propagation steps until the filter state is saved again */
+int8_t save_counter;
+
+static inline void b2_hff_get_past_accel(int back_n);
+static inline void b2_hff_rollback_filter(void);
+static inline void b2_hff_save_filter(void);
+#endif /* GPS_LAG */
+
+/* acceleration used in propagation step */
+float b2_hff_xaccel;
+float b2_hff_yaccel;
+
+/* last position measurement */
float b2_hff_x_meas;
float b2_hff_y_meas;
@@ -70,8 +118,8 @@ float b2_hff_y_meas;
static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias);
static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias);
-static inline void b2_hff_propagate_x(float xaccel);
-static inline void b2_hff_propagate_y(float yaccel);
+static inline void b2_hff_propagate_x(void);
+static inline void b2_hff_propagate_y(void);
static inline void b2_hff_update_x(float x_meas);
static inline void b2_hff_update_y(float y_meas);
@@ -82,8 +130,15 @@ static inline void b2_hff_update_ydot(float v);
void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias) {
- b2_hff_init_x(init_x, init_xdot, init_xbias);
- b2_hff_init_y(init_y, init_ydot, init_ybias);
+ b2_hff_init_x(init_x, init_xdot, init_xbias);
+ b2_hff_init_y(init_y, init_ydot, init_ybias);
+#ifdef GPS_LAG
+ buf_r = 0;
+ buf_w = 0;
+ buf_n = 0;
+ lag_counter = 0;
+ lag_counter_err = 0;
+#endif
}
static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias) {
@@ -115,9 +170,71 @@ static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias
else
b2_hff_yP[i][i] = INIT_PXX_BIAS;
}
-
}
+#ifdef GPS_LAG
+void b2_hff_store_accel(float x, float y) {
+ past_accel[buf_w].x = x;
+ past_accel[buf_w].y = y;
+ buf_w = (buf_w + 1) < BUF_MAXN ? (buf_w + 1) : 0;
+
+ if (buf_n < BUF_MAXN) {
+ buf_n++;
+ } else {
+ buf_r = (buf_r + 1) < BUF_MAXN ? (buf_r + 1) : 0;
+ }
+}
+
+/* get the accel values from back_n steps ago */
+static inline void b2_hff_get_past_accel(int back_n) {
+ int i;
+ if (back_n > buf_n) {
+ //printf("Cannot go back %d steps, going back only %d instead!\n", back_n, buf_n);
+ back_n = buf_n;
+ }
+ //i = (buf_r + n) < BUF_MAXN ? (buf_r + n) : (buf_r + n - BUF_MAXN);
+ i = (buf_w-1 - back_n) > 0 ? (buf_w-1 - back_n) : (buf_w-1 + BUF_MAXN - back_n);
+ b2_hff_xaccel = past_accel[i].x;
+ b2_hff_yaccel = past_accel[i].y;
+}
+
+/* rollback the state and covariance matrix to time when last saved */
+static inline void b2_hff_rollback_filter(void) {
+ b2_hff_x = b2_hff_x_sav;
+ b2_hff_xbias = b2_hff_xbias_sav;
+ b2_hff_xdot = b2_hff_xdot_sav;
+ b2_hff_xdotdot = b2_hff_xdotdot_sav;
+ b2_hff_y = b2_hff_y_sav;
+ b2_hff_ybias = b2_hff_ybias_sav;
+ b2_hff_ydot = b2_hff_ydot_sav;
+ b2_hff_ydotdot = b2_hff_ydotdot_sav;
+ for (int i=0; i < B2_HFF_STATE_SIZE; i++) {
+ for (int j=0; j < B2_HFF_STATE_SIZE; j++) {
+ b2_hff_xP[i][j] = b2_hff_xP_sav[i][j];
+ b2_hff_yP[i][j] = b2_hff_yP_sav[i][j];
+ }
+ }
+}
+
+/* save current state for later rollback */
+static inline void b2_hff_save_filter(void) {
+ b2_hff_x_sav = b2_hff_x;
+ b2_hff_xbias_sav = b2_hff_xbias;
+ b2_hff_xdot_sav = b2_hff_xdot;
+ b2_hff_xdotdot_sav = b2_hff_xdotdot;
+ b2_hff_y_sav = b2_hff_y;
+ b2_hff_ybias_sav = b2_hff_ybias;
+ b2_hff_ydot_sav = b2_hff_ydot;
+ b2_hff_ydotdot_sav = b2_hff_ydotdot;
+ for (int i=0; i < B2_HFF_STATE_SIZE; i++) {
+ for (int j=0; j < B2_HFF_STATE_SIZE; j++) {
+ b2_hff_xP_sav[i][j] = b2_hff_xP[i][j];
+ b2_hff_yP_sav[i][j] = b2_hff_yP[i][j];
+ }
+ }
+ lag_counter = 0;
+}
+#endif
/*
@@ -137,13 +254,30 @@ static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias
*/
void b2_hff_propagate(float xaccel, float yaccel) {
- b2_hff_propagate_x(xaccel);
- b2_hff_propagate_y(yaccel);
+#ifdef GPS_LAG
+ /* save filter if now is the estimated GPS validity point in time */
+ if (save_counter == 0) {
+ b2_hff_save_filter();
+ save_counter = -1;
+ } else if (save_counter > 0) {
+ save_counter--;
+ }
+#endif
+
+ b2_hff_xaccel = xaccel;
+ b2_hff_yaccel = yaccel;
+ b2_hff_propagate_x();
+ b2_hff_propagate_y();
+
+#ifdef GPS_LAG
+ if (lag_counter < 2*GPS_LAG_N) // prevent from overflowing if no gps available
+ lag_counter++;
+#endif
}
-static inline void b2_hff_propagate_x(float xaccel) {
+static inline void b2_hff_propagate_x(void) {
/* update state */
- b2_hff_xdotdot = xaccel - b2_hff_xbias;
+ b2_hff_xdotdot = b2_hff_xaccel - b2_hff_xbias;
b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot + DT_HFILTER * DT_HFILTER / 2 * b2_hff_xdotdot;
b2_hff_xdot = b2_hff_xdot + DT_HFILTER * b2_hff_xdotdot;
/* update covariance */
@@ -169,9 +303,9 @@ static inline void b2_hff_propagate_x(float xaccel) {
}
-static inline void b2_hff_propagate_y(float yaccel) {
+static inline void b2_hff_propagate_y(void) {
/* update state */
- b2_hff_ydotdot = yaccel - b2_hff_ybias;
+ b2_hff_ydotdot = b2_hff_yaccel - b2_hff_ybias;
b2_hff_y = b2_hff_y + DT_HFILTER * b2_hff_ydot;
b2_hff_ydot = b2_hff_ydot + DT_HFILTER * b2_hff_ydotdot;
/* update covariance */
@@ -198,6 +332,40 @@ static inline void b2_hff_propagate_y(float yaccel) {
}
+void b2_hff_update_gps(void) {
+#ifdef GPS_LAG
+ lag_counter_err = lag_counter - GPS_LAG_N;
+ /* roll back if state was saved approx when GPS was valid */
+ if (abs(lag_counter_err) < 3) {
+ b2_hff_rollback_filter();
+ }
+#endif
+
+#ifdef B2_HFF_UPDATE_POS
+ b2_hff_update_x(booz_ins_gps_pos_m_ned.x);
+ b2_hff_update_y(booz_ins_gps_pos_m_ned.y);
+#endif
+#ifdef B2_HFF_UPDATE_SPEED
+ b2_hff_update_xdot(booz_ins_gps_speed_m_s_ned.x);
+ b2_hff_update_ydot(booz_ins_gps_speed_m_s_ned.y);
+#endif
+
+#ifdef GPS_LAG
+ /* roll back if state was saved approx when GPS was valid */
+ if (abs(lag_counter_err) < 3) {
+ /* redo all propagation steps since GPS update */
+ for (int i=lag_counter-1; i >= 0; i--) {
+ b2_hff_get_past_accel(i);
+ b2_hff_propagate_x();
+ b2_hff_propagate_y();
+ }
+ }
+
+ /* reset save counter */
+ save_counter = (int)(HFF_FREQ / 4) % GPS_LAG_N;
+#endif
+}
+
/*
H = [1 0 0];
diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/booz/ins/booz2_hf_float.h
index 76b3c21b15..9a4c8bd48f 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.h
+++ b/sw/airborne/booz/ins/booz2_hf_float.h
@@ -24,6 +24,8 @@
#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
+#include
+
#define B2_HFF_STATE_SIZE 3
#ifndef HFF_PRESCALER
@@ -51,8 +53,16 @@ extern float b2_hff_y_meas;
extern void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias);
extern void b2_hff_propagate(float xaccel, float yaccel);
+extern void b2_hff_update_gps(void);
extern void b2_hff_update_pos(float xpos, float ypos);
extern void b2_hff_update_v(float xspeed, float yspeed);
+#ifdef GPS_LAG
+extern void b2_hff_store_accel(float x, float y);
+extern uint8_t lag_counter;
+extern int8_t lag_counter_err;
+extern int8_t save_counter;
+#endif
+
#endif /* BOOZ2_HF_FLOAT_H */