From 8d882b06ddb8cc13a87bd83b6b6e1cc19625da64 Mon Sep 17 00:00:00 2001 From: Caio Marcelo de Oliveira Filho Date: Fri, 20 Nov 2015 12:13:15 +0900 Subject: [PATCH] AP_NavEKF2: use millis/micros/panic functions --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index fb1f3d25d3..e9a94ba1e0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -320,7 +320,7 @@ void NavEKF2_core::readIMUData() dtIMUavg = 1.0f/ins.get_sample_rate(); // the imu sample time is used as a common time reference throughout the filter - imuSampleTime_ms = hal.scheduler->millis(); + imuSampleTime_ms = AP_HAL::millis(); // use the nominated imu or primary if not available if (ins.use_accel(imu_index)) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index b13cd502e0..d417c83500 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -98,7 +98,7 @@ void NavEKF2_core::InitialiseVariables() localFilterTimeStep_ms = max(localFilterTimeStep_ms,10); // initialise time stamps - imuSampleTime_ms = hal.scheduler->millis(); + imuSampleTime_ms = AP_HAL::millis(); lastHealthyMagTime_ms = imuSampleTime_ms; prevTasStep_ms = imuSampleTime_ms; prevBetaStep_ms = imuSampleTime_ms; @@ -393,7 +393,7 @@ void NavEKF2_core::UpdateFilter(bool predict) // TODO - in-flight restart method //get starting time for update step - imuSampleTime_ms = hal.scheduler->millis(); + imuSampleTime_ms = AP_HAL::millis(); // Check arm status and perform required checks and mode changes controlFilterModes();