Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: add support for OpticalFlow to MinnowBoardMax

Browse Source 
This commit adds support for OpticalFlow to MinnowBoardMax trying to
leave the OpticalFlow implementation as generic as it already is.

We had to add some format conversion and software crop to the cameras that
do not have this features.
This commit is contained in:
Ricardo de Almeida Gonzaga 2015-05-11 00:16:01 +00:00 committed by Lucas De Marchi
parent 8aded390e7
commit 7adbccac9a
8 changed files with 160 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -250,6 +250,22 @@
#define HAL_BARO_MS5611_I2C_BUS 1
#define HAL_BARO_MS5611_I2C_ADDR 0x77
#define HAL_COMPASS_DEFAULT HAL_COMPASS_HMC5843_MPU6000
#define HAL_OPTFLOW_ONBOARD_VDEV_PATH "/dev/video0"
#define HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH 320
#define HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT 240
#define HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH 64
#define HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT 64
#define HAL_OPTFLOW_ONBOARD_CROP_WIDTH 240
#define HAL_OPTFLOW_ONBOARD_CROP_HEIGHT 240
#define HAL_OPTFLOW_ONBOARD_NBUFS 8;
#define HAL_FLOW_PX4_MAX_FLOW_PIXEL 4
#define HAL_FLOW_PX4_BOTTOM_FLOW_FEATURE_THRESHOLD 30
#define HAL_FLOW_PX4_BOTTOM_FLOW_VALUE_THRESHOLD 5000
#define HAL_PARAM_DEFAULTS_PATH "/etc/arducopter/minlure.parm"
/* ELP-USBFHD01M-L21
* focal length 2.1 mm, pixel size 3 um
* 240x240 crop rescaled to 64x64 */
#define HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX (3.0 / (2.1 * 2.0 * 240 / 64))
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"

3
libraries/AP_HAL_Linux/Flow_PX4.cpp
View File

@ -37,7 +37,8 @@
*
****************************************************************************/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
#include "Flow_PX4.h"
#include <math.h>

3
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -175,7 +175,8 @@ static Empty::RCOutput rcoutDriver;
static Scheduler schedulerInstance;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
static OpticalFlow_Onboard opticalFlow;
#else
static Empty::OpticalFlow opticalFlow;

151
libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp
View File

