From 33ca577653a29c7ea62a57f1d6c86776b53f8178 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 2 Sep 2018 22:25:52 +1000
Subject: [PATCH] Copter: OpticalFlow takes care of its own logging

---
 ArduCopter/ArduCopter.cpp |  2 +-
 ArduCopter/Copter.h       |  1 -
 ArduCopter/Log.cpp        | 41 ---------------------------------------
 ArduCopter/defines.h      |  1 -
 ArduCopter/sensors.cpp    | 31 +----------------------------
 5 files changed, 2 insertions(+), 74 deletions(-)

diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp
index 6dbdd7d318..1d2bc3d17f 100644
--- a/ArduCopter/ArduCopter.cpp
+++ b/ArduCopter/ArduCopter.cpp
@@ -87,7 +87,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(throttle_loop,         50,     75),
     SCHED_TASK(update_GPS,            50,    200),
 #if OPTFLOW == ENABLED
-    SCHED_TASK(update_optical_flow,  200,    160),
+    SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),
 #endif
     SCHED_TASK(update_batt_compass,   10,    120),
     SCHED_TASK(read_aux_all,          10,     50),
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index bc1c77546d..cdfc1b107a 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -757,7 +757,6 @@ private:
     void landinggear_update();
 
     // Log.cpp
-    void Log_Write_Optflow();
     void Log_Write_Control_Tuning();
     void Log_Write_Performance();
     void Log_Write_Attitude();
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index 40e5184b64..08265bf53e 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -59,39 +59,6 @@ void Copter::ModeAutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_
 }
 #endif
 
-struct PACKED log_Optflow {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    uint8_t surface_quality;
-    float flow_x;
-    float flow_y;
-    float body_x;
-    float body_y;
-};
-
-// Write an optical flow packet
-void Copter::Log_Write_Optflow()
-{
- #if OPTFLOW == ENABLED
-    // 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_us         : AP_HAL::micros64(),
-        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     // OPTFLOW == ENABLED
-}
-
 struct PACKED log_Control_Tuning {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -507,10 +474,6 @@ const struct LogStructure Copter::log_structure[] = {
 #endif
     { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
       "PTUN", "QBfHHH",          "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },
-#if OPTFLOW == ENABLED
-    { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
-      "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
-#endif
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
       "CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
@@ -583,10 +546,6 @@ void Copter::Log_Write_Vehicle_Startup_Messages() {}
 void Copter::Log_Write_Heli() {}
 #endif
 
-#if OPTFLOW == ENABLED
-void Copter::Log_Write_Optflow() {}
-#endif
-
 void Copter::log_init(void) {}
 
 #endif // LOGGING_ENABLED
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 61563e5dce..50bf946dbc 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -240,7 +240,6 @@ enum LoggingParameters {
      TYPE_AIRSTART_MSG,
      TYPE_GROUNDSTART_MSG,
      LOG_CONTROL_TUNING_MSG,
-     LOG_OPTFLOW_MSG,
      LOG_EVENT_MSG,
      LOG_ERROR_MSG,
      LOG_DATA_INT16_MSG,
diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp
index 19dc71251b..18bc08ccb4 100644
--- a/ArduCopter/sensors.cpp
+++ b/ArduCopter/sensors.cpp
@@ -121,39 +121,10 @@ void Copter::init_optflow()
 {
 #if OPTFLOW == ENABLED
     // initialise optical flow sensor
-    optflow.init();
+    optflow.init(MASK_LOG_OPTFLOW);
 #endif      // OPTFLOW == ENABLED
 }
 
-// called at 200hz
-#if OPTFLOW == ENABLED
-void Copter::update_optical_flow(void)
-{
-    static uint32_t last_of_update = 0;
-
-    // exit immediately if not enabled
-    if (!optflow.enabled()) {
-        return;
-    }
-
-    // read from sensor
-    optflow.update();
-
-    // write to log and send to EKF if new data has arrived
-    if (optflow.last_update() != last_of_update) {
-        last_of_update = optflow.last_update();
-        uint8_t flowQuality = optflow.quality();
-        Vector2f flowRate = optflow.flowRate();
-        Vector2f bodyRate = optflow.bodyRate();
-        const Vector3f &posOffset = optflow.get_pos_offset();
-        ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset);
-        if (g.log_bitmask & MASK_LOG_OPTFLOW) {
-            Log_Write_Optflow();
-        }
-    }
-}
-#endif  // OPTFLOW == ENABLED
-
 void Copter::compass_cal_update()
 {
     static uint32_t compass_cal_stick_gesture_begin = 0;