Browse Source

AP_AHRS: added a getter function to get active source set once the ekf3 filter is started

master
nrt 3 years ago committed by Randy Mackay
parent
commit
c3d8f0fa45
  1. 10
      libraries/AP_AHRS/AP_AHRS.cpp
  2. 3
      libraries/AP_AHRS/AP_AHRS.h

10
libraries/AP_AHRS/AP_AHRS.cpp

@ -3148,6 +3148,16 @@ void AP_AHRS::set_posvelyaw_source_set(uint8_t source_set_idx) @@ -3148,6 +3148,16 @@ void AP_AHRS::set_posvelyaw_source_set(uint8_t source_set_idx)
#endif
}
//returns active source set used, 0=primary, 1=secondary, 2=tertiary
uint8_t AP_AHRS::get_posvelyaw_source_set() const
{
#if HAL_NAVEKF3_AVAILABLE
return EKF3.get_active_source_set();
#else
return 0;
#endif
}
void AP_AHRS::Log_Write()
{
#if HAL_NAVEKF2_AVAILABLE

3
libraries/AP_AHRS/AP_AHRS.h

@ -360,6 +360,9 @@ public: @@ -360,6 +360,9 @@ public:
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void set_posvelyaw_source_set(uint8_t source_set_idx);
//returns index of active source set used, 0=primary, 1=secondary, 2=tertiary
uint8_t get_posvelyaw_source_set() const;
void Log_Write();
// check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed

Loading…
Cancel
Save