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

AP_NavEKF3: use enum class for EK3_MAG_CAL values 

and use effective_magCal() everywhere for consistency
This commit is contained in:
Andrew Tridgell 2020-01-20 15:52:12 +11:00
parent e8fb082a9a
commit a3100251a8
3 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -40,14 +40,13 @@ void NavEKF3_core::controlFilterModes()
/*
return effective value for _magCal for this core
*/
uint8_t NavEKF3_core::effective_magCal(void) const
NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const
{
// force use of simple magnetic heading fusion for specified cores
if (frontend->_magMask & core_index) {
return 2;
} else {
return frontend->_magCal;
return MagCal::NEVER;
}
return MagCal(frontend->_magCal.get());
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
@ -92,16 +91,16 @@ void NavEKF3_core::setWindMagStateLearningMode()
}
// Determine if learning of magnetic field states has been requested by the user
uint8_t magCal = effective_magCal();
MagCal magCal = effective_magCal();
bool magCalRequested =
((magCal == 0) && inFlight) || // when flying
((magCal == 1) && manoeuvring) || // when manoeuvring
((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
(magCal == 4); // all the time
((magCal == MagCal::WHEN_FLYING) && inFlight) || // when flying
((magCal == MagCal::WHEN_MANOEUVRING) && manoeuvring) || // when manoeuvring
((magCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
(magCal == MagCal::ALWAYS); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
bool magCalDenied = !use_compass() || (magCal == MagCal::NEVER) || (onGround && magCal != MagCal::ALWAYS);
// Inhibit the magnetic field calibration if not requested or denied
bool setMagInhibit = !magCalRequested || magCalDenied;
@ -450,7 +449,7 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const
// return true if we should use the compass
bool NavEKF3_core::use_compass(void) const
{
return (frontend->_magCal != 5) && _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
return effective_magCal() != MagCal::EXTERNAL_YAW && _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
}
/*

2
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -264,7 +264,7 @@ void NavEKF3_core::SelectMagFusion()
magFusePerformed = false;
// Handle case where we are using an external yaw sensor instead of a magnetomer
if ((frontend->_magCal == 5)) {
if (effective_magCal() == MagCal::EXTERNAL_YAW) {
if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
if (tiltAlignComplete && !yawAlignComplete) {
alignYawAngle();

12
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -367,6 +367,16 @@ public:
// get timing statistics structure
void getTimingStatistics(struct ekf_timing &timing);
// values for EK3_MAG_CAL
enum class MagCal {
WHEN_FLYING = 0,
WHEN_MANOEUVRING = 1,
NEVER = 2,
AFTER_FIRST_CLIMB = 3,
ALWAYS = 4,
EXTERNAL_YAW = 5,
};
private:
// Reference to the global EKF frontend for parameters
NavEKF3 *frontend;
@ -830,7 +840,7 @@ private:
void recordMagReset();
// effective value of MAG_CAL
uint8_t effective_magCal(void) const;
MagCal effective_magCal(void) const;
// calculate the variances for the rotation vector equivalent
Vector3f calcRotVecVariances(void);