Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Enable simple heading fusion to be set for specific cores

Browse Source
This commit is contained in:
priseborough 2017-02-15 14:14:32 +11:00 committed by Francisco Ferreira
parent ccb952ba31
commit de7a679534
3 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -234,8 +234,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
AP_GROUPINFO("MAG_M_NSE", 13, NavEKF3, _magNoise, MAG_M_NSE_DEFAULT),
// @Param: MAG_CAL
// @DisplayName: Magnetometer calibration mode
// @Description: EKF_MAG_CAL = 0 enables calibration when airborne and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration when manoeuvreing. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition, is recommended if the external magnetic field is varying and is the default for rovers. EKF_MAG_CAL = 3 enables calibration when the first in-air field and yaw reset has completed and is the default for copters. EKF_MAG_CAL = 4 enables calibration all the time. This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states. This model is only suitable for use when the external magnetic field environment is stable.
// @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. 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.
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
// @User: Advanced
AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),
@ -524,6 +524,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m/s/s
AP_GROUPINFO("ACC_BIAS_LIM", 48, NavEKF3, _accBiasLim, 1.0f),
// @Param: MAG_MASK
// @DisplayName: Bitmask of active EKF cores that will always use heading fusion
// @Description: 1 byte bitmap of EKF cores that will disable magnetic field states and use simple magnetic heading fusion at all times. This parameter enables specified cores to be used as a backup for flight into an environment with high levels of external magnetic interference which may degrade the EKF attitude estimate when using 3-axis magnetometer fusion. NOTE : Use of a different magnetometer fusion algorithm on different cores makes unwanted EKF core switches due to magnetometer errors more likely.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced
AP_GROUPINFO("MAG_MASK", 49, NavEKF3, _magMask, 0),
AP_GROUPEND
};

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -358,6 +358,7 @@ private:
AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec)
AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
AP_Float _accBiasLim; // Accelerometer bias limit (m/s/s)
AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion.
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

7
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -45,7 +45,12 @@ void NavEKF3_core::controlFilterModes()
*/
uint8_t NavEKF3_core::effective_magCal(void) const
{
return frontend->_magCal;
// force use of simple magnetic heading fusion for specified cores
if (frontend->_magMask & core_index) {
return 2;
} else {
return frontend->_magCal;
}
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to