AP_OpticalFlow: added support for MSP based flow sensors

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -1,4 +1,4 @@
 /// @file   AP_OpticalFlow.h
 /// @brief  Catch-all header that defines all supported optical flow classes.
 
-#include "OpticalFlow.h"
+#include "OpticalFlow.h"

libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.cpp

@@ -0,0 +1,105 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include "AP_OpticalFlow_MSP.h"
+
+#if HAL_MSP_ENABLED
+
+#define OPTFLOW_MSP_TIMEOUT_SEC 0.5f // 2Hz
+
+extern const AP_HAL::HAL& hal;
+
+// detect the device
+AP_OpticalFlow_MSP *AP_OpticalFlow_MSP::detect(OpticalFlow &_frontend)
+{
+    // we assume msp messages will be sent into this driver
+    return new AP_OpticalFlow_MSP(_frontend);
+}
+
+// read latest values from sensor and fill in x,y and totals.
+void AP_OpticalFlow_MSP::update(void)
+{
+    // record gyro values as long as they are being used
+    // the sanity check of dt below ensures old gyro values are not used
+    if (gyro_sum_count < 1000) {
+        const Vector3f& gyro = AP::ahrs().get_gyro();
+        gyro_sum.x += gyro.x;
+        gyro_sum.y += gyro.y;
+        gyro_sum_count++;
+    }
+
+    // return without updating state if no readings
+    if (count == 0) {
+        return;
+    }
+
+    struct OpticalFlow::OpticalFlow_state state {};
+
+    state.surface_quality = quality_sum / count;
+
+    // calculate dt
+    const float dt = (latest_frame_us - prev_frame_us) * 1.0e-6;
+    prev_frame_us = latest_frame_us;
+
+    // sanity check dt
+    if (is_positive(dt) && (dt < OPTFLOW_MSP_TIMEOUT_SEC)) {
+        // calculate flow values
+        const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x;
+        const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y;
+
+        // copy flow rates to state structure and invert flow vector (needed for MSP flow message)
+        state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt * -1,
+                           ((float)flow_sum.y / count) * flow_scale_factor_y * dt * -1 };
+
+        // copy average body rate to state structure
+        state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count };
+
+        _applyYaw(state.flowRate);
+        _applyYaw(state.bodyRate);
+    } else {
+        // first frame received in some time so cannot calculate flow values
+        state.flowRate.zero();
+        state.bodyRate.zero();
+    }
+
+    _update_frontend(state);
+
+    // reset local buffers
+    flow_sum.zero();
+    quality_sum = 0;
+    count = 0;
+
+    // reset gyro sum
+    gyro_sum.zero();
+    gyro_sum_count = 0;
+}
+
+// handle OPTICAL_FLOW msp messages
+void AP_OpticalFlow_MSP::handle_msp(const msp_opflow_sensor_t &pkt)
+{
+    // record time message was received
+    // ToDo: add jitter correction
+    latest_frame_us = AP_HAL::micros64();
+
+    // add sensor values to sum
+    flow_sum.x += pkt.motion_x;
+    flow_sum.y += pkt.motion_y;
+    quality_sum += (int)pkt.quality * 100 / 255;
+    count++;
+}
+
+#endif //HAL_MSP_ENABLED

libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include "OpticalFlow.h"
+#include <AP_HAL/utility/OwnPtr.h>
+
+#if HAL_MSP_ENABLED
+
+class AP_OpticalFlow_MSP : public OpticalFlow_backend
+{
+public:
+    /// constructor
+    using OpticalFlow_backend::OpticalFlow_backend;
+
+    // initialise the sensor
+    void init() override {}
+
+    // read latest values from sensor and fill in x,y and totals.
+    void update(void) override;
+
+    // get update from msp
+    void handle_msp(const msp_opflow_sensor_t &pkt) override;
+
+    // detect if the sensor is available
+    static AP_OpticalFlow_MSP *detect(OpticalFlow &_frontend);
+
+private:
+
+    uint64_t prev_frame_us;             // system time of last message when update was last called
+    uint64_t latest_frame_us;           // system time of most recent messages processed
+    Vector2l flow_sum;                  // sum of sensor's flow_x and flow_y values since last call to update
+    uint16_t quality_sum;               // sum of sensor's quality values since last call to update
+    uint16_t count;                     // number of sensor readings since last call to update
+    Vector2f gyro_sum;                  // sum of gyro sensor values since last frame from flow sensor
+    uint16_t gyro_sum_count;            // number of gyro sensor values in sum
+};
+
+#endif //HAL_MSP_ENABLED

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -7,6 +7,7 @@
 #include "AP_OpticalFlow_CXOF.h"
 #include "AP_OpticalFlow_MAV.h"
 #include "AP_OpticalFlow_HereFlow.h"
+#include "AP_OpticalFlow_MSP.h"
 #include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL& hal;
@@ -25,7 +26,7 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Optical flow sensor type
     // @Description: Optical flow sensor type
-    // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:UAVCAN
+    // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:UAVCAN, 7:MSP
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO("_TYPE", 0,  OpticalFlow,    _type,   (int8_t)OPTICAL_FLOW_TYPE_DEFAULT),
@@ -135,6 +136,11 @@ void OpticalFlow::init(uint32_t log_bit)
         backend = new AP_OpticalFlow_HereFlow(*this);
 #endif
         break;
+#if HAL_MSP_ENABLED        
+    case OpticalFlowType::MSP:
+        backend = AP_OpticalFlow_MSP::detect(*this);
+        break;
+#endif //HAL_MSP_ENABLED
     case OpticalFlowType::SITL:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         backend = new AP_OpticalFlow_SITL(*this);
@@ -173,6 +179,20 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg)
     }
 }
 
+#if HAL_MSP_ENABLED
+void OpticalFlow::handle_msp(const msp_opflow_sensor_t &pkt)
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    if (backend != nullptr) {
+        backend->handle_msp(pkt);
+    }
+}
+#endif //HAL_MSP_ENABLED
+
 void OpticalFlow::update_state(const OpticalFlow_state &state)
 {
     _state = state;

libraries/AP_OpticalFlow/OpticalFlow.h

@@ -19,10 +19,15 @@
  *       Code by Randy Mackay. DIYDrones.com
  */
 
+#include <AP_MSP/msp.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 
+#if HAL_MSP_ENABLED
+using namespace MSP;
+#endif //HAL_MSP_ENABLED
+
 class OpticalFlow_backend;
 class AP_AHRS_NavEKF;
 
@@ -50,7 +55,8 @@ public:
         CXOF = 4,
         MAVLINK = 5,
         UAVCAN = 6,
-        SITL = 10
+        MSP = 7,
+        SITL = 10,
     };
 
     // init - initialise sensor
@@ -68,6 +74,11 @@ public:
     // handle optical flow mavlink messages
     void handle_msg(const mavlink_message_t &msg);
 
+#if HAL_MSP_ENABLED
+    // handle optical flow msp messages
+    void handle_msp(const msp_opflow_sensor_t &pkt);
+#endif //HAL_MSP_ENABLED
+
     // quality - returns the surface quality as a measure from 0 ~ 255
     uint8_t quality() const { return _state.surface_quality; }
 

libraries/AP_OpticalFlow/OpticalFlow_backend.h

@@ -38,6 +38,10 @@ public:
     // handle optical flow mavlink messages
     virtual void handle_msg(const mavlink_message_t &msg) {}
 
+#if HAL_MSP_ENABLED
+    // handle optical flow msp messages
+    virtual void handle_msp(const MSP::msp_opflow_sensor_t &pkt) {}
+#endif
 protected:
     // access to frontend
     OpticalFlow &frontend;