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_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 */
}
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++;
}
}
