OpticalFlow: add MAVLink driver

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

@@ -0,0 +1,107 @@
+/*
+   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_MAV.h"
+
+#define OPTFLOW_MAV_TIMEOUT_SEC 0.5f
+
+// detect the device
+AP_OpticalFlow_MAV *AP_OpticalFlow_MAV::detect(OpticalFlow &_frontend)
+{
+    // we assume mavlink messages will be sent into this driver
+    AP_OpticalFlow_MAV *sensor = new AP_OpticalFlow_MAV(_frontend);
+    return sensor;
+}
+
+// read latest values from sensor and fill in x,y and totals.
+void AP_OpticalFlow_MAV::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.device_id = sensor_id;
+    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_MAV_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
+        state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt,
+                           ((float)flow_sum.y / count) * flow_scale_factor_y * dt };
+
+        // 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 mavlink messages
+void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t *msg)
+{
+    mavlink_optical_flow_t packet;
+    mavlink_msg_optical_flow_decode(msg, &packet);
+
+    // record time message was received
+    // ToDo: add jitter correction
+    latest_frame_us = AP_HAL::micros64();
+
+    // add sensor values to sum
+    flow_sum.x += packet.flow_x;
+    flow_sum.y += packet.flow_y;
+    quality_sum += packet.quality;
+    count++;
+
+    // take sensor id from message
+    sensor_id = packet.sensor_id;
+}

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "OpticalFlow.h"
+#include <AP_HAL/utility/OwnPtr.h>
+
+class AP_OpticalFlow_MAV : 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 mavlink
+    void handle_msg(const mavlink_message_t *msg) override;
+
+    // detect if the sensor is available
+    static AP_OpticalFlow_MAV *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
+    uint8_t sensor_id;                  // sensor_id received in latest mavlink message
+    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
+};

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -5,6 +5,7 @@
 #include "AP_OpticalFlow_Pixart.h"
 #include "AP_OpticalFlow_PX4Flow.h"
 #include "AP_OpticalFlow_CXOF.h"
+#include "AP_OpticalFlow_MAV.h"
 #include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL& hal;
@@ -118,6 +119,9 @@ void OpticalFlow::init(uint32_t log_bit)
         if (backend == nullptr) {
             backend = AP_OpticalFlow_CXOF::detect(*this);
         }
+        if (backend == nullptr) {
+            backend = AP_OpticalFlow_MAV::detect(*this);
+        }
     }
 
     if (backend != nullptr) {
@@ -140,6 +144,18 @@ void OpticalFlow::update(void)
     _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
 }
 
+void OpticalFlow::handle_msg(const mavlink_message_t *msg)
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    if (backend != nullptr) {
+        backend->handle_msg(msg);
+    }
+}
+
 void OpticalFlow::update_state(const OpticalFlow_state &state)
 {
     _state = state;

libraries/AP_OpticalFlow/OpticalFlow.h

@@ -21,6 +21,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
 
 class OpticalFlow_backend;
 class AP_AHRS_NavEKF;
@@ -53,6 +54,9 @@ public:
     // read latest values from sensor and fill in x,y and totals.
     void update(void);
 
+    // handle optical flow mavlink messages
+    void handle_msg(const mavlink_message_t *msg);
+
     // 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

@@ -35,6 +35,9 @@ public:
     // read latest values from sensor and fill in x,y and totals.
     virtual void update() = 0;
 
+    // handle optical flow mavlink messages
+    virtual void handle_msg(const mavlink_message_t *msg) {}
+
 protected:
     // access to frontend
     OpticalFlow &frontend;