From 9d45669a580d99a1d7cb126c08aaa7c18052fe90 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 16 Oct 2020 18:03:11 +1100 Subject: [PATCH] AP_AHRS: reuse ins object for multiple calls, simplify delta_t variable --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index c84f8c4f77..39d0b612b9 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -69,23 +69,21 @@ 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(); } + AP_InertialSensor &_ins = AP::ins(); + if (!skip_ins_update) { // tell the IMU to grab some data - AP::ins().update(); + _ins.update(); } - const AP_InertialSensor &_ins = AP::ins(); - // ask the IMU how much time this sensor reading represents - delta_t = _ins.get_delta_time(); + const float 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