diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 6b7b99a0b4..63cf739e0d 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -116,10 +116,6 @@ void Rover::setup() // initialise the main loop scheduler scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks)); - - // setup initial performance counters - perf_info.set_loop_rate(scheduler.get_loop_rate_hz()); - perf_info.reset(); } /* @@ -127,30 +123,8 @@ void Rover::setup() */ void Rover::loop() { - // wait for an INS sample - ins.wait_for_sample(); - - const uint32_t timer = AP_HAL::micros(); - - // check loop time - perf_info.check_loop_time(timer - fast_loopTimer_us); - - G_Dt = (timer - fast_loopTimer_us) * 1.0e-6f; - fast_loopTimer_us = timer; - - // tell the scheduler one tick has passed - scheduler.tick(); - - // run all the tasks that are due to run. Note that we only - // have to call this once per loop, as the tasks are scheduled - // in multiples of the main loop tick. So if they don't run on - // the first call to the scheduler they won't run on a later - // call until scheduler.tick() is called again - uint32_t remaining = (timer + 20000) - micros(); - if (remaining > 19500) { - remaining = 19500; - } - scheduler.run(remaining); + scheduler.loop(); + G_Dt = scheduler.last_loop_time; } void Rover::update_soft_armed() diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index b98c61d230..772153bdda 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -24,9 +24,9 @@ void Rover::Log_Write_Performance() struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), time_us : AP_HAL::micros64(), - num_long_running : perf_info.get_num_long_running(), - num_loops : perf_info.get_num_loops(), - max_time : perf_info.get_max_time(), + num_long_running : scheduler.perf_info.get_num_long_running(), + num_loops : scheduler.perf_info.get_num_loops(), + max_time : scheduler.perf_info.get_max_time(), gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000), gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 812456e840..89c277e2d8 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -59,7 +59,6 @@ #include // APM relay #include // RSSI Library #include // main loop scheduler -#include // loop perf monitoring #include // Serial manager library #include #include // statistics library @@ -213,9 +212,6 @@ private: AP_ServoRelayEvents ServoRelayEvents{relay}; - // loop performance monitoring: - AP::PerfInfo perf_info; - // The rover's current location struct Location current_loc; @@ -334,10 +330,6 @@ private: // This is the time between calls to the DCM algorithm and is the Integration time for the gyros. float G_Dt; - // System Timers - // Time in microseconds of start of main control loop. - uint32_t fast_loopTimer_us; - // set if we are driving backwards bool in_reverse;