From 620d6ab4b827e9418db571ef5e654fe2b854b334 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 10 Feb 2018 11:48:20 +1100
Subject: [PATCH] AP_Scheduler: fixed merge issues

---
 libraries/AP_Scheduler/AP_Scheduler.h |  4 ++++
 libraries/AP_Scheduler/PerfInfo.cpp   | 23 ++++++++++++++++++++++-
 libraries/AP_Scheduler/PerfInfo.h     | 10 ++++------
 3 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h
index 6d9dffddc4..fb42ed2411 100644
--- a/libraries/AP_Scheduler/AP_Scheduler.h
+++ b/libraries/AP_Scheduler/AP_Scheduler.h
@@ -128,6 +128,10 @@ public:
         return _loop_period_s;
     }
 
+    float get_filtered_loop_time(void) const {
+        return perf_info.get_filtered_time();
+    }
+    
     static const struct AP_Param::GroupInfo var_info[];
 
     // current running task, or -1 if none. Used to debug stuck tasks
diff --git a/libraries/AP_Scheduler/PerfInfo.cpp b/libraries/AP_Scheduler/PerfInfo.cpp
index 1976b0b0f4..0ba0ba9de9 100644
--- a/libraries/AP_Scheduler/PerfInfo.cpp
+++ b/libraries/AP_Scheduler/PerfInfo.cpp
@@ -44,11 +44,15 @@ void AP::PerfInfo::check_loop_time(uint32_t time_in_micros)
     if( min_time == 0 || time_in_micros < min_time) {
         min_time = time_in_micros;
     }
-    if (time_in_micros > overtime_threshold_us) {
+    if (time_in_micros > overtime_threshold_micros) {
         long_running++;
     }
     sigma_time += time_in_micros;
     sigmasquared_time += time_in_micros * time_in_micros;
+
+    // we keep a filtered loop time for use as G_Dt which is the
+    // predicted time for the next loop
+    filtered_loop_time = 0.99 * filtered_loop_time + 0.01 * time_in_micros * 1.0e-6;
 }
 
 // get_num_loops: return number of loops used for recording performance
@@ -93,6 +97,12 @@ uint32_t AP::PerfInfo::get_stddev_time() const
     return sqrt((sigmasquared_time - (sigma_time*sigma_time)/loop_count) / loop_count);
 }
 
+// get_filtered_time - return low pass filtered loop time in seconds
+float AP::PerfInfo::get_filtered_time() const
+{
+    return filtered_loop_time;
+}
+
 void AP::PerfInfo::update_logging()
 {
     gcs().send_text(MAV_SEVERITY_WARNING,
@@ -104,3 +114,14 @@ void AP::PerfInfo::update_logging()
                     (unsigned long)get_avg_time(),
                     (unsigned long)get_stddev_time());
 }
+
+void AP::PerfInfo::set_loop_rate(uint16_t rate_hz)
+{
+    // allow a 20% overrun before we consider a loop "slow":
+    overtime_threshold_micros = 1000000/rate_hz * 1.2f;
+
+    if (loop_rate_hz != rate_hz) {
+        loop_rate_hz = rate_hz;
+        filtered_loop_time = 1.0 / rate_hz;
+    }
+}
diff --git a/libraries/AP_Scheduler/PerfInfo.h b/libraries/AP_Scheduler/PerfInfo.h
index 8f2e29959d..2118893b9d 100644
--- a/libraries/AP_Scheduler/PerfInfo.h
+++ b/libraries/AP_Scheduler/PerfInfo.h
@@ -22,15 +22,13 @@ public:
     uint32_t get_num_dropped() const;
     uint32_t get_avg_time() const;
     uint32_t get_stddev_time() const;
-
-    void set_loop_rate(uint16_t rate_hz) {
-        // allow a 20% overrun before we consider a loop "slow":
-        overtime_threshold_micros = 1000000/rate_hz * 1.2f;
-    }
+    float    get_filtered_time() const;
+    void set_loop_rate(uint16_t rate_hz);
 
     void update_logging();
 
 private:
+    uint16_t loop_rate_hz;
     uint16_t overtime_threshold_micros;
     uint16_t loop_count;
     uint32_t max_time; // in microseconds
@@ -39,7 +37,7 @@ private:
     uint64_t sigmasquared_time;
     uint16_t long_running;
     uint32_t log_dropped;
-    uint32_t overtime_threshold_us;
+    float filtered_loop_time;
     bool ignore_loop;
 
 };