AP_OpticalFlow: Add support for onboard optflow

Onboard Optical flow needs to have access to the ahrs to get
correct gyro values.
Therefore the constructor takes the ahrs as a param like it is done
for other classes that need to have access to these datas

libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp

@@ -0,0 +1,103 @@
+/*
+   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 "OpticalFlow.h"
+#include "AP_OpticalFlow_Onboard.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX &&\
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
+//#define FLOWONBOARD_DEBUG 1
+#define OPTICALFLOW_ONBOARD_ID 1
+extern const AP_HAL::HAL& hal;
+
+AP_OpticalFlow_Onboard::AP_OpticalFlow_Onboard(OpticalFlow &_frontend,
+                                               AP_AHRS_NavEKF& ahrs) :
+    OpticalFlow_backend(_frontend),
+    _ahrs(ahrs)
+{}
+
+void AP_OpticalFlow_Onboard::init(void)
+{
+    /* register callback to get gyro data */
+    hal.opticalflow->init(
+            FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Onboard::_get_gyro,
+                                void, float&, float&, float&));
+}
+
+void AP_OpticalFlow_Onboard::update()
+{
+    AP_HAL::OpticalFlow::Data_Frame data_frame;
+    // read at maximum 10Hz
+    uint32_t now = AP_HAL::millis();
+    if (now - _last_read_ms < 100) {
+        return;
+    }
+    _last_read_ms = now;
+
+    if (!hal.opticalflow->read(data_frame)) {
+        return;
+    }
+
+    struct OpticalFlow::OpticalFlow_state state;
+    state.device_id = OPTICALFLOW_ONBOARD_ID;
+    state.surface_quality = data_frame.quality;
+    if (data_frame.delta_time > 0) {
+        const Vector2f flowScaler = _flowScaler();
+        float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
+        float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
+
+        state.flowRate.x = flowScaleFactorX * 1000.0f /
+                           float(data_frame.delta_time) *
+                           data_frame.pixel_flow_x_integral;
+
+        state.flowRate.y = flowScaleFactorY * 1000.0f /
+                           float(data_frame.delta_time) *
+                           data_frame.pixel_flow_y_integral;
+
+        state.bodyRate.x = 1.0f / float(data_frame.delta_time) *
+                           data_frame.gyro_x_integral;
+
+        state.bodyRate.y = 1.0f / float(data_frame.delta_time) *
+                           data_frame.gyro_y_integral;
+    } else {
+        state.flowRate.zero();
+        state.bodyRate.zero();
+    }
+
+    // copy results to front end
+    _update_frontend(state);
+
+#if FLOWONBOARD_DEBUG
+    hal.console->printf("FLOW_ONBOARD qual:%u FlowRateX:%4.2f Y:%4.2f"
+                        "BodyRateX:%4.2f Y:%4.2f, delta_time = %u\n",
+                        (unsigned)state.surface_quality,
+                        (double)state.flowRate.x,
+                        (double)state.flowRate.y,
+                        (double)state.bodyRate.x,
+                        (double)state.bodyRate.y,
+                        data_frame.delta_time);
+#endif
+}
+
+void AP_OpticalFlow_Onboard::_get_gyro(float &rate_x, float &rate_y,
+                                       float &rate_z)
+{
+    Vector3f rates = _ahrs.get_gyro();
+    rate_x = rates.x;
+    rate_y = rates.y;
+    rate_z = rates.z;
+}
+
+#endif

libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.h

@@ -0,0 +1,35 @@
+/*
+   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/>.
+ */
+#ifndef __AP_OPTICALFLOW_ONBOARD_H__
+#define __AP_OPTICALFLOW_ONBOARD_H__
+
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_NavEKF/AP_NavEKF.h>
+#include <AP_NavEKF2/AP_NavEKF2.h>
+#include <AP_Math/AP_Math.h>
+
+class AP_OpticalFlow_Onboard : public OpticalFlow_backend
+{
+public:
+    AP_OpticalFlow_Onboard(OpticalFlow &_frontend, AP_AHRS_NavEKF &ahrs);
+    void init(void);
+    void update(void);
+private:
+    AP_AHRS &_ahrs;
+    void _get_gyro(float&, float&, float&);
+    uint32_t _last_read_ms;
+};
+
+#endif

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -1,6 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #include "OpticalFlow.h"
+#include "AP_OpticalFlow_Onboard.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -40,13 +41,16 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
 };
 
 // default constructor
-OpticalFlow::OpticalFlow(void) :
+OpticalFlow::OpticalFlow(AP_AHRS_NavEKF& ahrs) :
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     backend(new AP_OpticalFlow_PX4(*this)),
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
     backend(new AP_OpticalFlow_HIL(*this)),
-#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX &&\
+      CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_BEBOP
     backend(new AP_OpticalFlow_Linux(*this)),
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
+    backend(new AP_OpticalFlow_Onboard(*this, ahrs)),
 #else
     backend(NULL),
 #endif

libraries/AP_OpticalFlow/OpticalFlow.h

@@ -24,6 +24,7 @@
 #include <AP_Math/AP_Math.h>
 
 class OpticalFlow_backend;
+class AP_AHRS_NavEKF;
 
 class OpticalFlow
 {
@@ -31,7 +32,7 @@ class OpticalFlow
 
 public:
     // constructor
-    OpticalFlow(void);
+    OpticalFlow(AP_AHRS_NavEKF& ahrs);
 
     // init - initialise sensor
     void init(void);