Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: added EK3_PRIMARY parameter

Browse Source 
allows for selection of which IMU to use on startup
This commit is contained in:
Andrew Tridgell 2021-08-16 16:04:38 +10:00
parent 8bd04c17c8
commit 5c86cc6828
2 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -704,6 +704,14 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("GND_EFF_DZ", 7, NavEKF3, _baroGndEffectDeadZone, 4.0f), AP_GROUPINFO("GND_EFF_DZ", 7, NavEKF3, _baroGndEffectDeadZone, 4.0f),
// @Param: PRIMARY
// @DisplayName: Primary core number
// @Description: The core number (index in IMU mask) that will be used as the primary EKF core on startup. While disarmed the EKF will force the use of this core. A value of 0 corresponds to the first IMU in EK3_IMU_MASK.
// @Range: 0 2
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("PRIMARY", 8, NavEKF3, _primary_core, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -810,8 +818,8 @@ bool NavEKF3::InitialiseFilter(void)
// set relative error scores for all cores to 0 // set relative error scores for all cores to 0
resetCoreErrors(); resetCoreErrors();
// Set the primary initially to be the lowest index // Set the primary initially to be users selected primary
primary = 0; primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;
// invalidate shared origin // invalidate shared origin
common_origin_valid = false; common_origin_valid = false;
@ -956,16 +964,17 @@ void NavEKF3::UpdateFilter(void)
} }
} }
if (primary != 0 && core[0].healthy() && !armed) { const uint8_t user_primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;
// when on the ground and disarmed force the first lane. This if (primary != 0 && core[user_primary].healthy() && !armed) {
// avoids us ending with with a lottery for which IMU is used // when on the ground and disarmed force the selected primary
// in each flight. Otherwise the alignment of the timing of // core. This avoids us ending with with a lottery for which
// the lane updates with the timing of GPS updates can lead to // IMU is used in each flight. Otherwise the alignment of the
// a lane other than the first one being used as primary for // timing of the core selection updates with the timing of GPS
// some flights. As different IMUs may have quite different // updates can lead to a core other than the first one being
// noise characteristics this leads to inconsistent // used as primary for some flights. As different IMUs may
// performance // have quite different noise characteristics this leads to
primary = 0; // inconsistent performance
primary = user_primary;
} }
// align position of inactive sources to ahrs // align position of inactive sources to ahrs

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -427,6 +427,7 @@ private:
AP_Int8 _betaMask; // Bitmask controlling when sideslip angle fusion is used to estimate non wind states AP_Int8 _betaMask; // Bitmask controlling when sideslip angle fusion is used to estimate non wind states
AP_Float _ognmTestScaleFactor; // Scale factor applied to the thresholds used by the on ground not moving test AP_Float _ognmTestScaleFactor; // Scale factor applied to the thresholds used by the on ground not moving test
AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m) AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m)
AP_Int8 _primary_core; // initial core number
// Possible values for _flowUse // Possible values for _flowUse
#define FLOW_USE_NONE 0 #define FLOW_USE_NONE 0