From 7f69c5d26dc0768f5547727c2b8b8b45f7e499fa Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 13 Nov 2017 16:59:07 +1100 Subject: [PATCH] Copter: use scheduler ticks in place of mainloop_count --- ArduCopter/ArduCopter.cpp | 3 --- ArduCopter/Copter.cpp | 1 - ArduCopter/Copter.h | 2 -- ArduCopter/failsafe.cpp | 7 ++++--- 4 files changed, 4 insertions(+), 9 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 071d7d6377..346accb8b0 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -226,9 +226,6 @@ void Copter::loop() G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f; fast_loopTimer = timer; - // for mainloop failure monitoring - mainLoop_count++; - // Execute the fast loop // --------------------- fast_loop(); diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 7103e4eb5d..cc41573e23 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -48,7 +48,6 @@ Copter::Copter(void) inertial_nav(ahrs), pmTest1(0), fast_loopTimer(0), - mainLoop_count(0), auto_trim_counter(0), in_mavlink_delay(false), param_loader(var_info), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index ba13160cd3..d03d60d7c1 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -479,8 +479,6 @@ private: // -------------- // Time in microseconds of main control loop uint32_t fast_loopTimer; - // Counter of main loop executions. Used for performance monitoring and failsafe processing - uint16_t mainLoop_count; // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. uint32_t arm_time_ms; diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 15c09e3add..8c8816fa4b 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -8,7 +8,7 @@ // static bool failsafe_enabled = false; -static uint16_t failsafe_last_mainLoop_count; +static uint16_t failsafe_last_ticks; static uint32_t failsafe_last_timestamp; static bool in_failsafe; @@ -36,9 +36,10 @@ void Copter::failsafe_check() { uint32_t tnow = AP_HAL::micros(); - if (mainLoop_count != failsafe_last_mainLoop_count) { + const uint16_t ticks = scheduler.ticks(); + if (ticks != failsafe_last_ticks) { // the main loop is running, all is OK - failsafe_last_mainLoop_count = mainLoop_count; + failsafe_last_ticks = ticks; failsafe_last_timestamp = tnow; if (in_failsafe) { in_failsafe = false;