Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
27
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#include "generated/airframe.h"
43
45
46/* ABI messages sender ID */
47#ifndef OPTICFLOW_AGL_ID
48#define OPTICFLOW_AGL_ID ABI_BROADCAST
49#endif
51
52#ifndef OPTICFLOW_FPS
53#define OPTICFLOW_FPS 0
54#endif
55
56#ifndef OPTICFLOW_FPS_CAMERA2
57#define OPTICFLOW_FPS_CAMERA2 0
58#endif
61
62#ifdef OPTICFLOW_CAMERA2
63#define ACTIVE_CAMERAS 2
64#else
65#define ACTIVE_CAMERAS 1
66#endif
67
68/* The main opticflow variables */
71
73static pthread_mutex_t opticflow_mutex;
74
75/* Static functions */
76struct image_t *opticflow_module_calc(struct image_t *img,
77 uint8_t camera_id);
78
79#if PERIODIC_TELEMETRY
86static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
87{
88 pthread_mutex_lock(&opticflow_mutex);
89 for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
90 if (opticflow_result[idx_camera].noise_measurement < 0.8) {
91 // the message has int32 flow fields
92 int32_t flow_x = opticflow_result[idx_camera].flow_x;
93 int32_t flow_y = opticflow_result[idx_camera].flow_y;
94 int32_t flow_der_x = opticflow_result[idx_camera].flow_der_x;
95 int32_t flow_der_y = opticflow_result[idx_camera].flow_der_y;
96 pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
97 &opticflow_result[idx_camera].fps, &opticflow_result[idx_camera].corner_cnt,
98 &opticflow_result[idx_camera].tracked_cnt, &flow_x, &flow_y,
99 &flow_der_x, &flow_der_y, &opticflow_result[idx_camera].vel_body.x,
100 &opticflow_result[idx_camera].vel_body.y, &opticflow_result[idx_camera].vel_body.z,
101 &opticflow_result[idx_camera].div_size,
102 &opticflow_result[idx_camera].surface_roughness,
103 &opticflow_result[idx_camera].divergence,
104 &opticflow_result[idx_camera].camera_id); // TODO: no noise measurement here...
105 }
106 }
107 pthread_mutex_unlock(&opticflow_mutex);
108}
109#endif
110
115{
116 // Initialize the opticflow calculation
117 for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
118 opticflow_got_result[idx_camera] = false;
119 }
121
123#ifdef OPTICFLOW_CAMERA2
125#endif
126
127#if PERIODIC_TELEMETRY
129#endif
130
131}
132
138{
139 pthread_mutex_lock(&opticflow_mutex);
140 // Update the stabilization loops on the current calculation
141 for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
142 if (opticflow_got_result[idx_camera]) {
143 uint32_t now_ts = get_sys_time_usec();
144 AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID + idx_camera, now_ts,
145 opticflow_result[idx_camera].flow_x,
146 opticflow_result[idx_camera].flow_y,
147 opticflow_result[idx_camera].flow_der_x,
148 opticflow_result[idx_camera].flow_der_y,
149 opticflow_result[idx_camera].noise_measurement,
150 opticflow_result[idx_camera].div_size);
151 //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
152 if (opticflow_result[idx_camera].noise_measurement < 0.8) {
153 AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts,
154 opticflow_result[idx_camera].vel_body.x,
155 opticflow_result[idx_camera].vel_body.y,
156 0.0f, //opticflow_result.vel_body.z,
157 opticflow_result[idx_camera].noise_measurement,
158 opticflow_result[idx_camera].noise_measurement,
159 -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
160 );
161 }
162 opticflow_got_result[idx_camera] = false;
163 }
164 }
165 pthread_mutex_unlock(&opticflow_mutex);
166}
167
176struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id)
177{
178 // Copy the state
179 // TODO : put accelerometer values at pose of img timestamp
180 //struct opticflow_state_t temp_state;
181 struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
182 img->eulers = pose.eulers;
183
184 // Do the optical flow calculation
185 static struct opticflow_result_t
186 temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames
187 if (opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])) {
188 // Copy the result if finished
189 pthread_mutex_lock(&opticflow_mutex);
190 opticflow_result[camera_id] = temp_result[camera_id];
192 pthread_mutex_unlock(&opticflow_mutex);
193 }
194 return img;
195}
Main include for ABI (AirBorneInterface).
#define VEL_OPTICFLOW_ID
#define FLOW_OPTICFLOW_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
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
Computer vision framework for onboard processing.
static struct uart_periph * dev
uint32_t pprz_ts
The timestamp in us since system startup.
Definition image.h:50
struct FloatEulers eulers
Euler Angles at time of image.
Definition image.h:49
uint8_t camera_id
Camera id as passed to cv_add_to_device.
Encode images with the use of the JPEG encoding.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
void opticflow_calc_init(struct opticflow_t opticflow[])
Initialize the opticflow calculator.
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.
static struct opticflow_result_t opticflow_result[ACTIVE_CAMERAS]
The opticflow result.
static pthread_mutex_t opticflow_mutex
Mutex lock fo thread safety.
#define OPTICFLOW_AGL_ID
Default sonar/agl to use in opticflow visual_estimator.
#define OPTICFLOW_FPS_CAMERA2
Default FPS (zero means run at camera fps).
void opticflow_module_init(void)
Initialize the optical flow module for the bottom camera.
static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
Send optical flow telemetry information.
struct opticflow_t opticflow[ACTIVE_CAMERAS]
Opticflow calculations.
uint16_t fps_OF
void opticflow_module_run(void)
Update the optical flow state for the calculation thread and update the stabilization loops with the ...
static bool opticflow_got_result[ACTIVE_CAMERAS]
When we have an optical flow calculation.
struct image_t * opticflow_module_calc(struct image_t *img, uint8_t camera_id)
The main optical flow calculation thread.
#define OPTICFLOW_FPS
Default FPS (zero means run at camera fps).
#define ACTIVE_CAMERAS
optical-flow calculation for Parrot Drones
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...
struct FloatEulers eulers
Encodes a video stream with RTP Format 26 (Motion JPEG).
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
Capture images from a V4L2 device (Video for Linux 2).
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.