Browse Source

AP_NavEKF: source returns YawSource None if COMPASS_USE all zero

c415-sdk
Randy Mackay 4 years ago
parent
commit
4998b536b4
  1. 11
      libraries/AP_NavEKF/AP_NavEKF_Source.cpp
  2. 2
      libraries/AP_NavEKF/AP_NavEKF_Source.h

11
libraries/AP_NavEKF/AP_NavEKF_Source.cpp

@ -213,6 +213,17 @@ bool AP_NavEKF_Source::haveVelZSource() const @@ -213,6 +213,17 @@ bool AP_NavEKF_Source::haveVelZSource() const
return false;
}
// get yaw source
AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource() const
{
// check for special case of disabled compasses
if ((_source_set[active_source_set].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) {
return SourceYaw::NONE;
}
return _source_set[active_source_set].yaw;
}
// align position of inactive sources to ahrs
void AP_NavEKF_Source::align_inactive_sources()
{

2
libraries/AP_NavEKF/AP_NavEKF_Source.h

@ -71,7 +71,7 @@ public: @@ -71,7 +71,7 @@ public:
bool haveVelZSource() const;
// get yaw source
SourceYaw getYawSource() const { return _source_set[active_source_set].yaw; }
SourceYaw getYawSource() const;
// align position of inactive sources to ahrs
void align_inactive_sources();

Loading…
Cancel
Save