Browse Source

AP_NavEKF3: integrate Source for yaw

zr-v5.1
Randy Mackay 5 years ago
parent
commit
faed58a027
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 23
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
  3. 29
      libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
  4. 3
      libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
  5. 3
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
  6. 9
      libraries/AP_NavEKF3/AP_NavEKF3_core.h

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -233,7 +233,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: MAG_CAL // @Param: MAG_CAL
// @DisplayName: Magnetometer default fusion mode // @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. // @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 // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
// @User: Advanced // @User: Advanced
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT), AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),

23
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -38,14 +38,12 @@ void NavEKF3_core::controlFilterModes()
*/ */
NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const 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 // force use of simple magnetic heading fusion for specified cores
if ((magcal != MagCal::EXTERNAL_YAW) && (magcal != MagCal::EXTERNAL_YAW_FALLBACK) && (frontend->_magMask & core_index)) { if (frontend->_magMask & core_index) {
return MagCal::NEVER; return MagCal::NEVER;
} }
return magcal; return MagCal(frontend->_magCal.get());
} }
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
@ -94,7 +92,6 @@ void NavEKF3_core::setWindMagStateLearningMode()
((effectiveMagCal == MagCal::WHEN_FLYING) && inFlight) || // when flying ((effectiveMagCal == MagCal::WHEN_FLYING) && inFlight) || // when flying
((effectiveMagCal == MagCal::WHEN_MANOEUVRING) && manoeuvring) || // when manoeuvring ((effectiveMagCal == MagCal::WHEN_MANOEUVRING) && manoeuvring) || // when manoeuvring
((effectiveMagCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete ((effectiveMagCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
((effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) && inFlight) ||
(effectiveMagCal == MagCal::ALWAYS); // all the time (effectiveMagCal == MagCal::ALWAYS); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
@ -483,8 +480,14 @@ bool NavEKF3_core::readyToUseExtNav(void) const
bool NavEKF3_core::use_compass(void) const bool NavEKF3_core::use_compass(void) const
{ {
const auto *compass = dal.get_compass(); const auto *compass = dal.get_compass();
return effectiveMagCal != MagCal::EXTERNAL_YAW && const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
compass && if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) &&
(yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
// not using compass as a yaw source
return false;
}
return compass &&
compass->use_for_yaw(magSelectIndex) && compass->use_for_yaw(magSelectIndex) &&
!allMagSensorsFailed; !allMagSensorsFailed;
} }
@ -492,7 +495,8 @@ bool NavEKF3_core::use_compass(void) const
// are we using a yaw source other than the magnetomer? // are we using a yaw source other than the magnetomer?
bool NavEKF3_core::using_external_yaw(void) const bool NavEKF3_core::using_external_yaw(void) const
{ {
if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass()) { 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()) {
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
} }
return false; return false;
@ -558,7 +562,8 @@ void NavEKF3_core::checkGyroCalStatus(void)
{ {
// check delta angle bias variances // check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW) && (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK)) { const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
// which can make this check fail // which can make this check fail
Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]); Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);

29
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@ -199,17 +199,23 @@ void NavEKF3_core::SelectMagFusion()
// used for load levelling // used for load levelling
magFusePerformed = false; magFusePerformed = false;
effectiveMagCal = effective_magCal(); // get default yaw source
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource();
if (yaw_source != yaw_source_last) {
yaw_source_last = yaw_source;
yaw_source_reset = true;
}
// 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 and attempt to reset the yaw in
// flight using the output from the GSF yaw estimator. // flight using the output from the GSF yaw estimator.
if (!use_compass() && if (!use_compass() &&
effectiveMagCal != MagCal::EXTERNAL_YAW && yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL &&
effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) { yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
// because this type of reset event is not as time critical, require a continuous history of valid estimates // because this type of reset event is not as time critical, require a continuous history of valid estimates
if (!yawAlignComplete && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) { if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
yawAlignComplete = EKFGSF_resetMainFilterYaw(); yawAlignComplete = EKFGSF_resetMainFilterYaw();
yaw_source_reset = false;
} }
if (imuSampleTime_ms - lastSynthYawTime_ms > 140) { if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
@ -261,11 +267,12 @@ void NavEKF3_core::SelectMagFusion()
} }
// Handle case where we are using an external yaw sensor instead of a magnetomer // Handle case where we are using an external yaw sensor instead of a magnetomer
if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) { if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
bool have_fused_gps_yaw = false; bool have_fused_gps_yaw = false;
if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) { if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
if (tiltAlignComplete && !yawAlignComplete) { if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
alignYawAngle(); alignYawAngle();
yaw_source_reset = false;
} else if (tiltAlignComplete && yawAlignComplete) { } else if (tiltAlignComplete && yawAlignComplete) {
fuseEulerYaw(false, true); fuseEulerYaw(false, true);
} }
@ -296,7 +303,7 @@ void NavEKF3_core::SelectMagFusion()
} }
lastSynthYawTime_ms = imuSampleTime_ms; lastSynthYawTime_ms = imuSampleTime_ms;
} }
if (effectiveMagCal == MagCal::EXTERNAL_YAW) { if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL) {
// no fallback // no fallback
return; return;
} }
@ -344,9 +351,9 @@ void NavEKF3_core::SelectMagFusion()
magTimeout = true; magTimeout = true;
} }
if (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) { if (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
// check for and read new magnetometer measurements. We don't // check for and read new magnetometer measurements. We don't
// real for EXTERNAL_YAW_FALLBACK as it has already been read // read for EXTERNAL_COMPASS_FALLBACK as it has already been read
// above // above
readMagData(); readMagData();
} }
@ -356,6 +363,10 @@ void NavEKF3_core::SelectMagFusion()
// Control reset of yaw and magnetic field states if we are using compass data // Control reset of yaw and magnetic field states if we are using compass data
if (magDataToFuse) { if (magDataToFuse) {
if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS)) {
magYawResetRequest = true;
yaw_source_reset = false;
}
controlMagYawReset(); controlMagYawReset();
} }

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@ -1279,7 +1279,8 @@ float NavEKF3_core::MagDeclination(void) const
*/ */
void NavEKF3_core::updateMovementCheck(void) void NavEKF3_core::updateMovementCheck(void)
{ {
const bool runCheck = onGround && (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || !use_compass()); 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 || !use_compass());
if (!runCheck) if (!runCheck)
{ {
onGroundNotMoving = false; onGroundNotMoving = false;

3
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@ -534,8 +534,9 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
ResetPosition(resetDataSource::DEFAULT); ResetPosition(resetDataSource::DEFAULT);
ResetHeight(); ResetHeight();
// initialise the position source // initialise sources
pos_source_last = frontend->_sources.getPosXYSource(); pos_source_last = frontend->_sources.getPosXYSource();
yaw_source_last = frontend->_sources.getYawSource();
// define Earth rotation vector in the NED navigation frame // define Earth rotation vector in the NED navigation frame
calcEarthRateNED(earthRateNED, dal.get_home().lat); calcEarthRateNED(earthRateNED, dal.get_home().lat);

9
libraries/AP_NavEKF3/AP_NavEKF3_core.h

@ -418,9 +418,7 @@ public:
WHEN_MANOEUVRING = 1, WHEN_MANOEUVRING = 1,
NEVER = 2, NEVER = 2,
AFTER_FIRST_CLIMB = 3, AFTER_FIRST_CLIMB = 3,
ALWAYS = 4, ALWAYS = 4
EXTERNAL_YAW = 5,
EXTERNAL_YAW_FALLBACK = 6,
}; };
// are we using an external yaw source? This is needed by AHRS attitudes_consistent check // are we using an external yaw source? This is needed by AHRS attitudes_consistent check
@ -1025,7 +1023,7 @@ private:
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive. float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
MagCal effectiveMagCal; // the actual mag calibration and yaw fusion method being used as the default MagCal effectiveMagCal; // the actual mag calibration being used as the default
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
uint32_t lastMagUpdate_us; // last time compass was updated in usec uint32_t lastMagUpdate_us; // last time compass was updated in usec
@ -1477,5 +1475,6 @@ private:
// source reset handling // source reset handling
AP_NavEKF_Source::SourceXY pos_source_last; // position source on previous iteration (used to detect a changes) AP_NavEKF_Source::SourceXY pos_source_last; // position source on previous iteration (used to detect a changes)
bool pos_source_reset; // true when the position source has changed but the position has not yet been reset bool pos_source_reset; // true when the position source has changed but the position has not yet been reset
AP_NavEKF_Source::SourceYaw yaw_source_last; // yaw source on previous iteration (used to detect a change)
bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset
}; };

Loading…
Cancel
Save