Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/abi.h"
35 
36 #include "lib/v4l/v4l2.h"
37 #include "lib/encoding/jpeg.h"
38 #include "lib/encoding/rtp.h"
39 #include "errno.h"
40 
41 #include "cv.h"
42 
43 /* ABI messages sender ID */
44 #ifndef OPTICFLOW_AGL_ID
45 #define OPTICFLOW_AGL_ID ABI_BROADCAST
46 #endif
47 PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
48 
49 #ifndef OPTICFLOW_FPS
50 #define OPTICFLOW_FPS 0
51 #endif
52 
53 #ifndef OPTICFLOW_FPS_CAMERA2
54 #define OPTICFLOW_FPS_CAMERA2 0
55 #endif
56 PRINT_CONFIG_VAR(OPTICFLOW_FPS)
57 PRINT_CONFIG_VAR(OPTICFLOW_FPS_CAMERA2)
58 
59 #ifdef OPTICFLOW_CAMERA2
60 #define ACTIVE_CAMERAS 2
61 #else
62 #define ACTIVE_CAMERAS 1
63 #endif
64 
65 /* The main opticflow variables */
68 
70 static pthread_mutex_t opticflow_mutex;
71 
72 /* Static functions */
73 struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id);
74 
75 #if PERIODIC_TELEMETRY
82 static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
83 {
84  pthread_mutex_lock(&opticflow_mutex);
85  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
86  if (opticflow_result[idx_camera].noise_measurement < 0.8) {
87  pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
88  &opticflow_result[idx_camera].fps, &opticflow_result[idx_camera].corner_cnt,
89  &opticflow_result[idx_camera].tracked_cnt, &opticflow_result[idx_camera].flow_x,
90  &opticflow_result[idx_camera].flow_y, &opticflow_result[idx_camera].flow_der_x,
91  &opticflow_result[idx_camera].flow_der_y, &opticflow_result[idx_camera].vel_body.x,
92  &opticflow_result[idx_camera].vel_body.y, &opticflow_result[idx_camera].vel_body.z,
93  &opticflow_result[idx_camera].div_size,
95  &opticflow_result[idx_camera].divergence,
96  &opticflow_result[idx_camera].camera_id); // TODO: no noise measurement here...
97  }
98  }
99  pthread_mutex_unlock(&opticflow_mutex);
100 }
101 #endif
102 
107 {
108  // Initialize the opticflow calculation
109  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
110  opticflow_got_result[idx_camera] = false;
111  }
113 
114  cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS, 0);
115 #ifdef OPTICFLOW_CAMERA2
117 #endif
118 
119 #if PERIODIC_TELEMETRY
121 #endif
122 
123 }
124 
130 {
131  pthread_mutex_lock(&opticflow_mutex);
132  // Update the stabilization loops on the current calculation
133  for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
134  if (opticflow_got_result[idx_camera]) {
135  uint32_t now_ts = get_sys_time_usec();
136  AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID + idx_camera, now_ts,
137  opticflow_result[idx_camera].flow_x,
138  opticflow_result[idx_camera].flow_y,
139  opticflow_result[idx_camera].flow_der_x,
140  opticflow_result[idx_camera].flow_der_y,
141  opticflow_result[idx_camera].noise_measurement,
142  opticflow_result[idx_camera].div_size);
143  //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
144  if (opticflow_result[idx_camera].noise_measurement < 0.8) {
145  AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts,
146  opticflow_result[idx_camera].vel_body.x,
147  opticflow_result[idx_camera].vel_body.y,
148  0.0f, //opticflow_result.vel_body.z,
151  -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
152  );
153  }
154  opticflow_got_result[idx_camera] = false;
155  }
156  }
157  pthread_mutex_unlock(&opticflow_mutex);
158 }
159 
168 struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id)
169 {
170  // Copy the state
171  // TODO : put accelerometer values at pose of img timestamp
172  //struct opticflow_state_t temp_state;
173  struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
174  img->eulers = pose.eulers;
175 
176  // Do the optical flow calculation
177  static struct opticflow_result_t temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames
178  if(opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])){
179  // Copy the result if finished
180  pthread_mutex_lock(&opticflow_mutex);
181  opticflow_result[camera_id] = temp_result[camera_id];
183  pthread_mutex_unlock(&opticflow_mutex);
184  }
185  return img;
186 }
OPTICFLOW_AGL_ID
#define OPTICFLOW_AGL_ID
Default sonar/agl to use in opticflow visual_estimator.
Definition: opticflow_module.c:45
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
OPTICFLOW_FPS_CAMERA2
#define OPTICFLOW_FPS_CAMERA2
Default FPS (zero means run at camera fps)
Definition: opticflow_module.c:54
opticflow_got_result
static bool opticflow_got_result[ACTIVE_CAMERAS]
When we have an optical flow calculation.
Definition: opticflow_module.c:69
abi.h
VEL_OPTICFLOW_ID
#define VEL_OPTICFLOW_ID
Definition: abi_sender_ids.h:406
cv_add_to_device
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
opticflow_t
Definition: opticflow_calculator.h:40
opticflow_result_t::divergence
float divergence
Divergence as determined with a linear flow fit.
Definition: inter_thread_data.h:54
opticflow_result_t::surface_roughness
float surface_roughness
Surface roughness as determined with a linear optical flow fit.
Definition: inter_thread_data.h:53
opticflow_calc_frame
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.
Definition: opticflow_calculator.c:1169
FLOW_OPTICFLOW_ID
#define FLOW_OPTICFLOW_ID
Definition: abi_sender_ids.h:375
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
get_rotation_at_timestamp
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
telemetry.h
cv.h
opticflow_telem_send
static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
Send optical flow telemetry information.
Definition: opticflow_module.c:82
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
v4l2.h
opticflow_result_t::vel_body
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...
Definition: inter_thread_data.h:49
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
opticflow_module_run
void opticflow_module_run(void)
Update the optical flow state for the calculation thread and update the stabilization loops with the ...
Definition: opticflow_module.c:129
opticflow_result_t::camera_id
uint8_t camera_id
Camera id as passed to cv_add_to_device.
Definition: inter_thread_data.h:55
pose_history.h
opticflow_module_init
void opticflow_module_init(void)
Initialize the optical flow module for the bottom camera.
Definition: opticflow_module.c:106
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
opticflow_result_t::noise_measurement
float noise_measurement
noise of measurement, for state filter
Definition: inter_thread_data.h:57
pose_t::eulers
struct FloatEulers eulers
Definition: pose_history.h:33
rtp.h
OPTICFLOW_FPS
#define OPTICFLOW_FPS
Default FPS (zero means run at camera fps)
Definition: opticflow_module.c:50
opticflow_mutex
static pthread_mutex_t opticflow_mutex
Mutex lock fo thread safety.
Definition: opticflow_module.c:70
opticflow_result_t::div_size
float div_size
Divergence as determined with the size_divergence script.
Definition: inter_thread_data.h:51
opticflow_module_calc
struct image_t * opticflow_module_calc(struct image_t *img, uint8_t camera_id)
The main optical flow calculation thread.
Definition: opticflow_module.c:168
image_t::pprz_ts
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:50
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
opticflow
struct opticflow_t opticflow[ACTIVE_CAMERAS]
Opticflow calculations.
Definition: opticflow_module.c:66
ACTIVE_CAMERAS
#define ACTIVE_CAMERAS
Definition: opticflow_module.c:62
state.h
opticflow_result
static struct opticflow_result_t opticflow_result[ACTIVE_CAMERAS]
The opticflow result.
Definition: opticflow_module.c:67
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
opticflow_calc_init
void opticflow_calc_init(struct opticflow_t opticflow[])
Initialize the opticflow calculator.
Definition: opticflow_calculator.c:396
image_t
Definition: image.h:44
image_t::eulers
struct FloatEulers eulers
Euler Angles at time of image.
Definition: image.h:49
opticflow_result_t
Definition: inter_thread_data.h:37
jpeg.h
opticflow_module.h
optical-flow calculation for Parrot Drones
pose_t
Definition: pose_history.h:31