diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 20d1f35588..509ec136c5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "AP_DAL/AP_DAL.h" @@ -1591,6 +1592,95 @@ bool NavEKF3::getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_co return false; } +// parameter conversion of EKF3 parameters +void NavEKF3::convert_parameters() +{ + // exit immediately if param conversion has been done before + if (_sources.any_params_configured_in_storage()) { + return; + } + + // find EKF3's top level key + uint16_t k_param_ekf3; + if (!AP_Param::find_top_level_key_by_pointer(this, k_param_ekf3)) { + return; + } + + // use EK3_GPS_TYPE to set EK3_SRC1_POSXY, EK3_SRC1_VELXY, EK3_SRC1_VELZ + const AP_Param::ConversionInfo gps_type_info = {k_param_ekf3, 1, AP_PARAM_INT8, "EK3_GPS_TYPE"}; + AP_Int8 gps_type_old; + if (AP_Param::find_old_parameter(&gps_type_info, &gps_type_old)) { + switch (gps_type_old.get()) { + case 0: + // EK3_GPS_TYPE == 0 (GPS 3D Vel and 2D Pos), the default so do nothing + // sources default to EK3_SRC1_POSXY = GPS, EK3_SRC1_VELXY = GPS, EK3_SRC1_VELZ = GPS + break; + case 1: + // EK3_GPS_TYPE == 1 (GPS 2D Vel and 2D Pos) then EK3_SRC1_POSXY = GPS(1), EK3_SRC1_VELXY = GPS(1), EK3_SRC1_VELZ = NONE(0) + AP_Param::set_and_save_by_name("EK3_SRC1_POSXY", (int8_t)AP_NavEKF_Source::SourceXY::GPS); + AP_Param::set_and_save_by_name("EK3_SRC1_VELXY", (int8_t)AP_NavEKF_Source::SourceXY::GPS); + AP_Param::set_and_save_by_name("EK3_SRC1_VELZ", (int8_t)AP_NavEKF_Source::SourceZ::NONE); + break; + case 2: + // EK3_GPS_TYPE == 2 (GPS 2D Pos) then EK3_SRC1_POSXY = GPS(1), EK3_SRC1_VELXY = None(0), EK3_SRC1_VELZ = NONE(0) + AP_Param::set_and_save_by_name("EK3_SRC1_POSXY", (int8_t)AP_NavEKF_Source::SourceXY::GPS); + AP_Param::set_and_save_by_name("EK3_SRC1_VELXY", (int8_t)AP_NavEKF_Source::SourceXY::NONE); + AP_Param::set_and_save_by_name("EK3_SRC1_VELZ", (int8_t)AP_NavEKF_Source::SourceZ::NONE); + break; + case 3: + default: + // EK3_GPS_TYPE == 3 (No GPS) we don't know what to do, could be optical flow or external nav + AP_BoardConfig::config_error("Configure EK3_SRC1_POSXY and EK3_SRC1_VELXY"); + break; + } + } + + // use EK3_ALT_SOURCE to set EK3_SRC1_POSZ + const AP_Param::ConversionInfo alt_source_info = {k_param_ekf3, 9, AP_PARAM_INT8, "EK3_ALT_SOURCE"}; + AP_Int8 alt_source_old; + if (AP_Param::find_old_parameter(&alt_source_info, &alt_source_old)) { + switch (alt_source_old.get()) { + case 0: + // EK3_ALT_SOURCE = BARO, the default so do nothing + break; + case 1: + // EK3_ALT_SOURCE == 1 (RangeFinder) + AP_Param::set_and_save_by_name("EK3_SRC1_POSZ", (int8_t)AP_NavEKF_Source::SourceZ::RANGEFINDER); + break; + case 2: + // EK3_ALT_SOURCE == 2 (GPS) + AP_Param::set_and_save_by_name("EK3_SRC1_POSZ", (int8_t)AP_NavEKF_Source::SourceZ::GPS); + break; + case 3: + // EK3_ALT_SOURCE == 3 (Beacon) + AP_Param::set_and_save_by_name("EK3_SRC1_POSZ", (int8_t)AP_NavEKF_Source::SourceZ::BEACON); + break; + case 4: + // EK3_ALT_SOURCE == 4 (ExtNav) + AP_Param::set_and_save_by_name("EK3_SRC1_POSZ", (int8_t)AP_NavEKF_Source::SourceZ::EXTNAV); + break; + default: + // do nothing + break; + } + } + + // 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 + 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 + AP_Param::set_and_save_by_name("EK3_SRC1_YAW", (int8_t)AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK); + break; + default: + // do nothing + break; + } +} + // return data for debugging range beacon fusion bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow, Vector3f &posNED) const diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 1010f0e876..f6ac326835 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -427,6 +427,9 @@ public: // return false if data not available bool getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const; + // parameter conversion + void convert_parameters(); + private: uint8_t num_cores; // number of allocated cores uint8_t primary; // current primary core diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 335e76e9b9..666152f2d7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -43,7 +43,13 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const return MagCal::NEVER; } - return MagCal(frontend->_magCal.get()); + // handle deprecated values + const int8_t magCalParamVal = frontend->_magCal.get(); + if ((magCalParamVal == 5) || (magCalParamVal == 6)) { + return MagCal::NEVER; + } + + return MagCal(magCalParamVal); } // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 89f60f9a87..31b2be1654 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -423,6 +423,8 @@ public: NEVER = 2, AFTER_FIRST_CLIMB = 3, ALWAYS = 4 + // 5 was EXTERNAL_YAW (do not use) + // 6 was EXTERNAL_YAW_FALLBACK (do not use) }; // are we using an external yaw source? This is needed by AHRS attitudes_consistent check