From 8d253dec8e95138af21aa03edc4bce3e08e3577c Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 7 Jul 2016 17:12:27 +1000
Subject: [PATCH] DataFlash: support for mavlink SENSOR message

---
 libraries/DataFlash/DataFlash.cpp         | 19 ++++++++++++++++++
 libraries/DataFlash/DataFlash.h           |  7 +++++++
 libraries/DataFlash/DataFlash_Backend.h   |  4 ++++
 libraries/DataFlash/DataFlash_File.cpp    | 24 +++++++++++++++++++++++
 libraries/DataFlash/DataFlash_File.h      |  6 +++++-
 libraries/DataFlash/DataFlash_MAVLink.cpp |  5 +++++
 libraries/DataFlash/DataFlash_MAVLink.h   |  4 ++++
 7 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp
index e6eb530075f..1146f7aca46 100644
--- a/libraries/DataFlash/DataFlash.cpp
+++ b/libraries/DataFlash/DataFlash.cpp
@@ -40,6 +40,25 @@ const struct LogStructure *DataFlash_Class::structure(uint16_t num) const
     return &_structures[num];
 }
 
+bool DataFlash_Class::logging_present() const
+{
+    return _next_backend != 0;
+}
+bool DataFlash_Class::logging_enabled() const
+{
+    if (_next_backend == 0) {
+        return false;
+    }
+    return backends[0]->logging_enabled();
+}
+bool DataFlash_Class::logging_failed() const
+{
+    if (_next_backend < 1) {
+        // we should not have been called!
+        return true;
+    }
+    return backends[0]->logging_failed();
+}
 
 #define FOR_EACH_BACKEND(methodcall)              \
     do {                                          \
diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index b279d7fc202..594a4d5ff4d 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -200,6 +200,13 @@ public:
 
     const struct LogStructure *structure(uint16_t num) const;
 
+    // methods for mavlink SYS_STATUS message (send_extended_status1)
+    // these methods cover only the first logging backend used -
+    // typically DataFlash_File.
+    bool logging_present() const;
+    bool logging_enabled() const;
+    bool logging_failed() const;
+
 protected:
 
     const struct LogStructure *_structures;
diff --git a/libraries/DataFlash/DataFlash_Backend.h b/libraries/DataFlash/DataFlash_Backend.h
index f15fd93c10a..308049479d4 100644
--- a/libraries/DataFlash/DataFlash_Backend.h
+++ b/libraries/DataFlash/DataFlash_Backend.h
@@ -117,6 +117,10 @@ public:
     // values contained in arg_list:
     bool Log_Write(uint8_t msg_type, va_list arg_list, bool is_critical=false);
 
+    // these methods are used when reporting system status over mavlink
+    virtual bool logging_enabled() const = 0;
+    virtual bool logging_failed() const = 0;
+
 protected:
     uint32_t dropped;
     uint8_t internal_errors; // uint8_t - wishful thinking?
diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp
index 97f570abaa0..f4f67c6ea8b 100644
--- a/libraries/DataFlash/DataFlash_File.cpp
+++ b/libraries/DataFlash/DataFlash_File.cpp
@@ -1100,5 +1100,29 @@ void DataFlash_File::_io_timer(void)
     hal.util->perf_end(_perf_write);
 }
 
+// this sensor is enabled if we should be logging at the moment
+bool DataFlash_File::logging_enabled() const
+{
+    if (hal.util->get_soft_armed() ||
+        _front.log_while_disarmed()) {
+        return true;
+    }
+    return false;
+}
+
+bool DataFlash_File::logging_failed() const
+{
+    if (_write_fd == -1 &&
+        (hal.util->get_soft_armed() ||
+         _front.log_while_disarmed())) {
+        return true;
+    }
+    if (_open_error) {
+        return true;
+    }
+    return false;
+}
+
+
 #endif // HAL_OS_POSIX_IO
 
diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h
index 13771a4e9d1..a6802b245e7 100644
--- a/libraries/DataFlash/DataFlash_File.h
+++ b/libraries/DataFlash/DataFlash_File.h
@@ -66,7 +66,11 @@ public:
     void flush(void);
 #endif
     void periodic_fullrate(const uint32_t now);
-    
+
+    // this method is used when reporting system status over mavlink
+    bool logging_enabled() const;
+    bool logging_failed() const;
+
 private:
     int _write_fd;
     int _read_fd;
diff --git a/libraries/DataFlash/DataFlash_MAVLink.cpp b/libraries/DataFlash/DataFlash_MAVLink.cpp
index 6cdf75f03d9..3a472e649f0 100644
--- a/libraries/DataFlash/DataFlash_MAVLink.cpp
+++ b/libraries/DataFlash/DataFlash_MAVLink.cpp
@@ -52,6 +52,11 @@ void DataFlash_MAVLink::Init()
                              // the vehicles
 }
 
+bool DataFlash_MAVLink::logging_failed() const
+{
+    return !_sending_to_client;
+}
+
 uint16_t DataFlash_MAVLink::bufferspace_available() {
     return (_blockcount_free * 200 + remaining_space_in_current_block());
 }
diff --git a/libraries/DataFlash/DataFlash_MAVLink.h b/libraries/DataFlash/DataFlash_MAVLink.h
index b6e8bbbf001..28931eab472 100644
--- a/libraries/DataFlash/DataFlash_MAVLink.h
+++ b/libraries/DataFlash/DataFlash_MAVLink.h
@@ -127,6 +127,10 @@ protected:
         uint8_t state_sent_max;
     } stats;
 
+    // this method is used when reporting system status over mavlink
+    bool logging_enabled() const { return true; }
+    bool logging_failed() const;
+
 private:
     mavlink_channel_t _chan;
     uint8_t _target_system_id;