void b2_hff_propagate(void) {
if (b2_hff_lost_counter < b2_hff_lost_limit)
b2_hff_lost_counter++;
#ifdef GPS_LAG
/* continue re-propagating to catch up with the present */
if (b2_hff_rb_last->rollback) {
b2_hff_propagate_past(b2_hff_rb_last);
}
#endif
/* store body accelerations for mean computation */
b2_hff_store_accel_body();
/* propagate current state if it is time */
if (b2_hff_ps_counter == HFF_PRESCALER) {
b2_hff_ps_counter = 1;
if (b2_hff_lost_counter < b2_hff_lost_limit) {
/* compute float ltp mean acceleration */
b2_hff_compute_accel_body_mean(HFF_PRESCALER);
struct Int32Vect3 mean_accel_ltp;
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat, acc_body_mean);
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_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
#endif
/*
* propagate current state
*/
b2_hff_propagate_x(&b2_hff_state);
b2_hff_propagate_y(&b2_hff_state);
/* update ins state from horizontal filter */
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
/* increase lag counter on last saved state */
if (b2_hff_rb_n > 0)
b2_hff_rb_last->lag_counter++;
/* save filter state if needed */
if (save_counter == 0) {
PRINT_DBG(1, ("save current state\n"));
b2_hff_rb_put_state(&b2_hff_state);
save_counter = -1;
} else if (save_counter > 0) {
save_counter--;
}
#endif
}
} else {
b2_hff_ps_counter++;
}
}
void b2_hff_propagate(void)
{
if (b2_hff_lost_counter < b2_hff_lost_limit) {
b2_hff_lost_counter++;
}
#ifdef GPS_LAG
/* continue re-propagating to catch up with the present */
if (b2_hff_rb_last->rollback) {
b2_hff_propagate_past(b2_hff_rb_last);
}
#endif
/* rotate imu accel measurement to body frame and filter */
struct Int32Vect3 acc_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_transp_vmult(&acc_meas_body, body_to_imu_rmat, &imu.accel);
struct Int32Vect3 acc_body_filtered;
acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x);
acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, acc_meas_body.y);
acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, acc_meas_body.z);
/* propagate current state if it is time */
if (b2_hff_ps_counter == HFF_PRESCALER) {
b2_hff_ps_counter = 1;
if (b2_hff_lost_counter < b2_hff_lost_limit) {
struct Int32Vect3 filtered_accel_ltp;
struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered);
b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x);
b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y);
#ifdef GPS_LAG
b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
#endif
/*
* propagate current state
*/
b2_hff_propagate_x(&b2_hff_state, DT_HFILTER);
b2_hff_propagate_y(&b2_hff_state, DT_HFILTER);
#ifdef GPS_LAG
/* increase lag counter on last saved state */
if (b2_hff_rb_n > 0) {
b2_hff_rb_last->lag_counter++;
}
/* save filter state if needed */
if (save_counter == 0) {
PRINT_DBG(1, ("save current state\n"));
b2_hff_rb_put_state(&b2_hff_state);
save_counter = -1;
} else if (save_counter > 0) {
save_counter--;
}
#endif
}
} else {
b2_hff_ps_counter++;
}
}