@ -14,9 +14,11 @@
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
#include "OpticalFlow_Onboard.h"
#include <vector>
#include <stdio.h>
#include <pthread.h>
#include <sys/types.h>
@ -52,8 +54,26 @@ void OpticalFlow_Onboard::init(AP_HAL::OpticalFlow::Gyro_Cb get_gyro)
}
_get_gyro = get_gyro;
_videoin = new VideoIn;
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 to YxY, removing the lateral edges */
left = (HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH -
HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT) / 2;
if (device_path == NULL ||
!_videoin->open_device(device_path, memtype)) {
AP_HAL::panic("OpticalFlow_Onboard: couldn't open "
"video device");
}
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
_pwm = new PWM_Sysfs_Bebop(BEBOP_CAMV_PWM);
_pwm->set_freq(BEBOP_CAMV_PWM_FREQ);
_pwm->enable(true);
@ -67,37 +87,54 @@ void OpticalFlow_Onboard::init(AP_HAL::OpticalFlow::Gyro_Cb get_gyro)
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 to YxY, removing the lateral edges */
left = (HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH -
HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT) / 2;
_format = V4L2_PIX_FMT_NV12;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
std::vector<uint32_t> pixel_formats;
if (device_path == NULL ||
!_videoin->open_device(device_path, memtype)) {
AP_HAL::panic("OpticalFlow_Onboard: couldn't open "
"video device");
_videoin->get_pixel_formats(&pixel_formats);
for (uint32_t px_fmt : pixel_formats) {
if (px_fmt == V4L2_PIX_FMT_NV12 || px_fmt == V4L2_PIX_FMT_GREY) {
_format = px_fmt;
break;
}
if (!_videoin->set_crop(left, top, crop_width, crop_height)) {
AP_HAL::panic("OpticalFlow_Onboard: couldn't set video crop");
/* if V4L2_PIX_FMT_YUYV format is found we still iterate through
* the vector since the other formats need no conversions. */
if (px_fmt == V4L2_PIX_FMT_YUYV) {
_format = px_fmt;
}
}
if (!_format) {
AP_HAL::panic("OpticalFlow_Onboard: no supported format available");
}
#endif
if (!_videoin->set_format(&_width, &_height, &_format, &_bytesperline,
&_sizeimage)) {
AP_HAL::panic("OpticalFlow_Onboard: couldn't set video format");
return;
}
if (_format != V4L2_PIX_FMT_NV12 && _format != V4L2_PIX_FMT_GREY) {
AP_HAL::panic("OpticalFlow_Onboard: planar or monochrome format needed");
if (_format != V4L2_PIX_FMT_NV12 && _format != V4L2_PIX_FMT_GREY &&
_format != V4L2_PIX_FMT_YUYV) {
AP_HAL::panic("OpticalFlow_Onboard: format not supported\n");
}
if (_videoin->set_crop(left, top, crop_width, crop_height)) {
_crop_by_software = false;
} else {
/* here we store the actual camera output width and height to use them
* later on to software crop each frame. */
_crop_by_software = true;
_crop_by_software_width = _width;
_crop_by_software_height = _height;
/* we set these values here in order to the calculations be correct
* (such as PX4 init) even though we crop each frame later on. */
_width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
_height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
_bytesperline = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
}
if (!_videoin->allocate_buffers(nbufs)) {
@ -175,13 +212,75 @@ void OpticalFlow_Onboard::_run_optflow()
Vector3f gyro_rate;
Vector2f flow_rate;
VideoIn::Frame video_frame;
uint32_t convert_buffer_size = 0, crop_buffer_size = 0;
uint32_t crop_left = 0, crop_top = 0;
uint8_t *convert_buffer = NULL, *crop_buffer = NULL;
uint8_t qual;
if (_format == V4L2_PIX_FMT_YUYV) {
if (_crop_by_software) {
convert_buffer_size = _crop_by_software_width * _crop_by_software_height;
} else {
convert_buffer_size = _width * _height;
}
convert_buffer = (uint8_t *)malloc(convert_buffer_size);
if (!convert_buffer) {
AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate conversion buffer\n");
}
}
if (_crop_by_software) {
crop_buffer_size = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH *
HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
crop_buffer = (uint8_t *)malloc(crop_buffer_size);
if (!crop_buffer) {
if (convert_buffer) {
free(convert_buffer);
}
AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate crop buffer\n");
}
crop_left = _crop_by_software_width / 2 -
HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH / 2;
crop_top = _crop_by_software_height / 2 -
HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT / 2;
}
while(true) {
/* wait for next frame to come */
if (!_videoin->get_frame(video_frame)) {
if (convert_buffer) {
free(convert_buffer);
}
if (crop_buffer) {
free(crop_buffer);
}
AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame\n");
}
if (_format == V4L2_PIX_FMT_YUYV) {
VideoIn::yuyv_to_grey((uint8_t *)video_frame.data,
convert_buffer_size * 2, convert_buffer);
memset(video_frame.data, 0, convert_buffer_size * 2);
memcpy(video_frame.data, convert_buffer, convert_buffer_size);
}
if (_crop_by_software) {
VideoIn::crop_8bpp((uint8_t *)video_frame.data, crop_buffer,
_crop_by_software_width,
crop_left, HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH,
crop_top, HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT);
memset(video_frame.data, 0, _crop_by_software_width * _crop_by_software_height);
memcpy(video_frame.data, crop_buffer, crop_buffer_size);
}
/* if it is at least the second frame we receive
* since we have to compare 2 frames */
if (_last_video_frame.data == NULL) {
@ -243,5 +342,13 @@ void OpticalFlow_Onboard::_run_optflow()
_videoin->put_frame(_last_video_frame);
_last_video_frame = video_frame;
}
if (convert_buffer) {
free(convert_buffer);
}
if (crop_buffer) {
free(crop_buffer);
}
}
#endif

3
libraries/AP_HAL_Linux/OpticalFlow_Onboard.h
View File

@ -41,6 +41,9 @@ private:
pthread_mutex_t _mutex;
bool _initialized;
bool _data_available;
bool _crop_by_software;
uint32_t _crop_by_software_width;
uint32_t _crop_by_software_height;
uint32_t _width;
uint32_t _height;
uint32_t _format;

3
libraries/AP_HAL_Linux/VideoIn.cpp
View File

@ -18,7 +18,8 @@
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
#include "VideoIn.h"
#include <errno.h>

3
libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp
View File

@ -19,7 +19,8 @@
#include "OpticalFlow.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX &&\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
(CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE)
//#define FLOWONBOARD_DEBUG 1
#define OPTICALFLOW_ONBOARD_ID 1
extern const AP_HAL::HAL& hal;

8
libraries/AP_OpticalFlow/OpticalFlow.cpp
View File

@ -46,11 +46,11 @@ OpticalFlow::OpticalFlow(AP_AHRS_NavEKF& ahrs) :
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 &&\
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
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
backend(new AP_OpticalFlow_Onboard(*this, ahrs)),
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
backend(new AP_OpticalFlow_Linux(*this)),
#else
backend(NULL),
#endif