Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: make it easy to do EK2 timing tests

Browse Source 
for future benchmarking
This commit is contained in:
Andrew Tridgell 2019-10-06 08:33:44 +11:00
parent cd9c9e0809
commit 46628b4401
1 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -19,6 +19,20 @@ extern const AP_HAL::HAL& hal;
// maximum allowed gyro bias (rad/sec)
#define GYRO_BIAS_LIMIT 0.5f
/*
to run EK2 timing tests you need to set ENABLE_EKF_TIMING to 1, plus setup as follows:
- copter at 400Hz
- INS_FAST_SAMPLE=0
- EKF2_MAG_CAL=4
- GPS_TYPE=14
- load fakegps in mavproxy
- ensure a compass is enabled
- wait till EK2 reports "using GPS" (this is important, ignore earlier results)
DO NOT FLY WITH THIS ENABLED
*/
#define ENABLE_EKF_TIMING 0
// constructor
NavEKF2_core::NavEKF2_core(void) :
_perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")),
@ -524,8 +538,10 @@ void NavEKF2_core::UpdateFilter(bool predict)
}
// start the timer used for load measurement
#if EK2_DISABLE_INTERRUPTS
irqstate_t istate = irqsave();
#if ENABLE_EKF_TIMING
void *istate = hal.scheduler->disable_interrupts_save();
static uint32_t timing_start_us;
timing_start_us = AP_HAL::micros();
#endif
hal.util->perf_begin(_perf_UpdateFilter);
@ -575,8 +591,16 @@ void NavEKF2_core::UpdateFilter(bool predict)
// stop the timer used for load measurement
hal.util->perf_end(_perf_UpdateFilter);
#if EK2_DISABLE_INTERRUPTS
irqrestore(istate);
#if ENABLE_EKF_TIMING
static uint32_t total_us;
static uint32_t timing_counter;
total_us += AP_HAL::micros() - timing_start_us;
if (timing_counter++ == 4000) {
hal.console->printf("ekf2 avg %.2f us\n", total_us / float(timing_counter));
total_us = 0;
timing_counter = 0;
}
hal.scheduler->restore_interrupts(istate);
#endif
/*