Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: ExtNav yaw aligned when not used

This commit is contained in:
Randy Mackay 2024-03-27 14:31:38 +09:00
parent 440fb8b03b
commit 26f5490de4
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View File

@ -288,6 +288,20 @@ void AP_NavEKF_Source::align_inactive_sources()
}
}
visual_odom->align_position_to_ahrs(align_posxy, align_posz);
// consider aligning yaw:
if ((getYawSource() == SourceYaw::COMPASS) ||
(getYawSource() == SourceYaw::GPS) ||
(getYawSource() == SourceYaw::GPS_COMPASS_FALLBACK) ||
(getYawSource() == SourceYaw::GSF)) {
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].yaw == SourceYaw::EXTNAV) {
// ExtNav could potentially be used, so align it
visual_odom->request_align_yaw_to_ahrs();
break;
}
}
}
#endif
}