From ec460b4244e4e8ccad9b354684e536a1f5e3b915 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 25 Mar 2020 22:07:50 +1100
Subject: [PATCH] AP_Vehicle: send statustext at regular intervals after
 watchdog reset

---
 libraries/AP_Vehicle/AP_Vehicle.cpp | 25 +++++++++++++++++++++++++
 libraries/AP_Vehicle/AP_Vehicle.h   |  4 ++++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index 27ab400747..244e3b937b 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -115,6 +115,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       sample_gyros,      LOOP_RATE, 50),
     SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       update_parameters,         1, 50),
 #endif
+    SCHED_TASK(send_watchdog_reset_statustext,         0.1,     20),
 };
 
 void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks)
@@ -158,6 +159,30 @@ void AP_Vehicle::scheduler_delay_callback()
     logger.EnableWrites(true);
 }
 
+// if there's been a watchdog reset, notify the world via a statustext:
+void AP_Vehicle::send_watchdog_reset_statustext()
+{
+    if (!hal.util->was_watchdog_reset()) {
+        return;
+    }
+    const AP_HAL::Util::PersistentData &pd = hal.util->last_persistent_data;
+    gcs().send_text(MAV_SEVERITY_CRITICAL,
+                    "WDG: T%d SL%u FL%u FT%u FA%x FTP%u FLR%x FICSR%u MM%u MC%u IE%u IEC%u",
+                    pd.scheduler_task,
+                    pd.semaphore_line,
+                    pd.fault_line,
+                    pd.fault_type,
+                    (unsigned)pd.fault_addr,
+                    pd.fault_thd_prio,
+                    (unsigned)pd.fault_lr,
+                    (unsigned)pd.fault_icsr,
+                    pd.last_mavlink_msgid,
+                    pd.last_mavlink_cmd,
+                    (unsigned)pd.internal_errors,
+                    (unsigned)pd.internal_error_count
+        );
+}
+
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
 AP_Vehicle *AP_Vehicle::get_singleton()
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index 91061cf5b0..81f5e008ca 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -241,6 +241,10 @@ private:
     // delay() callback that processing MAVLink packets
     static void scheduler_delay_callback();
 
+    // if there's been a watchdog reset, notify the world via a
+    // statustext:
+    void send_watchdog_reset_statustext();
+
     bool likely_flying;         // true if vehicle is probably flying
     uint32_t _last_flying_ms;   // time when likely_flying last went true