diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 4afe7308bf..420eca80a0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1448,4 +1448,19 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_ } +/* + get timing statistics structure +*/ +void NavEKF2::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_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index d78d0a2489..70e7696df3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -310,6 +310,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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 4d38f7c5c5..06745e1a5d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -365,6 +365,8 @@ void NavEKF2_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; @@ -758,4 +760,35 @@ void NavEKF2_core::readRngBcnData() rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms); } + +/* + update timing statistics structure + */ +void NavEKF2_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 NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing) +{ + _timing = timing; + memset(&timing, 0, sizeof(timing)); +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 0dd1af4440..51b2ec025b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -296,6 +296,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 @@ -702,6 +705,9 @@ private: // effective value of MAG_CAL uint8_t effective_magCal(void) const; + + // update timing statistics structure + void updateTimingStatistics(void); // Length of FIFO buffers used for non-IMU sensor data. // Must be larger than the time period defined by IMU_BUFFER_LENGTH @@ -1113,6 +1119,9 @@ private: AP_HAL::Util::perf_counter_t _perf_FuseOptFlow; 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;