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 ins_update_gps(void) { #ifdef USE_GPS if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { if (!ins_ltp_initialised) { ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos); ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; ins_ltp_def.hmsl = booz_gps_state.hmsl; ins_ltp_initialised = TRUE; } ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos); ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel); #ifdef USE_HFF VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); if (ins_hf_realign) { ins_hf_realign = FALSE; #ifdef SITL struct FloatVect2 true_pos, true_speed; VECT2_COPY(true_pos, fdm.ltpprz_pos); VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); b2_hff_realign(true_pos, true_speed); #else const struct FloatVect2 zero = {0.0, 0.0}; b2_hff_realign(ins_gps_pos_m_ned, zero); #endif } b2_hff_update_gps(); #ifndef USE_VFF /* vff not used */ ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN; #endif /* vff not used */ #endif /* hff used */ #ifndef USE_HFF /* hff not used */ #ifndef USE_VFF /* neither hf nor vf used */ INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #else /* only vff used */ INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #endif #ifdef USE_GPS_LAG_HACK VECT2_COPY(d_pos, ins_ltp_speed); INT32_VECT2_RSHIFT(d_pos, d_pos, 11); VECT2_ADD(ins_ltp_pos, d_pos); #endif #endif /* hff not used */ INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos); INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed); INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel); } #endif /* USE_GPS */ }
void ins_update_gps(void) { #if USE_GPS if (gps.fix == GPS_FIX_3D) { if (!ins_ltp_initialised) { ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); ins_ltp_def.lla.alt = gps.lla_pos.alt; ins_ltp_def.hmsl = gps.hmsl; ins_ltp_initialised = TRUE; stateSetLocalOrigin_i(&ins_ltp_def); } ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); #if USE_HFF VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); if (ins.hf_realign) { ins.hf_realign = FALSE; #ifdef SITL struct FloatVect2 true_pos, true_speed; VECT2_COPY(true_pos, fdm.ltpprz_pos); VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); b2_hff_realign(true_pos, true_speed); #else const struct FloatVect2 zero = {0.0, 0.0}; b2_hff_realign(ins_gps_pos_m_ned, zero); #endif } b2_hff_update_gps(); #endif /* hff used */ #if !USE_HFF /* hff not used */ INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #endif /* hff not used */ } #endif /* USE_GPS */ }
void ins_reset_altitude_ref(void) { #if USE_GPS struct LlaCoor_i lla = { .lat = state.ned_origin_i.lla.lat, .lon = state.ned_origin_i.lla.lon, .alt = gps.lla_pos.alt }; ltp_def_from_lla_i(&ins_impl.ltp_def, &lla); ins_impl.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_impl.ltp_def); #endif ins_impl.vf_reset = TRUE; } void ins_propagate(float dt) { /* untilt accels */ struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); struct Int32Vect3 accel_meas_ltp; int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body); float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); if (ins_impl.baro_initialized) { vff_propagate(z_accel_meas_float, dt); ins_update_from_vff(); } else { // feed accel from the sensors // subtract -9.81m/s2 (acceleration measured due to gravity, // but vehicle not accelerating in ltp) ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); } #if USE_HFF /* propagate horizontal filter */ b2_hff_propagate(); /* convert and copy result to ins_impl */ ins_update_from_hff(); #else ins_impl.ltp_accel.x = accel_meas_ltp.x; ins_impl.ltp_accel.y = accel_meas_ltp.y; #endif /* USE_HFF */ ins_ned_to_state(); } static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { if (!ins_impl.baro_initialized && *pressure > 1e-7) { // wait for a first positive value ins_impl.qfe = *pressure; ins_impl.baro_initialized = TRUE; } if (ins_impl.baro_initialized) { if (ins_impl.vf_reset) { ins_impl.vf_reset = FALSE; ins_impl.qfe = *pressure; vff_realign(0.); ins_update_from_vff(); } else { ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); #if USE_VFF_EXTENDED vff_update_baro(ins_impl.baro_z); #else vff_update(ins_impl.baro_z); #endif } ins_ned_to_state(); } } #if USE_GPS void ins_update_gps(void) { if (gps.fix == GPS_FIX_3D) { if (!ins_impl.ltp_initialized) { ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; ins_impl.ltp_def.hmsl = gps.hmsl; ins_impl.ltp_initialized = TRUE; stateSetLocalOrigin_i(&ins_impl.ltp_def); } struct NedCoor_i gps_pos_cm_ned; ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); /// @todo maybe use gps.ned_vel directly?? struct NedCoor_i gps_speed_cm_s_ned; ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel); #if INS_USE_GPS_ALT vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS); #endif #if USE_HFF /* horizontal gps transformed to NED in meters as float */ struct FloatVect2 gps_pos_m_ned; VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y); VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f); struct FloatVect2 gps_speed_m_s_ned; VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y); VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.); if (ins_impl.hf_realign) { ins_impl.hf_realign = FALSE; const struct FloatVect2 zero = {0.0f, 0.0f}; b2_hff_realign(gps_pos_m_ned, zero); } // run horizontal filter b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned); // convert and copy result to ins_impl ins_update_from_hff(); #else /* hff not used */ /* simply copy horizontal pos/speed from gps */ INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #endif /* USE_HFF */ ins_ned_to_state(); } } #endif /* USE_GPS */ #if USE_SONAR static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) { static float last_offset = 0.; /* update filter assuming a flat ground */ if (*distance < INS_SONAR_MAX_RANGE && *distance > INS_SONAR_MIN_RANGE #ifdef INS_SONAR_THROTTLE_THRESHOLD && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD #endif #ifdef INS_SONAR_BARO_THRESHOLD && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */ #endif && ins_impl.update_on_agl && ins_impl.baro_initialized) { vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance)); last_offset = vff.offset; } else { /* update offset with last value to avoid divergence */ vff_update_offset(last_offset); } } #endif // USE_SONAR /** initialize the local origin (ltp_def) from flight plan position */ static void ins_init_origin_from_flightplan(void) { struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ llh_nav0.lat = NAV_LAT0; llh_nav0.lon = NAV_LON0; /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ llh_nav0.alt = NAV_ALT0 + NAV_MSL0; struct EcefCoor_i ecef_nav0; ecef_of_lla_i(&ecef_nav0, &llh_nav0); ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0); ins_impl.ltp_def.hmsl = NAV_ALT0; stateSetLocalOrigin_i(&ins_impl.ltp_def); } /** copy position and speed to state interface */ static void ins_ned_to_state(void) { stateSetPositionNed_i(&ins_impl.ltp_pos); stateSetSpeedNed_i(&ins_impl.ltp_speed); stateSetAccelNed_i(&ins_impl.ltp_accel); #if defined SITL && USE_NPS if (nps_bypass_ins) { sim_overwrite_ins(); } #endif }
void ins_reset_altitude_ref(void) { #if USE_GPS struct LlaCoor_i lla = { .lat = state.ned_origin_i.lla.lat, .lon = state.ned_origin_i.lla.lon, .alt = gps.lla_pos.alt }; ltp_def_from_lla_i(&ins_int.ltp_def, &lla); ins_int.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_int.ltp_def); #endif ins_int.vf_reset = TRUE; } void ins_int_propagate(struct Int32Vect3 *accel, float dt) { /* untilt accels */ struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel); struct Int32Vect3 accel_meas_ltp; int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body); float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); /* Propagate only if we got any measurement during the last INS_MAX_PROPAGATION_STEPS. * Otherwise halt the propagation to not diverge and only set the acceleration. * This should only be relevant in the startup phase when the baro is not yet initialized * and there is no gps fix yet... */ if (ins_int.propagation_cnt < INS_MAX_PROPAGATION_STEPS) { vff_propagate(z_accel_meas_float, dt); ins_update_from_vff(); } else { // feed accel from the sensors // subtract -9.81m/s2 (acceleration measured due to gravity, // but vehicle not accelerating in ltp) ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); } #if USE_HFF /* propagate horizontal filter */ b2_hff_propagate(); /* convert and copy result to ins_int */ ins_update_from_hff(); #else ins_int.ltp_accel.x = accel_meas_ltp.x; ins_int.ltp_accel.y = accel_meas_ltp.y; #endif /* USE_HFF */ ins_ned_to_state(); /* increment the propagation counter, while making sure it doesn't overflow */ if (ins_int.propagation_cnt < 100 * INS_MAX_PROPAGATION_STEPS) { ins_int.propagation_cnt++; } } static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { if (!ins_int.baro_initialized && pressure > 1e-7) { // wait for a first positive value ins_int.qfe = pressure; ins_int.baro_initialized = TRUE; } if (ins_int.baro_initialized) { if (ins_int.vf_reset) { ins_int.vf_reset = FALSE; ins_int.qfe = pressure; vff_realign(0.); ins_update_from_vff(); } else { ins_int.baro_z = -pprz_isa_height_of_pressure(pressure, ins_int.qfe); #if USE_VFF_EXTENDED vff_update_baro(ins_int.baro_z); #else vff_update(ins_int.baro_z); #endif } ins_ned_to_state(); /* reset the counter to indicate we just had a measurement update */ ins_int.propagation_cnt = 0; } } #if USE_GPS void ins_int_update_gps(struct GpsState *gps_s) { if (gps_s->fix < GPS_FIX_3D) { return; } if (!ins_int.ltp_initialized) { ins_reset_local_origin(); } struct NedCoor_i gps_pos_cm_ned; ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos); /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */ #ifdef INS_BODY_TO_GPS_X /* body2gps translation in body frame */ struct Int32Vect3 b2g_b = { .x = INS_BODY_TO_GPS_X, .y = INS_BODY_TO_GPS_Y, .z = INS_BODY_TO_GPS_Z }; /* rotate offset given in body frame to navigation/ltp frame using current attitude */ struct Int32Quat q_b2n = *stateGetNedToBodyQuat_i(); QUAT_INVERT(q_b2n, q_b2n); struct Int32Vect3 b2g_n; int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b); /* subtract body2gps translation in ltp from gps position */ VECT3_SUB(gps_pos_cm_ned, b2g_n); #endif /// @todo maybe use gps_s->ned_vel directly?? struct NedCoor_i gps_speed_cm_s_ned; ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel); #if INS_USE_GPS_ALT vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS); #endif #if INS_USE_GPS_ALT_SPEED vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS); #endif #if USE_HFF /* horizontal gps transformed to NED in meters as float */ struct FloatVect2 gps_pos_m_ned; VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y); VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f); struct FloatVect2 gps_speed_m_s_ned; VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y); VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.); if (ins_int.hf_realign) { ins_int.hf_realign = FALSE; const struct FloatVect2 zero = {0.0f, 0.0f}; b2_hff_realign(gps_pos_m_ned, zero); } // run horizontal filter b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned); // convert and copy result to ins_int ins_update_from_hff(); #else /* hff not used */ /* simply copy horizontal pos/speed from gps */ INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #endif /* USE_HFF */ ins_ned_to_state(); /* reset the counter to indicate we just had a measurement update */ ins_int.propagation_cnt = 0; } #else void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {} #endif /* USE_GPS */ #if USE_SONAR static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance) { static float last_offset = 0.; /* update filter assuming a flat ground */ if (distance < INS_SONAR_MAX_RANGE && distance > INS_SONAR_MIN_RANGE #ifdef INS_SONAR_THROTTLE_THRESHOLD && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD #endif #ifdef INS_SONAR_BARO_THRESHOLD && ins_int.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */ #endif && ins_int.update_on_agl && ins_int.baro_initialized) { vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance)); last_offset = vff.offset; } else { /* update offset with last value to avoid divergence */ vff_update_offset(last_offset); } /* reset the counter to indicate we just had a measurement update */ ins_int.propagation_cnt = 0; } #endif // USE_SONAR /** initialize the local origin (ltp_def) from flight plan position */ static void ins_init_origin_from_flightplan(void) { struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ llh_nav0.lat = NAV_LAT0; llh_nav0.lon = NAV_LON0; /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ llh_nav0.alt = NAV_ALT0 + NAV_MSL0; struct EcefCoor_i ecef_nav0; ecef_of_lla_i(&ecef_nav0, &llh_nav0); ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_nav0); ins_int.ltp_def.hmsl = NAV_ALT0; stateSetLocalOrigin_i(&ins_int.ltp_def); } /** copy position and speed to state interface */ static void ins_ned_to_state(void) { stateSetPositionNed_i(&ins_int.ltp_pos); stateSetSpeedNed_i(&ins_int.ltp_speed); stateSetAccelNed_i(&ins_int.ltp_accel); #if defined SITL && USE_NPS if (nps_bypass_ins) { sim_overwrite_ins(); } #endif }
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 */ }