Browse Source

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

gps-1.3.1
Randy Mackay 4 years ago committed by Andrew Tridgell
parent
commit
e5bde1a085
  1. 4
      libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

4
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@ -129,8 +129,8 @@ void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const @@ -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;
}

Loading…
Cancel
Save