Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: improve MAG_CAL vs EK3_SRCn_YAW checks

Browse Source 
MAG_CAL param description include deprecated values
Pre-arm check of MAG_CAL using deprecated values
effective_magCal interprets 5 (was EXTERNAL_YAW) as Never, 6 (was EXTERNAL_YAW_FALLBACK) as WhenFlying
Update comments in param conversion from MAG_CAL to EK3_SRC1_YAW
This commit is contained in:
Randy Mackay 2020-11-20 10:57:12 +09:00
parent 36c946113d
commit 04e3623620
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -234,7 +234,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: MAG_CAL
// @DisplayName: Magnetometer default fusion mode
// @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. EK3_MAG_CAL = 5 uses an external yaw sensor with simple heading fusion. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. EK3_MAG_CAL = 6 uses an external yaw sensor with fallback to compass when the external sensor is not available if we are flying. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting all COMPASS_USE, COMPASS_USE2, COMPASS_USE3, etc parameters to 0 and setting COMPASS_ENABLE to 0. If this is done, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK. A yaw angle derived from IMU and GPS velocity data using a Gaussian Sum Filter (GSF) will then be used to align the yaw when flight commences and there is sufficient movement.
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor (Deprecated in 4.1+ see EK3_SRCn_YAW),6:External yaw sensor with compass fallback (Deprecated in 4.1+ see EK3_SRCn_YAW)
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),
@ -1025,6 +1025,15 @@ bool NavEKF3::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
return false;
}
// check if using compass (i.e. EK3_SRCn_YAW) with deprecated MAG_CAL values (5 was EXTERNAL_YAW, 6 was EXTERNAL_YAW_FALLBACK)
const int8_t magCalParamVal = _magCal.get();
const AP_NavEKF_Source::SourceYaw yaw_source = sources.getYawSource();
if (((magCalParamVal == 5) || (magCalParamVal == 6)) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL)) {
// yaw source is configured to use compass but MAG_CAL valid is deprecated
AP::dal().snprintf(failure_msg, failure_msg_len, "EK3_MAG_CAL and EK3_SRC1_YAW inconsistent");
return false;
}
if (!core) {
AP::dal().snprintf(failure_msg, failure_msg_len, "no EKF3 cores");
return false;
@ -1682,11 +1691,11 @@ void NavEKF3::convert_parameters()
// use EK3_MAG_CAL to set EK3_SRC1_YAW
switch (_magCal.get()) {
case 5:
// EK3_MAG_CAL = 5 (External Yaw sensor). We rely on effective_magCal to interpret old "5" values as Never
// EK3_MAG_CAL = 5 (External Yaw sensor). We rely on effective_magCal to interpret old "5" values as "Never"
AP_Param::set_and_save_by_name("EK3_SRC1_YAW", (int8_t)AP_NavEKF_Source::SourceYaw::EXTERNAL);
break;
case 6:
// EK3_MAG_CAL = 6 (ExtYaw with Compass fallback). We rely on effective_magCal to interpret old "5" values as Never
// EK3_MAG_CAL = 6 (ExtYaw with Compass fallback). We rely on effective_magCal to interpret old "6" values as "When Flying"
AP_Param::set_and_save_by_name("EK3_SRC1_YAW", (int8_t)AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK);
break;
default:

5
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -45,9 +45,12 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const
// handle deprecated MagCal::EXTERNAL_YAW and MagCal::EXTERNAL_YAW_FALLBACK values
const int8_t magCalParamVal = frontend->_magCal.get();
if ((magCalParamVal == 5) || (magCalParamVal == 6)) {
if (magCalParamVal == 5) {
return MagCal::NEVER;
}
if (magCalParamVal == 6) {
return MagCal::WHEN_FLYING;
}
return MagCal(magCalParamVal);
}