Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: make it easy to do EK2 timing tests 

for future benchmarking
This commit is contained in:
Andrew Tridgell 2019-09-28 07:18:52 +10:00
parent e820219202
commit 8dd2119b14
1 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
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(NavEKF2 *_frontend) :
_perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")),
@ -534,8 +548,10 @@ void NavEKF2_core::UpdateFilter(bool predict)
}
// start the timer used for load measurement
#if EK2_DISABLE_INTERRUPTS
#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);
@ -585,7 +601,15 @@ void NavEKF2_core::UpdateFilter(bool predict)
// stop the timer used for load measurement
hal.util->perf_end(_perf_UpdateFilter);
#if EK2_DISABLE_INTERRUPTS
#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