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

AP_NavEKF: added functions to see if a nav source is enabled 

this allows us to avoid initialising ring buffers when not needed
This commit is contained in:
Andrew Tridgell 2020-11-27 09:37:13 +11:00
parent ab917ed92e
commit 4fdbbd8984
2 changed files with 57 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View File

@ -477,3 +477,50 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
return true;
}
// return true if ext nav is enabled on any source
bool AP_NavEKF_Source::ext_nav_enabled(void) const
{
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
const auto &src = _source_set[i];
if (SourceXY(src.posxy.get()) == SourceXY::EXTNAV) {
return true;
}
if (SourceZ(src.posz.get()) == SourceZ::EXTNAV) {
return true;
}
if (SourceXY(src.velxy.get()) == SourceXY::EXTNAV) {
return true;
}
if (SourceZ(src.velz.get()) == SourceZ::EXTNAV) {
return true;
}
}
return false;
}
// return true if wheel encoder is enabled on any source
bool AP_NavEKF_Source::wheel_encoder_enabled(void) const
{
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
const auto &src = _source_set[i];
if (SourceXY(src.velxy.get()) == SourceXY::WHEEL_ENCODER) {
return true;
}
}
return false;
}
// return true if ext yaw is enabled on any source
bool AP_NavEKF_Source::ext_yaw_enabled(void) const
{
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
const auto &src = _source_set[i];
const SourceYaw yaw = SourceYaw(src.yaw.get());
if (yaw == SourceYaw::EXTERNAL ||
yaw == SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
return true;
}
}
return false;
}

11
libraries/AP_NavEKF/AP_NavEKF_Source.h
View File

@ -93,10 +93,19 @@ public:
static const struct AP_Param::GroupInfo var_info[];
// return true if ext nav is enabled on any source
bool ext_nav_enabled(void) const;
// return true if ext yaw is enabled on any source
bool ext_yaw_enabled(void) const;
// return true if wheel encoder is enabled on any source
bool wheel_encoder_enabled(void) const;
private:
// Parameters
struct {
struct SourceSet {
AP_Int8 posxy; // xy position source
AP_Int8 velxy; // xy velocity source
AP_Int8 posz; // position z (aka altitude or height) source