// 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;
}
