From 9f49b8fa4888b7b1e1b39c10b5abdbd2b5d95300 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 23 Jul 2013 17:07:35 +1000 Subject: [PATCH] Rover: use the new load_average() API --- APMrover2/APMrover2.pde | 12 ++---------- APMrover2/GCS_Mavlink.pde | 2 +- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index e2f37c8dbd..08c1b22358 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -543,9 +543,6 @@ static uint8_t delta_ms_fast_loop; // Counter of main loop executions. Used for performance monitoring and failsafe processing static uint16_t mainLoop_count; -// % MCU cycles used -static float load; - //////////////////////////////////////////////////////////////////////////////// // Top-level logic //////////////////////////////////////////////////////////////////////////////// @@ -607,7 +604,6 @@ void loop() uint16_t num_samples = ins.num_samples_available(); if (num_samples >= 1) { delta_ms_fast_loop = timer - fast_loopTimer; - load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop; G_Dt = (float)delta_ms_fast_loop / 1000.f; fast_loopTimer = timer; @@ -620,12 +616,8 @@ void loop() // tell the scheduler one tick has passed scheduler.tick(); fast_loopTimeStamp = millis(); - } else { - uint16_t dt = timer - fast_loopTimer; - if (dt < 20) { - uint16_t time_to_next_loop = 20 - dt; - scheduler.run(time_to_next_loop * 1000U); - } + + scheduler.run(19000U); } } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 1a9be99652..bf370c5ed9 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -191,7 +191,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack control_sensors_present, control_sensors_enabled, control_sensors_health, - (uint16_t)(load * 1000), + (uint16_t)(scheduler.load_average(20000) * 1000), battery_voltage1 * 1000, // mV battery_current, // in 10mA units battery_remaining, // in %