Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
Data Structures | Macros | Functions | Variables
tag_tracking.c File Reference
#include "modules/computer_vision/tag_tracking.h"
#include "modules/sensors/cameras/jevois.h"
#include "filters/simple_kinematic_kalman.h"
#include "generated/modules.h"
#include "state.h"
#include "modules/core/abi.h"
#include <math.h>
#include "modules/datalink/downlink.h"
#include "generated/flight_plan.h"
+ Include dependency graph for tag_tracking.c:

Go to the source code of this file.

Data Structures

struct  tag_tracking
 

Macros

#define TAG_TRACKING_BODY_TO_CAM_PHI   0.f
 
#define TAG_TRACKING_BODY_TO_CAM_THETA   0.f
 
#define TAG_TRACKING_BODY_TO_CAM_PSI   M_PI_2
 
#define TAG_TRACKING_CAM_POS_X   0.f
 
#define TAG_TRACKING_CAM_POS_Y   0.f
 
#define TAG_TRACKING_CAM_POS_Z   0.f
 
#define TAG_TRACKING_COORD_TO_M   (1.f / 1000.f)
 
#define TAG_TRACKING_R   1.f
 
#define TAG_TRACKING_Q_SIGMA2   1.f
 
#define TAG_TRACKING_P0_POS   10.f
 
#define TAG_TRACKING_P0_SPEED   10.f
 
#define TAG_TRACKING_TIMEOUT   5.f
 
#define TAG_TRACKING_PREDICT_TIME   1.f
 
#define TAG_TRACKING_MAX_OFFSET   2.0f
 
#define TAG_TRACKING_KP   0.5f
 
#define TAG_TRACKING_KPZ   0.2f
 
#define TAG_TRACKING_MAX_SPEED   4.f
 
#define TAG_TRACKING_MAX_VZ   2.f
 
#define TAG_TRACKING_ID   ABI_BROADCAST
 

Functions

static void update_tag_position (void)
 
static void tag_track_cb (uint8_t sender_id UNUSED, uint8_t type, char *id, uint8_t nb UNUSED, int16_t *coord, uint16_t *dim UNUSED, struct FloatQuat quat UNUSED, char *extra UNUSED)
 
void tag_tracking_parse_target_pos (uint8_t *buf)
 
static void update_wp (bool report UNUSED)
 
void tag_tracking_init ()
 
void tag_tracking_propagate ()
 
void tag_tracking_propagate_start ()
 
void tag_tracking_report ()
 
void tag_tracking_compute_speed (void)
 Control function. More...
 

Variables

static const float tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD
 
static struct tag_tracking tag_track_private
 
static struct SimpleKinematicKalman kalman
 
struct tag_tracking_public tag_tracking
 
static abi_event tag_track_ev
 

Data Structure Documentation

◆ tag_tracking

struct tag_tracking

Definition at line 157 of file tag_tracking.c.

+ Collaboration diagram for tag_tracking:
Data Fields
struct FloatRMat body_to_cam Body to camera rotation.
struct FloatVect3 cam_pos Position of camera in body frame.
uint8_t id ID of detected tag.
struct FloatVect3 meas measured position
float timeout timeout for lost flag [sec]
bool updated updated state

Macro Definition Documentation

◆ TAG_TRACKING_BODY_TO_CAM_PHI

#define TAG_TRACKING_BODY_TO_CAM_PHI   0.f

Definition at line 82 of file tag_tracking.c.

◆ TAG_TRACKING_BODY_TO_CAM_PSI

#define TAG_TRACKING_BODY_TO_CAM_PSI   M_PI_2

Definition at line 90 of file tag_tracking.c.

◆ TAG_TRACKING_BODY_TO_CAM_THETA

#define TAG_TRACKING_BODY_TO_CAM_THETA   0.f

Definition at line 86 of file tag_tracking.c.

◆ TAG_TRACKING_CAM_POS_X

#define TAG_TRACKING_CAM_POS_X   0.f

Definition at line 94 of file tag_tracking.c.

◆ TAG_TRACKING_CAM_POS_Y

#define TAG_TRACKING_CAM_POS_Y   0.f

Definition at line 98 of file tag_tracking.c.

◆ TAG_TRACKING_CAM_POS_Z

#define TAG_TRACKING_CAM_POS_Z   0.f

Definition at line 102 of file tag_tracking.c.

