AP_OpticalFlow: split library into frontend/backend

this will make it easier to add a SITL backend

libraries/AP_OpticalFlow/AP_OpticalFlow.h

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

libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.cpp

@@ -0,0 +1,37 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+
+/*
+ * AP_OpticalFlow_HIL.cpp - HIL emulation of optical flow sensor.
+ * This is a dummy class, with the work done in setHIL()
+ */
+
+#include <AP_HAL.h>
+#include "OpticalFlow.h"
+
+extern const AP_HAL::HAL& hal;
+
+AP_OpticalFlow_HIL::AP_OpticalFlow_HIL(OpticalFlow &_frontend) : 
+    OpticalFlow_backend(_frontend) 
+{}
+
+void AP_OpticalFlow_HIL::init(void)
+{
+}
+
+void AP_OpticalFlow_HIL::update(void)
+{
+}

libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.h

@@ -0,0 +1,22 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef AP_OpticalFlow_HIL_H
+#define AP_OpticalFlow_HIL_H
+
+#include "OpticalFlow.h"
+
+class AP_OpticalFlow_HIL : public OpticalFlow_backend
+{
+public:
+    /// constructor
+    AP_OpticalFlow_HIL(OpticalFlow &_frontend);
+
+    // init - initialise the sensor
+    void init();
+
+    // update - read latest values from sensor and fill in x,y and totals.
+    void update(void);
+};
+
+#endif // AP_OpticalFlow_HIL_H
+

libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp

@@ -20,11 +20,10 @@
  */
 
 #include <AP_HAL.h>
+#include "OpticalFlow.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 
-#include "AP_OpticalFlow_PX4.h"
-
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -35,7 +34,10 @@
 
 extern const AP_HAL::HAL& hal;
 
-// Public Methods //////////////////////////////////////////////////////////////
+AP_OpticalFlow_PX4::AP_OpticalFlow_PX4(OpticalFlow &_frontend) : 
+OpticalFlow_backend(_frontend) 
+{}
+
 
 void AP_OpticalFlow_PX4::init(void)
 {
@@ -50,39 +52,38 @@ void AP_OpticalFlow_PX4::init(void)
     if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) {
         hal.console->printf("Unable to set flow rate to 10Hz\n");        
     }
-
-    // if we got this far, the sensor must be healthy
-    _flags.healthy = true;
 }
 
 // update - read latest values from sensor and fill in x,y and totals.
 void AP_OpticalFlow_PX4::update(void)
 {
-    // return immediately if not healthy
+    // return immediately if not initialised
-    if (!_flags.healthy) {
+    if (_fd == -1) {
         return;
     }
 
     struct optical_flow_s report;
-    while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && report.timestamp != _last_timestamp) {
+    while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && 
-        _device_id = report.sensor_id;
+           report.timestamp != _last_timestamp) {
-        _surface_quality = report.quality;
+        struct OpticalFlow::OpticalFlow_state state;
+        state.device_id = report.sensor_id;
+        state.surface_quality = report.quality;
         if (report.integration_timespan > 0) {
-            float flowScaleFactorX = 1.0f + 0.001f * float(_flowScalerX);
+            const Vector2f flowScaler = _flowScaler();
-            float flowScaleFactorY = 1.0f + 0.001f * float(_flowScalerY);
+            float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
+            float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
             float integralToRate = 1e6f / float(report.integration_timespan);
-            _flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
+            state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
-            _flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
+            state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
-            _bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
+            state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
-            _bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
+            state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
         } else {
-            _flowRate.x = 0.0f;
+            state.flowRate.zero();
-            _flowRate.y = 0.0f;
+            state.bodyRate.zero();
-            _bodyRate.x = 0.0f;
-            _bodyRate.y = 0.0f;
         }
         _last_timestamp = report.timestamp;
-        _last_update = hal.scheduler->millis();
+
+        _update_frontend(state);
     }
 }
 

libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h

