AP_OpticalFlow : Update PX4Flow interface

Interface now uses the integral of flow and body rates

libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp

@@ -63,10 +63,10 @@ void AP_OpticalFlow_PX4::update(void)
     while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && report.timestamp != _last_timestamp) {
         _device_id = report.sensor_id;
         _surface_quality = report.quality;
-        _raw.x = report.flow_raw_x;
-        _raw.y = report.flow_raw_y;
-        _velocity.x = report.flow_comp_x_m;
-        _velocity.y = report.flow_comp_y_m;
+        _flowRate.x = report.pixel_flow_x_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the X sensor axis
+        _flowRate.y = report.pixel_flow_y_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the Y sensor axis
+        _bodyRate.x = report.gyro_x_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the X sensor axis
+        _bodyRate.y = report.gyro_y_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the Y sensor axis
         _last_timestamp = report.timestamp;
         _last_update = hal.scheduler->millis();
     }

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -18,7 +18,6 @@ OpticalFlow::OpticalFlow(const AP_AHRS &ahrs) :
     _ahrs(ahrs),
     _device_id(0),
     _surface_quality(0),
-    _ground_distance_m(0.0f),
     _last_update(0)
 {
     AP_Param::setup_object_defaults(this, var_info);

libraries/AP_OpticalFlow/OpticalFlow.h

@@ -54,17 +54,14 @@ public:
     uint8_t quality() const { return _surface_quality; }
 
     // raw - returns the raw movement from the sensor
-    const Vector2i& raw() const { return _raw; }
+    const Vector2f& flowRate() const { return _flowRate; }
 
     // velocity - returns the velocity in m/s
-    const Vector2f& velocity() const { return _velocity; }
+    const Vector2f& bodyRate() const { return _bodyRate; }
 
     // device_id - returns device id
     uint8_t device_id() const { return _device_id; }
 
-    // return ground distance in meters (if available)
-    float ground_distance_m() const { return _ground_distance_m; }
-
     // last_update() - returns system time of last sensor update
     uint32_t last_update() const { return _last_update; }
 
@@ -85,10 +82,9 @@ protected:
 
     // internal variables
     uint8_t _device_id;             // device id
-    uint8_t  _surface_quality;      // image quality (below 15 you can't trust the dx,dy values returned)
-    Vector2i _raw;                  // raw x,y values from sensor
-    Vector2f _velocity;             // x, y velocity in m/s
-    float    _ground_distance_m;    // ground distance in m
+    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.
     uint32_t _last_update;          // millis() time of last update
 };