Plane : Add data logging for optical flow use with EKF

ArduPlane/APM_Config.h

@@ -20,5 +20,3 @@
  *  // 2. HIL_MODE_SENSORS: full sensor simulation
  *
  */
-#define OPTFLOW               ENABLED             // enable optical flow sensor
-

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;

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,

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),

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

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)