Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
Data Structures | Macros | Functions | Variables
cv_target_localization.c File Reference
#include "modules/computer_vision/cv_target_localization.h"
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "state.h"
#include "subsystems/abi.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/navigation/waypoints.h"
#include "generated/flight_plan.h"
+ Include dependency graph for cv_target_localization.c:

Go to the source code of this file.

Data Structures

struct  target_loc_t
 

Macros

#define TARGET_LOC_BODY_TO_CAM_PHI   0.f
 
#define TARGET_LOC_BODY_TO_CAM_THETA   0.f
 
#define TARGET_LOC_BODY_TO_CAM_PSI   M_PI_2
 
#define TARGET_LOC_CAM_POS_X   0.f
 
#define TARGET_LOC_CAM_POS_Y   0.f
 
#define TARGET_LOC_CAM_POS_Z   0.f
 
#define TARGET_LOC_PIXEL_TO_IMAGE_X   (1.f / 1000.f)
 
#define TARGET_LOC_PIXEL_TO_IMAGE_Y   TARGET_LOC_PIXEL_TO_IMAGE_X
 
#define TARGET_LOC_WP_T1_ID   1
 
#define TARGET_LOC_WP_T2_ID   2
 
#define TARGET_LOC_WP_T3_ID   3
 
#define TARGET_LOC_ID   ABI_BROADCAST
 

Functions

static void detection_cb (uint8_t sender_id UNUSED, int16_t pixel_x, int16_t pixel_y, int16_t pixel_width UNUSED, int16_t pixel_height UNUSED, int32_t quality UNUSED, int16_t extra)
 
void target_localization_init (void)
 
void target_localization_report (void)
 
void cv_target_localization_report_mark (uint8_t mark)
 

Variables

static struct target_loc_t target_loc
 
uint8_t target_localization_mark
 
bool target_localization_update_wp
 
uint8_t target_loc_wp_tab [][2]
 
abi_event detection_ev
 

Detailed Description

Author
Gautier Hattenberger gauti.nosp@m.er.h.nosp@m.atten.nosp@m.berg.nosp@m.er@en.nosp@m.ac.f.nosp@m.r

Compute georeferenced position of a target from visual detection assuming that the target is on a flat ground

Definition in file cv_target_localization.c.

Data Structure Documentation

◆ target_loc_t

struct target_loc_t

Definition at line 80 of file cv_target_localization.c.

+ Collaboration diagram for target_loc_t:
Data Fields
struct FloatRMat body_to_cam Body to camera rotation.
struct FloatVect3 cam_pos Position of camera in body frame.
struct LlaCoor_f pos_lla Target global position in LLA.
int16_t px Target in camera frame.
int16_t py Target in camera frame.
struct NedCoor_f target Target position in LTP-NED frame.
uint8_t type Type of target.
bool valid True if a target have been seen.

Macro Definition Documentation

◆ TARGET_LOC_BODY_TO_CAM_PHI

#define TARGET_LOC_BODY_TO_CAM_PHI   0.f

Definition at line 45 of file cv_target_localization.c.

◆ TARGET_LOC_BODY_TO_CAM_PSI

#define TARGET_LOC_BODY_TO_CAM_PSI   M_PI_2

Definition at line 53 of file cv_target_localization.c.

◆ TARGET_LOC_BODY_TO_CAM_THETA

#define TARGET_LOC_BODY_TO_CAM_THETA   0.f

Definition at line 49 of file cv_target_localization.c.

◆ TARGET_LOC_CAM_POS_X

#define TARGET_LOC_CAM_POS_X   0.f

Definition at line 57 of file cv_target_localization.c.

◆ TARGET_LOC_CAM_POS_Y

#define TARGET_LOC_CAM_POS_Y   0.f

Definition at line 61 of file cv_target_localization.c.

◆ TARGET_LOC_CAM_POS_Z

#define TARGET_LOC_CAM_POS_Z   0.f

Definition at line 65 of file cv_target_localization.c.

◆ TARGET_LOC_ID

#define TARGET_LOC_ID   ABI_BROADCAST

Definition at line 128 of file cv_target_localization.c.

◆ TARGET_LOC_PIXEL_TO_IMAGE_X

#define TARGET_LOC_PIXEL_TO_IMAGE_X   (1.f / 1000.f)

Definition at line 71 of file cv_target_localization.c.

◆ TARGET_LOC_PIXEL_TO_IMAGE_Y

