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 = FALSE;
/* forward write pointer */
INC_RB_POINTER(b2_hff_rb_put);
/* increase fill count and forward last pointer if neccessary */
if (b2_hff_rb_n < HFF_RB_MAXN) {
b2_hff_rb_n++;
} else {
INC_RB_POINTER(b2_hff_rb_last);
}
PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n));
}
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;
}
}
}
