Paparazzi UAS  v6.0_unstable-53-gfe8bbd3-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
opticflow_module.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Hann Woei Ho
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "opticflow_module.h"
29 
30 #include <stdio.h>
31 #include <pthread.h>
32 #include "state.h"
33 #include "subsystems/abi.h"
34 
35 #include "lib/v4l/v4l2.h"
36 #include "lib/encoding/jpeg.h"
37 #include "lib/encoding/rtp.h"
38 #include "errno.h"
39 
40 #include "cv.h"
41 
42 /* ABI messages sender ID */
43 #ifndef OPTICFLOW_AGL_ID
44 #define OPTICFLOW_AGL_ID ABI_BROADCAST
45 #endif
46 PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
47 
48 #ifndef OPTICFLOW_FPS
49 #define OPTICFLOW_FPS 0
50 #endif
51 
52 #ifndef OPTICFLOW_FPS_CAMERA2
53 #define OPTICFLOW_FPS_CAMERA2 0
54 #endif
55 PRINT_CONFIG_VAR(OPTICFLOW_FPS)
56 PRINT_CONFIG_VAR(OPTICFLOW_FPS_CAMERA2)
57 
58 #ifdef OPTICFLOW_CAMERA2
59 #define ACTIVE_CAMERAS 2
60 #else
61 #define ACTIVE_CAMERAS 1
62 #endif
63 
64 /* The main opticflow variables */
67 
69 static pthread_mutex_t opticflow_mutex;
70 
71 /* Static functions */
72 struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id);
73 
74 #if PERIODIC_TELEMETRY
81 static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
82 {
83  pthread_mutex_lock(&opticflow_mutex);
84  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
85  if (opticflow_result[idx_camera].noise_measurement < 0.8) {
86  pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
87  &opticflow_result[idx_camera].fps, &opticflow_result[idx_camera].corner_cnt,
88  &opticflow_result[idx_camera].tracked_cnt, &opticflow_result[idx_camera].flow_x,
89  &opticflow_result[idx_camera].flow_y, &opticflow_result[idx_camera].flow_der_x,
90  &opticflow_result[idx_camera].flow_der_y, &opticflow_result[idx_camera].vel_body.x,
91  &opticflow_result[idx_camera].vel_body.y, &opticflow_result[idx_camera].vel_body.z,
92  &opticflow_result[idx_camera].div_size,
94  &opticflow_result[idx_camera].divergence,
95  &opticflow_result[idx_camera].camera_id); // TODO: no noise measurement here...
96  }
97  }
98  pthread_mutex_unlock(&opticflow_mutex);
99 }
100 #endif
101 
106 {
107  // Initialize the opticflow calculation
108  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
109  opticflow_got_result[idx_camera] = false;
110  }
112 
113  cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS, 0);
114 #ifdef OPTICFLOW_CAMERA2
116 #endif
117 
118 #if PERIODIC_TELEMETRY
120 #endif
121 
122 }
123 
129 {
130  pthread_mutex_lock(&opticflow_mutex);
131  // Update the stabilization loops on the current calculation
132  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
133  if (opticflow_got_result[idx_camera]) {
134  uint32_t now_ts = get_sys_time_usec();
135  AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID + idx_camera, now_ts,
136  opticflow_result[idx_camera].flow_x,
137  opticflow_result[idx_camera].flow_y,
138  opticflow_result[idx_camera].flow_der_x,
139  opticflow_result[idx_camera].flow_der_y,
140  opticflow_result[idx_camera].noise_measurement,
141  opticflow_result[idx_camera].div_size);
142  //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
143  if (opticflow_result[idx_camera].noise_measurement < 0.8) {
144  AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts,
145  opticflow_result[idx_camera].vel_body.x,
146  opticflow_result[idx_camera].vel_body.y,
147  0.0f, //opticflow_result.vel_body.z,
150  -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
151  );
152  }
153  opticflow_got_result[idx_camera] = false;
154  }
155  }
156  pthread_mutex_unlock(&opticflow_mutex);
157 }
158 
167 struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id)
168 {
169  // Copy the state
170  // TODO : put accelerometer values at pose of img timestamp
171  //struct opticflow_state_t temp_state;
172  struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
173  img->eulers = pose.eulers;
174 
175  // Do the optical flow calculation
176  static struct opticflow_result_t temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames
177  if(opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])){
178  // Copy the result if finished
179  pthread_mutex_lock(&opticflow_mutex);
180  opticflow_result[camera_id] = temp_result[camera_id];
181  opticflow_got_result[camera_id] = true;
182  pthread_mutex_unlock(&opticflow_mutex);
183  }
184  return img;
185 }
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
float div_size
Divergence as determined with the size_divergence script.
bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result)
Run the optical flow on a new image frame.
Periodic telemetry system header (includes downlink utility and generated code).
optical-flow calculation for Parrot Drones
Capture images from a V4L2 device (Video for Linux 2)
Main include for ABI (AirBorneInterface).
Definition: image.h:44
struct pose_t get_rotation_at_timestamp(uint32_t timestamp)
Given a pprz timestamp in used (obtained with get_sys_time_usec) we return the pose in FloatEulers cl...
Definition: pose_history.c:53
#define FLOW_OPTICFLOW_ID
struct image_t * opticflow_module_calc(struct image_t *img, uint8_t camera_id)
The main optical flow calculation thread.
struct opticflow_t opticflow[ACTIVE_CAMERAS]
Opticflow calculations.
struct FloatVect3 vel_body
The velocity in body frame (m/s) with X positive to the front of the aircraft, Y positive to the righ...
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:50
void opticflow_module_init(void)
Initialize the optical flow module for the bottom camera.
static pthread_mutex_t opticflow_mutex
Mutex lock fo thread safety.
#define OPTICFLOW_FPS_CAMERA2
Default FPS (zero means run at camera fps)
uint8_t camera_id
Camera id as passed to cv_add_to_device.
struct FloatEulers eulers
Definition: pose_history.h:33
Computer vision framework for onboard processing.
static bool opticflow_got_result[ACTIVE_CAMERAS]
When we have an optical flow calculation.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void opticflow_calc_init(struct opticflow_t opticflow[])
Initialize the opticflow calculator.
Encode images with the use of the JPEG encoding.
#define OPTICFLOW_FPS
Default FPS (zero means run at camera fps)
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
Definition: cv.c:46
API to get/set the generic vehicle states.
#define ACTIVE_CAMERAS
void opticflow_module_run(void)
Update the optical flow state for the calculation thread and update the stabilization loops with the ...
Encodes a video stream with RTP Format 26 (Motion JPEG)
#define OPTICFLOW_AGL_ID
Default sonar/agl to use in opticflow visual_estimator.
#define VEL_OPTICFLOW_ID
struct FloatEulers eulers
Euler Angles at time of image.
Definition: image.h:49
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
float noise_measurement
noise of measurement, for state filter
float divergence
Divergence as determined with a linear flow fit.
float surface_roughness
Surface roughness as determined with a linear optical flow fit.
static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
Send optical flow telemetry information.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static struct opticflow_result_t opticflow_result[ACTIVE_CAMERAS]
The opticflow result.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78