Paparazzi UAS  v5.14.0_stable-0-g3f680d1
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 PRINT_CONFIG_VAR(OPTICFLOW_FPS)
52 
53 /* The main opticflow variables */
56 
57 static bool opticflow_got_result;
58 static pthread_mutex_t opticflow_mutex;
59 
60 /* Static functions */
61 struct image_t *opticflow_module_calc(struct image_t *img);
62 
63 #if PERIODIC_TELEMETRY
70 static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
71 {
72  pthread_mutex_lock(&opticflow_mutex);
73  if (opticflow_result.noise_measurement < 0.8) {
74  pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
75  &opticflow_result.fps, &opticflow_result.corner_cnt,
76  &opticflow_result.tracked_cnt, &opticflow_result.flow_x,
77  &opticflow_result.flow_y, &opticflow_result.flow_der_x,
78  &opticflow_result.flow_der_y, &opticflow_result.vel_body.x,
79  &opticflow_result.vel_body.y, &opticflow_result.vel_body.z,
80  &opticflow_result.div_size, &opticflow_result.surface_roughness,
81  &opticflow_result.divergence); // TODO: no noise measurement here...
82  }
83  pthread_mutex_unlock(&opticflow_mutex);
84 }
85 #endif
86 
91 {
92  // Initialize the opticflow calculation
93  opticflow_got_result = false;
94  opticflow_calc_init(&opticflow);
95 
97 
98 #if PERIODIC_TELEMETRY
100 #endif
101 
102 }
103 
109 {
110  pthread_mutex_lock(&opticflow_mutex);
111  // Update the stabilization loops on the current calculation
112  if (opticflow_got_result) {
113  uint32_t now_ts = get_sys_time_usec();
114  AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID, now_ts,
115  opticflow_result.flow_x,
116  opticflow_result.flow_y,
117  opticflow_result.flow_der_x,
118  opticflow_result.flow_der_y,
119  opticflow_result.noise_measurement,
120  opticflow_result.div_size);
121  //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
122  if (opticflow_result.noise_measurement < 0.8) {
123  AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID, now_ts,
124  opticflow_result.vel_body.x,
125  opticflow_result.vel_body.y,
126  0.0f, //opticflow_result.vel_body.z,
127  opticflow_result.noise_measurement,
128  opticflow_result.noise_measurement,
129  -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
130  );
131  }
132  opticflow_got_result = false;
133  }
134  pthread_mutex_unlock(&opticflow_mutex);
135 }
136 
145 {
146  // Copy the state
147  // TODO : put accelerometer values at pose of img timestamp
148  //struct opticflow_state_t temp_state;
149  struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
150  img->eulers = pose.eulers;
151 
152  // Do the optical flow calculation
153  static struct opticflow_result_t temp_result; // static so that the number of corners is kept between frames
154  bool flow_successful = opticflow_calc_frame(&opticflow, img, &temp_result);
155 
156  // Copy the result if finished
157  pthread_mutex_lock(&opticflow_mutex);
158  opticflow_result = temp_result;
159  opticflow_got_result = flow_successful;
160 
161  // release the mutex as we are done with editing the opticflow result
162  pthread_mutex_unlock(&opticflow_mutex);
163  return img;
164 }
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.
struct opticflow_t opticflow
Opticflow calculations.
Periodic telemetry system header (includes downlink utility and generated code).
static bool opticflow_got_result
When we have an optical flow calculation.
optical-flow calculation for Parrot Drones
Capture images from a V4L2 device (Video for Linux 2)
Main include for ABI (AirBorneInterface).
Definition: image.h:43
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
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:49
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps)
Definition: cv.c:46
void opticflow_module_init(void)
Initialize the optical flow module for the bottom camera.
struct image_t * opticflow_module_calc(struct image_t *img)
The main optical flow calculation thread.
static pthread_mutex_t opticflow_mutex
Mutex lock fo thread safety.
void opticflow_calc_init(struct opticflow_t *opticflow)
Initialize the opticflow calculator.
struct FloatEulers eulers
Definition: pose_history.h:33
unsigned long uint32_t
Definition: types.h:18
Computer vision framework for onboard processing.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static struct opticflow_result_t opticflow_result
The opticflow result.
Encode images with the use of the JPEG encoding.
#define OPTICFLOW_FPS
Default FPS (zero means run at camera fps)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
API to get/set the generic vehicle states.
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:48
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
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