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

AP_NavEKF3: add support for GSF as yaw source

This commit is contained in:
Randy Mackay 2020-12-22 16:15:43 +09:00 committed by Andrew Tridgell
parent 0fb168d6ab
commit c14b4a8b6c
3 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -528,7 +528,8 @@ bool NavEKF3_core::use_compass(void) const
bool NavEKF3_core::using_external_yaw(void) const
{
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()) {
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
}
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {

14
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -219,12 +219,13 @@ void NavEKF3_core::SelectMagFusion()
yawAngDataStatic.time_ms = imuDataDelayed.time_ms;
}
// Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in
// Handle case where we are not using a yaw sensor of any type and attempt to reset the yaw in
// flight using the output from the GSF yaw estimator.
if (!use_compass() &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV) {
if ((yaw_source == AP_NavEKF_Source::SourceYaw::GSF) ||
(!use_compass() &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
// because this type of reset event is not as time critical, require a continuous history of valid estimates
if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
@ -1518,7 +1519,8 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
EKFGSF_yaw_reset_ms = imuSampleTime_ms;
EKFGSF_yaw_reset_count++;
if (!use_compass() || dal.compass().get_num_enabled() == 0) {
if ((frontend->sources.getYawSource() == AP_NavEKF_Source::SourceYaw::GSF) ||
!use_compass() || (dal.compass().get_num_enabled() == 0)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset",(unsigned)imu_index);

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -1291,7 +1291,7 @@ void NavEKF3_core::updateMovementCheck(void)
{
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || !use_compass());
yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass());
if (!runCheck)
{
onGroundNotMoving = false;