// run a full DCM update round void AP_AHRS_DCM::update(bool skip_ins_update) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); float delta_t; if (_last_startup_ms == 0) { _last_startup_ms = AP_HAL::millis(); load_watchdog_home(); } if (!skip_ins_update) { // tell the IMU to grab some data AP::ins().update(); } const AP_InertialSensor &_ins = AP::ins(); // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); // if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter if (delta_t > 0.2f) { memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum)); _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // update trig values including _cos_roll, cos_pitch update_trig(); // update AOA and SSA update_AOA_SSA(); backup_attitude(); }
// return calculated SSA float AP_AHRS::getSSA(void) { update_AOA_SSA(); return _SSA; }