#define TARGET_LOC_PIXEL_TO_IMAGE_Y   TARGET_LOC_PIXEL_TO_IMAGE_X

Definition at line 76 of file cv_target_localization.c.

◆ TARGET_LOC_WP_T1_ID

#define TARGET_LOC_WP_T1_ID   1

Definition at line 105 of file cv_target_localization.c.

◆ TARGET_LOC_WP_T2_ID

#define TARGET_LOC_WP_T2_ID   2

Definition at line 108 of file cv_target_localization.c.

◆ TARGET_LOC_WP_T3_ID

#define TARGET_LOC_WP_T3_ID   3

Definition at line 111 of file cv_target_localization.c.

Function Documentation

◆ cv_target_localization_report_mark()

void cv_target_localization_report_mark ( uint8_t  mark)

Definition at line 232 of file cv_target_localization.c.

References DefaultChannel, DefaultDevice, LlaCoor_f::lat, LlaCoor_f::lon, stateGetPositionLla_f(), and target_localization_mark.

+ Here is the call graph for this function:

◆ detection_cb()

static void detection_cb ( uint8_t sender_id  UNUSED,
int16_t  pixel_x,
int16_t  pixel_y,
int16_t pixel_width  UNUSED,
int16_t pixel_height  UNUSED,
int32_t quality  UNUSED,
int16_t  extra 
)
static

Definition at line 133 of file cv_target_localization.c.

References target_loc_t::body_to_cam, target_loc_t::cam_pos, ecef_of_ned_point_f(), float_rmat_comp(), float_rmat_transp_vmult(), float_rmat_vmult(), lla_of_ecef_f(), State::ned_origin_f, POS_BFP_OF_REAL, target_loc_t::pos_lla, target_loc_t::px, target_loc_t::py, scale, state, stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), target_loc_t::target, target_loc, TARGET_LOC_PIXEL_TO_IMAGE_X, TARGET_LOC_PIXEL_TO_IMAGE_Y, target_loc_wp_tab, target_localization_update_wp, target_loc_t::type, target_loc_t::valid, VECT3_ADD, VECT3_SUM_SCALED, waypoint_move_xy_i(), FloatVect3::x, NedCoor_f::x, NedCoor_f::y, and FloatVect3::z.

Referenced by target_localization_init().

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

◆ target_localization_init()

void target_localization_init ( void  )

Definition at line 190 of file cv_target_localization.c.

References target_loc_t::body_to_cam, target_loc_t::cam_pos, detection_cb(), detection_ev, float_rmat_of_eulers, FLOAT_VECT3_ZERO, target_loc_t::px, target_loc_t::py, target_loc_t::target, target_loc, TARGET_LOC_BODY_TO_CAM_PHI, TARGET_LOC_BODY_TO_CAM_PSI, TARGET_LOC_BODY_TO_CAM_THETA, TARGET_LOC_CAM_POS_X, TARGET_LOC_CAM_POS_Y, TARGET_LOC_CAM_POS_Z, TARGET_LOC_ID, target_localization_mark, target_localization_update_wp, target_loc_t::valid, and VECT3_ASSIGN.

+ Here is the call graph for this function:

◆ target_localization_report()

void target_localization_report ( void  )

Definition at line 218 of file cv_target_localization.c.

References DefaultChannel, DefaultDevice, LlaCoor_f::lat, LlaCoor_f::lon, target_loc_t::pos_lla, target_loc, target_loc_t::type, and target_loc_t::valid.

Variable Documentation

◆ detection_ev

abi_event detection_ev

Definition at line 131 of file cv_target_localization.c.

Referenced by target_localization_init().

◆ target_loc

struct target_loc_t target_loc
static

Definition at line 94 of file cv_target_localization.c.

Referenced by detection_cb(), target_localization_init(), and target_localization_report().

◆ target_loc_wp_tab

uint8_t target_loc_wp_tab[][2]
Initial value:
= {
{ 0, 0 }
}

Definition at line 113 of file cv_target_localization.c.

Referenced by detection_cb().

◆ target_localization_mark

uint8_t target_localization_mark

Definition at line 99 of file cv_target_localization.c.

Referenced by cv_target_localization_report_mark(), and target_localization_init().

◆ target_localization_update_wp

bool target_localization_update_wp

Definition at line 101 of file cv_target_localization.c.

Referenced by detection_cb(), and target_localization_init().