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); 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 // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx) void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx)
{ {
// ensure init has been run
init();
// sanity check source idx // sanity check source idx
if (source_set_idx >= AP_NAKEKF_SOURCE_SET_MAX) { if (source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) {
return; 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 // 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) const
{ {
if (velxy_source == _active_source_set.velxy) { if (velxy_source == _source_set[active_source_set].velxy) {
return true; return true;
} }
@ -202,7 +176,7 @@ bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
bool AP_NavEKF_Source::useVelZSource(SourceZ velz_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; return true;
} }
@ -222,7 +196,7 @@ bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
// true if a velocity source is configured // true if a velocity source is configured
bool AP_NavEKF_Source::haveVelZSource() const bool AP_NavEKF_Source::haveVelZSource() const
{ {
if (_active_source_set.velz != SourceZ::NONE) { if (_source_set[active_source_set].velz != SourceZ::NONE) {
return true; return true;
} }

20
libraries/AP_NavEKF/AP_NavEKF_Source.h
View File

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