void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
{
b2_hff_lost_counter = 0;
#if USE_GPS_ACC4R
Rgps_pos = (float) gps.pacc / 100.;
if (Rgps_pos < HFF_R_POS_MIN) {
Rgps_pos = HFF_R_POS_MIN;
}
Rgps_vel = (float) gps.sacc / 100.;
if (Rgps_vel < HFF_R_SPEED_MIN) {
Rgps_vel = HFF_R_SPEED_MIN;
}
#endif
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif
/* update filter state with measurement */
b2_hff_update_x(&b2_hff_state, pos_ned->x, Rgps_pos);
b2_hff_update_y(&b2_hff_state, pos_ned->y, Rgps_pos);
#if HFF_UPDATE_SPEED
b2_hff_update_xdot(&b2_hff_state, speed_ned->x, Rgps_vel);
b2_hff_update_ydot(&b2_hff_state, speed_ned->y, Rgps_vel);
#endif
#ifdef GPS_LAG
} else 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;
PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter,
lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
b2_hff_rb_last->rollback = true;
b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos);
b2_hff_update_y(b2_hff_rb_last, pos_ned->y, Rgps_pos);
#if HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel);
b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel);
#endif
past_save_counter = GPS_DT_N - 1; // + lag_counter_err;
PRINT_DBG(2, ("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 - (GPS_LAG_TOL_N + 1)) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
b2_hff_rb_drop_last();
b2_hff_update_gps(pos_ned, speed_ned);
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N - 1 - (GPS_LAG_N % GPS_DT_N);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
#endif /* GPS_LAG */
}
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) {
b2_hff_state.x = pos.x;
b2_hff_state.y = pos.y;
b2_hff_state.xdot = vel.x;
b2_hff_state.ydot = vel.y;
#ifdef GPS_LAG
while (b2_hff_rb_n > 0) {
b2_hff_rb_drop_last();
}
save_counter = -1;
past_save_counter = SAVE_DONE;
#endif
}
static void b2_hff_propagate_past(struct HfilterFloat *hff_past)
{
PRINT_DBG(1, ("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++) {
if (hff_past->lag_counter > 0) {
b2_hff_get_past_accel(hff_past->lag_counter);
PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
b2_hff_propagate_x(hff_past, DT_HFILTER);
b2_hff_propagate_y(hff_past, DT_HFILTER);
hff_past->lag_counter--;
if (past_save_counter > 0) {
past_save_counter--;
PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter));
} else if (past_save_counter == SAVE_NOW) {
/* next GPS measurement valid at this state -> save */
PRINT_DBG(2, ("save past state\n"));
b2_hff_rb_put_state(hff_past);
past_save_counter = SAVING;
} else if (past_save_counter == SAVING) {
/* 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_drop_last();
past_save_counter = SAVE_DONE;
break;
}
}
}
void b2_hff_update_gps(void) {
b2_hff_lost_counter = 0;
#if USE_GPS_ACC4R
Rgps_pos = (float) gps.pacc / 100.;
if (Rgps_pos < HFF_R_POS_MIN)
Rgps_pos = HFF_R_POS_MIN;
Rgps_vel = (float) gps.sacc / 100.;
if (Rgps_vel < HFF_R_SPEED_MIN)
Rgps_vel = HFF_R_SPEED_MIN;
#endif
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif
/* update filter state with measurement */
b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
/* update ins state */
ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifdef GPS_LAG
} else 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;
PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
past_save_counter = GPS_DT_N-1;// + lag_counter_err;
PRINT_DBG(2, ("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 - (GPS_LAG_TOL_N+1)) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
b2_hff_rb_drop_last();
b2_hff_update_gps();
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
#endif /* GPS_LAG */
}
