diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 5c655c07d5..54f7bf892d 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -137,6 +137,9 @@ public: return _airspeed; } + // return the index of the primary core or -1 if no primary core selected + virtual int8_t get_primary_core_index() const { return -1; } + // get the index of the current primary accelerometer sensor virtual uint8_t get_primary_accel_index(void) const { return AP::ins().get_primary_accel(); diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 1c5997e645..740e8f9034 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -2151,6 +2151,32 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const return get_accel_ef(get_primary_accel_index()); } +// return the index of the primary core or -1 if no primary core selected +int8_t AP_AHRS_NavEKF::get_primary_core_index() const +{ + switch (active_EKF_type()) { + case EKFType::NONE: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: +#endif + // SITL and DCM have only one core + return 0; + +#if HAL_NAVEKF2_AVAILABLE + case EKFType::TWO: + return EKF2.getPrimaryCoreIndex(); +#endif + +#if HAL_NAVEKF3_AVAILABLE + case EKFType::THREE: + return EKF3.getPrimaryCoreIndex(); +#endif + } + + // we should never get here + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return -1; +} // get the index of the current primary accelerometer sensor uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index e94fb0334d..df348c6ecf 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -278,6 +278,9 @@ public: // is the EKF backend doing its own sensor logging? bool have_ekf_logging(void) const override; + // return the index of the primary core or -1 if no primary core selected + int8_t get_primary_core_index() const override; + // get the index of the current primary accelerometer sensor uint8_t get_primary_accel_index(void) const override;