Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Source's active_source_set becomes index

Browse Source
This commit is contained in:
Andrew Tridgell 2020-12-15 21:59:15 +11:00 committed by Randy Mackay
parent 89f9b9e9b0
commit 218c0e4385
2 changed files with 11 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View File

@ -145,45 +145,19 @@ AP_NavEKF_Source::AP_NavEKF_Source()
AP_Param::setup_object_defaults(this, var_info);
}
void AP_NavEKF_Source::init()
{
// ensure init is only run once
if (initialised) {
return;
}
// initialise active sources
_active_source_set.posxy = _source_set[0].posxy;
_active_source_set.velxy = _source_set[0].velxy;
_active_source_set.posz = _source_set[0].posz;
_active_source_set.velz = _source_set[0].velz;
_active_source_set.yaw = _source_set[0].yaw;
initialised = true;
}
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx)
{
// ensure init has been run
init();
// sanity check source idx
if (source_set_idx >= AP_NAKEKF_SOURCE_SET_MAX) {
return;
if (source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) {
active_source_set = source_set_idx;
}
_active_source_set.posxy = _source_set[source_set_idx].posxy;
_active_source_set.velxy = _source_set[source_set_idx].velxy;
_active_source_set.posz = _source_set[source_set_idx].posz;
_active_source_set.velz = _source_set[source_set_idx].velz;
_active_source_set.yaw = _source_set[source_set_idx].yaw;
}
// true/false of whether velocity source should be used
bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
{
if (velxy_source == _active_source_set.velxy) {
if (velxy_source == _source_set[active_source_set].velxy) {
return true;
}
@ -202,7 +176,7 @@ bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
{
if (velz_source == _active_source_set.velz) {
if (velz_source == _source_set[active_source_set].velz) {
return true;
}
@ -222,7 +196,7 @@ bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
// true if a velocity source is configured
bool AP_NavEKF_Source::haveVelZSource() const
{
if (_active_source_set.velz != SourceZ::NONE) {
if (_source_set[active_source_set].velz != SourceZ::NONE) {
return true;
}

20
libraries/AP_NavEKF/AP_NavEKF_Source.h
View File

@ -53,15 +53,15 @@ public:
void init();
// get current position source
SourceXY getPosXYSource() const { return _active_source_set.posxy; }
SourceZ getPosZSource() const { return _active_source_set.posz; }
SourceXY getPosXYSource() const { return _source_set[active_source_set].posxy; }
SourceZ getPosZSource() const { return _source_set[active_source_set].posz; }
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void setPosVelYawSourceSet(uint8_t source_set_idx);
// get/set velocity source
SourceXY getVelXYSource() const { return _active_source_set.velxy; }
SourceZ getVelZSource() const { return _active_source_set.velz; }
SourceXY getVelXYSource() const { return _source_set[active_source_set].velxy; }
SourceZ getVelZSource() const { return _source_set[active_source_set].velz; }
// true/false of whether velocity source should be used
bool useVelXYSource(SourceXY velxy_source) const;
@ -71,7 +71,7 @@ public:
bool haveVelZSource() const;
// get yaw source
SourceYaw getYawSource() const { return _active_source_set.yaw; }
SourceYaw getYawSource() const { return _source_set[active_source_set].yaw; }
// align position of inactive sources to ahrs
void align_inactive_sources();
@ -114,14 +114,6 @@ private:
AP_Int16 _options; // source options bitmask
// active sources
struct {
SourceXY posxy; // current xy position source
SourceZ posz; // current z position source
SourceXY velxy; // current xy velocity source
SourceZ velz; // current z velocity source
SourceYaw yaw; // current yaw source
} _active_source_set;
bool initialised; // true once init has been run
uint8_t active_source_set; // index of active source set
bool config_in_storage; // true once configured in storage has returned true
};