#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:

Data Structures
struct	tag_tracking

struct	tag_info

struct	wp_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_UNUSED_ID -1

#define	TAG_TRACKING_ID ABI_BROADCAST

Functions
static void	tag_tracking_propagate_start_tag (struct tag_info *tag_info)

struct tag_tracking_public *	tag_tracking_get (int16_t tag_id)

uint8_t	tag_tracking_get_status (int16_t tag_id)

uint8_t	tag_tracking_get_motion_type (int16_t tag_id)

float	tag_tracking_get_heading (int16_t tag_id)

static void	update_tag_position (struct tag_info *tag_info)

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 (struct tag_info *tag_info UNUSED, 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...

bool	tag_tracking_set_tracker_id (int16_t tag_id, uint8_t wp_id)

void	tag_tracking_set_motion_type (float motion_type)

void	tag_tracking_set_predict_time (float predict_time)

void	tag_tracking_set_kp (float kp)

void	tag_tracking_set_kpz (float kpz)

void	tag_tracking_set_setting_id (float id)

Variables
static const float	tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD

struct wp_tracking	wp_track [] = {}

const uint8_t	tag_tracking_wps_len = 0

struct tag_info	tag_infos [TAG_TRACKING_NB_MAX]

struct tag_tracking_public	dummy = {0}

struct FloatQuat	rot_x_quat

static abi_event	tag_track_ev

int	tag_tracking_setting_id

float	tag_tracking_motion_type

float	tag_tracking_predict_time

float	tag_tracking_kp

float	tag_tracking_kpz

Data Structure Documentation

◆ tag_tracking

struct tag_tracking

Definition at line 162 of file tag_tracking.c.

Collaboration diagram for tag_tracking:

Data Fields
struct FloatRMat	body_to_cam	Body to camera rotation.
struct FloatQuat	body_to_cam_quat	Body to camera rotation in quaternion.
struct FloatVect3	cam_pos	Position of camera in body frame.
struct FloatQuat	cam_to_tag_quat	measured quat
int16_t	id	ID of detected tag.
struct FloatVect3	meas	measured position
float	timeout	timeout for lost flag [sec]
bool	updated	updated state

◆ tag_info

struct tag_info

Definition at line 178 of file tag_tracking.c.

Collaboration diagram for tag_info:

Data Fields
struct SimpleKinematicKalman	kalman
struct tag_tracking	tag_track_private
struct tag_tracking_public	tag_tracking
uint8_t	wp_id

◆ wp_tracking

struct wp_tracking

Definition at line 185 of file tag_tracking.c.

Data Fields
int16_t	tag_id
uint8_t	wp_id

Macro Definition Documentation

◆ TAG_TRACKING_BODY_TO_CAM_PHI

#define TAG_TRACKING_BODY_TO_CAM_PHI 0.f

Definition at line 84 of file tag_tracking.c.

◆ TAG_TRACKING_BODY_TO_CAM_PSI

#define TAG_TRACKING_BODY_TO_CAM_PSI M_PI_2

Definition at line 92 of file tag_tracking.c.

◆ TAG_TRACKING_BODY_TO_CAM_THETA

#define TAG_TRACKING_BODY_TO_CAM_THETA 0.f

Definition at line 88 of file tag_tracking.c.

◆ TAG_TRACKING_CAM_POS_X

#define TAG_TRACKING_CAM_POS_X 0.f

Definition at line 96 of file tag_tracking.c.

◆ TAG_TRACKING_CAM_POS_Y

#define TAG_TRACKING_CAM_POS_Y 0.f

Definition at line 100 of file tag_tracking.c.

◆ TAG_TRACKING_CAM_POS_Z

#define TAG_TRACKING_CAM_POS_Z 0.f

Definition at line 104 of file tag_tracking.c.

◆ TAG_TRACKING_COORD_TO_M

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

Definition at line 108 of file tag_tracking.c.

◆ TAG_TRACKING_ID

#define TAG_TRACKING_ID ABI_BROADCAST

Definition at line 208 of file tag_tracking.c.

◆ TAG_TRACKING_KP

#define TAG_TRACKING_KP 0.5f

Definition at line 140 of file tag_tracking.c.

◆ TAG_TRACKING_KPZ

#define TAG_TRACKING_KPZ 0.2f

Definition at line 144 of file tag_tracking.c.

◆ TAG_TRACKING_MAX_OFFSET

#define TAG_TRACKING_MAX_OFFSET 2.0f

Definition at line 136 of file tag_tracking.c.

◆ TAG_TRACKING_MAX_SPEED

#define TAG_TRACKING_MAX_SPEED 4.f

Definition at line 148 of file tag_tracking.c.

◆ TAG_TRACKING_MAX_VZ

#define TAG_TRACKING_MAX_VZ 2.f

Definition at line 152 of file tag_tracking.c.

◆ TAG_TRACKING_P0_POS

#define TAG_TRACKING_P0_POS 10.f

Definition at line 120 of file tag_tracking.c.

◆ TAG_TRACKING_P0_SPEED

#define TAG_TRACKING_P0_SPEED 10.f

Definition at line 124 of file tag_tracking.c.

◆ TAG_TRACKING_PREDICT_TIME

#define TAG_TRACKING_PREDICT_TIME 1.f

Definition at line 132 of file tag_tracking.c.

◆ TAG_TRACKING_Q_SIGMA2

#define TAG_TRACKING_Q_SIGMA2 1.f

Definition at line 116 of file tag_tracking.c.

◆ TAG_TRACKING_R

#define TAG_TRACKING_R 1.f

Definition at line 112 of file tag_tracking.c.

◆ TAG_TRACKING_TIMEOUT

#define TAG_TRACKING_TIMEOUT 5.f

Definition at line 128 of file tag_tracking.c.

◆ TAG_UNUSED_ID

#define TAG_UNUSED_ID -1

Definition at line 156 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 288 of file tag_tracking.c.

References float_quat_comp(), float_quat_normalize(), tag_tracking::id, jevois_extract_nb(), JEVOIS_MSG_D3, tag_tracking::meas, rot_x_quat, tag_infos, tag_info::tag_track_private, TAG_TRACKING_COORD_TO_M, TAG_TRACKING_NB_MAX, TAG_UNUSED_ID, 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 509 of file tag_tracking.c.

References FLOAT_VECT3_ZERO, tag_tracking_public::kp, tag_tracking_public::kpz, tag_tracking_public::pos, tag_tracking_public::speed, tag_tracking_public::speed_cmd, stateGetPositionNed_f(), tag_infos, tag_info::tag_tracking, TAG_TRACKING_MAX_SPEED, TAG_TRACKING_MAX_VZ, TAG_TRACKING_NB_MAX, TAG_TRACKING_RUNNING, VECT2_STRIM, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.