From 8b59e564ba3a14af5d0dfe85f84279fc9c734ab0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 27 Feb 2014 08:11:04 +1100 Subject: [PATCH] AP_NavEKF: improved replay timing --- libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde b/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde index 7f1b4ea7de..7006d622f1 100644 --- a/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde +++ b/libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde @@ -195,11 +195,13 @@ static void read_sensors(uint8_t type) uint32_t update_delta_usec = 1e6 / update_rate; uint8_t update_count = update_rate>0?update_rate/50:1; for (uint8_t i=0; istop_clock(hal.scheduler->micros() + i*update_delta_usec); ahrs.update(); if (ahrs.get_home().lat != 0) { inertial_nav.update(ins.get_delta_time()); } + hal.scheduler->stop_clock(hal.scheduler->micros() + (i+1)*update_delta_usec); + ins.set_gyro(0, ins.get_gyro()); + ins.set_accel(0, ins.get_accel()); } } else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) || (type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {