Browse Source

AP_NavEKF3: Simplify setting EK3_MAG_CAL

Don't require user to separately set EK3_MAG_CAL to fly without a magnetomer
c415-sdk
Paul Riseborough 5 years ago committed by Peter Barker
parent
commit
9a4108f55e
  1. 4
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 9
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
  3. 4
      libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
  4. 6
      libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
  5. 1
      libraries/AP_NavEKF3/AP_NavEKF3_core.h

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -244,8 +244,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { @@ -244,8 +244,8 @@ 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. EK3_MAG_CAL = 7 uses a yaw angle derived from IMU and GPS velocity data using a Gaussian Sum Filter (GSF). NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: When using EK3_MAG_CAL = 7, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK and COMPASS_ENABLE must be set to 0 and yaw alignment will not occur until after takeoff with sufficient horizontal movement.
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor,6:External yaw sensor with compass fallback,7:Use GSF yaw estimate
// @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,5:Use external yaw sensor,6:External yaw sensor with compass fallback
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),

9
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -45,7 +45,7 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const @@ -45,7 +45,7 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const
MagCal magcal = MagCal(frontend->_magCal.get());
// force use of simple magnetic heading fusion for specified cores
if ((magcal != MagCal::EXTERNAL_YAW) && (magcal != MagCal::EXTERNAL_YAW_FALLBACK) && (magcal != MagCal::GSF_YAW) && (frontend->_magMask & core_index)) {
if ((magcal != MagCal::EXTERNAL_YAW) && (magcal != MagCal::EXTERNAL_YAW_FALLBACK) && (frontend->_magMask & core_index)) {
return MagCal::NEVER;
}
@ -286,7 +286,7 @@ void NavEKF3_core::setAidingMode() @@ -286,7 +286,7 @@ void NavEKF3_core::setAidingMode()
// This is a special case for when we are a fixed wing aircraft vehicle that has landed and
// has no yaw measurement that works when static. Declare the yaw as unaligned (unknown)
// and declare attitude aiding loss so that we fall back into a non-aiding mode
if (assume_zero_sideslip() && onGround && (effectiveMagCal == MagCal::GSF_YAW) && !use_compass()){
if (assume_zero_sideslip() && onGround && !use_compass()){
yawAlignComplete = false;
finalInflightYawInit = false;
attAidLossCritical = true;
@ -479,7 +479,6 @@ bool NavEKF3_core::readyToUseExtNav(void) const @@ -479,7 +479,6 @@ bool NavEKF3_core::readyToUseExtNav(void) const
bool NavEKF3_core::use_compass(void) const
{
return effectiveMagCal != MagCal::EXTERNAL_YAW &&
effectiveMagCal != MagCal::GSF_YAW &&
_ahrs->get_compass() &&
_ahrs->get_compass()->use_for_yaw(magSelectIndex) &&
!allMagSensorsFailed;
@ -488,7 +487,7 @@ bool NavEKF3_core::use_compass(void) const @@ -488,7 +487,7 @@ bool NavEKF3_core::use_compass(void) const
// are we using a yaw source other than the magnetomer?
bool NavEKF3_core::using_external_yaw(void) const
{
if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || effectiveMagCal == MagCal::GSF_YAW) {
if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass()) {
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
}
return false;
@ -553,7 +552,7 @@ void NavEKF3_core::checkGyroCalStatus(void) @@ -553,7 +552,7 @@ void NavEKF3_core::checkGyroCalStatus(void)
{
// check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
if (effectiveMagCal == MagCal::GSF_YAW) {
if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW)) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass
// which can make this check fail
Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);

4
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@ -261,7 +261,7 @@ void NavEKF3_core::SelectMagFusion() @@ -261,7 +261,7 @@ void NavEKF3_core::SelectMagFusion()
// Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in
// flight using the output from the GSF yaw estimator.
if ((effectiveMagCal == MagCal::GSF_YAW || !use_compass()) &&
if (!use_compass() &&
effectiveMagCal != MagCal::EXTERNAL_YAW &&
effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) {
@ -1496,7 +1496,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() @@ -1496,7 +1496,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
EKFGSF_yaw_reset_ms = imuSampleTime_ms;
EKFGSF_yaw_reset_count++;
if (effectiveMagCal == MagCal::GSF_YAW || AP::compass().get_num_enabled() == 0) {
if (!use_compass() || AP::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);

6
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@ -1150,10 +1150,8 @@ float NavEKF3_core::MagDeclination(void) const @@ -1150,10 +1150,8 @@ float NavEKF3_core::MagDeclination(void) const
*/
void NavEKF3_core::updateMovementCheck(void)
{
if (!onGround &&
effectiveMagCal != MagCal::EXTERNAL_YAW &&
effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK &&
effectiveMagCal != MagCal::GSF_YAW)
const bool runCheck = onGround && (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass());
if (!runCheck)
{
onGroundNotMoving = false;
return;

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h

@ -426,7 +426,6 @@ public: @@ -426,7 +426,6 @@ public:
ALWAYS = 4,
EXTERNAL_YAW = 5,
EXTERNAL_YAW_FALLBACK = 6,
GSF_YAW = 7,
};
// are we using an external yaw source? This is needed by AHRS attitudes_consistent check

Loading…
Cancel
Save