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

AP_NavEKF3: rename source and yawFusionMethod from EXTERNAL to GPS

This commit is contained in:
Randy Mackay 2020-12-22 17:08:30 +09:00 committed by Andrew Tridgell
parent 77af6df730
commit 27c998ad94
6 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1070,7 +1070,7 @@ bool NavEKF3::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
// 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)) {
if (((magCalParamVal == 5) || (magCalParamVal == 6)) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS)) {
// 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;
@ -1672,11 +1672,11 @@ void NavEKF3::convert_parameters()
switch (_magCal.get()) {
case 5:
// 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);
AP_Param::set_and_save_by_name("EK3_SRC1_YAW", (int8_t)AP_NavEKF_Source::SourceYaw::GPS);
break;
case 6:
// 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);
AP_Param::set_and_save_by_name("EK3_SRC1_YAW", (int8_t)AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK);
break;
default:
// do nothing

6
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -513,7 +513,7 @@ bool NavEKF3_core::use_compass(void) const
{
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) &&
(yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
(yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) {
// not using compass as a yaw source
return false;
}
@ -528,7 +528,7 @@ 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 ||
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_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;
}
@ -599,7 +599,7 @@ void NavEKF3_core::checkGyroCalStatus(void)
// check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
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) &&
if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) &&
(yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
// 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

22
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -223,8 +223,8 @@ void NavEKF3_core::SelectMagFusion()
// flight using the output from the GSF yaw estimator.
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::GPS &&
yaw_source != AP_NavEKF_Source::SourceYaw::GPS_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
@ -259,14 +259,14 @@ void NavEKF3_core::SelectMagFusion()
}
// Handle case where we are using GPS yaw sensor instead of a magnetomer
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
bool have_fused_gps_yaw = false;
if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
alignYawAngle(yawAngDataDelayed);
yaw_source_reset = false;
} else if (tiltAlignComplete && yawAlignComplete) {
fuseEulerYaw(yawFusionMethod::EXTERNAL);
fuseEulerYaw(yawFusionMethod::GPS);
}
have_fused_gps_yaw = true;
last_gps_yaw_fusion_ms = imuSampleTime_ms;
@ -286,7 +286,7 @@ void NavEKF3_core::SelectMagFusion()
lastSynthYawTime_ms = imuSampleTime_ms;
}
}
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL) {
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS) {
// no fallback
return;
}
@ -362,9 +362,9 @@ void NavEKF3_core::SelectMagFusion()
magTimeout = true;
}
if (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
if (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
// check for and read new magnetometer measurements. We don't
// read for EXTERNAL_COMPASS_FALLBACK as it has already been read
// read for GPS_COMPASS_FALLBACK as it has already been read
// above
readMagData();
}
@ -375,7 +375,7 @@ void NavEKF3_core::SelectMagFusion()
// Control reset of yaw and magnetic field states if we are using compass data
if (magDataToFuse) {
if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS ||
yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) {
magYawResetRequest = true;
yaw_source_reset = false;
}
@ -887,7 +887,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
// yaw measurement error variance (rad^2)
float R_YAW;
switch (method) {
case yawFusionMethod::EXTERNAL:
case yawFusionMethod::GPS:
R_YAW = sq(yawAngDataDelayed.yawAngErr);
break;
@ -913,7 +913,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
// determine if a 321 or 312 Euler sequence is best
rotationOrder order;
switch (method) {
case yawFusionMethod::EXTERNAL:
case yawFusionMethod::GPS:
order = yawAngDataDelayed.order;
break;
@ -1074,7 +1074,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
break;
}
case yawFusionMethod::EXTERNAL:
case yawFusionMethod::GPS:
// both external sensor yaw and last yaw when static are stored in yawAngDataDelayed.yawAng
innovYaw = wrap_PI(yawAngPredicted - yawAngDataDelayed.yawAng);
break;

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -1290,7 +1290,7 @@ float NavEKF3_core::MagDeclination(void) const
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 ||
const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass());
if (!runCheck)
{

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -123,7 +123,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
// initialise to same length of IMU to allow for multiple wheel sensors
return false;
}
if(frontend->sources.ext_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) {
if(frontend->sources.gps_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) {
return false;
}
// Note: the use of dual range finders potentially doubles the amount of data to be stored

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -621,7 +621,7 @@ private:
// specifies the method to be used when fusing yaw observations
enum class yawFusionMethod {
MAGNETOMETER=0,
EXTERNAL=1,
GPS=1,
GSF=2,
STATIC=3,
PREDICTED=4,