From ee069a081b0e60f2d99bbda4d97fcdc2e7d28140 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 20 Jul 2021 22:16:30 +1000 Subject: [PATCH] AP_Mount: rename for AHRS restructuring --- libraries/AP_Mount/SoloGimbal.cpp | 6 +++--- libraries/AP_Mount/SoloGimbalEKF.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp index 9d8e87ad60..01b8aec829 100644 --- a/libraries/AP_Mount/SoloGimbal.cpp +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -28,7 +28,7 @@ bool SoloGimbal::aligned() gimbal_mode_t SoloGimbal::get_mode() { - const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); + const auto &_ahrs = AP::ahrs(); if ((_gimbalParams.initialized() && is_zero(_gimbalParams.get_K_rate())) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) { return GIMBAL_MODE_IDLE; @@ -281,7 +281,7 @@ Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst) float dt = _measurement.delta_time; float alpha = dt/(dt+tc); - const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); + const auto &_ahrs = AP::ahrs(); Matrix3f Tve = _ahrs.get_rotation_body_to_ned(); Matrix3f Teg; quatEst.inverse().rotation_matrix(Teg); @@ -380,7 +380,7 @@ Vector3f SoloGimbal::get_ang_vel_dem_body_lock() joint_rates_to_gimbal_ang_vel(gimbalRateDemVecBodyLock, gimbalRateDemVecBodyLock); // Add a feedforward term from vehicle gyros - const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); + const auto &_ahrs = AP::ahrs(); gimbalRateDemVecBodyLock += Tvg * _ahrs.get_gyro(); return gimbalRateDemVecBodyLock; diff --git a/libraries/AP_Mount/SoloGimbalEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp index 16715b6024..fbb609371a 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.cpp +++ b/libraries/AP_Mount/SoloGimbalEKF.cpp @@ -76,7 +76,7 @@ void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const bool main_ekf_healthy = false; nav_filter_status main_ekf_status; - const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); + const auto &_ahrs = AP::ahrs(); if (_ahrs.get_filter_status(main_ekf_status)) { if (main_ekf_status.flags.attitude) { @@ -597,7 +597,7 @@ void SoloGimbalEKF::predictCovariance() // Fuse the SoloGimbalEKF velocity estimates - this enables alevel reference to be maintained during constant turns void SoloGimbalEKF::fuseVelocity() { - const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); + const auto &_ahrs = AP::ahrs(); if (!_ahrs.have_inertial_nav()) { return; @@ -668,7 +668,7 @@ void SoloGimbalEKF::fuseVelocity() // check for new magnetometer data and update store measurements if available void SoloGimbalEKF::readMagData() { - const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); + const auto &_ahrs = AP::ahrs(); if (_ahrs.get_compass() && _ahrs.get_compass()->use_for_yaw() && @@ -867,7 +867,7 @@ float SoloGimbalEKF::calcMagHeadingInnov() Tms[1][2] = sinPhi; Tms[2][2] = cosTheta*cosPhi; - const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); + const auto &_ahrs = AP::ahrs(); // get earth magnetic field estimate from main ekf if available to take advantage of main ekf magnetic field learning Vector3f earth_magfield = Vector3f(0,0,0);