From 5044189ac977d404f4c8da57d0c36028ecd87b21 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Wed, 5 Aug 2015 13:12:42 +1000 Subject: [PATCH] Rover: scheduler remaining time loop calc made common Just making the improved scheduler loop remaining time calculation in line with Plane and Copter. --- APMrover2/APMrover2.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 8deaf49814..86ac7e41c7 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -124,7 +124,17 @@ void Rover::loop() // tell the scheduler one tick has passed scheduler.tick(); - scheduler.run(19500U); + + // 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); } // update AHRS system