◆ TAG_TRACKING_COORD_TO_M

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

Definition at line 106 of file tag_tracking.c.

◆ TAG_TRACKING_ID

#define TAG_TRACKING_ID   ABI_BROADCAST

Definition at line 176 of file tag_tracking.c.

◆ TAG_TRACKING_KP

#define TAG_TRACKING_KP   0.5f

Definition at line 138 of file tag_tracking.c.

◆ TAG_TRACKING_KPZ

#define TAG_TRACKING_KPZ   0.2f

Definition at line 142 of file tag_tracking.c.

◆ TAG_TRACKING_MAX_OFFSET

#define TAG_TRACKING_MAX_OFFSET   2.0f

Definition at line 134 of file tag_tracking.c.

◆ TAG_TRACKING_MAX_SPEED

#define TAG_TRACKING_MAX_SPEED   4.f

Definition at line 146 of file tag_tracking.c.

◆ TAG_TRACKING_MAX_VZ

#define TAG_TRACKING_MAX_VZ   2.f

Definition at line 150 of file tag_tracking.c.

◆ TAG_TRACKING_P0_POS

#define TAG_TRACKING_P0_POS   10.f

Definition at line 118 of file tag_tracking.c.

◆ TAG_TRACKING_P0_SPEED

#define TAG_TRACKING_P0_SPEED   10.f

Definition at line 122 of file tag_tracking.c.

◆ TAG_TRACKING_PREDICT_TIME

#define TAG_TRACKING_PREDICT_TIME   1.f

Definition at line 130 of file tag_tracking.c.

◆ TAG_TRACKING_Q_SIGMA2

#define TAG_TRACKING_Q_SIGMA2   1.f

Definition at line 114 of file tag_tracking.c.

◆ TAG_TRACKING_R

#define TAG_TRACKING_R   1.f

Definition at line 110 of file tag_tracking.c.

◆ TAG_TRACKING_TIMEOUT

#define TAG_TRACKING_TIMEOUT   5.f

Definition at line 126 of file tag_tracking.c.

Function Documentation

◆ tag_track_cb()

static void tag_track_cb ( uint8_t sender_id  UNUSED,
uint8_t  type,
char *  id,
uint8_t nb  UNUSED,
int16_t *  coord,
uint16_t *dim  UNUSED,
struct FloatQuat quat  UNUSED,
char *extra  UNUSED 
)
static

Definition at line 216 of file tag_tracking.c.

References tag_tracking::id, jevois_extract_nb(), JEVOIS_MSG_D3, tag_tracking::meas, tag_track_private, TAG_TRACKING_COORD_TO_M, tag_tracking::timeout, update_tag_position(), tag_tracking::updated, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by tag_tracking_init().

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

◆ tag_tracking_compute_speed()

void tag_tracking_compute_speed ( void  )

Control function.

calling this function only updates the command vector it can be applied to the guidance control using the guided mode or from the flight plan with 'guided' instruction

Definition at line 393 of file tag_tracking.c.

◆ tag_tracking_init()

void tag_tracking_init ( void  )

Definition at line 276 of file tag_tracking.c.

References tag_tracking::body_to_cam, tag_tracking::cam_pos, float_rmat_of_eulers, FLOAT_VECT3_ZERO, tag_tracking::meas, tag_track_cb(), tag_track_ev, tag_track_private, TAG_TRACKING_BODY_TO_CAM_PHI, TAG_TRACKING_BODY_TO_CAM_PSI, TAG_TRACKING_BODY_TO_CAM_THETA, TAG_TRACKING_CAM_POS_X, TAG_TRACKING_CAM_POS_Y, TAG_TRACKING_CAM_POS_Z, TAG_TRACKING_FIXED_POS, TAG_TRACKING_ID, TAG_TRACKING_KP, TAG_TRACKING_KPZ, TAG_TRACKING_PREDICT_TIME, TAG_TRACKING_SEARCHING, tag_tracking::timeout, tag_tracking::updated, and VECT3_ASSIGN.

+ Here is the call graph for this function:

◆ tag_tracking_parse_target_pos()

void tag_tracking_parse_target_pos ( uint8_t *  buf)

Definition at line 236 of file tag_tracking.c.

References tag_tracking::id, tag_tracking::meas, tag_track_private, TAG_TRACKING_COORD_TO_M, tag_tracking::timeout, update_tag_position(), tag_tracking::updated, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

