diff --git a/conf/messages.xml b/conf/messages.xml
index a520dcaa5d..f20807dacb 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1066,26 +1066,28 @@
-
+
+
-
+
+
-
-
-
+
+
+
@@ -1304,7 +1306,7 @@
-
+
@@ -1323,7 +1325,7 @@
-
+
@@ -1341,7 +1343,7 @@
-
+
@@ -1351,7 +1353,7 @@
-
+
@@ -1361,7 +1363,7 @@
-
+
@@ -1371,6 +1373,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1.h b/conf/simulator/nps/nps_sensors_params_booz2_a1.h
index 19f124f201..36db559a76 100644
--- a/conf/simulator/nps/nps_sensors_params_booz2_a1.h
+++ b/conf/simulator/nps/nps_sensors_params_booz2_a1.h
@@ -27,7 +27,7 @@
#include "airframe.h"
-#if 1
+#if 0
#define NPS_BODY_TO_IMU_PHI RadOfDeg(4.)
#define NPS_BODY_TO_IMU_THETA RadOfDeg(3.)
#define NPS_BODY_TO_IMU_PSI RadOfDeg(0.)
@@ -63,8 +63,8 @@
-/*
- * Gyrometer
+/*
+ * Gyrometer
*/
#define NPS_GYRO_RESOLUTION 65536
@@ -85,9 +85,9 @@
#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
/* s */
#define NPS_GYRO_DT (1./512.)
@@ -148,23 +148,23 @@
#define NPS_GPS_POS_BIAS_INITIAL_X 0.
#define NPS_GPS_POS_BIAS_INITIAL_Y 0.
#define NPS_GPS_POS_BIAS_INITIAL_Z 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
#define NPS_GPS_POS_LATENCY 0.
#else
-#define NPS_GPS_SPEED_NOISE_STD_DEV 1e-1
-#define NPS_GPS_SPEED_LATENCY 0.25
-#define NPS_GPS_POS_NOISE_STD_DEV 1e-1
+#define NPS_GPS_SPEED_NOISE_STD_DEV 1
+#define NPS_GPS_SPEED_LATENCY 0.8
+#define NPS_GPS_POS_NOISE_STD_DEV 2
#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1
#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1
#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
-#define NPS_GPS_POS_LATENCY 0.25
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
+#define NPS_GPS_POS_LATENCY 0.8
#endif /* GPS_PERFECT */
diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml
index edea79fea9..7840ba1199 100644
--- a/conf/telemetry/telemetry_booz2_flixr.xml
+++ b/conf/telemetry/telemetry_booz2_flixr.xml
@@ -22,15 +22,15 @@
-
+
-
-
+
+
@@ -90,6 +90,7 @@
+
diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index b6af40f28c..21f6658873 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -148,7 +148,7 @@ void booz_ins_propagate() {
booz_ins_ltp_accel.x = accel_ltp.x;
booz_ins_ltp_accel.y = accel_ltp.y;
#endif /* USE_HFF */
-
+
INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
@@ -213,7 +213,7 @@ void booz_ins_update_gps(void) {
VECT2_COPY(d_pos, booz_ins_ltp_speed);
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
VECT2_ADD(booz_ins_ltp_pos, d_pos);
-#endif
+#endif
#ifndef USE_VFF /* neither hf nor vf used */
INT32_VECT3_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, 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 d1f1f41ab1..aaf9acce03 100644
--- a/sw/airborne/booz/booz2_ins.h
+++ b/sw/airborne/booz/booz2_ins.h
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
+ * Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_INS_H
@@ -39,7 +39,7 @@ extern struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
extern int32_t booz_ins_baro_alt;
extern int32_t booz_ins_qfe;
extern bool_t booz_ins_baro_initialised;
-extern bool_t booz_ins_vff_realign;
+extern bool_t booz_ins_vff_realign;
#endif
/* output LTP NED */
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index d82007f044..d107c926f9 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -456,22 +456,25 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#include "ins/booz2_hf_float.h"
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \
- &b2_hff_x_meas, \
- &b2_hff_state.xdotdot, \
- &b2_hff_state.x, \
- &b2_hff_state.xdot, \
- &b2_hff_state.xP[0][0], \
- &b2_hff_state.xP[1][1]); \
+ &b2_hff_x_meas, \
+ &b2_hff_xd_meas, \
+ &b2_hff_state.x, \
+ &b2_hff_state.xdot, \
+ &b2_hff_state.xdotdot, \
+ &b2_hff_state.xP[0][0], \
+ &b2_hff_state.xP[1][1]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \
- DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
- &b2_hff_y_meas, \
- &b2_hff_state.ydotdot, \
- &b2_hff_state.y, \
- &b2_hff_state.ydot, \
- &b2_hff_state.yP[0][0], \
- &b2_hff_state.yP[1][1]); \
+ DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
+ &b2_hff_y_meas, \
+ &b2_hff_yd_meas, \
+ &b2_hff_state.y, \
+ &b2_hff_state.ydot, \
+ &b2_hff_state.ydotdot, \
+ &b2_hff_state.yP[0][0], \
+ &b2_hff_state.yP[1][1]); \
}
+#ifdef GPS_LAG
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
&b2_hff_rb_last->lag_counter, \
@@ -479,6 +482,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&save_counter); \
}
#else
+#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
+#endif
+#else
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
@@ -534,6 +540,19 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#define PERIODIC_SEND_BOOZ2_INS3(_chan) {}
#endif /* USE_GPS */
+#define PERIODIC_SEND_BOOZ_INS(_chan) { \
+ DOWNLINK_SEND_BOOZ_INS(_chan, \
+ &booz_ins_ltp_pos.x, \
+ &booz_ins_ltp_pos.y, \
+ &booz_ins_ltp_pos.z, \
+ &booz_ins_ltp_speed.x, \
+ &booz_ins_ltp_speed.y, \
+ &booz_ins_ltp_speed.z, \
+ &booz_ins_ltp_accel.x, \
+ &booz_ins_ltp_accel.y, \
+ &booz_ins_ltp_accel.z); \
+ }
+
#define PERIODIC_SEND_BOOZ2_INS_REF(_chan) { \
DOWNLINK_SEND_BOOZ2_INS_REF(_chan, \
&booz_ins_ltp_def.ecef.x, \
diff --git a/sw/airborne/booz/booz_ahrs.c b/sw/airborne/booz/booz_ahrs.c
index f6ff2713f5..1dca75a885 100644
--- a/sw/airborne/booz/booz_ahrs.c
+++ b/sw/airborne/booz/booz_ahrs.c
@@ -64,10 +64,14 @@ void booz_ahrs_compute_accel_mean(uint8_t n) {
if (n > rb_n) {
n = rb_n;
}
- for (i = 0; i < n; i++) {
- j = (rb_r + i) < RB_MAXN ? rb_r + i : rb_r + i - RB_MAXN;
+ for (i = 1; i <= n; i++) {
+ j = (rb_w - i) > 0 ? rb_w - i : rb_w - i + RB_MAXN;
VECT3_ADD(sum, accel_buf[j]);
}
- VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
+ if (n > 1) {
+ VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
+ } else {
+ VECT3_COPY(booz_ahrs_accel_mean, sum);
+ }
}
diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c
index 064aa8b7a7..b5af0a85bd 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.c
+++ b/sw/airborne/booz/ins/booz2_hf_float.c
@@ -27,12 +27,15 @@
#include "booz_imu.h"
#include "booz_ahrs.h"
#include
+#include
+#define PRINT_DBG 0
+
/* horizontal filter propagation frequency */
-#define HFF_FREQ 512./HFF_PRESCALER
-#define DT_HFILTER 1./HFF_FREQ
+#define HFF_FREQ (512./HFF_PRESCALER)
+#define DT_HFILTER (1./HFF_FREQ)
/* initial covariance diagonal */
#define INIT_PXX 1.
@@ -53,14 +56,15 @@
*/
/* output filter states */
struct HfilterFloat b2_hff_state;
-/* pointer to filter states to operate on */
-//struct HfilterFloat *b2_hff_work;
+/* last acceleration measurement */
+float b2_hff_xdd_meas;
+float b2_hff_ydd_meas;
-/* acceleration used in propagation step */
-float b2_hff_xaccel;
-float b2_hff_yaccel;
+/* last velocity measurement */
+float b2_hff_xd_meas;
+float b2_hff_yd_meas;
/* last position measurement */
float b2_hff_x_meas;
@@ -76,7 +80,6 @@ int b2_hff_ps_counter;
*
*
*/
-
#ifdef GPS_LAG
/*
* GPS_LAG is defined in seconds in airframe file
@@ -88,16 +91,19 @@ int b2_hff_ps_counter;
#define GPS_DT_N ((int) (HFF_FREQ / 4))
/* maximum number of past propagation steps to rerun per ap cycle
- * make sure GPS_LAG_N/MAX_PP_STEPS < GPS_DT_N
+ * make sure GPS_LAG_N/MAX_PP_STEPS < 128
+ * GPS_LAG_N/MAX_PP_STEPS/512 = seconds until re-propagation catches up with the present
*/
#define MAX_PP_STEPS 6
/* variables for accel buffer */
-#define ACC_BUF_MAXN GPS_LAG_N+10
+#define ACC_BUF_MAXN (GPS_LAG_N+10)
+#define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; }
+
struct FloatVect2 past_accel[ACC_BUF_MAXN]; /* buffer with past mean accel values for redoing the propagation */
-uint8_t acc_buf_r; /* pos to read from, oldest measurement */
-uint8_t acc_buf_w; /* pos to write to */
-uint8_t acc_buf_n; /* number of elements in buffer */
+unsigned int acc_buf_r; /* pos to read from, oldest measurement */
+unsigned int acc_buf_w; /* pos to write to */
+unsigned int acc_buf_n; /* number of elements in buffer */
/*
@@ -106,9 +112,9 @@ uint8_t acc_buf_n; /* number of elements in buffer */
#define HFF_RB_MAXN ((int) (GPS_LAG * 4))
#define INC_RB_POINTER(ptr) { \
if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \
- ptr = b2_hff_rb; \
- else \
- ptr++; \
+ ptr = b2_hff_rb; \
+ else \
+ ptr++; \
}
struct HfilterFloat b2_hff_rb[HFF_RB_MAXN]; /* ringbuffer with state and covariance when GPS was valid */
@@ -120,16 +126,16 @@ int b2_hff_rb_n; /* fill count */
/* by how many steps the estimated GPS validity point in time differed from GPS_LAG_N */
-int8_t lag_counter_err;
+int lag_counter_err;
/* counts down the propagation steps until the filter state is saved again */
-int8_t save_counter;
-int8_t past_save_counter;
+int save_counter;
+int past_save_counter;
#ifdef GPS_LAG
-static inline void b2_hff_get_past_accel(int back_n);
+static inline void b2_hff_get_past_accel(unsigned int back_n);
static inline void b2_hff_rb_put_state(struct HfilterFloat* source);
-static inline void b2_hff_rb_next(void);
+static inline void b2_hff_rb_drop_last(void);
static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source);
#endif
@@ -158,14 +164,24 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
acc_buf_n = 0;
b2_hff_rb_put = b2_hff_rb;
b2_hff_rb_last = b2_hff_rb;
+ b2_hff_rb_last->rollback = FALSE;
+ b2_hff_rb_last->lag_counter = 0;
b2_hff_state.lag_counter = GPS_LAG_N;
+#ifdef SITL
+ printf("GPS_LAG: %f\n", GPS_LAG);
+ printf("GPS_LAG_N: %d\n", GPS_LAG_N);
+ printf("GPS_DT_N: %d\n", GPS_DT_N);
+ printf("DT_HFILTER: %f\n", DT_HFILTER);
+#endif
#else
- b2_hff_rb_last = NULL;
+ b2_hff_rb_last = &b2_hff_state;
b2_hff_state.lag_counter = 0;
#endif
b2_hff_rb_n = 0;
- b2_hff_state.rollback = 0;
+ b2_hff_state.rollback = FALSE;
lag_counter_err = 0;
+ save_counter = -1;
+ past_save_counter = -1;
b2_hff_ps_counter = 1;
}
@@ -196,35 +212,40 @@ static inline void b2_hff_init_y(float init_y, float init_ydot) {
void b2_hff_store_accel(float x, float y) {
past_accel[acc_buf_w].x = x;
past_accel[acc_buf_w].y = y;
- acc_buf_w = (acc_buf_w + 1) < ACC_BUF_MAXN ? (acc_buf_w + 1) : 0;
+ INC_ACC_IDX(acc_buf_w);
if (acc_buf_n < ACC_BUF_MAXN) {
acc_buf_n++;
} else {
- acc_buf_r = (acc_buf_r + 1) < ACC_BUF_MAXN ? (acc_buf_r + 1) : 0;
+ INC_ACC_IDX(acc_buf_r);
}
}
/* get the accel values from back_n steps ago */
-static inline void b2_hff_get_past_accel(int back_n) {
+static inline void b2_hff_get_past_accel(unsigned int back_n) {
int i;
if (back_n > acc_buf_n) {
- //printf("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n);
+ if (PRINT_DBG)
+ printf("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n);
back_n = acc_buf_n;
- } else if (back_n <= 0) {
- //printf("Cannot go back %d steps, not getting past accel value!\n", back_n);
+ } else if (back_n == 0) {
+ if (PRINT_DBG)
+ printf("Cannot go back zero steps!\n");
return;
}
- i = (acc_buf_w - back_n) > 0 ? (acc_buf_w - back_n) : (acc_buf_w + ACC_BUF_MAXN - back_n);
- b2_hff_xaccel = past_accel[i].x;
- b2_hff_yaccel = past_accel[i].y;
+ if ((int)(acc_buf_w - back_n) < 0)
+ i = acc_buf_w - back_n + ACC_BUF_MAXN;
+ else
+ i = acc_buf_w - back_n;
+ b2_hff_xdd_meas = past_accel[i].x;
+ b2_hff_ydd_meas = past_accel[i].y;
}
static inline void b2_hff_rb_put_state(struct HfilterFloat* source) {
/* copy state from source into buffer */
b2_hff_set_state(b2_hff_rb_put, source);
b2_hff_rb_put->lag_counter = 0;
- b2_hff_rb_put->rollback = 0;
+ b2_hff_rb_put->rollback = FALSE;
/* forward write pointer */
INC_RB_POINTER(b2_hff_rb_put);
@@ -235,15 +256,22 @@ static inline void b2_hff_rb_put_state(struct HfilterFloat* source) {
} else {
INC_RB_POINTER(b2_hff_rb_last);
}
+ if (PRINT_DBG)
+ printf("put state. fill count now: %d\n", b2_hff_rb_n);
}
-static inline void b2_hff_rb_next(void) {
+static inline void b2_hff_rb_drop_last(void) {
if (b2_hff_rb_n > 0) {
INC_RB_POINTER(b2_hff_rb_last);
b2_hff_rb_n--;
} else {
- //printf("hff ringbuffer empty!\n");
+ if (PRINT_DBG)
+ printf("hff ringbuffer empty!\n");
+ b2_hff_rb_last->lag_counter = 0;
+ b2_hff_rb_last->rollback = FALSE;
}
+ if (PRINT_DBG)
+ printf("drop last state. fill count now: %d\n", b2_hff_rb_n);
}
@@ -264,24 +292,41 @@ static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFlo
}
static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
+ if (PRINT_DBG)
+ printf("enter propagate past: %d\n", hff_past->lag_counter);
/* run max MAX_PP_STEPS propagation steps */
for (int i=0; i < MAX_PP_STEPS; i++) {
- b2_hff_get_past_accel(hff_past->lag_counter);
- b2_hff_propagate_x(hff_past);
- b2_hff_propagate_y(hff_past);
- hff_past->lag_counter--;
+ if (hff_past->lag_counter > 0) {
+ b2_hff_get_past_accel(hff_past->lag_counter);
+ //if (PRINT_DBG)
+ //printf("propagate past: %d\n", hff_past->lag_counter);
+ b2_hff_propagate_x(hff_past);
+ b2_hff_propagate_y(hff_past);
+ hff_past->lag_counter--;
- if (past_save_counter == 0) {
- /* next GPS measurement valid at this state -> save */
- b2_hff_rb_put_state(hff_past);
- past_save_counter = -1;
- } else if (save_counter > 0) {
- past_save_counter--;
+ if (past_save_counter == 0) {
+ /* next GPS measurement valid at this state -> save */
+ if (PRINT_DBG)
+ printf("save past state\n");
+ b2_hff_rb_put_state(hff_past);
+ past_save_counter = -1;
+ } else if (past_save_counter > 0) {
+ past_save_counter--;
+ //if (PRINT_DBG)
+ //printf("dec past_save_counter: %d\n", past_save_counter);
+ } else {
+ /* increase lag counter on if next state is already saved */
+ if (hff_past == &b2_hff_rb[HFF_RB_MAXN-1])
+ b2_hff_rb[0].lag_counter++;
+ else
+ (hff_past+1)->lag_counter++;
+ }
}
+ /* finished re-propagating the past values */
if (hff_past->lag_counter == 0) {
b2_hff_set_state(&b2_hff_state, hff_past);
- b2_hff_rb_next();
+ b2_hff_rb_drop_last();
break;
}
}
@@ -292,7 +337,7 @@ static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
void b2_hff_propagate(void) {
#ifdef GPS_LAG
- /* continue to redo the propagation to catch up with the present */
+ /* continue re-propagating to catch up with the present */
if (b2_hff_rb_last->rollback) {
b2_hff_propagate_past(b2_hff_rb_last);
}
@@ -308,8 +353,11 @@ void b2_hff_propagate(void) {
INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, booz_ahrs_accel_mean);
struct Int32Vect3 mean_accel_ltp;
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body);
- b2_hff_xaccel = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
- b2_hff_yaccel = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
+ b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
+ b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
+#ifdef GPS_LAG
+ b2_hff_store_accel(b2_hff_xdd_meas, b2_hff_ydd_meas);
+#endif
/*
* propagate current state
@@ -319,14 +367,14 @@ void b2_hff_propagate(void) {
b2_hff_propagate_y(&b2_hff_state);
}
#ifdef GPS_LAG
- b2_hff_store_accel(b2_hff_xaccel, b2_hff_yaccel);
- /* increase all lag counters */
- for (int i=0; i < b2_hff_rb_n; i++ ) {
- (b2_hff_rb_last+i)->lag_counter++;
- }
+ /* increase lag counter on last saved state */
+ if (b2_hff_rb_n > 0)
+ b2_hff_rb_last->lag_counter++;
- /* save filter state if ringbuffer is empty */
+ /* save filter state if needed */
if (save_counter == 0) {
+ if (PRINT_DBG)
+ printf("save current state\n");
b2_hff_rb_put_state(&b2_hff_state);
save_counter = -1;
} else if (save_counter > 0) {
@@ -348,24 +396,30 @@ void b2_hff_update_gps(void) {
if (b2_hff_rb_n > 0) {
/* roll back if state was saved approx when GPS was valid */
lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
+ //if (PRINT_DBG)
+ //printf("update. rb_n: %d lag cnt err: %d\n", b2_hff_rb_n, lag_counter_err);
if (abs(lag_counter_err) < 3) {
- b2_hff_rb_last->rollback = 1;
+ b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, booz_ins_gps_pos_m_ned.x);
b2_hff_update_y(b2_hff_rb_last, booz_ins_gps_pos_m_ned.y);
#ifdef B2_HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.x);
b2_hff_update_ydot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.y);
#endif
- past_save_counter = GPS_DT_N + lag_counter_err;
+ past_save_counter = GPS_DT_N-1 + lag_counter_err;
+ if (PRINT_DBG)
+ printf("gps updated. past_save_counter: %d\n", past_save_counter);
b2_hff_propagate_past(b2_hff_rb_last);
} else if (lag_counter_err >= GPS_DT_N-2) {
/* apparently missed a GPS update, try next saved state */
- b2_hff_rb_next();
+ b2_hff_rb_drop_last();
b2_hff_update_gps();
}
- } else {
+ } else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
- save_counter = GPS_DT_N - (GPS_LAG_N % GPS_DT_N);
+ save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
+ if (PRINT_DBG)
+ printf("rb empty, save counter set: %d\n", save_counter);
}
#else /* GPS_LAG */
@@ -403,7 +457,7 @@ void b2_hff_update_gps(void) {
*/
static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) {
/* update state */
- hff_work->xdotdot = b2_hff_xaccel;
+ hff_work->xdotdot = b2_hff_xdd_meas;
hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot;
hff_work->xdot = hff_work->xdot + DT_HFILTER * hff_work->xdotdot;
/* update covariance */
@@ -420,8 +474,8 @@ static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) {
static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work) {
/* update state */
- hff_work->ydotdot = b2_hff_yaccel;
- hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot;
+ hff_work->ydotdot = b2_hff_ydd_meas;
+ hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot + DT_HFILTER*DT_HFILTER/2 * hff_work->ydotdot;
hff_work->ydot = hff_work->ydot + DT_HFILTER * hff_work->ydotdot;
/* update covariance */
const float FPF00 = hff_work->yP[0][0] + DT_HFILTER * ( hff_work->yP[1][0] + hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1] );
@@ -532,6 +586,8 @@ void b2_hff_update_v(float xspeed, float yspeed) {
}
static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float v) {
+ b2_hff_xd_meas = v;
+
const float yd = v - hff_work->xdot;
const float S = hff_work->xP[1][1] + Rspeed;
const float K1 = hff_work->xP[0][1] * 1/S;
@@ -552,6 +608,8 @@ static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float v) {
}
static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float v) {
+ b2_hff_yd_meas = v;
+
const float yd = v - hff_work->ydot;
const float S = hff_work->yP[1][1] + Rspeed;
const float K1 = hff_work->yP[0][1] * 1/S;
diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/booz/ins/booz2_hf_float.h
index 7fa91b41bd..82cbc771bf 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.h
+++ b/sw/airborne/booz/ins/booz2_hf_float.h
@@ -24,7 +24,8 @@
#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
-#include
+#include "std.h"
+#include "math/pprz_algebra_float.h"
#define B2_HFF_STATE_SIZE 3
@@ -46,26 +47,31 @@ struct HfilterFloat {
float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
uint8_t lag_counter;
- uint8_t rollback;
+ bool_t rollback;
};
extern struct HfilterFloat b2_hff_state;
extern float b2_hff_x_meas;
extern float b2_hff_y_meas;
+extern float b2_hff_xd_meas;
+extern float b2_hff_yd_meas;
+extern float b2_hff_xdd_meas;
+extern float b2_hff_ydd_meas;
extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
extern void b2_hff_propagate(void);
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);
+extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 speed);
#ifdef GPS_LAG
extern void b2_hff_store_accel(float x, float y);
#endif
extern struct HfilterFloat *b2_hff_rb_last;
-extern int8_t lag_counter_err;
-extern int8_t save_counter;
+extern int lag_counter_err;
+extern int save_counter;
#endif /* BOOZ2_HF_FLOAT_H */