AP_Scheduler: move logging of PM messages to AP_Scheduler

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -25,6 +25,7 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <DataFlash/DataFlash.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
+
 #include <stdio.h>
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
@@ -241,9 +242,29 @@ void AP_Scheduler::loop()
     perf_info.check_loop_time(AP_HAL::micros() - loop_start);
 }
 
-void AP_Scheduler::update_logging()
+void AP_Scheduler::update_logging(const bool log_to_dataflash)
 {
     if (debug()) {
         perf_info.update_logging();
     }
+    if (log_to_dataflash) {
+        Log_Write_Performance();
+    }
+    perf_info.set_loop_rate(get_loop_rate_hz());
+    perf_info.reset();
+}
+
+// Write a performance monitoring packet
+void AP_Scheduler::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(),
+        ins_error_count  : AP::ins().error_count(),
+        mem_avail        : hal.util->available_memory()
+    };
+    DataFlash_Class::instance()->WriteCriticalBlock(&pkt, sizeof(pkt));
 }

libraries/AP_Scheduler/AP_Scheduler.h

@@ -79,7 +79,10 @@ public:
     void loop();
 
     // call to update any logging the scheduler might do; call at 1Hz
-    void update_logging();
+    void update_logging(bool log_to_dataflash);
+
+    // write out PERF message to dataflash
+    void Log_Write_Performance();
 
     // call when one tick has passed
     void tick(void);

libraries/AP_Scheduler/PerfInfo.cpp

@@ -10,7 +10,7 @@
 //
 
 // reset - reset all records of loop time to zero
-void AP::PerfInfo::reset(uint16_t loop_rate_hz)
+void AP::PerfInfo::reset()
 {
     loop_count = 0;
     max_time = 0;
@@ -19,8 +19,6 @@ void AP::PerfInfo::reset(uint16_t loop_rate_hz)
     log_dropped = DataFlash_Class::instance()->num_dropped();
     sigma_time = 0;
     sigmasquared_time = 0;
-    // 500us threshold for overtime
-    overtime_threshold_us = (1000000UL / loop_rate_hz) + 500;
 }
 
 // ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)

libraries/AP_Scheduler/PerfInfo.h

@@ -12,7 +12,7 @@ public:
     PerfInfo(const PerfInfo &other) = delete;
     PerfInfo &operator=(const PerfInfo&) = delete;
 
-    void reset(uint16_t loop_rate_hz);
+    void reset();
     void ignore_this_loop();
     void check_loop_time(uint32_t time_in_micros);
     uint16_t get_num_loops() const;