mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_VisualOdom: fix T265 criteria for aligning to AHRS
This commit is contained in:
parent
f53242b323
commit
1ee7f00192
@ -135,8 +135,13 @@ bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position,
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// if ahrs's yaw is from the compass, wait until it has been initialised
|
// do not align to ahrs if it is using us as its yaw source
|
||||||
if (!AP::ahrs().is_ext_nav_used_for_yaw() && !AP::ahrs().yaw_initialised()) {
|
if (AP::ahrs().is_ext_nav_used_for_yaw()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// do not align until ahrs yaw initialised
|
||||||
|
if (!AP::ahrs().initialised() || !AP::ahrs().yaw_initialised()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user