AP_HAL_Linux: Add Onboard Optical Flow interface

Implementation of AP_HAL::OpticalFlow for an embedded camera sensor

There is the possibility to record video and also the gyro datas in order
to process the video off-board and debug possible issues.

libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp

@@ -0,0 +1,263 @@
+/*
+    This class has been implemented based on
+    yavta -- Yet Another V4L2 Test Application written by:
+    Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+
+   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 "GPIO.h"
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
+#include "OpticalFlow_Onboard.h"
+#include "CameraSensor_Mt9v117.h"
+#include "PWM_Sysfs.h"
+#include <stdio.h>
+#include <pthread.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <linux/v4l2-mediabus.h>
+
+//#define OPTICALFLOW_ONBOARD_RECORD_VIDEO 1
+//#define OPTICALFLOW_ONBOARD_VIDEO_FILE "/data/ftp/internal_000/optflow.bin"
+//#define OPTICALFLOW_ONBOARD_RECORD_METADATAS 1
+#define OPTICAL_FLOW_ONBOARD_RTPRIO 11
+
+extern const AP_HAL::HAL& hal;
+
+using namespace Linux;
+
+void OpticalFlow_Onboard::init(AP_HAL::OpticalFlow::Gyro_Cb get_gyro)
+{
+    uint32_t top, left;
+    uint32_t crop_width, crop_height;
+    uint32_t memtype = V4L2_MEMORY_MMAP;
+    unsigned int nbufs = 0;
+    int ret;
+    pthread_attr_t attr;
+    struct sched_param param = {
+        .sched_priority = OPTICAL_FLOW_ONBOARD_RTPRIO
+    };
+
+    if (_initialized) {
+        return;
+    }
+
+    _get_gyro = get_gyro;
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
+    _videoin = new VideoIn;
+    _pwm = new PWM_Sysfs_Bebop(BEBOP_CAMV_PWM);
+    _pwm->set_freq(BEBOP_CAMV_PWM_FREQ);
+    _pwm->enable(true);
+
+    _camerasensor = new CameraSensor_Mt9v117(HAL_OPTFLOW_ONBOARD_SUBDEV_PATH,
+                                             hal.i2c, 0x5D, MT9V117_QVGA,
+                                             BEBOP_GPIO_CAMV_NRST,
+                                             BEBOP_CAMV_PWM_FREQ);
+    if (!_camerasensor->set_format(HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH,
+                                   HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT,
+                                   V4L2_MBUS_FMT_UYVY8_2X8)) {
+        AP_HAL::panic("OpticalFlow_Onboard: couldn't set subdev fmt\n");
+    }
+    const char* device_path = HAL_OPTFLOW_ONBOARD_VDEV_PATH;
+    memtype = V4L2_MEMORY_MMAP;
+    nbufs = HAL_OPTFLOW_ONBOARD_NBUFS;
+    _width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
+    _height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
+    crop_width = HAL_OPTFLOW_ONBOARD_CROP_WIDTH;
+    crop_height = HAL_OPTFLOW_ONBOARD_CROP_HEIGHT;
+    top = 0;
+    /* make the image square by cropping in the center */
+    left = (HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH -
+            HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT) / 2;
+    _format = V4L2_PIX_FMT_NV12;
+#else
+    AP_HAL::panic("OpticalFlow_Onboard: unsupported board\n");
+#endif
+
+    if (device_path == NULL ||
+        !_videoin->open_device(device_path, memtype)) {
+        AP_HAL::panic("OpticalFlow_Onboard: couldn't open "
+                             "video device\n");
+        return;
+    }
+
+    if (!_videoin->set_format(&_width, &_height, &_format, &_bytesperline,
+                              &_sizeimage)) {
+        AP_HAL::panic("OpticalFlow_Onboard: couldn't set video format\n");
+        return;
+    }
+
+    if (_format != V4L2_PIX_FMT_NV12 && _format != V4L2_PIX_FMT_GREY) {
+        AP_HAL::panic("OpticalFlow_Onboard: planar or monochrome format"
+                      "needed\n");
+    }
+
+    if (!_videoin->set_crop(left, top, crop_width, crop_height)) {
+        AP_HAL::panic("OpticalFlow_Onboard: couldn't set video crop\n");
+        return;
+    }
+
+    if (!_videoin->allocate_buffers(nbufs)) {
+        AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate "
+                             "video buffers\n");
+        return;
+    }
+
+    _videoin->prepare_capture();
+
+    /* Use px4 algorithm for optical flow */
+    _flow = new Flow_PX4(_width, _bytesperline,
+                         HAL_FLOW_PX4_MAX_FLOW_PIXEL,
+                         HAL_FLOW_PX4_BOTTOM_FLOW_FEATURE_THRESHOLD,
+                         HAL_FLOW_PX4_BOTTOM_FLOW_VALUE_THRESHOLD);
+
+    /* Create the thread that will be waiting for frames
+     * Initialize thread and mutex */
+    ret = pthread_mutex_init(&_mutex, NULL);
+    if (ret != 0) {
+        perror("OpticalFlow_Onboard: failed to init mutex\n");
+        return;
+    }
+
+    ret = pthread_attr_init(&attr);
+    if (ret != 0) {
+        perror("OpticalFlow_Onboard: failed to init attr\n");
+        return;
+    }
+    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
+    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
+    pthread_attr_setschedparam(&attr, &param);
+    ret = pthread_create(&_thread, &attr, _read_thread, this);
+    if (ret != 0) {
+        perror("OpticalFlow_Onboard: failed to create thread\n");
+        return;
+    }
+    _initialized = true;
+}
+
+bool OpticalFlow_Onboard::read(AP_HAL::OpticalFlow::Data_Frame& frame)
+{
+    bool ret;
+
+    pthread_mutex_lock(&_mutex);
+    if (!_data_available) {
+        ret = false;
+        goto end;
+    }
+    frame.pixel_flow_x_integral = _pixel_flow_x_integral;
+    frame.pixel_flow_y_integral = _pixel_flow_y_integral;
+    frame.gyro_x_integral = _gyro_x_integral;
+    frame.gyro_y_integral = _gyro_y_integral;
+    frame.delta_time = _integration_timespan;
+    frame.quality = _surface_quality;
+    _integration_timespan = 0;
+    _pixel_flow_x_integral = 0;
+    _pixel_flow_y_integral = 0;
+    _gyro_x_integral = 0;
+    _gyro_y_integral = 0;
+    _data_available = false;
+    ret = true;
+end:
+    pthread_mutex_unlock(&_mutex);
+    return ret;
+}
+
+void* OpticalFlow_Onboard::_read_thread(void *arg)
+{
+    OpticalFlow_Onboard* optflow_onboard = (OpticalFlow_Onboard *) arg;
+
+    optflow_onboard->_run_optflow();
+    return NULL;
+}
+
+void OpticalFlow_Onboard::_run_optflow()
+{
+    float rate_x, rate_y, rate_z;
+    Vector3f gyro_rate;
+    Vector2f flow_rate;
+    VideoIn::Frame video_frame;
+    uint8_t qual;
+
+    while(true) {
+        /* wait for next frame to come */
+        if (!_videoin->get_frame(video_frame)) {
+            AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame\n");
+        }
+        /* if it is at least the second frame we receive
+         * since we have to compare 2 frames */
+        if (_last_video_frame.data == NULL) {
+            _last_video_frame = video_frame;
+            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;
+
+#ifdef OPTICALFLOW_ONBOARD_RECORD_VIDEO
+        int fd = open(OPTICALFLOW_ONBOARD_VIDEO_FILE, O_CREAT | O_WRONLY
+                | O_APPEND, S_IRUSR | S_IWUSR | S_IRGRP |
+                S_IWGRP | S_IROTH | S_IWOTH);
+	    if (fd != -1) {
+	        write(fd, video_frame.data, _sizeimage);
+#ifdef OPTICALFLOW_ONBOARD_RECORD_METADATAS
+            struct PACKED {
+                uint32_t timestamp;
+                float x;
+                float y;
+                float z;
+            } metas = { video_frame.timestamp, rate_x, rate_y, rate_z};
+            write(fd, &metas, sizeof(metas));
+#endif
+	        close(fd);
+        }
+#endif
+
+        /* compute gyro data and video frames
+         * get flow rate to send it to the opticalflow driver
+         */
+        qual = _flow->compute_flow((uint8_t*)_last_video_frame.data,
+                                   (uint8_t *)video_frame.data,
+                                   video_frame.timestamp -
+                                   _last_video_frame.timestamp,
+                                   &flow_rate.x, &flow_rate.y);
+
+        /* fill data frame for upper layers */
+        pthread_mutex_lock(&_mutex);
+        _pixel_flow_x_integral += flow_rate.x /
+                                  HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX;
+        _pixel_flow_y_integral += flow_rate.y /
+                                  HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX;
+        _integration_timespan += video_frame.timestamp -
+                                 _last_video_frame.timestamp;
+        _gyro_x_integral       += gyro_rate.x * (video_frame.timestamp -
+                                 _last_video_frame.timestamp);
+        _gyro_y_integral       += gyro_rate.y * (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_video_frame = video_frame;
+    }
+}
+#endif

libraries/AP_HAL_Linux/OpticalFlow_Onboard.h

@@ -0,0 +1,55 @@
+/*
+   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 __OPTICALFLOW_ONBOARD_H__
+#define __OPTICALFLOW_ONBOARD_H__
+
+#include "AP_HAL_Linux.h"
+#include "CameraSensor.h"
+#include "Flow_PX4.h"
+#include "VideoIn.h"
+#include <AP_Math/AP_Math.h>
+#include <linux/videodev2.h>
+
+class Linux::OpticalFlow_Onboard : public AP_HAL::OpticalFlow {
+public:
+    void init(AP_HAL::OpticalFlow::Gyro_Cb);
+    bool read(AP_HAL::OpticalFlow::Data_Frame& frame);
+
+private:
+    void _run_optflow();
+    static void *_read_thread(void *arg);
+    VideoIn* _videoin;
+    VideoIn::Frame _last_video_frame;
+    PWM_Sysfs_Base* _pwm;
+    CameraSensor* _camerasensor;
+    Flow_PX4* _flow;
+    pthread_t _thread;
+    pthread_mutex_t _mutex;
+    bool _initialized = false;
+    bool _data_available = false;
+    uint32_t _width = 0;
+    uint32_t _height = 0;
+    uint32_t _format = 0;
+    uint32_t _bytesperline = 0;
+    uint32_t _sizeimage = 0;
+    float _pixel_flow_x_integral = 0;
+    float _pixel_flow_y_integral = 0;
+    float _gyro_x_integral = 0;
+    float _gyro_y_integral = 0;
+    uint32_t _integration_timespan = 0;
+    uint8_t _surface_quality = 0;
+    AP_HAL::OpticalFlow::Gyro_Cb _get_gyro;
+};
+#endif