mirror of https://github.com/ArduPilot/ardupilot
AP_VisualOdom: use AP_AHRS::using_extnav_for_yaw to protect against aligning to oneself
This commit is contained in:
parent
1c08866342
commit
e5bde1a085
|
@ -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…
Reference in New Issue