void ahrs_update_gps(void) { #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS if (gps.fix == GPS_FIX_3D) { ahrs_impl.ltp_vel_norm = gps.speed_3d / 100.; ahrs_impl.ltp_vel_norm_valid = TRUE; } else { ahrs_impl.ltp_vel_norm_valid = FALSE; } #endif #if AHRS_USE_GPS_HEADING && USE_GPS //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg if(gps.fix == GPS_FIX_3D && gps.gspeed >= 500 && gps.cacc <= RadOfDeg(10*1e7)) { // gps.course is in rad * 1e7, we need it in rad float course = gps.course / 1e7; if (ahrs_impl.heading_aligned) { /* the assumption here is that there is no side-slip, so heading=course */ ahrs_update_heading(course); } else { /* hard reset the heading if this is the first measurement */ ahrs_realign_heading(course); } } #endif }
void aos_run(void) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_ALIGNEMENT if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); } } else { #endif /* DISABLE_ALIGNEMENT */ ahrs_propagate(aos.dt); ahrs_update_accel(); #ifndef DISABLE_MAG_UPDATE ahrs_update_mag(); #endif #if AHRS_USE_GPS_HEADING #if AHRS_TYPE == AHRS_TYPE_ICQ int32_t heading = ANGLE_BFP_OF_REAL(aos.heading_meas); #endif #if AHRS_TYPE == AHRS_TYPE_FCQ float heading = aos.heading_meas; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_course = aos.heading_meas; ahrs_impl.gps_course_valid = true; #else if (aos.time > 10) { if (!ahrs_impl.heading_aligned) { ahrs_realign_heading(heading); } else { RunOnceEvery(100, ahrs_update_heading(heading)); } } #endif #endif // AHRS_USE_GPS_HEADING #ifndef DISABLE_ALIGNEMENT } #endif }