From ac8376548784a8f7610dc34e86c046653c0b4988 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 2 Sep 2018 22:27:29 +1000
Subject: [PATCH] OpticalFlow: take care of own logging

---
 libraries/AP_OpticalFlow/OpticalFlow.cpp      | 50 ++++++++++++++++++-
 libraries/AP_OpticalFlow/OpticalFlow.h        |  9 +++-
 .../AP_OpticalFlow/OpticalFlow_backend.cpp    |  3 +-
 3 files changed, 58 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp
index 65ed130867..d190b1cd74 100644
--- a/libraries/AP_OpticalFlow/OpticalFlow.cpp
+++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp
@@ -4,6 +4,7 @@
 #include "AP_OpticalFlow_SITL.h"
 #include "AP_OpticalFlow_Pixart.h"
 #include "AP_OpticalFlow_PX4Flow.h"
+#include <DataFlash/DataFlash.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -76,8 +77,10 @@ OpticalFlow::OpticalFlow()
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-void OpticalFlow::init(void)
+void OpticalFlow::init(uint32_t log_bit)
 {
+     _log_bit = log_bit;
+
     // return immediately if not enabled
     if (!_enabled) {
         return;
@@ -117,13 +120,58 @@ void OpticalFlow::init(void)
 
 void OpticalFlow::update(void)
 {
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
     if (backend != nullptr) {
         backend->update();
     }
+
     // only healthy if the data is less than 0.5s old
     _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
 }
 
+void OpticalFlow::update_state(const OpticalFlow_state &state)
+{
+    _state = state;
+    _last_update_ms = AP_HAL::millis();
+
+    // write to log and send to EKF if new data has arrived
+    AP::ahrs_navekf().writeOptFlowMeas(quality(),
+                                       _state.flowRate,
+                                       _state.bodyRate,
+                                       _last_update_ms,
+                                       get_pos_offset());
+    Log_Write_Optflow();
+}
+
+void OpticalFlow::Log_Write_Optflow()
+{
+    DataFlash_Class *instance = DataFlash_Class::instance();
+    if (instance == nullptr) {
+        return;
+    }
+    if (_log_bit != (uint32_t)-1 &&
+        !instance->should_log(_log_bit)) {
+        return;
+    }
+
+    struct log_Optflow pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
+        time_us         : AP_HAL::micros64(),
+        surface_quality : _state.surface_quality,
+        flow_x          : _state.flowRate.x,
+        flow_y          : _state.flowRate.y,
+        body_x          : _state.bodyRate.x,
+        body_y          : _state.bodyRate.y
+    };
+    instance->WriteBlock(&pkt, sizeof(pkt));
+}
+
+
+
 // singleton instance
 OpticalFlow *OpticalFlow::_singleton;
 
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h
index e679442168..85eabe2bc9 100644
--- a/libraries/AP_OpticalFlow/OpticalFlow.h
+++ b/libraries/AP_OpticalFlow/OpticalFlow.h
@@ -42,7 +42,7 @@ public:
     }
 
     // init - initialise sensor
-    void init(void);
+    void init(uint32_t log_bit);
 
     // enabled - returns true if optical flow is enabled
     bool enabled() const { return _enabled; }
@@ -101,10 +101,17 @@ private:
     AP_Vector3f _pos_offset;        // position offset of the flow sensor in the body frame
     AP_Int8  _address;              // address on the bus (allows selecting between 8 possible I2C addresses for px4flow)
 
+    // method called by backend to update frontend state:
+    void update_state(const OpticalFlow_state &state);
+
     // state filled in by backend
     struct OpticalFlow_state _state;
 
     uint32_t _last_update_ms;        // millis() time of last update
+
+    void Log_Write_Optflow();
+    uint32_t _log_bit = -1;     // bitmask bit which indicates if we should log.  -1 means we always log
+
 };
 
 namespace AP {
diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp b/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp
index 2895d02836..ffb770b158 100644
--- a/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp
+++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp
@@ -29,8 +29,7 @@ OpticalFlow_backend::~OpticalFlow_backend(void)
 // update the frontend
 void OpticalFlow_backend::_update_frontend(const struct OpticalFlow::OpticalFlow_state &state)
 {
-    frontend._state = state;
-    frontend._last_update_ms = AP_HAL::millis();
+    frontend.update_state(state);
 }
 
 // apply yaw angle to a vector