void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { Rgps_pos = HFF_R_POS; Rgps_vel = HFF_R_SPEED; b2_hff_init_x(init_x, init_xdot); b2_hff_init_y(init_y, init_ydot); #ifdef GPS_LAG /* init buffer for past mean accel values */ acc_buf_r = 0; acc_buf_w = 0; acc_buf_n = 0; b2_hff_rb_put = b2_hff_rb; b2_hff_rb_last = b2_hff_rb; b2_hff_rb_last->rollback = false; b2_hff_rb_last->lag_counter = 0; b2_hff_state.lag_counter = GPS_LAG_N; #ifdef SITL printf("GPS_LAG: %f\n", GPS_LAG); printf("GPS_LAG_N: %d\n", GPS_LAG_N); printf("GPS_DT_N: %d\n", GPS_DT_N); printf("DT_HFILTER: %f\n", DT_HFILTER); printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N); #endif #else b2_hff_rb_last = &b2_hff_state; b2_hff_state.lag_counter = 0; #endif b2_hff_rb_n = 0; b2_hff_state.rollback = false; lag_counter_err = 0; save_counter = -1; past_save_counter = SAVE_DONE; b2_hff_ps_counter = 1; b2_hff_lost_counter = 0; b2_hff_lost_limit = HFF_LOST_LIMIT; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF, send_hff); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_DBG, send_hff_debug); #ifdef GPS_LAG register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_GPS, send_hff_gps); #endif #endif init_butterworth_2_low_pass_int(&filter_x, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0); init_butterworth_2_low_pass_int(&filter_y, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0); init_butterworth_2_low_pass_int(&filter_z, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0); }
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { Rgps_pos = HFF_R_POS; Rgps_vel = HFF_R_SPEED; b2_hff_init_x(init_x, init_xdot); b2_hff_init_y(init_y, init_ydot); /* init buffer for mean accel calculation */ acc_body.r = 0; acc_body.w = 0; acc_body.n = 0; acc_body.size = ACC_RB_MAXN; #ifdef GPS_LAG /* init buffer for past mean accel values */ acc_buf_r = 0; acc_buf_w = 0; acc_buf_n = 0; b2_hff_rb_put = b2_hff_rb; b2_hff_rb_last = b2_hff_rb; b2_hff_rb_last->rollback = FALSE; b2_hff_rb_last->lag_counter = 0; b2_hff_state.lag_counter = GPS_LAG_N; #ifdef SITL printf("GPS_LAG: %f\n", GPS_LAG); printf("GPS_LAG_N: %d\n", GPS_LAG_N); printf("GPS_DT_N: %d\n", GPS_DT_N); printf("DT_HFILTER: %f\n", DT_HFILTER); printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N); #endif #else b2_hff_rb_last = &b2_hff_state; b2_hff_state.lag_counter = 0; #endif b2_hff_rb_n = 0; b2_hff_state.rollback = FALSE; lag_counter_err = 0; save_counter = -1; past_save_counter = SAVE_DONE; b2_hff_ps_counter = 1; b2_hff_lost_counter = 0; b2_hff_lost_limit = HFF_LOST_LIMIT; }