From 764fa3671692f027622b8cfe892132f8e0ea59fd Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sun, 12 Jul 2015 22:06:21 +0900
Subject: [PATCH] Copter: log ERR when compass, baro unhealthy

---
 ArduCopter/ArduCopter.cpp |  5 +++++
 ArduCopter/Copter.cpp     |  4 ++++
 ArduCopter/Copter.h       |  7 +++++++
 ArduCopter/Log.cpp        | 16 ++++++++++++++++
 ArduCopter/defines.h      |  1 +
 5 files changed, 33 insertions(+)

diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp
index 9a54eb81a7..d119d4a632 100644
--- a/ArduCopter/ArduCopter.cpp
+++ b/ArduCopter/ArduCopter.cpp
@@ -272,6 +272,11 @@ void Copter::fast_loop()
 
     // check if we've landed or crashed
     update_land_and_crash_detectors();
+
+    // log sensor health
+    if (should_log(MASK_LOG_ANY)) {
+        Log_Sensor_Health();
+    }
 }
 
 // rc_loops - reads user input from transmitter/receiver
diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index 86f5779d63..9d63ae9a98 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -125,6 +125,10 @@ Copter::Copter(void) :
     param_loader(var_info)
 {
     memset(&current_loc, 0, sizeof(current_loc));
+
+    // init sensor error logging flags
+    sensor_health.baro = true;
+    sensor_health.compass = true;
 }
 
 Copter copter;
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 48a17cd9de..118d07bd29 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -271,6 +271,12 @@ private:
         uint32_t last_heartbeat_ms;      // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
     } failsafe;
 
+    // sensor health for logging
+    struct {
+        uint8_t baro        : 1;    // true if baro is healthy
+        uint8_t compass     : 1;    // true if compass is healthy
+    } sensor_health;
+
     // Motor Output
 #if FRAME_CONFIG == QUAD_FRAME
  #define MOTOR_CLASS AP_MotorsQuad
@@ -598,6 +604,7 @@ private:
     void Log_Write_Baro(void);
     void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high);
     void Log_Write_Home_And_Origin();
+    void Log_Sensor_Health();
 #if FRAME_CONFIG == HELI_FRAME
     void Log_Write_Heli(void);
 #endif
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index e906835ad0..47a5af9203 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -646,6 +646,22 @@ void Copter::Log_Write_Home_And_Origin()
     }
 }
 
+// logs when baro or compass becomes unhealthy
+void Copter::Log_Sensor_Health()
+{
+    // check baro
+    if (sensor_health.baro != barometer.healthy()) {
+        sensor_health.baro = barometer.healthy();
+        Log_Write_Error(ERROR_SUBSYSTEM_BARO, (sensor_health.baro ? ERROR_CODE_ERROR_RESOLVED : ERROR_CODE_UNHEALTHY));
+    }
+
+    // check compass
+    if (sensor_health.compass != compass.healthy()) {
+        sensor_health.compass = compass.healthy();
+        Log_Write_Error(ERROR_SUBSYSTEM_COMPASS, (sensor_health.compass ? ERROR_CODE_ERROR_RESOLVED : ERROR_CODE_UNHEALTHY));
+    }
+}
+
 struct PACKED log_Heli {
     LOG_PACKET_HEADER;
     uint64_t time_us;
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index ea5d0662ef..0eab81337b 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -353,6 +353,7 @@ enum FlipState {
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
+#define ERROR_CODE_UNHEALTHY                4
 // subsystem specific error codes -- radio
 #define ERROR_CODE_RADIO_LATE_FRAME         2
 // subsystem specific error codes -- failsafe_thr, batt, gps