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

AP_NavEKF2: added EK2_OPTIONS 

this allows for disabling of external navigation data (such as visual
odomotry) in EKF2. This is very useful when using EKF2 as a fallback
and truth reference when testing visual odomotry on EKF3
This commit is contained in:
Andrew Tridgell 2024-08-16 08:31:14 +10:00 committed by Peter Barker
parent 2e1364e3b0
commit 1b4fb403f2
2 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -598,6 +598,13 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("GSF_RST_MAX", 57, NavEKF2, _gsfResetMaxCount, 2),
// @Param: OPTIONS
// @DisplayName: Optional EKF behaviour
// @Description: optional EKF2 behaviour. Disabling external navigation prevents use of external vision data in the EKF2 solution
// @Bitmask: 0:DisableExternalNavigation
// @User: Advanced
AP_GROUPINFO("OPTIONS", 58, NavEKF2, _options, 0),
AP_GROUPEND
};
@ -1514,8 +1521,7 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
AP::dal().writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
if (core) {
if (!option_is_set(Option::DisableExternalNav) && core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
}
@ -1568,8 +1574,7 @@ void NavEKF2::writeDefaultAirSpeed(float airspeed)
void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
AP::dal().writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
if (core) {
if (!option_is_set(Option::DisableExternalNav) && core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
}

11
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -364,7 +364,18 @@ private:
AP_Int8 _gsfRunMask; // mask controlling which EKF2 instances run a separate EKF-GSF yaw estimator
AP_Int8 _gsfUseMask; // mask controlling which EKF2 instances will use EKF-GSF yaw estimator data to assit with yaw resets
AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF2 is allowed to reset it's yaw to the EKF-GSF estimate
AP_Int32 _options; // optional behaviour bitmask
// enum for processing options
enum class Option {
DisableExternalNav = (1U<<0),
};
// return true if an option is set
bool option_is_set(Option option) const {
return (uint32_t(option) & uint32_t(_options)) != 0;
}
// Possible values for _flowUse
#define FLOW_USE_NONE 0
#define FLOW_USE_NAV 1