mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-15 14:53:32 +08:00
AP_NavEKF: Allow EK3 cores to run different sources
This commit is contained in:
rishabsingh3003
committed by
Andrew Tridgell
Andrew Tridgell
parent
4f20cc2442
commit
f172c2fd03
@@ -134,8 +134,8 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
|
||||
|
||||
// @Param: _OPTIONS
|
||||
// @DisplayName: EKF Source Options
|
||||
// @Description: EKF Source Options
|
||||
// @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow
|
||||
// @Description: EKF Source Options. Bit 0: Fuse all velocity sources present in EK3_SRCx_VEL_. Bit 1: Align external navigation position when using optical flow. Bit 3: Use SRC per core. By default, EKF source selection is controlled via the EK3_SRC parameters, allowing only one source to be active at a time across all cores (switchable via MAVLink, Lua, or RC). Enabling this bit maps EKF core 1 to SRC1, core 2 to SRC2, etc., allowing each core to run independently with a dedicated source.
|
||||
// @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow, 3: UsePerCoreEKFSources
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES),
|
||||
|
||||
@@ -165,14 +165,14 @@ void AP_NavEKF_Source::setPosVelYawSourceSet(AP_NavEKF_Source::SourceSetSelectio
|
||||
}
|
||||
|
||||
// true/false of whether velocity source should be used
|
||||
bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
|
||||
bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source, uint8_t core_index) const
|
||||
{
|
||||
if (velxy_source == _source_set[active_source_set].velxy) {
|
||||
if (velxy_source == _source_set[getActiveSourceSet(core_index)].velxy) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// check for fuse all velocities
|
||||
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
|
||||
if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
|
||||
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
|
||||
if (_source_set[i].velxy == velxy_source) {
|
||||
return true;
|
||||
@@ -184,14 +184,14 @@ bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
|
||||
bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source, uint8_t core_index) const
|
||||
{
|
||||
if (velz_source == _source_set[active_source_set].velz) {
|
||||
if (velz_source == _source_set[getActiveSourceSet(core_index)].velz) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// check for fuse all velocities
|
||||
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
|
||||
if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
|
||||
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
|
||||
if (_source_set[i].velz == velz_source) {
|
||||
return true;
|
||||
@@ -204,14 +204,14 @@ bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
|
||||
}
|
||||
|
||||
// true if a velocity source is configured
|
||||
bool AP_NavEKF_Source::haveVelZSource() const
|
||||
bool AP_NavEKF_Source::haveVelZSource(uint8_t core_index) const
|
||||
{
|
||||
if (_source_set[active_source_set].velz != SourceZ::NONE) {
|
||||
if (_source_set[getActiveSourceSet(core_index)].velz != SourceZ::NONE) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// check for fuse all velocities
|
||||
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
|
||||
if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
|
||||
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
|
||||
if (_source_set[i].velz != SourceZ::NONE) {
|
||||
return true;
|
||||
@@ -224,30 +224,30 @@ bool AP_NavEKF_Source::haveVelZSource() const
|
||||
}
|
||||
|
||||
// get yaw source
|
||||
AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource() const
|
||||
AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource(uint8_t core_index) const
|
||||
{
|
||||
// check for special case of disabled compasses
|
||||
if ((_source_set[active_source_set].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) {
|
||||
if ((_source_set[getActiveSourceSet(core_index)].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) {
|
||||
return SourceYaw::NONE;
|
||||
}
|
||||
|
||||
return _source_set[active_source_set].yaw;
|
||||
return _source_set[getActiveSourceSet(core_index)].yaw;
|
||||
}
|
||||
|
||||
// get pos Z source
|
||||
AP_NavEKF_Source::SourceZ AP_NavEKF_Source::getPosZSource() const
|
||||
AP_NavEKF_Source::SourceZ AP_NavEKF_Source::getPosZSource(uint8_t core_index) const
|
||||
{
|
||||
#ifdef HAL_BARO_ALLOW_INIT_NO_BARO
|
||||
// check for special case of missing baro
|
||||
if ((_source_set[active_source_set].posz == SourceZ::BARO) && (AP::dal().baro().num_instances() == 0)) {
|
||||
if ((_source_set[getActiveSourceSet(core_index)].posz == SourceZ::BARO) && (AP::dal().baro().num_instances() == 0)) {
|
||||
return SourceZ::NONE;
|
||||
}
|
||||
#endif
|
||||
return _source_set[active_source_set].posz;
|
||||
return _source_set[getActiveSourceSet(core_index)].posz;
|
||||
}
|
||||
|
||||
// align position of inactive sources to ahrs
|
||||
void AP_NavEKF_Source::align_inactive_sources()
|
||||
void AP_NavEKF_Source::align_inactive_sources(uint8_t core_number)
|
||||
{
|
||||
// align visual odometry
|
||||
#if HAL_VISUALODOM_ENABLED
|
||||
@@ -259,9 +259,9 @@ void AP_NavEKF_Source::align_inactive_sources()
|
||||
|
||||
// consider aligning ExtNav XY position:
|
||||
bool align_posxy = false;
|
||||
if ((getPosXYSource() == SourceXY::GPS) ||
|
||||
(getPosXYSource() == SourceXY::BEACON) ||
|
||||
((getVelXYSource() == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW))) {
|
||||
if ((getPosXYSource(core_number) == SourceXY::GPS) ||
|
||||
(getPosXYSource(core_number) == SourceXY::BEACON) ||
|
||||
((getVelXYSource(core_number) == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW))) {
|
||||
// align ExtNav position if active source is GPS, Beacon or (optionally) Optflow
|
||||
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
|
||||
if (_source_set[i].posxy == SourceXY::EXTNAV) {
|
||||
@@ -274,10 +274,10 @@ void AP_NavEKF_Source::align_inactive_sources()
|
||||
|
||||
// consider aligning Z position:
|
||||
bool align_posz = false;
|
||||
if ((getPosZSource() == SourceZ::BARO) ||
|
||||
(getPosZSource() == SourceZ::RANGEFINDER) ||
|
||||
(getPosZSource() == SourceZ::GPS) ||
|
||||
(getPosZSource() == SourceZ::BEACON)) {
|
||||
if ((getPosZSource(core_number) == SourceZ::BARO) ||
|
||||
(getPosZSource(core_number) == SourceZ::RANGEFINDER) ||
|
||||
(getPosZSource(core_number) == SourceZ::GPS) ||
|
||||
(getPosZSource(core_number) == SourceZ::BEACON)) {
|
||||
// ExtNav is not the active source; we do not want to align active source!
|
||||
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
|
||||
if (_source_set[i].posz == SourceZ::EXTNAV) {
|
||||
@@ -292,13 +292,13 @@ void AP_NavEKF_Source::align_inactive_sources()
|
||||
}
|
||||
|
||||
// sensor specific helper functions
|
||||
bool AP_NavEKF_Source::usingGPS() const
|
||||
bool AP_NavEKF_Source::usingGPS(uint8_t core_index) const
|
||||
{
|
||||
return getPosXYSource() == SourceXY::GPS ||
|
||||
getPosZSource() == SourceZ::GPS ||
|
||||
getVelXYSource() == SourceXY::GPS ||
|
||||
getVelZSource() == SourceZ::GPS ||
|
||||
getYawSource() == SourceYaw::GSF;
|
||||
return getPosXYSource(core_index) == SourceXY::GPS ||
|
||||
getPosZSource(core_index) == SourceZ::GPS ||
|
||||
getVelXYSource(core_index) == SourceXY::GPS ||
|
||||
getVelZSource(core_index) == SourceZ::GPS ||
|
||||
getYawSource(core_index) == SourceYaw::GSF;
|
||||
}
|
||||
|
||||
// true if some parameters have been configured (used during parameter conversion)
|
||||
|
||||
@@ -48,7 +48,9 @@ public:
|
||||
// enum for OPTIONS parameter
|
||||
enum class SourceOptions {
|
||||
FUSE_ALL_VELOCITIES = (1 << 0), // fuse all velocities configured in source sets
|
||||
ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1) // align position of inactive sources to ahrs when using optical flow
|
||||
ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1), // align position of inactive sources to ahrs when using optical flow
|
||||
// reserved = (1 << 2), // reserved for future use
|
||||
SRC_PER_CORE = (1 << 3) // use a separate source set for each core
|
||||
};
|
||||
|
||||
enum class SourceSetSelection : uint8_t {
|
||||
@@ -60,27 +62,41 @@ public:
|
||||
// initialisation
|
||||
void init();
|
||||
|
||||
// This function will get the active source set or get the source set for the core index if SRC_PER_CORE SourceOption is set
|
||||
uint8_t getActiveSourceSet(uint8_t core_index) const {
|
||||
// check if we are using a separate source set for each core
|
||||
if (option_is_set(SourceOptions::SRC_PER_CORE)) {
|
||||
if (core_index >= AP_NAKEKF_SOURCE_SET_MAX) {
|
||||
// we need to return a valid source set
|
||||
return active_source_set;
|
||||
}
|
||||
return core_index;
|
||||
}
|
||||
return active_source_set;
|
||||
}
|
||||
|
||||
// get current position source
|
||||
SourceXY getPosXYSource() const { return _source_set[active_source_set].posxy; }
|
||||
SourceZ getPosZSource() const;
|
||||
SourceXY getPosXYSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].posxy; }
|
||||
|
||||
SourceZ getPosZSource(uint8_t core_index) const;
|
||||
|
||||
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
|
||||
void setPosVelYawSourceSet(SourceSetSelection source_set_idx);
|
||||
uint8_t getPosVelYawSourceSet() const { return active_source_set; }
|
||||
|
||||
// get/set velocity source
|
||||
SourceXY getVelXYSource() const { return _source_set[active_source_set].velxy; }
|
||||
SourceZ getVelZSource() const { return _source_set[active_source_set].velz; }
|
||||
SourceXY getVelXYSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].velxy; }
|
||||
SourceZ getVelZSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].velz; }
|
||||
|
||||
// true/false of whether velocity source should be used
|
||||
bool useVelXYSource(SourceXY velxy_source) const;
|
||||
bool useVelZSource(SourceZ velz_source) const;
|
||||
bool useVelXYSource(SourceXY velxy_source, uint8_t core_index) const;
|
||||
bool useVelZSource(SourceZ velz_source, uint8_t core_index) const;
|
||||
|
||||
// true if a velocity source is configured
|
||||
bool haveVelZSource() const;
|
||||
bool haveVelZSource(uint8_t core_index) const;
|
||||
|
||||
// get yaw source
|
||||
SourceYaw getYawSource() const;
|
||||
SourceYaw getYawSource(uint8_t core_index) const;
|
||||
|
||||
// align position of inactive sources to ahrs
|
||||
void align_inactive_sources();
|
||||
@@ -88,7 +104,7 @@ public:
|
||||
// sensor-specific helper functions
|
||||
|
||||
// true if any source is GPS
|
||||
bool usingGPS() const;
|
||||
bool usingGPS(uint8_t core_index) const;
|
||||
|
||||
// true if source parameters have been configured (used for parameter conversion)
|
||||
bool configured();
|
||||
|
||||
Reference in New Issue
Block a user