OpticalFlow_Onboard: add gyro integration support

Integrate the gyro values pushed by the inertial sensor backend using
bias values sent by EKF.
Use the unblocking RingBuffer to avoid locking the callers.

libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp

@@ -25,14 +25,17 @@
 #include <stdio.h>
 #include <sys/stat.h>
 #include <sys/types.h>
+#include <time.h>
 #include <unistd.h>
 #include <vector>
 
 #include "CameraSensor_Mt9v117.h"
 #include "GPIO.h"
 #include "PWM_Sysfs.h"
+#include "AP_HAL/utility/RingBuffer.h"
 
 #define OPTICAL_FLOW_ONBOARD_RTPRIO 11
+static const unsigned int OPTICAL_FLOW_GYRO_BUFFER_LEN = 400;
 
 extern const AP_HAL::HAL& hal;
 
@@ -188,6 +191,8 @@ void OpticalFlow_Onboard::init(AP_HAL::OpticalFlow::Gyro_Cb get_gyro)
         AP_HAL::panic("OpticalFlow_Onboard: failed to create thread");
     }
 
+    _gyro_ring_buffer = new ObjectBuffer<GyroSample>(OPTICAL_FLOW_GYRO_BUFFER_LEN);
+
     _initialized = true;
 }
 
@@ -218,6 +223,42 @@ end:
     return ret;
 }
 
+void OpticalFlow_Onboard::push_gyro(float gyro_x, float gyro_y, float dt)
+{
+    GyroSample sample;
+    struct timespec ts;
+
+    if (!_gyro_ring_buffer) {
+        return;
+    }
+
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    _integrated_gyro.x += (gyro_x - _gyro_bias.x) * dt;
+    _integrated_gyro.y += (gyro_y - _gyro_bias.y) * dt;
+    sample.gyro = _integrated_gyro;
+    sample.time_us = 1.0e6 * (ts.tv_sec + (ts.tv_nsec*1.0e-9));
+
+    _gyro_ring_buffer->push(sample);
+}
+
+void OpticalFlow_Onboard::_get_integrated_gyros(uint64_t timestamp, GyroSample &gyro)
+{
+    GyroSample integrated_gyro_at_time = {};
+    unsigned int retries = 0;
+
+    // pop all samples prior to frame time
+    while (_gyro_ring_buffer->pop(integrated_gyro_at_time) &&
+            integrated_gyro_at_time.time_us < timestamp &&
+            retries++ < OPTICAL_FLOW_GYRO_BUFFER_LEN);
+    gyro = integrated_gyro_at_time;
+}
+
+void OpticalFlow_Onboard::push_gyro_bias(float gyro_bias_x, float gyro_bias_y)
+{
+    _gyro_bias.x = gyro_bias_x;
+    _gyro_bias.y = gyro_bias_y;
+}
+
 void *OpticalFlow_Onboard::_read_thread(void *arg)
 {
     OpticalFlow_Onboard *optflow_onboard = (OpticalFlow_Onboard *) arg;
@@ -228,8 +269,7 @@ void *OpticalFlow_Onboard::_read_thread(void *arg)
 
 void OpticalFlow_Onboard::_run_optflow()
 {
-    float rate_x, rate_y, rate_z;
-    Vector3f gyro_rate;
+    GyroSample gyro_sample;
     Vector2f flow_rate;
     VideoIn::Frame video_frame;
     uint32_t convert_buffer_size = 0, output_buffer_size = 0;
@@ -336,11 +376,8 @@ void OpticalFlow_Onboard::_run_optflow()
             continue;
         }
 
-        /* read gyro data from EKF via the opticalflow driver */
-        _get_gyro(rate_x, rate_y, rate_z);
-        gyro_rate.x = rate_x;
-        gyro_rate.y = rate_y;
-        gyro_rate.z = rate_z;
+        /* read the integrated gyro data */
+        _get_integrated_gyros(video_frame.timestamp, gyro_sample);
 
 #ifdef OPTICALFLOW_ONBOARD_RECORD_VIDEO
         int fd = open(OPTICALFLOW_ONBOARD_VIDEO_FILE, O_CLOEXEC | O_CREAT | O_WRONLY
@@ -378,20 +415,21 @@ void OpticalFlow_Onboard::_run_optflow()
                                   HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX;
         _integration_timespan += video_frame.timestamp -
                                  _last_video_frame.timestamp;
-        _gyro_x_integral       += (gyro_rate.x + _last_gyro_rate.x) / 2.0f *
-                                  (video_frame.timestamp -
-                                  _last_video_frame.timestamp);
-        _gyro_y_integral       += (gyro_rate.y + _last_gyro_rate.y) / 2.0f *
-                                  (video_frame.timestamp -
-                                  _last_video_frame.timestamp);
+        _gyro_x_integral       += (gyro_sample.gyro.x - _last_gyro_rate.x) *
+                                  (video_frame.timestamp - _last_video_frame.timestamp) /
+                                  (gyro_sample.time_us - _last_integration_time);
+        _gyro_y_integral       += (gyro_sample.gyro.y - _last_gyro_rate.y) /
+                                  (gyro_sample.time_us - _last_integration_time) *
+                                  (video_frame.timestamp - _last_video_frame.timestamp);
         _surface_quality = qual;
         _data_available = true;
         pthread_mutex_unlock(&_mutex);
 
         /* give the last frame back to the video input driver */
         _videoin->put_frame(_last_video_frame);
+        _last_integration_time = gyro_sample.time_us;
         _last_video_frame = video_frame;
-        _last_gyro_rate = gyro_rate;
+        _last_gyro_rate = gyro_sample.gyro;
     }
 
     if (convert_buffer) {

libraries/AP_HAL_Linux/OpticalFlow_Onboard.h

@@ -24,17 +24,27 @@
 #include "Flow_PX4.h"
 #include "PWM_Sysfs.h"
 #include "VideoIn.h"
+#include "AP_HAL/utility/RingBuffer.h"
 
 namespace Linux {
 
+class GyroSample {
+public:
+    Vector2f gyro;
+    uint64_t time_us;
+};
+
 class OpticalFlow_Onboard : public AP_HAL::OpticalFlow {
 public:
     void init(AP_HAL::OpticalFlow::Gyro_Cb);
     bool read(AP_HAL::OpticalFlow::Data_Frame& frame);
+    void push_gyro(float gyro_x, float gyro_y, float dt);
+    void push_gyro_bias(float gyro_bias_x, float gyro_bias_y);
 
 private:
     void _run_optflow();
     static void *_read_thread(void *arg);
+    void _get_integrated_gyros(uint64_t timestamp, GyroSample &gyro);
     VideoIn* _videoin;
     VideoIn::Frame _last_video_frame;
     PWM_Sysfs_Base* _pwm;
@@ -57,10 +67,14 @@ private:
     float _pixel_flow_y_integral;
     float _gyro_x_integral;
     float _gyro_y_integral;
-    uint32_t _integration_timespan;
+    uint64_t _integration_timespan;
     uint8_t _surface_quality;
     AP_HAL::OpticalFlow::Gyro_Cb _get_gyro;
-    Vector3f _last_gyro_rate;
+    Vector2f _last_gyro_rate;
+    Vector2f _gyro_bias;
+    Vector2f _integrated_gyro;
+    uint64_t _last_integration_time;
+    ObjectBuffer<GyroSample> *_gyro_ring_buffer;
 };
 
 }