From 40957ec4307712c891aceddd63c4fee5bdb225c6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 10 Mar 2018 20:35:50 +1100 Subject: [PATCH] AP_NavEKF2: use ins singleton --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 7 +++---- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 6 +++--- libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 10 ++++++---- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 96962aca44..0ec84e15e7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -583,8 +583,7 @@ void NavEKF2::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; } @@ -599,7 +598,7 @@ bool NavEKF2::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(); @@ -688,7 +687,7 @@ void NavEKF2::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(); // average IMU sampling rate dtIMUavg = ins.get_loop_delta_t(); @@ -391,7 +391,7 @@ void NavEKF2_core::readIMUData() // read the delta velocity and corresponding time interval from the IMU // return false if data is not available bool NavEKF2_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); @@ -534,7 +534,7 @@ void NavEKF2_core::readGpsData() // read the delta angle and corresponding time interval from the IMU // return false if data is not available bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt) { - 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_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 2277dc1f50..796f9b087f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -452,7 +452,7 @@ void NavEKF2_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_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index f5d13c955e..870548eeeb 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -62,7 +62,7 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c than 100Hz is downsampled. For 50Hz main loop rate we need a shorter buffer. */ - if (_ahrs->get_ins().get_sample_rate() < 100) { + if (AP::ins().get_sample_rate() < 100) { imu_buffer_length = 13; } else { // maximum 260 msec delay at 100 Hz fusion rate @@ -110,7 +110,7 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c void NavEKF2_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,10); @@ -355,8 +355,10 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) // set re-used variables to zero InitialiseVariables(); + const AP_InertialSensor &ins = AP::ins(); + // Initialise IMU data - dtIMUavg = _ahrs->get_ins().get_loop_delta_t(); + dtIMUavg = ins.get_loop_delta_t(); readIMUData(); storedIMU.reset_history(imuDataNew); imuDataDelayed = imuDataNew; @@ -365,7 +367,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) Vector3f initAccVec; // TODO we should average accel readings over several cycles - initAccVec = _ahrs->get_ins().get_accel(imu_index); + initAccVec = ins.get_accel(imu_index); // read the magnetometer data readMagData();