32#include "generated/modules.h"
38#include "generated/flight_plan.h"
42#if !(defined TAG_TRACKING_SIM_WP) && (defined WP_TARGET)
43#define TAG_TRACKING_SIM_WP WP_TARGET
46#if defined SITL && defined TAG_TRACKING_SIM_WP
47static void tag_tracking_sim(
void);
48static void tag_motion_sim(
void);
55#define TAG_MOTION_NONE 0
56#define TAG_MOTION_LINE 1
57#define TAG_MOTION_CIRCLE 2
59#define TAG_MOTION_SPEED_X 0.25f
60#define TAG_MOTION_SPEED_Y 0.f
61#define TAG_MOTION_RANGE_X 4.f
62#define TAG_MOTION_RANGE_Y 4.f
64#ifndef TAG_TRACKING_SIM_ID
65#define TAG_TRACKING_SIM_ID "U1"
68static uint8_t tag_motion_sim_type = TAG_MOTION_NONE;
69static struct FloatVect3 tag_motion_speed = { TAG_MOTION_SPEED_X, TAG_MOTION_SPEED_Y, 0.f };
73float time_circle_corrected;
74float speed_circle = 0.03;
83#ifndef TAG_TRACKING_BODY_TO_CAM_PHI
84#define TAG_TRACKING_BODY_TO_CAM_PHI 0.f
87#ifndef TAG_TRACKING_BODY_TO_CAM_THETA
88#define TAG_TRACKING_BODY_TO_CAM_THETA 0.f
91#ifndef TAG_TRACKING_BODY_TO_CAM_PSI
92#define TAG_TRACKING_BODY_TO_CAM_PSI M_PI_2
95#ifndef TAG_TRACKING_CAM_POS_X
96#define TAG_TRACKING_CAM_POS_X 0.f
99#ifndef TAG_TRACKING_CAM_POS_Y
100#define TAG_TRACKING_CAM_POS_Y 0.f
103#ifndef TAG_TRACKING_CAM_POS_Z
104#define TAG_TRACKING_CAM_POS_Z 0.f
107#ifndef TAG_TRACKING_COORD_TO_M
108#define TAG_TRACKING_COORD_TO_M (1.f / 1000.f)
111#ifndef TAG_TRACKING_R
112#define TAG_TRACKING_R 1.f
115#ifndef TAG_TRACKING_Q_SIGMA2
116#define TAG_TRACKING_Q_SIGMA2 1.f
119#ifndef TAG_TRACKING_P0_POS
120#define TAG_TRACKING_P0_POS 10.f
123#ifndef TAG_TRACKING_P0_SPEED
124#define TAG_TRACKING_P0_SPEED 10.f
127#ifndef TAG_TRACKING_TIMEOUT
128#define TAG_TRACKING_TIMEOUT 5.f
131#ifndef TAG_TRACKING_PREDICT_TIME
132#define TAG_TRACKING_PREDICT_TIME 1.f
135#ifndef TAG_TRACKING_MAX_OFFSET
136#define TAG_TRACKING_MAX_OFFSET 2.0f
139#ifndef TAG_TRACKING_KP
140#define TAG_TRACKING_KP 0.5f
143#ifndef TAG_TRACKING_KPZ
144#define TAG_TRACKING_KPZ 0.2f
147#ifndef TAG_TRACKING_MAX_SPEED
148#define TAG_TRACKING_MAX_SPEED 4.f
151#ifndef TAG_TRACKING_MAX_VZ
152#define TAG_TRACKING_MAX_VZ 2.f
156#define TAG_UNUSED_ID -1
191#if (defined TAG_TRACKING_WPS)
207#ifndef TAG_TRACKING_ID
208#define TAG_TRACKING_ID ABI_BROADCAST
217 if(
tag_infos[i].tag_track_private.id == tag_id) {
246 return DegOfRad(e.
psi);
252 struct FloatVect3 target_pos_ned, target_pos_body;
298 tag_infos[i].tag_track_private.id = tag_id;
301 if (
tag_infos[i].tag_track_private.id == tag_id) {
313 tag_infos[i].tag_track_private.timeout = 0.f;
314 tag_infos[i].tag_track_private.updated =
true;
331 tag_infos[i].tag_track_private.id = DL_TARGET_POS_target_id(buf);
333 tag_infos[i].tag_track_private.timeout = 0.f;
334 tag_infos[i].tag_track_private.updated =
true;
346 struct FloatVect3 target_pos_enu, target_pos_pred;
353 VECT3_ADD(target_pos_enu, target_pos_pred);
395 tag_infos[i].tag_track_private.timeout = 0.f;
396 tag_infos[i].tag_track_private.updated =
false;
414#if defined SITL && defined TAG_TRACKING_SIM_WP
415 if (tag_motion_sim_type != TAG_MOTION_NONE) {
474#if TAG_TRACKING_DEBUG
493 float lat_deg = DegOfRad(tag_lla.
lat);
494 float lon_deg = DegOfRad(tag_lla.
lon);
498 tag_infos[i].tag_track_private.updated =
false;
532 tag_infos[i].tag_track_private.id = tag_id;
541#if defined SITL && defined TAG_TRACKING_SIM_WP
542static void tag_tracking_sim(
void)
561 if (fabsf(target_cam.z) > 1.) {
564 if (fabsf(target_cam.x / target_cam.z) < 0.3f &&
565 fabsf(target_cam.y / target_cam.z) < 0.3f) {
569 (
int16_t) (target_cam.x * 1000.f),
570 (
int16_t) (target_cam.y * 1000.f),
571 (
int16_t) (target_cam.z * 1000.f)
576 AbiSendMsgJEVOIS_MSG(42,
JEVOIS_MSG_D3, TAG_TRACKING_SIM_ID, 3, coord, dim, quat,
"");
581static void tag_motion_sim(
void)
583 switch (tag_motion_sim_type) {
584 case TAG_MOTION_LINE:
587 struct FloatVect3 speed_dt = tag_motion_speed;
589 if (pos.
x < -TAG_MOTION_RANGE_X || pos.
x > TAG_MOTION_RANGE_X ||
590 pos.
y < -TAG_MOTION_RANGE_Y || pos.
y > TAG_MOTION_RANGE_Y) {
591 tag_motion_speed.
x = -tag_motion_speed.
x;
592 tag_motion_speed.
y = -tag_motion_speed.
y;
593 speed_dt.
x = -speed_dt.
x;
594 speed_dt.
y = -speed_dt.
y;
602 case TAG_MOTION_CIRCLE:
605 time_circle_corrected = time_circle * 0.02;
607 struct FloatVect3 speed_dt = tag_motion_speed;
609 tag_motion_speed.
x = speed_circle * cos(time_circle_corrected);
610 tag_motion_speed.
y = speed_circle * sin(time_circle_corrected);
611 speed_dt.
x = speed_circle * cos(time_circle_corrected);;
612 speed_dt.
y = speed_circle * sin(time_circle_corrected);
649 tag_infos[i].tag_tracking.predict_time = predict_time;
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Common code for AP and FBW telemetry.
static void float_quat_normalize(struct FloatQuat *q)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define FLOAT_VECT3_ZERO(_v)
#define float_rmat_of_eulers
void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZYX'
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b, const struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
#define VECT2_ADD(_a, _b)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_STRIM(_v, _min, _max)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ADD(_a, _b)
#define ENU_OF_TO_NED(_po, _pi)
#define ENU_BFP_OF_REAL(_o, _i)
vector in East North Up coordinates
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
int jevois_extract_nb(char *in)
Extract a number from jevoid ID field.
void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos)
void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates Units: meters
void simple_kinematic_kalman_get_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 *pos, struct FloatVect3 *speed)
Get current state.
void simple_kinematic_kalman_predict(struct SimpleKinematicKalman *kalman)
propagate dynamic model
void simple_kinematic_kalman_set_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos, struct FloatVect3 speed)
Set initial state vector.
void simple_kinematic_kalman_update_pos(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos)
correction step for position
void simple_kinematic_kalman_init(struct SimpleKinematicKalman *kalman, float P0_pos, float P0_speed, float Q_sigma2, float r, float dt)
Init SimpleKinematicKalman internal struct.
void simple_kinematic_kalman_update_speed(struct SimpleKinematicKalman *kalman, struct FloatVect3 speed, uint8_t type)
correction step for speed
#define SIMPLE_KINEMATIC_KALMAN_SPEED_3D
API to get/set the generic vehicle states.
struct tag_tracking_public tag_tracking
void tag_tracking_propagate_start()
void tag_tracking_set_kpz(float kpz)
static void update_tag_position(struct tag_info *tag_info)
int tag_tracking_setting_id
static abi_event tag_track_ev
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 const float tag_track_dt
#define TAG_TRACKING_TIMEOUT
struct tag_tracking_public * tag_tracking_get(int16_t tag_id)
bool tag_tracking_set_tracker_id(int16_t tag_id, uint8_t wp_id)
struct FloatVect3 meas
measured position
void tag_tracking_set_kp(float kp)
#define TAG_TRACKING_MAX_VZ
#define TAG_TRACKING_CAM_POS_Z
#define TAG_TRACKING_CAM_POS_Y
float tag_tracking_predict_time
#define TAG_TRACKING_BODY_TO_CAM_THETA
uint8_t tag_tracking_get_motion_type(int16_t tag_id)
void tag_tracking_propagate()
#define TAG_TRACKING_PREDICT_TIME
#define TAG_TRACKING_Q_SIGMA2
float tag_tracking_motion_type
#define TAG_TRACKING_BODY_TO_CAM_PSI
float timeout
timeout for lost flag [sec]
void tag_tracking_set_setting_id(float id)
struct FloatVect3 cam_pos
Position of camera in body frame.
#define TAG_TRACKING_BODY_TO_CAM_PHI
bool updated
updated state
struct tag_tracking_public dummy
#define TAG_TRACKING_COORD_TO_M
void tag_tracking_report()
void tag_tracking_parse_target_pos(uint8_t *buf)
const uint8_t tag_tracking_wps_len
static void tag_tracking_propagate_start_tag(struct tag_info *tag_info)
static void update_wp(struct tag_info *tag_info UNUSED, bool report UNUSED)
struct FloatQuat body_to_cam_quat
Body to camera rotation in quaternion.
struct tag_tracking tag_track_private
void tag_tracking_set_predict_time(float predict_time)
#define TAG_TRACKING_P0_POS
struct tag_info tag_infos[TAG_TRACKING_NB_MAX]
#define TAG_TRACKING_MAX_SPEED
struct FloatRMat body_to_cam
Body to camera rotation.
struct SimpleKinematicKalman kalman
#define TAG_TRACKING_CAM_POS_X
uint8_t tag_tracking_get_status(int16_t tag_id)
#define TAG_TRACKING_MAX_OFFSET
int16_t id
ID of detected tag.
void tag_tracking_set_motion_type(float motion_type)
#define TAG_TRACKING_P0_SPEED
struct FloatQuat rot_x_quat
struct wp_tracking wp_track[]
void tag_tracking_compute_speed(void)
Control function.
float tag_tracking_get_heading(int16_t tag_id)
struct FloatQuat cam_to_tag_quat
measured quat
struct FloatQuat ned_to_tag_quat
estimated attitude in NED frame
#define TAG_TRACKING_MOVING
#define TAG_TRACKING_DISABLE
struct FloatVect3 pos
estimated position
#define TAG_TRACKING_RUNNING
uint8_t status
tracking status flag
#define TAG_TRACKING_FIXED_POS
float predict_time
prediction time for WP tag
#define TAG_TRACKING_NB_MAX
#define TAG_TRACKING_SEARCHING
struct FloatQuat body_to_tag_quat
estimated attitude in body frame
#define TAG_TRACKING_LOST
uint8_t motion_type
type of tag motion
struct FloatVect3 speed
estimated speed
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.