APM:Libraries
OpticalFlow_Onboard.cpp
Go to the documentation of this file.
1 /*
2  This program is free software: you can redistribute it and/or modify
3  it under the terms of the GNU General Public License as published by
4  the Free Software Foundation, either version 3 of the License, or
5  (at your option) any later version.
6 
7  This program is distributed in the hope that it will be useful,
8  but WITHOUT ANY WARRANTY; without even the implied warranty of
9  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  GNU General Public License for more details.
11 
12  You should have received a copy of the GNU General Public License
13  along with this program. If not, see <http://www.gnu.org/licenses/>.
14  */
15 
16 #include <AP_HAL/AP_HAL.h>
17 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
18  CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
19 #include "OpticalFlow_Onboard.h"
20 
21 #include <fcntl.h>
22 #include <linux/v4l2-mediabus.h>
23 #include <pthread.h>
24 #include <stdio.h>
25 #include <sys/stat.h>
26 #include <sys/types.h>
27 #include <time.h>
28 #include <unistd.h>
29 #include <vector>
30 
31 #include "CameraSensor_Mt9v117.h"
32 #include "GPIO.h"
33 #include "PWM_Sysfs.h"
35 
36 #define OPTICAL_FLOW_ONBOARD_RTPRIO 11
37 static const unsigned int OPTICAL_FLOW_GYRO_BUFFER_LEN = 400;
38 
39 extern const AP_HAL::HAL& hal;
40 
41 using namespace Linux;
42 
44 {
45  uint32_t top, left;
46  uint32_t crop_width, crop_height;
47  uint32_t memtype = V4L2_MEMORY_MMAP;
48  unsigned int nbufs = 0;
49  int ret;
50  pthread_attr_t attr;
51  struct sched_param param = {
52  .sched_priority = OPTICAL_FLOW_ONBOARD_RTPRIO
53  };
54 
55  if (_initialized) {
56  return;
57  }
58 
59  _videoin = new VideoIn;
60  const char* device_path = HAL_OPTFLOW_ONBOARD_VDEV_PATH;
61  memtype = V4L2_MEMORY_MMAP;
62  nbufs = HAL_OPTFLOW_ONBOARD_NBUFS;
63  _width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
64  _height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
65  crop_width = HAL_OPTFLOW_ONBOARD_CROP_WIDTH;
66  crop_height = HAL_OPTFLOW_ONBOARD_CROP_HEIGHT;
67  top = 0;
68  /* make the image square by cropping to YxY, removing the lateral edges */
69  left = (HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH -
70  HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT) / 2;
71 
72  if (device_path == nullptr ||
73  !_videoin->open_device(device_path, memtype)) {
74  AP_HAL::panic("OpticalFlow_Onboard: couldn't open "
75  "video device");
76  }
77 
78 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
79  _pwm = new PWM_Sysfs_Bebop(BEBOP_CAMV_PWM);
80  _pwm->init();
81  _pwm->set_freq(BEBOP_CAMV_PWM_FREQ);
82  _pwm->enable(true);
83 
84  _camerasensor = new CameraSensor_Mt9v117(HAL_OPTFLOW_ONBOARD_SUBDEV_PATH,
85  hal.i2c_mgr->get_device(0, 0x5D),
88  BEBOP_CAMV_PWM_FREQ);
89  if (!_camerasensor->set_format(HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH,
90  HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT,
91  V4L2_MBUS_FMT_UYVY8_2X8)) {
92  AP_HAL::panic("OpticalFlow_Onboard: couldn't set subdev fmt\n");
93  }
94  _format = V4L2_PIX_FMT_NV12;
95 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
96  std::vector<uint32_t> pixel_formats;
97 
98  _videoin->get_pixel_formats(&pixel_formats);
99 
100  for (uint32_t px_fmt : pixel_formats) {
101  if (px_fmt == V4L2_PIX_FMT_NV12 || px_fmt == V4L2_PIX_FMT_GREY) {
102  _format = px_fmt;
103  break;
104  }
105 
106  /* if V4L2_PIX_FMT_YUYV format is found we still iterate through
107  * the vector since the other formats need no conversions. */
108  if (px_fmt == V4L2_PIX_FMT_YUYV) {
109  _format = px_fmt;
110  }
111  }
112 #endif
113 
115  &_sizeimage)) {
116  AP_HAL::panic("OpticalFlow_Onboard: couldn't set video format");
117  }
118 
119  if (_format != V4L2_PIX_FMT_NV12 && _format != V4L2_PIX_FMT_GREY &&
120  _format != V4L2_PIX_FMT_YUYV) {
121  AP_HAL::panic("OpticalFlow_Onboard: format not supported\n");
122  }
123 
124  if (_width == HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH &&
125  _height == HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT) {
126  _shrink_by_software = false;
127  } else {
128  /* here we store the actual camera output width and height to use
129  * them later on to software shrink each frame. */
130  _shrink_by_software = true;
133 
134  /* we set these values here in order to the calculations be correct
135  * (such as PX4 init) even though we shrink each frame later on. */
136  _width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
137  _height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
138  _bytesperline = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
139  }
140 
141  if (_videoin->set_crop(left, top, crop_width, crop_height)) {
142  _crop_by_software = false;
143  } else {
144  _crop_by_software = true;
145 
146  if (!_shrink_by_software) {
147  /* here we store the actual camera output width and height to use
148  * them later on to software crop each frame. */
151 
152  /* we set these values here in order to the calculations be correct
153  * (such as PX4 init) even though we crop each frame later on. */
154  _width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
155  _height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
156  _bytesperline = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
157  }
158  }
159 
160  if (!_videoin->allocate_buffers(nbufs)) {
161  AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate video buffers");
162  }
163 
165 
166  /* Use px4 algorithm for optical flow */
168  HAL_FLOW_PX4_MAX_FLOW_PIXEL,
169  HAL_FLOW_PX4_BOTTOM_FLOW_FEATURE_THRESHOLD,
170  HAL_FLOW_PX4_BOTTOM_FLOW_VALUE_THRESHOLD);
171 
172  /* Create the thread that will be waiting for frames
173  * Initialize thread and mutex */
174  ret = pthread_mutex_init(&_mutex, nullptr);
175  if (ret != 0) {
176  AP_HAL::panic("OpticalFlow_Onboard: failed to init mutex");
177  }
178 
179  ret = pthread_attr_init(&attr);
180  if (ret != 0) {
181  AP_HAL::panic("OpticalFlow_Onboard: failed to init attr");
182  }
183  pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
184  pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
185  pthread_attr_setschedparam(&attr, &param);
186  ret = pthread_create(&_thread, &attr, _read_thread, this);
187  if (ret != 0) {
188  AP_HAL::panic("OpticalFlow_Onboard: failed to create thread");
189  }
190 
192 
193  _initialized = true;
194 }
195 
197 {
198  bool ret;
199 
200  pthread_mutex_lock(&_mutex);
201  if (!_data_available) {
202  ret = false;
203  goto end;
204  }
210  frame.quality = _surface_quality;
214  _gyro_x_integral = 0;
215  _gyro_y_integral = 0;
216  _data_available = false;
217  ret = true;
218 end:
219  pthread_mutex_unlock(&_mutex);
220  return ret;
221 }
222 
223 void OpticalFlow_Onboard::push_gyro(float gyro_x, float gyro_y, float dt)
224 {
225  GyroSample sample;
226  struct timespec ts;
227 
228  if (!_gyro_ring_buffer) {
229  return;
230  }
231 
232  clock_gettime(CLOCK_MONOTONIC, &ts);
233  _integrated_gyro.x += (gyro_x - _gyro_bias.x) * dt;
234  _integrated_gyro.y += (gyro_y - _gyro_bias.y) * dt;
235  sample.gyro = _integrated_gyro;
236  sample.time_us = 1.0e6 * (ts.tv_sec + (ts.tv_nsec*1.0e-9));
237 
238  _gyro_ring_buffer->push(sample);
239 }
240 
242 {
243  GyroSample integrated_gyro_at_time = {};
244  unsigned int retries = 0;
245 
246  // pop all samples prior to frame time
247  while (_gyro_ring_buffer->pop(integrated_gyro_at_time) &&
248  integrated_gyro_at_time.time_us < timestamp &&
249  retries++ < OPTICAL_FLOW_GYRO_BUFFER_LEN);
250  gyro = integrated_gyro_at_time;
251 }
252 
253 void OpticalFlow_Onboard::push_gyro_bias(float gyro_bias_x, float gyro_bias_y)
254 {
255  _gyro_bias.x = gyro_bias_x;
256  _gyro_bias.y = gyro_bias_y;
257 }
258 
260 {
261  OpticalFlow_Onboard *optflow_onboard = (OpticalFlow_Onboard *) arg;
262 
263  optflow_onboard->_run_optflow();
264  return nullptr;
265 }
266 
268 {
270  Vector2f flow_rate;
271  VideoIn::Frame video_frame;
272  uint32_t convert_buffer_size = 0, output_buffer_size = 0;
273  uint32_t crop_left = 0, crop_top = 0;
274  uint32_t shrink_scale = 0, shrink_width = 0, shrink_height = 0;
275  uint32_t shrink_width_offset = 0, shrink_height_offset = 0;
276  uint8_t *convert_buffer = nullptr, *output_buffer = nullptr;
277  uint8_t qual;
278 
279  if (_format == V4L2_PIX_FMT_YUYV) {
281  convert_buffer_size = _camera_output_width * _camera_output_height;
282  } else {
283  convert_buffer_size = _width * _height;
284  }
285 
286  convert_buffer = (uint8_t *)calloc(1, convert_buffer_size);
287  if (!convert_buffer) {
288  AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate conversion buffer\n");
289  }
290  }
291 
293  output_buffer_size = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH *
294  HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
295 
296  output_buffer = (uint8_t *)calloc(1, output_buffer_size);
297  if (!output_buffer) {
298  if (convert_buffer) {
299  free(convert_buffer);
300  }
301 
302  AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate crop buffer\n");
303  }
304  }
305 
306  if (_shrink_by_software) {
308  shrink_scale = (uint32_t) _camera_output_height /
309  HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
310  } else {
311  shrink_scale = (uint32_t) _camera_output_width /
312  HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
313  }
314 
315  shrink_width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH * shrink_scale;
316  shrink_height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT * shrink_scale;
317 
318  shrink_width_offset = (_camera_output_width - shrink_width) / 2;
319  shrink_height_offset = (_camera_output_height - shrink_height) / 2;
320  } else if (_crop_by_software) {
321  crop_left = _camera_output_width / 2 -
322  HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH / 2;
323  crop_top = _camera_output_height / 2 -
324  HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT / 2;
325  }
326 
327  while(true) {
328  /* wait for next frame to come */
329  if (!_videoin->get_frame(video_frame)) {
330  if (convert_buffer) {
331  free(convert_buffer);
332  }
333 
334  if (output_buffer) {
335  free(output_buffer);
336  }
337 
338  AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame\n");
339  }
340 
341  if (_format == V4L2_PIX_FMT_YUYV) {
342  VideoIn::yuyv_to_grey((uint8_t *)video_frame.data,
343  convert_buffer_size * 2, convert_buffer);
344 
345  memset(video_frame.data, 0, convert_buffer_size * 2);
346  memcpy(video_frame.data, convert_buffer, convert_buffer_size);
347  }
348 
349  if (_shrink_by_software) {
350  /* shrink_8bpp() will shrink a selected area using the offsets,
351  * therefore, we don't need the crop. */
352  VideoIn::shrink_8bpp((uint8_t *)video_frame.data, output_buffer,
354  shrink_width_offset, shrink_width,
355  shrink_height_offset, shrink_height,
356  shrink_scale, shrink_scale);
357  memset(video_frame.data, 0, _camera_output_width * _camera_output_height);
358  memcpy(video_frame.data, output_buffer, output_buffer_size);
359  } else if (_crop_by_software) {
360  VideoIn::crop_8bpp((uint8_t *)video_frame.data, output_buffer,
362  crop_left, HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH,
363  crop_top, HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT);
364 
365  memset(video_frame.data, 0, _camera_output_width * _camera_output_height);
366  memcpy(video_frame.data, output_buffer, output_buffer_size);
367  }
368 
369  /* if it is at least the second frame we receive
370  * since we have to compare 2 frames */
371  if (_last_video_frame.data == nullptr) {
372  _last_video_frame = video_frame;
373  continue;
374  }
375 
376  /* read the integrated gyro data */
377  _get_integrated_gyros(video_frame.timestamp, gyro_sample);
378 
379 #ifdef OPTICALFLOW_ONBOARD_RECORD_VIDEO
380  int fd = open(OPTICALFLOW_ONBOARD_VIDEO_FILE, O_CLOEXEC | O_CREAT | O_WRONLY
381  | O_APPEND, S_IRUSR | S_IWUSR | S_IRGRP |
382  S_IWGRP | S_IROTH | S_IWOTH);
383  if (fd != -1) {
384  write(fd, video_frame.data, _sizeimage);
385 #ifdef OPTICALFLOW_ONBOARD_RECORD_METADATAS
386  struct PACKED {
387  uint32_t timestamp;
388  float x;
389  float y;
390  float z;
391  } metas = { video_frame.timestamp, rate_x, rate_y, rate_z};
392  write(fd, &metas, sizeof(metas));
393 #endif
394  close(fd);
395  }
396 #endif
397 
398  /* compute gyro data and video frames
399  * get flow rate to send it to the opticalflow driver
400  */
401  qual = _flow->compute_flow((uint8_t*)_last_video_frame.data,
402  (uint8_t *)video_frame.data,
403  video_frame.timestamp -
405  &flow_rate.x, &flow_rate.y);
406 
407  /* fill data frame for upper layers */
408  pthread_mutex_lock(&_mutex);
409  _pixel_flow_x_integral += flow_rate.x /
410  HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX;
411  _pixel_flow_y_integral += flow_rate.y /
412  HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX;
413  _integration_timespan += video_frame.timestamp -
415  _gyro_x_integral += (gyro_sample.gyro.x - _last_gyro_rate.x) *
416  (video_frame.timestamp - _last_video_frame.timestamp) /
417  (gyro_sample.time_us - _last_integration_time);
418  _gyro_y_integral += (gyro_sample.gyro.y - _last_gyro_rate.y) /
419  (gyro_sample.time_us - _last_integration_time) *
420  (video_frame.timestamp - _last_video_frame.timestamp);
421  _surface_quality = qual;
422  _data_available = true;
423  pthread_mutex_unlock(&_mutex);
424 
425  /* give the last frame back to the video input driver */
427  _last_integration_time = gyro_sample.time_us;
428  _last_video_frame = video_frame;
429  _last_gyro_rate = gyro_sample.gyro;
430  }
431 
432  if (convert_buffer) {
433  free(convert_buffer);
434  }
435 
436  if (output_buffer) {
437  free(output_buffer);
438  }
439 }
440 #endif
uint32_t timestamp
Definition: VideoIn.h:36
bool read(AP_HAL::OpticalFlow::Data_Frame &frame)
void clock_gettime(uint32_t a1, void *a2)
Definition: syscalls.c:201
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
Definition: posix.c:885
static void shrink_8bpp(uint8_t *buffer, uint8_t *new_buffer, uint32_t width, uint32_t height, uint32_t left, uint32_t selection_width, uint32_t top, uint32_t selection_height, uint32_t fx, uint32_t fy)
Definition: VideoIn.cpp:253
static void * _read_thread(void *arg)
bool set_crop(uint32_t left, uint32_t top, uint32_t width, uint32_t height)
Definition: VideoIn.cpp:219
#define OPTICAL_FLOW_ONBOARD_RTPRIO
ssize_t write(int fd, const void *buf, size_t count)
POSIX Write count bytes from *buf to fileno fd.
Definition: posix.c:1169
void _get_integrated_gyros(uint64_t timestamp, GyroSample &gyro)
ObjectBuffer< GyroSample > * _gyro_ring_buffer
static void yuyv_to_grey(uint8_t *buffer, uint32_t buffer_size, uint8_t *new_buffer)
Definition: VideoIn.cpp:310
void put_frame(Frame &frame)
Definition: VideoIn.cpp:58
bool set_format(uint32_t width, uint32_t height, uint32_t format)
void * calloc(size_t nmemb, size_t size)
Definition: malloc.c:76
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
T y
Definition: vector2.h:37
bool open_device(const char *device_path, uint32_t memtype)
Definition: VideoIn.cpp:63
#define x(i)
void get_pixel_formats(std::vector< uint32_t > *formats)
Definition: VideoIn.cpp:163
bool get_frame(Frame &frame)
Definition: VideoIn.cpp:45
int close(int fileno)
POSIX Close a file with fileno handel.
Definition: posix.c:675
bool allocate_buffers(uint32_t nbufs)
Definition: VideoIn.cpp:97
void push_gyro_bias(float gyro_bias_x, float gyro_bias_y)
void prepare_capture()
Definition: VideoIn.cpp:243
AP_HAL::I2CDeviceManager * i2c_mgr
Definition: HAL.h:106
void free(void *ptr)
Definition: malloc.c:81
static void crop_8bpp(uint8_t *buffer, uint8_t *new_buffer, uint32_t width, uint32_t left, uint32_t crop_width, uint32_t top, uint32_t crop_height)
Definition: VideoIn.cpp:292
T x
Definition: vector2.h:37
void push_gyro(float gyro_x, float gyro_y, float dt)
bool set_format(uint32_t *width, uint32_t *height, uint32_t *format, uint32_t *bytesperline, uint32_t *sizeimage)
Definition: VideoIn.cpp:177
#define PACKED
Definition: AP_Common.h:28
void enable(bool value)
Definition: PWM_Sysfs.cpp:76
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
virtual OwnPtr< AP_HAL::I2CDevice > get_device(uint8_t bus, uint8_t address, uint32_t bus_clock=400000, bool use_smbus=false, uint32_t timeout_ms=4)=0
static const unsigned int OPTICAL_FLOW_GYRO_BUFFER_LEN
uint8_t compute_flow(uint8_t *image1, uint8_t *image2, uint32_t delta_time, float *pixel_flow_x, float *pixel_flow_y)
Definition: Flow_PX4.cpp:234
void set_freq(uint32_t freq)
Definition: PWM_Sysfs.cpp:117