+ Here is the call graph for this function:

◆ tag_tracking_propagate()

void tag_tracking_propagate ( void  )

Definition at line 308 of file tag_tracking.c.

References kalman, simple_kinematic_kalman_get_state(), simple_kinematic_kalman_predict(), SIMPLE_KINEMATIC_KALMAN_SPEED_3D, simple_kinematic_kalman_update_speed(), tag_track_dt, tag_track_private, TAG_TRACKING_FIXED_POS, TAG_TRACKING_LOST, tag_tracking_propagate_start(), TAG_TRACKING_RUNNING, TAG_TRACKING_SEARCHING, TAG_TRACKING_TIMEOUT, tag_tracking::timeout, and update_wp().

+ Here is the call graph for this function:

◆ tag_tracking_propagate_start()

void tag_tracking_propagate_start ( void  )

Definition at line 349 of file tag_tracking.c.

References kalman, simple_kinematic_kalman_init(), tag_track_dt, tag_track_private, TAG_TRACKING_P0_POS, TAG_TRACKING_P0_SPEED, TAG_TRACKING_Q_SIGMA2, TAG_TRACKING_R, TAG_TRACKING_SEARCHING, and tag_tracking::timeout.

Referenced by tag_tracking_propagate().

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

◆ tag_tracking_report()

void tag_tracking_report ( void  )

Definition at line 357 of file tag_tracking.c.

References DefaultChannel, DefaultDevice, ecef_of_ned_point_f(), tag_tracking::id, kalman, LlaCoor_f::lat, lla_of_ecef_f(), LlaCoor_f::lon, msg, State::ned_origin_f, SimpleKinematicKalman::state, state, tag_track_private, TAG_TRACKING_RUNNING, update_wp(), and tag_tracking::updated.

+ Here is the call graph for this function:

◆ update_tag_position()

static void update_tag_position ( void  )
static

Definition at line 182 of file tag_tracking.c.

References tag_tracking::body_to_cam, tag_tracking::cam_pos, float_rmat_transp_vmult(), FLOAT_VECT3_ZERO, kalman, tag_tracking::meas, simple_kinematic_kalman_get_state(), simple_kinematic_kalman_set_state(), simple_kinematic_kalman_update_pos(), stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), tag_track_private, TAG_TRACKING_DISABLE, TAG_TRACKING_RUNNING, and VECT3_ADD.

Referenced by tag_track_cb(), and tag_tracking_parse_target_pos().

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

◆ update_wp()

static void update_wp ( bool report  UNUSED)
static

Definition at line 252 of file tag_tracking.c.

References ENU_BFP_OF_REAL, ENU_OF_TO_NED, TAG_TRACKING_MAX_OFFSET, TAG_TRACKING_MOVING, VECT2_SMUL, VECT2_STRIM, VECT3_ADD, waypoint_move_enu_i(), and waypoint_set_enu_i().

Referenced by tag_tracking_propagate(), and tag_tracking_report().

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

Variable Documentation

◆ kalman

struct SimpleKinematicKalman kalman
static

Definition at line 154 of file tag_tracking.c.

Referenced by georeference_filter(), simple_kinematic_kalman_get_pos(), simple_kinematic_kalman_init(), simple_kinematic_kalman_predict(), simple_kinematic_kalman_set_state(), simple_kinematic_kalman_update(), simple_kinematic_kalman_update_noise(), simple_kinematic_kalman_update_pos(), simple_kinematic_kalman_update_speed(), tag_tracking_propagate(), tag_tracking_propagate_start(), tag_tracking_report(), and update_tag_position().

◆ tag_track_dt

const float tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD
static

Definition at line 154 of file tag_tracking.c.

Referenced by tag_tracking_propagate(), and tag_tracking_propagate_start().

◆ tag_track_ev

abi_event tag_track_ev
static

Definition at line 179 of file tag_tracking.c.

Referenced by tag_tracking_init().

◆ tag_track_private

struct tag_tracking tag_track_private
static

Definition at line 154 of file tag_tracking.c.

Referenced by tag_track_cb(), tag_tracking_init(), tag_tracking_parse_target_pos(), tag_tracking_propagate(), tag_tracking_propagate_start(), tag_tracking_report(), and update_tag_position().

◆ tag_tracking

struct tag_tracking_public tag_tracking

Definition at line 154 of file tag_tracking.c.