mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: added recording of timing statistics
This commit is contained in:
parent
ae3df89d17
commit
ee5c032f27
|
@ -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
|
||||
|
|
|
@ -311,6 +311,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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -297,6 +297,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
|
||||
NavEKF2 *frontend;
|
||||
|
@ -703,6 +706,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
|
||||
static const uint32_t OBS_BUFFER_LENGTH = 5;
|
||||
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue