From 2500f7e9c2c696c4f04470529c4bed26c0c475db Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sun, 2 Nov 2014 17:26:05 +1100
Subject: [PATCH] Plane : Add data logging for optical flow use with EKF

---
 ArduPlane/APM_Config.h   |  2 --
 ArduPlane/Log.pde        | 40 ++++++++++++++++++++++++++++++++++++++++
 ArduPlane/Parameters.h   |  3 +++
 ArduPlane/Parameters.pde |  6 ++++++
 ArduPlane/config.h       | 16 ++++++++++++++++
 ArduPlane/defines.h      |  3 +++
 6 files changed, 68 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h
index 0a505f9e2e..5a92990766 100644
--- a/ArduPlane/APM_Config.h
+++ b/ArduPlane/APM_Config.h
@@ -20,5 +20,3 @@
  *  // 2. HIL_MODE_SENSORS: full sensor simulation
  *
  */
-#define OPTFLOW               ENABLED             // enable optical flow sensor
-
diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index b33753bc26..97344e427b 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -374,6 +374,39 @@ static void Log_Write_Sonar()
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+struct PACKED log_Optflow {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint8_t surface_quality;
+    float flow_x;
+    float flow_y;
+    float body_x;
+    float body_y;
+};
+
+#if OPTFLOW == ENABLED
+// Write an optical flow packet
+static void Log_Write_Optflow()
+{
+    // exit immediately if not enabled
+    if (!optflow.enabled()) {
+        return;
+    }
+    const Vector2f &flowRate = optflow.flowRate();
+    const Vector2f &bodyRate = optflow.bodyRate();
+    struct log_Optflow pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
+        time_ms         : hal.scheduler->millis(),
+        surface_quality : optflow.quality(),
+        flow_x           : flowRate.x,
+        flow_y           : flowRate.y,
+        body_x           : bodyRate.x,
+        body_y           : bodyRate.y
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+#endif
+
 struct PACKED log_Current {
     LOG_PACKET_HEADER;
     uint32_t time_ms;
@@ -563,6 +596,10 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "ARSP",  "Iffcff",   "TimeMS,Airspeed,DiffPress,Temp,RawPress,Offset" },
     { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP),
       "ATRP", "IBBcfff",  "TimeMS,Type,State,Servo,Demanded,Achieved,P" },
+#if OPTFLOW == ENABLED
+    { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
+      "OF",   "IBffff",   "TimeMS,Qual,flowX,flowY,bodyX,bodyY" },
+#endif
     TECS_LOG_FORMAT(LOG_TECS_MSG)
 };
 
@@ -614,6 +651,9 @@ static void Log_Write_IMU() {}
 static void Log_Write_RC() {}
 static void Log_Write_Airspeed(void) {}
 static void Log_Write_Baro(void) {}
+#if OPTFLOW == ENABLED
+static void Log_Write_Optflow() {}
+#endif
 
 static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {
     return 0;
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index d547a095de..856f2abfea 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -130,6 +130,9 @@ public:
         k_param_rtl_autoland,
         k_param_override_channel,
         k_param_stall_prevention,
+#if OPTFLOW == ENABLED
+        k_param_optflow,
+#endif
 
         // 100: Arming parameters
         k_param_arming = 100,
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index a47f09c016..7f5eb2bc0a 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -1144,6 +1144,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECTN(ahrs.get_NavEKF(), NavEKF, "EKF_", NavEKF),
 #endif
 
+#if OPTFLOW == ENABLED
+    // @Group: FLOW
+    // @Path: ../libraries/AP_OpticalFlow/OpticalFlow.cpp
+    GOBJECT(optflow,   "FLOW", OpticalFlow),
+#endif
+
     // @Group: MIS_
     // @Path: ../libraries/AP_Mission/AP_Mission.cpp
     GOBJECT(mission, "MIS_",       AP_Mission),
diff --git a/ArduPlane/config.h b/ArduPlane/config.h
index 9c86216098..17a5b71b43 100644
--- a/ArduPlane/config.h
+++ b/ArduPlane/config.h
@@ -80,6 +80,10 @@
  #ifndef FRSKY_TELEM_ENABLED
  # define FRSKY_TELEM_ENABLED DISABLED
  #endif
+#ifndef OPTFLOW
+# define OPTFLOW DISABLED
+#endif
+
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -135,6 +139,18 @@
 #endif
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Optical flow sensor support
+//
+
+#ifndef OPTFLOW
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ # define OPTFLOW DISABLED
+#else
+ # define OPTFLOW ENABLED
+#endif
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////
 // RADIO CONFIGURATION
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 224bb286f5..e0894966cd 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -128,6 +128,9 @@ enum log_messages {
     LOG_ARM_DISARM_MSG,
     LOG_AIRSPEED_MSG,
     LOG_COMPASS3_MSG
+#if OPTFLOW == ENABLED
+    ,LOG_OPTFLOW_MSG
+#endif
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)