AP_VisualOdom: use AP_AHRS::using_extnav_for_yaw to protect against aligning to oneself

This commit is contained in:
Randy Mackay 2021-08-18 16:45:49 +09:00 committed by Andrew Tridgell
parent 1c08866342
commit e5bde1a085
1 changed files with 2 additions and 2 deletions

View File

@ -129,8 +129,8 @@ void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const
// use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude
bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude)
{
// do not align to ahrs if it is using us as its yaw source
if (AP::ahrs().using_noncompass_for_yaw()) {
// do not align to ahrs if we are its yaw source
if (AP::ahrs().using_extnav_for_yaw()) {
return false;
}