diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 7f861fa46d..db8f616817 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -637,8 +637,7 @@ void NavEKF3::check_log_write(void) logging.log_baro = false; } if (logging.log_imu) { - const AP_InertialSensor &ins = _ahrs->get_ins(); - DataFlash_Class::instance()->Log_Write_IMUDT(ins, imuSampleTime_us, _logging_mask.get()); + DataFlash_Class::instance()->Log_Write_IMUDT(imuSampleTime_us, _logging_mask.get()); logging.log_imu = false; } @@ -653,7 +652,7 @@ bool NavEKF3::InitialiseFilter(void) if (_enable == 0) { return false; } - const AP_InertialSensor &ins = _ahrs->get_ins(); + const AP_InertialSensor &ins = AP::ins(); imuSampleTime_us = AP_HAL::micros64(); @@ -756,7 +755,7 @@ void NavEKF3::UpdateFilter(void) imuSampleTime_us = AP_HAL::micros64(); - const AP_InertialSensor &ins = _ahrs->get_ins(); + const AP_InertialSensor &ins = AP::ins(); bool statePredictEnabled[num_cores]; for (uint8_t i=0; iget_ins(); + const AP_InertialSensor &ins = AP::ins(); // calculate an averaged IMU update rate using a spike and lowpass filter combination dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg; @@ -437,7 +437,7 @@ void NavEKF3_core::readIMUData() // read the delta velocity and corresponding time interval from the IMU // return false if data is not available bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) { - const AP_InertialSensor &ins = _ahrs->get_ins(); + const AP_InertialSensor &ins = AP::ins(); if (ins_index < ins.get_accel_count()) { ins.get_delta_velocity(ins_index,dVel); @@ -583,7 +583,7 @@ void NavEKF3_core::readGpsData() // read the delta angle and corresponding time interval from the IMU // return false if data is not available bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { - const AP_InertialSensor &ins = _ahrs->get_ins(); + const AP_InertialSensor &ins = AP::ins(); if (ins_index < ins.get_gyro_count()) { ins.get_delta_angle(ins_index,dAng); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 834cb7277f..1ecb25a7a1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -451,7 +451,7 @@ void NavEKF3_core::detectOptFlowTakeoff(void) { if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff - const AP_InertialSensor &ins = _ahrs->get_ins(); + const AP_InertialSensor &ins = AP::ins(); Vector3f angRateVec; Vector3f gyroBias; getGyroBias(gyroBias); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index aef7ffff83..c70a2a23b3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -53,8 +53,8 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c */ // Calculate the expected EKF time step - if (_ahrs->get_ins().get_sample_rate() > 0) { - dtEkfAvg = 1.0f / _ahrs->get_ins().get_sample_rate(); + if (AP::ins().get_sample_rate() > 0) { + dtEkfAvg = 1.0f / AP::ins().get_sample_rate(); dtEkfAvg = MAX(dtEkfAvg,EKF_TARGET_DT); } else { return false; @@ -155,7 +155,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c void NavEKF3_core::InitialiseVariables() { // calculate the nominal filter update rate - const AP_InertialSensor &ins = _ahrs->get_ins(); + const AP_InertialSensor &ins = AP::ins(); localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t()); localFilterTimeStep_ms = MAX(localFilterTimeStep_ms, (uint8_t)EKF_TARGET_DT_MS); @@ -430,7 +430,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) Vector3f initAccVec; // TODO we should average accel readings over several cycles - initAccVec = _ahrs->get_ins().get_accel(imu_index); + initAccVec = AP::ins().get_accel(imu_index); // normalise the acceleration vector float pitch=0, roll=0;