Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
Data Structures | Macros | Functions | Variables
detect_gate.c File Reference
#include <stdio.h>
#include "detect_gate.h"
#include "modules/computer_vision/lib/vision/image.h"
#include "modules/computer_vision/lib/vision/PnP_AHRS.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_simple_matrix.h"
#include "subsystems/abi.h"
#include "subsystems/abi_sender_ids.h"
#include "modules/computer_vision/snake_gate_detection.h"
#include "subsystems/datalink/telemetry.h"
+ Include dependency graph for detect_gate.c:

Go to the source code of this file.

Data Structures

struct  vision_relative_position_struct
 

Macros

#define DETECT_GATE_JUST_FILTER   0
 
#define DETECT_GATE_FPS   0
 Default FPS (zero means run at camera fps) More...
 
#define DETECT_GATE_CAMERA   "front_camera"
 
#define DETECT_GATE_N_SAMPLES   2000
 
#define DETECT_GATE_MIN_N_SIDES   3
 
#define DETECT_GATE_MIN_PIX_SIZE   30
 
#define DETECT_GATE_MIN_GATE_QUALITY   0.15
 
#define DETECT_GATE_GATE_THICKNESS   0.0f
 
#define DETECT_GATE_EXCLUDE_PIXELS_TOP   0
 
#define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM   0
 
#define DETECT_GATE_SIMPLIFIED_PNP   0
 

Functions

static struct image_t * detect_gate_func (struct image_t *img, uint8_t camera_id)
 
void detect_gate_event (void)
 
static void send_detect_gate_visual_position (struct transport_tx *trans, struct link_device *dev)
 
void detect_gate_init (void)
 

Variables

int just_filtering
 
int n_samples
 
int min_n_sides
 
int min_px_size
 
float min_gate_quality
 
float gate_thickness
 
uint8_t color_Ym
 
uint8_t color_YM
 
uint8_t color_Um
 
uint8_t color_UM
 
uint8_t color_Vm
 
uint8_t color_VM
 
int exclude_top
 
int exclude_bottom
 
struct FloatVect3 drone_position
 
struct gate_img best_gate
 
struct gate_img gates_c [MAX_GATES]
 
struct FloatVect3 world_corners [4]
 
float gate_size_m = 1.4
 
float gate_center_height = 0.0
 
int n_corners = 3
 
struct FloatEulers cam_body
 
volatile int detect_gate_has_new_data
 
volatile float detect_gate_x
 
volatile float detect_gate_y
 
volatile float detect_gate_z
 
static pthread_mutex_t gate_detect_mutex
 Mutex lock fo thread safety. More...
 
struct vision_relative_position_struct detectgate_vision_position = {false, 0, 0.0f, 0.0f, 0.0f}
 

Data Structure Documentation

◆ vision_relative_position_struct

struct vision_relative_position_struct

Definition at line 123 of file detect_gate.c.

Data Fields
int cnt
int received
float x
float y
float z

Macro Definition Documentation

◆ DETECT_GATE_CAMERA

#define DETECT_GATE_CAMERA   "front_camera"

Definition at line 38 of file detect_gate.c.

◆ DETECT_GATE_EXCLUDE_PIXELS_BOTTOM

#define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM   0

Definition at line 73 of file detect_gate.c.

◆ DETECT_GATE_EXCLUDE_PIXELS_TOP

#define DETECT_GATE_EXCLUDE_PIXELS_TOP   0

Definition at line 68 of file detect_gate.c.

◆ DETECT_GATE_FPS

#define DETECT_GATE_FPS   0

Default FPS (zero means run at camera fps)

Definition at line 33 of file detect_gate.c.

◆ DETECT_GATE_GATE_THICKNESS

#define DETECT_GATE_GATE_THICKNESS   0.0f

Definition at line 63 of file detect_gate.c.

◆ DETECT_GATE_JUST_FILTER

#define DETECT_GATE_JUST_FILTER   0

Definition at line 28 of file detect_gate.c.

◆ DETECT_GATE_MIN_GATE_QUALITY

#define DETECT_GATE_MIN_GATE_QUALITY   0.15

Definition at line 58 of file detect_gate.c.

◆ DETECT_GATE_MIN_N_SIDES

#define DETECT_GATE_MIN_N_SIDES   3

Definition at line 48 of file detect_gate.c.

◆ DETECT_GATE_MIN_PIX_SIZE

#define DETECT_GATE_MIN_PIX_SIZE   30

Definition at line 53 of file detect_gate.c.

◆ DETECT_GATE_N_SAMPLES

#define DETECT_GATE_N_SAMPLES   2000

Definition at line 43 of file detect_gate.c.

◆ DETECT_GATE_SIMPLIFIED_PNP

#define DETECT_GATE_SIMPLIFIED_PNP   0

Definition at line 78 of file detect_gate.c.

Function Documentation

◆ detect_gate_event()

void detect_gate_event ( void  )

Definition at line 264 of file detect_gate.c.

References vision_relative_position_struct::cnt, DETECT_GATE_ABI_ID, detect_gate_has_new_data, detect_gate_x, detect_gate_y, detect_gate_z, detectgate_vision_position, gate_detect_mutex, vision_relative_position_struct::received, vision_relative_position_struct::x, vision_relative_position_struct::y, and vision_relative_position_struct::z.

◆ detect_gate_func()

static struct image_t* detect_gate_func ( struct image_t *  img,
uint8_t  camera_id 
)
static

Definition at line 134 of file detect_gate.c.