@@ -5,18 +5,17 @@
 
 #include "OpticalFlow.h"
 
-class AP_OpticalFlow_PX4 : public OpticalFlow
+class AP_OpticalFlow_PX4 : public OpticalFlow_backend
 {
 public:
-
     /// constructor
-    AP_OpticalFlow_PX4(const AP_AHRS &ahrs) : OpticalFlow(ahrs) {};
+    AP_OpticalFlow_PX4(OpticalFlow &_frontend);
 
     // init - initialise the sensor
-    virtual void init();
+    void init();
 
     // update - read latest values from sensor and fill in x,y and totals.
-    virtual void update(void);
+    void update(void);
 
 private:
     int         _fd;                // file descriptor for sensor

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -1,7 +1,10 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
 #include <AP_Progmem.h>
 #include "OpticalFlow.h"
 
+extern const AP_HAL::HAL& hal;
+
 const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
     // @Param: ENABLE
     // @DisplayName: Optical flow enable/disable
@@ -30,14 +33,44 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
 };
 
 // default constructor
-OpticalFlow::OpticalFlow(const AP_AHRS &ahrs) :
+OpticalFlow::OpticalFlow(void) :
-    _ahrs(ahrs),
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-    _device_id(0),
+    backend(new AP_OpticalFlow_PX4(*this))
-    _surface_quality(0),
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-    _last_update(0)
+    backend(new AP_OpticalFlow_HIL(*this))
+#else
+    backend(NULL)
+#endif
 {
     AP_Param::setup_object_defaults(this, var_info);
 
-    // healthy flag will be overwritten when init is called
+    memset(&_state, 0, sizeof(_state));
+
+    // healthy flag will be overwritten on update
     _flags.healthy = false;
 };
+
+void OpticalFlow::init(void)
+{
+    if (backend != NULL) {
+        backend->init();
+    } else {
+        _enabled = 0;
+    }
+}
+
+void OpticalFlow::update(void)
+{
+    if (backend != NULL) {
+        backend->update();
+    }
+    // only healthy if the data is less than 0.5s old
+    _flags.healthy = (_last_update_ms - hal.scheduler->millis() < 500);
+}
+
+void OpticalFlow::setHIL(const struct OpticalFlow::OpticalFlow_state &state)
+{ 
+    if (backend) {
+        backend->_update_frontend(state); 
+    }
+}

libraries/AP_OpticalFlow/OpticalFlow.h

@@ -18,76 +18,82 @@
 /*
  *       OpticalFlow.h - OpticalFlow Base Class for Ardupilot
  *       Code by Randy Mackay. DIYDrones.com
- *
- *       Methods:
- *               init() : initializate sensor and library.
- *               read   : reads latest value from OpticalFlow and
- *                        stores values in x,y, surface_quality parameter
- *               read_register()  : reads a value from the sensor (will be
- *                                  sensor specific)
- *               write_register() : writes a value to one of the sensor's
- *                                  register (will be sensor specific)
  */
 
+#include <AP_HAL.h>
 #include <AP_Math.h>
