Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: rename for AHRS restructuring

Browse Source
This commit is contained in:
Peter Barker 2021-07-20 22:16:30 +10:00 committed by Peter Barker
parent da11b6be77
commit ee069a081b
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Mount/SoloGimbal.cpp
View File

@ -28,7 +28,7 @@ bool SoloGimbal::aligned()
gimbal_mode_t SoloGimbal::get_mode() 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()))) { 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; return GIMBAL_MODE_IDLE;
@ -281,7 +281,7 @@ Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst)
float dt = _measurement.delta_time; float dt = _measurement.delta_time;
float alpha = dt/(dt+tc); 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 Tve = _ahrs.get_rotation_body_to_ned();
Matrix3f Teg; Matrix3f Teg;
quatEst.inverse().rotation_matrix(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); joint_rates_to_gimbal_ang_vel(gimbalRateDemVecBodyLock, gimbalRateDemVecBodyLock);
// Add a feedforward term from vehicle gyros // 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(); gimbalRateDemVecBodyLock += Tvg * _ahrs.get_gyro();
return gimbalRateDemVecBodyLock; return gimbalRateDemVecBodyLock;

8
libraries/AP_Mount/SoloGimbalEKF.cpp
View File

@ -76,7 +76,7 @@ void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const
bool main_ekf_healthy = false; bool main_ekf_healthy = false;
nav_filter_status main_ekf_status; 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 (_ahrs.get_filter_status(main_ekf_status)) {
if (main_ekf_status.flags.attitude) { 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 // Fuse the SoloGimbalEKF velocity estimates - this enables alevel reference to be maintained during constant turns
void SoloGimbalEKF::fuseVelocity() void SoloGimbalEKF::fuseVelocity()
{ {
const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); const auto &_ahrs = AP::ahrs();
if (!_ahrs.have_inertial_nav()) { if (!_ahrs.have_inertial_nav()) {
return; return;
@ -668,7 +668,7 @@ void SoloGimbalEKF::fuseVelocity()
// check for new magnetometer data and update store measurements if available // check for new magnetometer data and update store measurements if available
void SoloGimbalEKF::readMagData() void SoloGimbalEKF::readMagData()
{ {
const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); const auto &_ahrs = AP::ahrs();
if (_ahrs.get_compass() && if (_ahrs.get_compass() &&
_ahrs.get_compass()->use_for_yaw() && _ahrs.get_compass()->use_for_yaw() &&
@ -867,7 +867,7 @@ float SoloGimbalEKF::calcMagHeadingInnov()
Tms[1][2] = sinPhi; Tms[1][2] = sinPhi;
Tms[2][2] = cosTheta*cosPhi; 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 // 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); Vector3f earth_magfield = Vector3f(0,0,0);