aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: add convert_params for source

Browse Source
This commit is contained in:
Randy Mackay 2020-08-10 13:36:53 +09:00
parent 849e94cc24
commit 6c0bb6a198
4 changed files with 102 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -4,6 +4,7 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#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

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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

8
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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