diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c index 2f527c6fda..fbc246aeb3 100644 --- a/sw/airborne/booz/booz2_ins.c +++ b/sw/airborne/booz/booz2_ins.c @@ -153,12 +153,12 @@ void booz_ins_propagate() { /* propagate horizontal filter */ b2_hff_propagate(x_accel_mean_f, y_accel_mean_f); /* update ins state from horizontal filter */ - 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); + booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); + booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); + booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); + booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); + booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); + booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); } } } else { @@ -216,12 +216,12 @@ void booz_ins_update_gps(void) { #ifdef USE_HFF 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); + booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); + booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); + booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); + booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); + booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); + booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.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_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index 21e87d7005..f404366868 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -457,28 +457,28 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \ DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \ &b2_hff_x_meas, \ - &b2_hff_xdotdot, \ - &b2_hff_x, \ - &b2_hff_xdot, \ - &b2_hff_xbias, \ - & b2_hff_xP[0][0], \ - & b2_hff_xP[1][1], \ - & b2_hff_xP[2][2]); \ + &b2_hff_state.xdotdot, \ + &b2_hff_state.x, \ + &b2_hff_state.xdot, \ + &b2_hff_state.xbias, \ + &b2_hff_state.xP[0][0], \ + &b2_hff_state.xP[1][1], \ + &b2_hff_state.xP[2][2]); \ } #define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \ DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \ &b2_hff_y_meas, \ - &b2_hff_ydotdot, \ - &b2_hff_y, \ - &b2_hff_ydot, \ - &b2_hff_ybias, \ - & b2_hff_yP[0][0], \ - & b2_hff_yP[1][1], \ - & b2_hff_yP[2][2]); \ + &b2_hff_state.ydotdot, \ + &b2_hff_state.y, \ + &b2_hff_state.ydot, \ + &b2_hff_state.ybias, \ + &b2_hff_state.yP[0][0], \ + &b2_hff_state.yP[1][1], \ + &b2_hff_state.yP[2][2]); \ } #define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \ DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \ - &lag_counter, \ + &b2_hff_save.lag_counter, \ &lag_counter_err, \ &save_counter); \ } diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c index 962bbfa1d4..33970c029d 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.c +++ b/sw/airborne/booz/ins/booz2_hf_float.c @@ -50,20 +50,26 @@ X_y = [ y ydot ybias ] #define Rpos 5. #define Rspeed 1. -/* filter states */ -float b2_hff_x; -float b2_hff_xbias; -float b2_hff_xdot; -float b2_hff_xdotdot; +/* output filter states */ +struct hfilter_f b2_hff_state; +/* saved state when gps was valid */ +struct hfilter_f b2_hff_save; +/* pointer to filter states to operate on */ +struct hfilter_f *b2_hff_work; -float b2_hff_y; -float b2_hff_ybias; -float b2_hff_ydot; -float b2_hff_ydotdot; +/* float b2_hff_x; */ +/* float b2_hff_xbias; */ +/* float b2_hff_xdot; */ +/* float b2_hff_xdotdot; */ + +/* float b2_hff_y; */ +/* float b2_hff_ybias; */ +/* float b2_hff_ydot; */ +/* float b2_hff_ydotdot; */ /* filter covariance matrices */ -float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; -float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; +/* 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 @@ -73,16 +79,16 @@ float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; #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]; +/* 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 */ @@ -94,7 +100,7 @@ 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; +/* 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; @@ -132,43 +138,45 @@ 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_work = &b2_hff_state; #ifdef GPS_LAG buf_r = 0; buf_w = 0; buf_n = 0; - lag_counter = 0; + b2_hff_save.lag_counter = 0; + b2_hff_state.lag_counter = GPS_LAG_N; lag_counter_err = 0; #endif } static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias) { - b2_hff_x = init_x; - b2_hff_xdot = init_xdot; - b2_hff_xbias = init_xbias; + b2_hff_state.x = init_x; + b2_hff_state.xdot = init_xdot; + b2_hff_state.xbias = init_xbias; int i, j; for (i=0; ix; + b2_hff_state.xbias = source->xbias; + b2_hff_state.xdot = source->xdot; + b2_hff_state.xdotdot = source->xdotdot; + b2_hff_state.y = source->y; + b2_hff_state.ybias = source->ybias; + b2_hff_state.ydot = source->ydot; + b2_hff_state.ydotdot = source->ydotdot; + for (int i=0; i < B2_HFF_STATE_SIZE; i++) { + for (int j=0; j < B2_HFF_STATE_SIZE; j++) { + b2_hff_state.xP[i][j] = source->xP[i][j]; + b2_hff_state.yP[i][j] = source->yP[i][j]; } } - lag_counter = 0; } #endif @@ -270,71 +297,71 @@ void b2_hff_propagate(float xaccel, float yaccel) { b2_hff_propagate_y(); #ifdef GPS_LAG - if (lag_counter < 2*GPS_LAG_N) // prevent from overflowing if no gps available - lag_counter++; + if (b2_hff_save.lag_counter < 2*GPS_LAG_N) // prevent from overflowing if no gps available + b2_hff_save.lag_counter++; #endif } static inline void b2_hff_propagate_x(void) { /* update state */ - 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; + b2_hff_work->xdotdot = b2_hff_xaccel - b2_hff_work->xbias; + b2_hff_work->x = b2_hff_work->x + DT_HFILTER * b2_hff_work->xdot + DT_HFILTER * DT_HFILTER / 2 * b2_hff_work->xdotdot; + b2_hff_work->xdot = b2_hff_work->xdot + DT_HFILTER * b2_hff_work->xdotdot; /* update covariance */ - const float FPF00 = b2_hff_xP[0][0] + DT_HFILTER * ( b2_hff_xP[1][0] + b2_hff_xP[0][1] + DT_HFILTER * b2_hff_xP[1][1] ); - const float FPF01 = b2_hff_xP[0][1] + DT_HFILTER * ( b2_hff_xP[1][1] - b2_hff_xP[0][2] - DT_HFILTER * b2_hff_xP[1][2] ); - const float FPF02 = b2_hff_xP[0][2] + DT_HFILTER * ( b2_hff_xP[1][2] ); - const float FPF10 = b2_hff_xP[1][0] + DT_HFILTER * (-b2_hff_xP[2][0] + b2_hff_xP[1][1] - DT_HFILTER * b2_hff_xP[2][1] ); - const float FPF11 = b2_hff_xP[1][1] + DT_HFILTER * (-b2_hff_xP[2][1] - b2_hff_xP[1][2] + DT_HFILTER * b2_hff_xP[2][2] ); - const float FPF12 = b2_hff_xP[1][2] + DT_HFILTER * (-b2_hff_xP[2][2] ); - const float FPF20 = b2_hff_xP[2][0] + DT_HFILTER * ( b2_hff_xP[2][1] ); - const float FPF21 = b2_hff_xP[2][1] + DT_HFILTER * (-b2_hff_xP[2][2] ); - const float FPF22 = b2_hff_xP[2][2]; + const float FPF00 = b2_hff_work->xP[0][0] + DT_HFILTER * ( b2_hff_work->xP[1][0] + b2_hff_work->xP[0][1] + DT_HFILTER * b2_hff_work->xP[1][1] ); + const float FPF01 = b2_hff_work->xP[0][1] + DT_HFILTER * ( b2_hff_work->xP[1][1] - b2_hff_work->xP[0][2] - DT_HFILTER * b2_hff_work->xP[1][2] ); + const float FPF02 = b2_hff_work->xP[0][2] + DT_HFILTER * ( b2_hff_work->xP[1][2] ); + const float FPF10 = b2_hff_work->xP[1][0] + DT_HFILTER * (-b2_hff_work->xP[2][0] + b2_hff_work->xP[1][1] - DT_HFILTER * b2_hff_work->xP[2][1] ); + const float FPF11 = b2_hff_work->xP[1][1] + DT_HFILTER * (-b2_hff_work->xP[2][1] - b2_hff_work->xP[1][2] + DT_HFILTER * b2_hff_work->xP[2][2] ); + const float FPF12 = b2_hff_work->xP[1][2] + DT_HFILTER * (-b2_hff_work->xP[2][2] ); + const float FPF20 = b2_hff_work->xP[2][0] + DT_HFILTER * ( b2_hff_work->xP[2][1] ); + const float FPF21 = b2_hff_work->xP[2][1] + DT_HFILTER * (-b2_hff_work->xP[2][2] ); + const float FPF22 = b2_hff_work->xP[2][2]; - b2_hff_xP[0][0] = FPF00 + Q; - b2_hff_xP[0][1] = FPF01; - b2_hff_xP[0][2] = FPF02; - b2_hff_xP[1][0] = FPF10; - b2_hff_xP[1][1] = FPF11 + Qdotdot; - b2_hff_xP[1][2] = FPF12; - b2_hff_xP[2][0] = FPF20; - b2_hff_xP[2][1] = FPF21; - b2_hff_xP[2][2] = FPF22 + Qbiasbias; + b2_hff_work->xP[0][0] = FPF00 + Q; + b2_hff_work->xP[0][1] = FPF01; + b2_hff_work->xP[0][2] = FPF02; + b2_hff_work->xP[1][0] = FPF10; + b2_hff_work->xP[1][1] = FPF11 + Qdotdot; + b2_hff_work->xP[1][2] = FPF12; + b2_hff_work->xP[2][0] = FPF20; + b2_hff_work->xP[2][1] = FPF21; + b2_hff_work->xP[2][2] = FPF22 + Qbiasbias; } static inline void b2_hff_propagate_y(void) { /* update state */ - 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; + b2_hff_work->ydotdot = b2_hff_yaccel - b2_hff_work->ybias; + b2_hff_work->y = b2_hff_work->y + DT_HFILTER * b2_hff_work->ydot; + b2_hff_work->ydot = b2_hff_work->ydot + DT_HFILTER * b2_hff_work->ydotdot; /* update covariance */ - const float FPF00 = b2_hff_yP[0][0] + DT_HFILTER * ( b2_hff_yP[1][0] + b2_hff_yP[0][1] + DT_HFILTER * b2_hff_yP[1][1] ); - const float FPF01 = b2_hff_yP[0][1] + DT_HFILTER * ( b2_hff_yP[1][1] - b2_hff_yP[0][2] - DT_HFILTER * b2_hff_yP[1][2] ); - const float FPF02 = b2_hff_yP[0][2] + DT_HFILTER * ( b2_hff_yP[1][2] ); - const float FPF10 = b2_hff_yP[1][0] + DT_HFILTER * (-b2_hff_yP[2][0] + b2_hff_yP[1][1] - DT_HFILTER * b2_hff_yP[2][1] ); - const float FPF11 = b2_hff_yP[1][1] + DT_HFILTER * (-b2_hff_yP[2][1] - b2_hff_yP[1][2] + DT_HFILTER * b2_hff_yP[2][2] ); - const float FPF12 = b2_hff_yP[1][2] + DT_HFILTER * (-b2_hff_yP[2][2] ); - const float FPF20 = b2_hff_yP[2][0] + DT_HFILTER * ( b2_hff_yP[2][1] ); - const float FPF21 = b2_hff_yP[2][1] + DT_HFILTER * (-b2_hff_yP[2][2] ); - const float FPF22 = b2_hff_yP[2][2]; + const float FPF00 = b2_hff_work->yP[0][0] + DT_HFILTER * ( b2_hff_work->yP[1][0] + b2_hff_work->yP[0][1] + DT_HFILTER * b2_hff_work->yP[1][1] ); + const float FPF01 = b2_hff_work->yP[0][1] + DT_HFILTER * ( b2_hff_work->yP[1][1] - b2_hff_work->yP[0][2] - DT_HFILTER * b2_hff_work->yP[1][2] ); + const float FPF02 = b2_hff_work->yP[0][2] + DT_HFILTER * ( b2_hff_work->yP[1][2] ); + const float FPF10 = b2_hff_work->yP[1][0] + DT_HFILTER * (-b2_hff_work->yP[2][0] + b2_hff_work->yP[1][1] - DT_HFILTER * b2_hff_work->yP[2][1] ); + const float FPF11 = b2_hff_work->yP[1][1] + DT_HFILTER * (-b2_hff_work->yP[2][1] - b2_hff_work->yP[1][2] + DT_HFILTER * b2_hff_work->yP[2][2] ); + const float FPF12 = b2_hff_work->yP[1][2] + DT_HFILTER * (-b2_hff_work->yP[2][2] ); + const float FPF20 = b2_hff_work->yP[2][0] + DT_HFILTER * ( b2_hff_work->yP[2][1] ); + const float FPF21 = b2_hff_work->yP[2][1] + DT_HFILTER * (-b2_hff_work->yP[2][2] ); + const float FPF22 = b2_hff_work->yP[2][2]; - b2_hff_yP[0][0] = FPF00 + Q; - b2_hff_yP[0][1] = FPF01; - b2_hff_yP[0][2] = FPF02; - b2_hff_yP[1][0] = FPF10; - b2_hff_yP[1][1] = FPF11 + Qdotdot; - b2_hff_yP[1][2] = FPF12; - b2_hff_yP[2][0] = FPF20; - b2_hff_yP[2][1] = FPF21; - b2_hff_yP[2][2] = FPF22 + Qbiasbias; + b2_hff_work->yP[0][0] = FPF00 + Q; + b2_hff_work->yP[0][1] = FPF01; + b2_hff_work->yP[0][2] = FPF02; + b2_hff_work->yP[1][0] = FPF10; + b2_hff_work->yP[1][1] = FPF11 + Qdotdot; + b2_hff_work->yP[1][2] = FPF12; + b2_hff_work->yP[2][0] = FPF20; + b2_hff_work->yP[2][1] = FPF21; + b2_hff_work->yP[2][2] = FPF22 + Qbiasbias; } void b2_hff_update_gps(void) { #ifdef GPS_LAG - lag_counter_err = lag_counter - GPS_LAG_N; + lag_counter_err = b2_hff_save.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(); @@ -354,11 +381,13 @@ void b2_hff_update_gps(void) { /* 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--) { + for (int i=b2_hff_save.lag_counter-1; i >= 0; i--) { b2_hff_get_past_accel(i); b2_hff_propagate_x(); b2_hff_propagate_y(); } + b2_hff_set_state(b2_hff_work); + b2_hff_work = &b2_hff_state; } /* reset save counter */ @@ -389,70 +418,70 @@ void b2_hff_update_pos (float posx, float posy) { static inline void b2_hff_update_x(float x_meas) { b2_hff_x_meas = x_meas; - const float y = x_meas - b2_hff_x; - const float S = b2_hff_xP[0][0] + Rpos; - const float K1 = b2_hff_xP[0][0] * 1/S; - const float K2 = b2_hff_xP[1][0] * 1/S; - const float K3 = b2_hff_xP[2][0] * 1/S; + const float y = x_meas - b2_hff_work->x; + const float S = b2_hff_work->xP[0][0] + Rpos; + const float K1 = b2_hff_work->xP[0][0] * 1/S; + const float K2 = b2_hff_work->xP[1][0] * 1/S; + const float K3 = b2_hff_work->xP[2][0] * 1/S; - b2_hff_x = b2_hff_x + K1 * y; - b2_hff_xdot = b2_hff_xdot + K2 * y; - b2_hff_xbias = b2_hff_xbias + K3 * y; + b2_hff_work->x = b2_hff_work->x + K1 * y; + b2_hff_work->xdot = b2_hff_work->xdot + K2 * y; + b2_hff_work->xbias = b2_hff_work->xbias + K3 * y; - const float P11 = (1. - K1) * b2_hff_xP[0][0]; - const float P12 = (1. - K1) * b2_hff_xP[0][1]; - const float P13 = (1. - K1) * b2_hff_xP[0][2]; - const float P21 = -K2 * b2_hff_xP[0][0] + b2_hff_xP[1][0]; - const float P22 = -K2 * b2_hff_xP[0][1] + b2_hff_xP[1][1]; - const float P23 = -K2 * b2_hff_xP[0][2] + b2_hff_xP[1][2]; - const float P31 = -K3 * b2_hff_xP[0][0] + b2_hff_xP[2][0]; - const float P32 = -K3 * b2_hff_xP[0][1] + b2_hff_xP[2][1]; - const float P33 = -K3 * b2_hff_xP[0][2] + b2_hff_xP[2][2]; + const float P11 = (1. - K1) * b2_hff_work->xP[0][0]; + const float P12 = (1. - K1) * b2_hff_work->xP[0][1]; + const float P13 = (1. - K1) * b2_hff_work->xP[0][2]; + const float P21 = -K2 * b2_hff_work->xP[0][0] + b2_hff_work->xP[1][0]; + const float P22 = -K2 * b2_hff_work->xP[0][1] + b2_hff_work->xP[1][1]; + const float P23 = -K2 * b2_hff_work->xP[0][2] + b2_hff_work->xP[1][2]; + const float P31 = -K3 * b2_hff_work->xP[0][0] + b2_hff_work->xP[2][0]; + const float P32 = -K3 * b2_hff_work->xP[0][1] + b2_hff_work->xP[2][1]; + const float P33 = -K3 * b2_hff_work->xP[0][2] + b2_hff_work->xP[2][2]; - b2_hff_xP[0][0] = P11; - b2_hff_xP[0][1] = P12; - b2_hff_xP[0][2] = P13; - b2_hff_xP[1][0] = P21; - b2_hff_xP[1][1] = P22; - b2_hff_xP[1][2] = P23; - b2_hff_xP[2][0] = P31; - b2_hff_xP[2][1] = P32; - b2_hff_xP[2][2] = P33; + b2_hff_work->xP[0][0] = P11; + b2_hff_work->xP[0][1] = P12; + b2_hff_work->xP[0][2] = P13; + b2_hff_work->xP[1][0] = P21; + b2_hff_work->xP[1][1] = P22; + b2_hff_work->xP[1][2] = P23; + b2_hff_work->xP[2][0] = P31; + b2_hff_work->xP[2][1] = P32; + b2_hff_work->xP[2][2] = P33; } static inline void b2_hff_update_y(float y_meas) { b2_hff_y_meas = y_meas; - const float y = y_meas - b2_hff_y; - const float S = b2_hff_yP[0][0] + Rpos; - const float K1 = b2_hff_yP[0][0] * 1/S; - const float K2 = b2_hff_yP[1][0] * 1/S; - const float K3 = b2_hff_yP[2][0] * 1/S; + const float y = y_meas - b2_hff_work->y; + const float S = b2_hff_work->yP[0][0] + Rpos; + const float K1 = b2_hff_work->yP[0][0] * 1/S; + const float K2 = b2_hff_work->yP[1][0] * 1/S; + const float K3 = b2_hff_work->yP[2][0] * 1/S; - b2_hff_y = b2_hff_y + K1 * y; - b2_hff_ydot = b2_hff_ydot + K2 * y; - b2_hff_ybias = b2_hff_ybias + K3 * y; + b2_hff_work->y = b2_hff_work->y + K1 * y; + b2_hff_work->ydot = b2_hff_work->ydot + K2 * y; + b2_hff_work->ybias = b2_hff_work->ybias + K3 * y; - const float P11 = (1. - K1) * b2_hff_yP[0][0]; - const float P12 = (1. - K1) * b2_hff_yP[0][1]; - const float P13 = (1. - K1) * b2_hff_yP[0][2]; - const float P21 = -K2 * b2_hff_yP[0][0] + b2_hff_yP[1][0]; - const float P22 = -K2 * b2_hff_yP[0][1] + b2_hff_yP[1][1]; - const float P23 = -K2 * b2_hff_yP[0][2] + b2_hff_yP[1][2]; - const float P31 = -K3 * b2_hff_yP[0][0] + b2_hff_yP[2][0]; - const float P32 = -K3 * b2_hff_yP[0][1] + b2_hff_yP[2][1]; - const float P33 = -K3 * b2_hff_yP[0][2] + b2_hff_yP[2][2]; + const float P11 = (1. - K1) * b2_hff_work->yP[0][0]; + const float P12 = (1. - K1) * b2_hff_work->yP[0][1]; + const float P13 = (1. - K1) * b2_hff_work->yP[0][2]; + const float P21 = -K2 * b2_hff_work->yP[0][0] + b2_hff_work->yP[1][0]; + const float P22 = -K2 * b2_hff_work->yP[0][1] + b2_hff_work->yP[1][1]; + const float P23 = -K2 * b2_hff_work->yP[0][2] + b2_hff_work->yP[1][2]; + const float P31 = -K3 * b2_hff_work->yP[0][0] + b2_hff_work->yP[2][0]; + const float P32 = -K3 * b2_hff_work->yP[0][1] + b2_hff_work->yP[2][1]; + const float P33 = -K3 * b2_hff_work->yP[0][2] + b2_hff_work->yP[2][2]; - b2_hff_yP[0][0] = P11; - b2_hff_yP[0][1] = P12; - b2_hff_yP[0][2] = P13; - b2_hff_yP[1][0] = P21; - b2_hff_yP[1][1] = P22; - b2_hff_yP[1][2] = P23; - b2_hff_yP[2][0] = P31; - b2_hff_yP[2][1] = P32; - b2_hff_yP[2][2] = P33; + b2_hff_work->yP[0][0] = P11; + b2_hff_work->yP[0][1] = P12; + b2_hff_work->yP[0][2] = P13; + b2_hff_work->yP[1][0] = P21; + b2_hff_work->yP[1][1] = P22; + b2_hff_work->yP[1][2] = P23; + b2_hff_work->yP[2][0] = P31; + b2_hff_work->yP[2][1] = P32; + b2_hff_work->yP[2][2] = P33; } @@ -479,68 +508,68 @@ void b2_hff_update_v(float xspeed, float yspeed) { } static inline void b2_hff_update_xdot(float v) { - const float yd = v - b2_hff_xdot; - const float S = b2_hff_xP[1][1] + Rspeed; - const float K1 = b2_hff_xP[0][1] * 1/S; - const float K2 = b2_hff_xP[1][1] * 1/S; - const float K3 = b2_hff_xP[2][1] * 1/S; + const float yd = v - b2_hff_work->xdot; + const float S = b2_hff_work->xP[1][1] + Rspeed; + const float K1 = b2_hff_work->xP[0][1] * 1/S; + const float K2 = b2_hff_work->xP[1][1] * 1/S; + const float K3 = b2_hff_work->xP[2][1] * 1/S; - b2_hff_x = b2_hff_x + K1 * yd; - b2_hff_xdot = b2_hff_xdot + K2 * yd; - b2_hff_xbias = b2_hff_xbias + K3 * yd; + b2_hff_work->x = b2_hff_work->x + K1 * yd; + b2_hff_work->xdot = b2_hff_work->xdot + K2 * yd; + b2_hff_work->xbias = b2_hff_work->xbias + K3 * yd; - const float P11 = -K1 * b2_hff_xP[1][0] + b2_hff_xP[0][0]; - const float P12 = -K1 * b2_hff_xP[1][1] + b2_hff_xP[0][1]; - const float P13 = -K1 * b2_hff_xP[1][2] + b2_hff_xP[0][2]; - const float P21 = (1. - K2) * b2_hff_xP[1][0]; - const float P22 = (1. - K2) * b2_hff_xP[1][1]; - const float P23 = (1. - K2) * b2_hff_xP[1][2]; - const float P31 = -K3 * b2_hff_xP[1][0] + b2_hff_xP[2][0]; - const float P32 = -K3 * b2_hff_xP[1][1] + b2_hff_xP[2][1]; - const float P33 = -K3 * b2_hff_xP[1][2] + b2_hff_xP[2][2]; + const float P11 = -K1 * b2_hff_work->xP[1][0] + b2_hff_work->xP[0][0]; + const float P12 = -K1 * b2_hff_work->xP[1][1] + b2_hff_work->xP[0][1]; + const float P13 = -K1 * b2_hff_work->xP[1][2] + b2_hff_work->xP[0][2]; + const float P21 = (1. - K2) * b2_hff_work->xP[1][0]; + const float P22 = (1. - K2) * b2_hff_work->xP[1][1]; + const float P23 = (1. - K2) * b2_hff_work->xP[1][2]; + const float P31 = -K3 * b2_hff_work->xP[1][0] + b2_hff_work->xP[2][0]; + const float P32 = -K3 * b2_hff_work->xP[1][1] + b2_hff_work->xP[2][1]; + const float P33 = -K3 * b2_hff_work->xP[1][2] + b2_hff_work->xP[2][2]; - b2_hff_xP[0][0] = P11; - b2_hff_xP[0][1] = P12; - b2_hff_xP[0][2] = P13; - b2_hff_xP[1][0] = P21; - b2_hff_xP[1][1] = P22; - b2_hff_xP[1][2] = P23; - b2_hff_xP[2][0] = P31; - b2_hff_xP[2][1] = P32; - b2_hff_xP[2][2] = P33; + b2_hff_work->xP[0][0] = P11; + b2_hff_work->xP[0][1] = P12; + b2_hff_work->xP[0][2] = P13; + b2_hff_work->xP[1][0] = P21; + b2_hff_work->xP[1][1] = P22; + b2_hff_work->xP[1][2] = P23; + b2_hff_work->xP[2][0] = P31; + b2_hff_work->xP[2][1] = P32; + b2_hff_work->xP[2][2] = P33; } static inline void b2_hff_update_ydot(float v) { - const float yd = v - b2_hff_ydot; - const float S = b2_hff_yP[1][1] + Rspeed; - const float K1 = b2_hff_yP[0][1] * 1/S; - const float K2 = b2_hff_yP[1][1] * 1/S; - const float K3 = b2_hff_yP[2][1] * 1/S; + const float yd = v - b2_hff_work->ydot; + const float S = b2_hff_work->yP[1][1] + Rspeed; + const float K1 = b2_hff_work->yP[0][1] * 1/S; + const float K2 = b2_hff_work->yP[1][1] * 1/S; + const float K3 = b2_hff_work->yP[2][1] * 1/S; - b2_hff_y = b2_hff_y + K1 * yd; - b2_hff_ydot = b2_hff_ydot + K2 * yd; - b2_hff_ybias = b2_hff_ybias + K3 * yd; + b2_hff_work->y = b2_hff_work->y + K1 * yd; + b2_hff_work->ydot = b2_hff_work->ydot + K2 * yd; + b2_hff_work->ybias = b2_hff_work->ybias + K3 * yd; - const float P11 = -K1 * b2_hff_yP[1][0] + b2_hff_yP[0][0]; - const float P12 = -K1 * b2_hff_yP[1][1] + b2_hff_yP[0][1]; - const float P13 = -K1 * b2_hff_yP[1][2] + b2_hff_yP[0][2]; - const float P21 = (1. - K2) * b2_hff_yP[1][0]; - const float P22 = (1. - K2) * b2_hff_yP[1][1]; - const float P23 = (1. - K2) * b2_hff_yP[1][2]; - const float P31 = -K3 * b2_hff_yP[1][0] + b2_hff_yP[2][0]; - const float P32 = -K3 * b2_hff_yP[1][1] + b2_hff_yP[2][1]; - const float P33 = -K3 * b2_hff_yP[1][2] + b2_hff_yP[2][2]; + const float P11 = -K1 * b2_hff_work->yP[1][0] + b2_hff_work->yP[0][0]; + const float P12 = -K1 * b2_hff_work->yP[1][1] + b2_hff_work->yP[0][1]; + const float P13 = -K1 * b2_hff_work->yP[1][2] + b2_hff_work->yP[0][2]; + const float P21 = (1. - K2) * b2_hff_work->yP[1][0]; + const float P22 = (1. - K2) * b2_hff_work->yP[1][1]; + const float P23 = (1. - K2) * b2_hff_work->yP[1][2]; + const float P31 = -K3 * b2_hff_work->yP[1][0] + b2_hff_work->yP[2][0]; + const float P32 = -K3 * b2_hff_work->yP[1][1] + b2_hff_work->yP[2][1]; + const float P33 = -K3 * b2_hff_work->yP[1][2] + b2_hff_work->yP[2][2]; - b2_hff_yP[0][0] = P11; - b2_hff_yP[0][1] = P12; - b2_hff_yP[0][2] = P13; - b2_hff_yP[1][0] = P21; - b2_hff_yP[1][1] = P22; - b2_hff_yP[1][2] = P23; - b2_hff_yP[2][0] = P31; - b2_hff_yP[2][1] = P32; - b2_hff_yP[2][2] = P33; + b2_hff_work->yP[0][0] = P11; + b2_hff_work->yP[0][1] = P12; + b2_hff_work->yP[0][2] = P13; + b2_hff_work->yP[1][0] = P21; + b2_hff_work->yP[1][1] = P22; + b2_hff_work->yP[1][2] = P23; + b2_hff_work->yP[2][0] = P31; + b2_hff_work->yP[2][1] = P32; + b2_hff_work->yP[2][2] = P33; } diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/booz/ins/booz2_hf_float.h index 9a4c8bd48f..659f2de01e 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.h +++ b/sw/airborne/booz/ins/booz2_hf_float.h @@ -35,18 +35,36 @@ #define B2_HFF_UPDATE_SPEED #define B2_HFF_UPDATE_POS -extern float b2_hff_x; -extern float b2_hff_xbias; -extern float b2_hff_xdot; -extern float b2_hff_xdotdot; +struct hfilter_f { + float x; + float xbias; + float xdot; + float xdotdot; + float y; + float ybias; + float ydot; + float ydotdot; + float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; + float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; + uint8_t lag_counter; +}; -extern float b2_hff_y; -extern float b2_hff_ybias; -extern float b2_hff_ydot; -extern float b2_hff_ydotdot; +extern struct hfilter_f b2_hff_state; +extern struct hfilter_f b2_hff_save; +extern struct hfilter_f *b2_hff_work; -extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; -extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; +/* extern float b2_hff_x; */ +/* extern float b2_hff_xbias; */ +/* extern float b2_hff_xdot; */ +/* extern float b2_hff_xdotdot; */ + +/* extern float b2_hff_y; */ +/* extern float b2_hff_ybias; */ +/* extern float b2_hff_ydot; */ +/* extern float b2_hff_ydotdot; */ + +/* extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */ +/* extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */ extern float b2_hff_x_meas; extern float b2_hff_y_meas; @@ -59,7 +77,7 @@ 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 uint8_t lag_counter; */ extern int8_t lag_counter_err; extern int8_t save_counter; #endif