-#include <AP_AHRS.h>
+
+class OpticalFlow_backend;
 
 class OpticalFlow
 {
+    friend class OpticalFlow_backend;
+
 public:
     // constructor
-    OpticalFlow(const AP_AHRS &ahrs);
+    OpticalFlow(void);
 
     // init - initialise sensor
-    virtual void init() {}
+    void init(void);
 
     // enabled - returns true if optical flow is enabled
     bool enabled() const { return _enabled; }
 
     // healthy - return true if the sensor is healthy
-    bool healthy() const { return _flags.healthy; }
+    bool healthy() const { return backend != NULL && _flags.healthy; }
 
     // read latest values from sensor and fill in x,y and totals.
-    virtual void update() {}
+    void update(void);
 
     // quality - returns the surface quality as a measure from 0 ~ 255
-    uint8_t quality() const { return _surface_quality; }
+    uint8_t quality() const { return _state.surface_quality; }
 
     // raw - returns the raw movement from the sensor
-    const Vector2f& flowRate() const { return _flowRate; }
+    const Vector2f& flowRate() const { return _state.flowRate; }
 
     // velocity - returns the velocity in m/s
-    const Vector2f& bodyRate() const { return _bodyRate; }
+    const Vector2f& bodyRate() const { return _state.bodyRate; }
 
     // device_id - returns device id
-    uint8_t device_id() const { return _device_id; }
+    uint8_t device_id() const { return _state.device_id; }
 
     // last_update() - returns system time of last sensor update
-    uint32_t last_update() const { return _last_update; }
+    uint32_t last_update() const { return _last_update_ms; }
 
     // parameter var info table
     static const struct AP_Param::GroupInfo var_info[];
 
-protected:
+    struct OpticalFlow_state {
+        uint8_t device_id;          // device id
+        uint8_t  surface_quality;   // image quality (below TBD you can't trust the dx,dy values returned)
+        Vector2f flowRate;          // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
+        Vector2f bodyRate;          // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
+    };
+
+    // support for HIL/SITL
+    void setHIL(const struct OpticalFlow_state &state);
+
+private:
+    OpticalFlow_backend *backend;
 
     struct AP_OpticalFlow_Flags {
         uint8_t healthy     : 1;    // true if sensor is healthy
     } _flags;
 
-    // external references
-    const AP_AHRS &_ahrs;           // ahrs object
-
     // parameters
     AP_Int8  _enabled;              // enabled/disabled flag
     AP_Int16 _flowScalerX;          // X axis flow scale factor correction - parts per thousand
     AP_Int16 _flowScalerY;          // Y axis flow scale factor correction - parts per thousand
 
-    // internal variables
+
-    uint8_t _device_id;             // device id
+    // state filled in by backend
-    uint8_t  _surface_quality;      // image quality (below TBD you can't trust the dx,dy values returned)
+    struct OpticalFlow_state _state;
-    Vector2f _flowRate;             // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
+
-    Vector2f _bodyRate;             // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
+    uint32_t _last_update_ms;        // millis() time of last update
-    uint32_t _last_update;          // millis() time of last update
 };
 
+#include "OpticalFlow_backend.h"
+#include "AP_OpticalFlow_HIL.h"
+#include "AP_OpticalFlow_PX4.h"
+
 #endif

libraries/AP_OpticalFlow/OpticalFlow_backend.cpp

@@ -0,0 +1,26 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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 "OpticalFlow.h"
+
+extern const AP_HAL::HAL& hal;
+
+// update the frontend
+void OpticalFlow_backend::_update_frontend(const struct OpticalFlow::OpticalFlow_state &state)
+{
+    frontend._state = state;
+    frontend._last_update_ms = hal.scheduler->millis();
+}

libraries/AP_OpticalFlow/OpticalFlow_backend.h

@@ -0,0 +1,50 @@
+#ifndef __OpticalFlow_backend_H__
+#define __OpticalFlow_backend_H__
+/*
+   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/>.
+ */
+
+/*
+  OpticalFlow backend class for ArduPilot
+ */
+
+#include "OpticalFlow.h"
+
+class OpticalFlow_backend
+{
+    friend class OpticalFlow;
+
+public:
+    // constructor
+    OpticalFlow_backend(OpticalFlow &_frontend) : frontend(_frontend) {}
+
+    // init - initialise sensor
+    virtual void init() = 0;
+
+    // read latest values from sensor and fill in x,y and totals.
+    virtual void update() = 0;
+
+protected:
+    // access to frontend
+    OpticalFlow &frontend;
+
+    // update the frontend
+    void _update_frontend(const struct OpticalFlow::OpticalFlow_state &state);
+
+    // get the flow scaling parameters
+    Vector2f _flowScaler(void) const { return Vector2f(frontend._flowScalerX, frontend._flowScalerY); }
+};
+
+#endif // __OpticalFlow_backend_H__
+