diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 0dd2fecbaa..7b6c0e0ff2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1487,4 +1487,19 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_ } +/* + get timing statistics structure +*/ +void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) +{ + if (instance < 0 || instance >= num_cores) { + instance = primary; + } + if (core) { + core[instance].getTimingStatistics(timing); + } else { + memset(&timing, 0, sizeof(timing)); + } +} + #endif //HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index d89ab1fe7a..b48adb1596 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -328,6 +328,9 @@ public: // are we doing sensor logging inside the EKF? bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; } + // get timing statistics structure + void getTimingStatistics(int8_t instance, struct ekf_timing &timing); + private: uint8_t num_cores; // number of allocated cores uint8_t primary; // current primary core diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index ad1dea5e24..bfd331bf67 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -387,6 +387,8 @@ void NavEKF3_core::readIMUData() imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); + updateTimingStatistics(); + // correct the extracted IMU data for sensor errors delAngCorrected = imuDataDelayed.delAng; delVelCorrected = imuDataDelayed.delVel; @@ -787,4 +789,34 @@ void NavEKF3_core::readRngBcnData() } +/* + update timing statistics structure + */ +void NavEKF3_core::updateTimingStatistics(void) +{ + if (timing.count == 0) { + timing.dtIMUavg_max = dtIMUavg; + timing.dtIMUavg_min = dtIMUavg; + timing.delAngDT_max = imuDataDelayed.delAngDT; + timing.delAngDT_min = imuDataDelayed.delAngDT; + timing.delVelDT_max = imuDataDelayed.delVelDT; + timing.delVelDT_min = imuDataDelayed.delVelDT; + } else { + timing.dtIMUavg_max = MAX(timing.dtIMUavg_max, dtIMUavg); + timing.dtIMUavg_min = MIN(timing.dtIMUavg_min, dtIMUavg); + timing.delAngDT_max = MAX(timing.delAngDT_max, imuDataDelayed.delAngDT); + timing.delAngDT_min = MIN(timing.delAngDT_min, imuDataDelayed.delAngDT); + timing.delVelDT_max = MAX(timing.delVelDT_max, imuDataDelayed.delVelDT); + timing.delVelDT_min = MIN(timing.delVelDT_min, imuDataDelayed.delVelDT); + } + timing.count++; +} + +// get timing statistics structure +void NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing) +{ + _timing = timing; + memset(&timing, 0, sizeof(timing)); +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 9e16e7a966..fb35972ab5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -331,6 +331,9 @@ public: // get the IMU index uint8_t getIMUIndex(void) const { return imu_index; } + // get timing statistics structure + void getTimingStatistics(struct ekf_timing &timing); + private: // Reference to the global EKF frontend for parameters NavEKF3 *frontend; @@ -761,6 +764,9 @@ private: // initialise the quaternion covariances using rotation vector variances void initialiseQuatCovariances(Vector3f &rotVarVec); + // update timing statistics structure + void updateTimingStatistics(void); + // Variables bool statesInitialised; // boolean true when filter states have been initialised bool velHealth; // boolean true if velocity measurements have passed innovation consistency check @@ -1205,6 +1211,9 @@ private: AP_HAL::Util::perf_counter_t _perf_FuseBodyOdom; AP_HAL::Util::perf_counter_t _perf_test[10]; + // timing statistics + struct ekf_timing timing; + // should we assume zero sideslip? bool assume_zero_sideslip(void) const;