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++; } }