Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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


#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_KP   0.5f
#define TAG_TRACKING_KPZ   0.2f
#define TAG_TRACKING_MAX_VZ   2.f


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...


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



Definition at line 82 of file tag_tracking.c.



Definition at line 90 of file tag_tracking.c.



Definition at line 86 of file tag_tracking.c.


#define TAG_TRACKING_CAM_POS_X   0.f

Definition at line 94 of file tag_tracking.c.


#define TAG_TRACKING_CAM_POS_Y   0.f

Definition at line 98 of file tag_tracking.c.


#define TAG_TRACKING_CAM_POS_Z   0.f

Definition at line 102 of file tag_tracking.c.


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

Definition at line 106 of file tag_tracking.c.



Definition at line 176 of file tag_tracking.c.


#define TAG_TRACKING_KP   0.5f

Definition at line 138 of file tag_tracking.c.


#define TAG_TRACKING_KPZ   0.2f

Definition at line 142 of file tag_tracking.c.



Definition at line 134 of file tag_tracking.c.



Definition at line 146 of file tag_tracking.c.


#define TAG_TRACKING_MAX_VZ   2.f

Definition at line 150 of file tag_tracking.c.


#define TAG_TRACKING_P0_POS   10.f

Definition at line 118 of file tag_tracking.c.


#define TAG_TRACKING_P0_SPEED   10.f

Definition at line 122 of file tag_tracking.c.



Definition at line 130 of file tag_tracking.c.


#define TAG_TRACKING_Q_SIGMA2   1.f

Definition at line 114 of file tag_tracking.c.


#define TAG_TRACKING_R   1.f

Definition at line 110 of file tag_tracking.c.



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 

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()

◆ tag_tracking_parse_target_pos()

void tag_tracking_parse_target_pos ( uint8_t buf)

◆ tag_tracking_propagate()

◆ 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()

◆ update_tag_position()

◆ update_wp()

static void update_wp ( bool report  UNUSED)

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

◆ tag_track_dt

const float tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD

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

Definition at line 179 of file tag_tracking.c.

Referenced by tag_tracking_init().

◆ tag_track_private

◆ tag_tracking

Definition at line 154 of file tag_tracking.c.