zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF3: use AP_Enum to make code clearer

zr-v5.1
Andrew Tridgell 4 years ago committed by Randy Mackay
parent
9bf89e5635
commit
89f9b9e9b0
2 changed files with 28 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 40
      libraries/AP_NavEKF/AP_NavEKF_Source.cpp
  2. 16
      libraries/AP_NavEKF/AP_NavEKF_Source.h

40
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
Unescape View File

@ -153,11 +153,11 @@ void AP_NavEKF_Source::init() @@ -153,11 +153,11 @@ void AP_NavEKF_Source::init()
}
// initialise active sources
_active_source_set.posxy = (SourceXY)_source_set[0].posxy.get();
_active_source_set.velxy = (SourceXY)_source_set[0].velxy.get();
_active_source_set.posz = (SourceZ)_source_set[0].posz.get();
_active_source_set.velz = (SourceZ)_source_set[0].velz.get();
_active_source_set.yaw = (SourceYaw)_source_set[0].yaw.get();
_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;
}
@ -173,11 +173,11 @@ void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx) @@ -173,11 +173,11 @@ void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx)
return;
}
_active_source_set.posxy = (SourceXY)_source_set[source_set_idx].posxy.get();
_active_source_set.velxy = (SourceXY)_source_set[source_set_idx].velxy.get();
_active_source_set.posz = (SourceZ)_source_set[source_set_idx].posz.get();
_active_source_set.velz = (SourceZ)_source_set[source_set_idx].velz.get();
_active_source_set.yaw = (SourceYaw)_source_set[source_set_idx].yaw.get();
_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
@ -190,7 +190,7 @@ bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const @@ -190,7 +190,7 @@ bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
// check for fuse all velocities
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if ((SourceXY)_source_set[i].velxy.get() == velxy_source) {
if (_source_set[i].velxy == velxy_source) {
return true;
}
}
@ -209,7 +209,7 @@ bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const @@ -209,7 +209,7 @@ bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
// check for fuse all velocities
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if ((SourceZ)_source_set[i].velz.get() == velz_source) {
if (_source_set[i].velz == velz_source) {
return true;
}
}
@ -229,7 +229,7 @@ bool AP_NavEKF_Source::haveVelZSource() const @@ -229,7 +229,7 @@ bool AP_NavEKF_Source::haveVelZSource() const
// check for fuse all velocities
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if ((SourceZ)_source_set[i].velz.get() != SourceZ::NONE) {
if (_source_set[i].velz != SourceZ::NONE) {
return true;
}
}
@ -256,7 +256,7 @@ void AP_NavEKF_Source::align_inactive_sources() @@ -256,7 +256,7 @@ void AP_NavEKF_Source::align_inactive_sources()
(getPosXYSource() == SourceXY::BEACON)) {
// only align position if active source is GPS or Beacon
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if ((SourceXY)_source_set[i].posxy.get() == SourceXY::EXTNAV) {
if (_source_set[i].posxy == SourceXY::EXTNAV) {
// ExtNav could potentially be used, so align it
align_posxy = true;
break;
@ -272,7 +272,7 @@ void AP_NavEKF_Source::align_inactive_sources() @@ -272,7 +272,7 @@ void AP_NavEKF_Source::align_inactive_sources()
(getPosZSource() == 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 ((SourceZ)_source_set[i].posz.get() == SourceZ::EXTNAV) {
if (_source_set[i].posz == SourceZ::EXTNAV) {
// ExtNav could potentially be used, so align it
align_posz = true;
break;
@ -489,16 +489,16 @@ bool AP_NavEKF_Source::ext_nav_enabled(void) const @@ -489,16 +489,16 @@ 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) {
if (src.posxy == SourceXY::EXTNAV) {
return true;
}
if (SourceZ(src.posz.get()) == SourceZ::EXTNAV) {
if (src.posz == SourceZ::EXTNAV) {
return true;
}
if (SourceXY(src.velxy.get()) == SourceXY::EXTNAV) {
if (src.velxy == SourceXY::EXTNAV) {
return true;
}
if (SourceZ(src.velz.get()) == SourceZ::EXTNAV) {
if (src.velz == SourceZ::EXTNAV) {
return true;
}
}
@ -510,7 +510,7 @@ bool AP_NavEKF_Source::wheel_encoder_enabled(void) const @@ -510,7 +510,7 @@ 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) {
if (src.velxy == SourceXY::WHEEL_ENCODER) {
return true;
}
}

16
libraries/AP_NavEKF/AP_NavEKF_Source.h
Unescape View File

@ -15,7 +15,7 @@ public: @@ -15,7 +15,7 @@ public:
AP_NavEKF_Source(const AP_NavEKF_Source &other) = delete;
AP_NavEKF_Source &operator=(const AP_NavEKF_Source&) = delete;
enum class SourceXY {
enum class SourceXY : uint8_t {
NONE = 0,
// BARO = 1 (not applicable)
// RANGEFINDER = 2 (not applicable)
@ -26,7 +26,7 @@ public: @@ -26,7 +26,7 @@ public:
WHEEL_ENCODER = 7
};
enum class SourceZ {
enum class SourceZ : uint8_t {
NONE = 0,
BARO = 1,
RANGEFINDER = 2,
@ -37,7 +37,7 @@ public: @@ -37,7 +37,7 @@ public:
// WHEEL_ENCODER = 7 (not applicable)
};
enum class SourceYaw {
enum class SourceYaw : uint8_t {
NONE = 0,
COMPASS = 1,
EXTERNAL = 2,
@ -105,11 +105,11 @@ private: @@ -105,11 +105,11 @@ private:
// Parameters
struct SourceSet {
AP_Int8 posxy; // xy position source
AP_Int8 velxy; // xy velocity source
AP_Int8 posz; // position z (aka altitude or height) source
AP_Int8 velz; // velocity z source
AP_Int8 yaw; // yaw source
AP_Enum<SourceXY> posxy; // xy position source
AP_Enum<SourceXY> velxy; // xy velocity source
AP_Enum<SourceZ> posz; // position z (aka altitude or height) source
AP_Enum<SourceZ> velz; // velocity z source
AP_Enum<SourceYaw> yaw; // yaw source
} _source_set[AP_NAKEKF_SOURCE_SET_MAX];
AP_Int16 _options; // source options bitmask

Write Preview
Loading…
Cancel
Save