References best_gate, cam_body, color_Um, color_UM, color_Vm, color_VM, color_Ym, color_YM, DETECT_GATE_CAMERA, detect_gate_has_new_data, DETECT_GATE_SIMPLIFIED_PNP, detect_gate_x, detect_gate_y, detect_gate_z, drone_position, exclude_bottom, exclude_top, f, gate_detect_mutex, gate_size_m, gate_thickness, gates_c, get_world_position_from_image_points(), image_yuv422_colorfilt(), just_filtering, min_gate_quality, min_n_sides, min_px_size, n_corners, n_samples, gate_img::quality, snake_gate_detection(), world_corners, FloatVect3::x, gate_img::x_corners, FloatVect3::y, gate_img::y_corners, and FloatVect3::z.

Referenced by detect_gate_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ detect_gate_init()

void detect_gate_init ( void  )

Definition at line 305 of file detect_gate.c.

References cam_body, color_Um, color_UM, color_Vm, color_VM, color_Ym, color_YM, cv_add_to_device(), DefaultPeriodic, DETECT_GATE_CAMERA, DETECT_GATE_EXCLUDE_PIXELS_BOTTOM, DETECT_GATE_EXCLUDE_PIXELS_TOP, DETECT_GATE_FPS, detect_gate_func(), DETECT_GATE_GATE_THICKNESS, detect_gate_has_new_data, DETECT_GATE_JUST_FILTER, DETECT_GATE_MIN_GATE_QUALITY, DETECT_GATE_MIN_N_SIDES, DETECT_GATE_MIN_PIX_SIZE, DETECT_GATE_N_SAMPLES, detect_gate_x, detect_gate_y, detect_gate_z, exclude_bottom, exclude_top, f, gate_center_height, gate_detect_mutex, gate_size_m, gate_thickness, just_filtering, min_gate_quality, min_n_sides, min_px_size, n_samples, FloatEulers::phi, FloatEulers::psi, register_periodic_telemetry(), send_detect_gate_visual_position(), FloatEulers::theta, VECT3_ASSIGN, and world_corners.

+ Here is the call graph for this function:

◆ send_detect_gate_visual_position()

static void send_detect_gate_visual_position ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 288 of file detect_gate.c.

References vision_relative_position_struct::cnt, detectgate_vision_position, dev, foo, vision_relative_position_struct::received, vision_relative_position_struct::x, vision_relative_position_struct::y, and vision_relative_position_struct::z.

Referenced by detect_gate_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ best_gate

struct gate_img best_gate

Definition at line 101 of file detect_gate.c.

Referenced by detect_gate_func(), and snake_gate_detection().

◆ cam_body

struct FloatEulers cam_body

Definition at line 112 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and get_world_position_from_image_points().

◆ color_Um

uint8_t color_Um

Definition at line 92 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ color_UM

uint8_t color_UM

Definition at line 93 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ color_Vm

uint8_t color_Vm

Definition at line 94 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ color_VM

uint8_t color_VM

Definition at line 95 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ color_Ym

uint8_t color_Ym

Definition at line 90 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ color_YM

uint8_t color_YM

Definition at line 91 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ detect_gate_has_new_data

volatile int detect_gate_has_new_data

Definition at line 115 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

◆ detect_gate_x

volatile float detect_gate_x

Definition at line 116 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

◆ detect_gate_y

volatile float detect_gate_y

Definition at line 117 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

◆ detect_gate_z

volatile float detect_gate_z

Definition at line 118 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

◆ detectgate_vision_position

struct vision_relative_position_struct detectgate_vision_position = {false, 0, 0.0f, 0.0f, 0.0f}

Referenced by detect_gate_event(), and send_detect_gate_visual_position().

◆ drone_position

struct FloatVect3 drone_position

Definition at line 100 of file detect_gate.c.

Referenced by detect_gate_func().

◆ exclude_bottom

int exclude_bottom

Definition at line 97 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ exclude_top

int exclude_top

Definition at line 96 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ gate_center_height

float gate_center_height = 0.0

Definition at line 108 of file detect_gate.c.

Referenced by detect_gate_init().

◆ gate_detect_mutex

pthread_mutex_t gate_detect_mutex
static

Mutex lock fo thread safety.

Definition at line 120 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

◆ gate_size_m

float gate_size_m = 1.4

Definition at line 107 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

◆ gate_thickness

float gate_thickness

Definition at line 89 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ gates_c

struct gate_img gates_c[MAX_GATES]

Definition at line 103 of file detect_gate.c.

Referenced by detect_gate_func(), and snake_gate_detection().

◆ just_filtering

int just_filtering

Definition at line 84 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

◆ min_gate_quality

float min_gate_quality

Definition at line 88 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ min_n_sides

int min_n_sides

Definition at line 86 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ min_px_size

int min_px_size

Definition at line 87 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

◆ n_corners

int n_corners = 3

Definition at line 109 of file detect_gate.c.

Referenced by detect_gate_func(), and get_world_position_from_image_points().

◆ n_samples

int n_samples

Definition at line 85 of file detect_gate.c.

Referenced by analyze_linear_flow_field(), calc_fast9_lukas_kanade(), check_inside(), detect_gate_func(), detect_gate_init(), fit_linear_flow_field(), fit_linear_model(), fit_linear_model_prior(), get_indices_without_replacement(), get_size_divergence(), RANSAC_linear_model(), and snake_gate_detection().

◆ world_corners

struct FloatVect3 world_corners[4]

Definition at line 106 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and get_world_position_from_image_points().