32 #include "generated/modules.h"
38 #include "generated/flight_plan.h"
39 #if !(defined TAG_TRACKING_WP) && (defined WP_TAG)
40 #define TAG_TRACKING_WP WP_TAG
44 #if !(defined TAG_TRACKING_SIM_WP) && (defined WP_TARGET)
45 #define TAG_TRACKING_SIM_WP WP_TARGET
48 #if defined SITL && defined TAG_TRACKING_SIM_WP
49 static void tag_tracking_sim(
void);
50 static void tag_motion_sim(
void);
57 #define TAG_MOTION_NONE 0
58 #define TAG_MOTION_LINE 1
59 #define TAG_MOTION_CIRCLE 2
61 #define TAG_MOTION_SPEED_X 0.25f
62 #define TAG_MOTION_SPEED_Y 0.f
63 #define TAG_MOTION_RANGE_X 4.f
64 #define TAG_MOTION_RANGE_Y 4.f
66 static uint8_t tag_motion_sim_type = TAG_MOTION_NONE;
67 static struct FloatVect3 tag_motion_speed = { TAG_MOTION_SPEED_X, TAG_MOTION_SPEED_Y, 0.f };
71 float time_circle_corrected;
72 float speed_circle = 0.03;
81 #ifndef TAG_TRACKING_BODY_TO_CAM_PHI
82 #define TAG_TRACKING_BODY_TO_CAM_PHI 0.f
85 #ifndef TAG_TRACKING_BODY_TO_CAM_THETA
86 #define TAG_TRACKING_BODY_TO_CAM_THETA 0.f
89 #ifndef TAG_TRACKING_BODY_TO_CAM_PSI
90 #define TAG_TRACKING_BODY_TO_CAM_PSI M_PI_2
93 #ifndef TAG_TRACKING_CAM_POS_X
94 #define TAG_TRACKING_CAM_POS_X 0.f
97 #ifndef TAG_TRACKING_CAM_POS_Y
98 #define TAG_TRACKING_CAM_POS_Y 0.f
101 #ifndef TAG_TRACKING_CAM_POS_Z
102 #define TAG_TRACKING_CAM_POS_Z 0.f
105 #ifndef TAG_TRACKING_COORD_TO_M
106 #define TAG_TRACKING_COORD_TO_M (1.f / 1000.f)
109 #ifndef TAG_TRACKING_R
110 #define TAG_TRACKING_R 1.f
113 #ifndef TAG_TRACKING_Q_SIGMA2
114 #define TAG_TRACKING_Q_SIGMA2 1.f
117 #ifndef TAG_TRACKING_P0_POS
118 #define TAG_TRACKING_P0_POS 10.f
121 #ifndef TAG_TRACKING_P0_SPEED
122 #define TAG_TRACKING_P0_SPEED 10.f
125 #ifndef TAG_TRACKING_TIMEOUT
126 #define TAG_TRACKING_TIMEOUT 5.f
129 #ifndef TAG_TRACKING_PREDICT_TIME
130 #define TAG_TRACKING_PREDICT_TIME 1.f
133 #ifndef TAG_TRACKING_MAX_OFFSET
134 #define TAG_TRACKING_MAX_OFFSET 2.0f
137 #ifndef TAG_TRACKING_KP
138 #define TAG_TRACKING_KP 0.5f
141 #ifndef TAG_TRACKING_KPZ
142 #define TAG_TRACKING_KPZ 0.2f
145 #ifndef TAG_TRACKING_MAX_SPEED
146 #define TAG_TRACKING_MAX_SPEED 4.f
149 #ifndef TAG_TRACKING_MAX_VZ
150 #define TAG_TRACKING_MAX_VZ 2.f
175 #ifndef TAG_TRACKING_ID
176 #define TAG_TRACKING_ID ABI_BROADCAST
184 struct FloatVect3 target_pos_ned, target_pos_body;
254 #ifdef TAG_TRACKING_WP
255 struct FloatVect3 target_pos_enu, target_pos_pred;
262 VECT3_ADD(target_pos_enu, target_pos_pred);
310 #if defined SITL && defined TAG_TRACKING_SIM_WP
311 if (tag_motion_sim_type != TAG_MOTION_NONE) {
359 #if TAG_TRACKING_DEBUG
378 float lat_deg = DegOfRad(tag_lla.
lat);
379 float lon_deg = DegOfRad(tag_lla.
lon);
411 #if defined SITL && defined TAG_TRACKING_SIM_WP
412 static void tag_tracking_sim(
void)
431 if (fabsf(target_cam.z) > 1.) {
434 if (fabsf(target_cam.x / target_cam.z) < 0.3f &&
435 fabsf(target_cam.y / target_cam.z) < 0.3f) {
439 (
int16_t) (target_cam.x * 1000.f),
440 (
int16_t) (target_cam.y * 1000.f),
441 (
int16_t) (target_cam.z * 1000.f)
446 AbiSendMsgJEVOIS_MSG(42,
JEVOIS_MSG_D3,
"1", 3, coord, dim, quat,
"");
451 static void tag_motion_sim(
void)
453 switch (tag_motion_sim_type) {
454 case TAG_MOTION_LINE:
457 struct FloatVect3 speed_dt = tag_motion_speed;
459 if (pos.
x < -TAG_MOTION_RANGE_X || pos.
x > TAG_MOTION_RANGE_X ||
460 pos.
y < -TAG_MOTION_RANGE_Y || pos.
y > TAG_MOTION_RANGE_Y) {
461 tag_motion_speed.
x = -tag_motion_speed.
x;
462 tag_motion_speed.
y = -tag_motion_speed.
y;
463 speed_dt.
x = -speed_dt.
x;
464 speed_dt.
y = -speed_dt.
y;
472 case TAG_MOTION_CIRCLE:
475 time_circle_corrected = time_circle * 0.02;
477 struct FloatVect3 speed_dt = tag_motion_speed;
479 tag_motion_speed.
x = speed_circle * cos(time_circle_corrected);
480 tag_motion_speed.
y = speed_circle * sin(time_circle_corrected);
481 speed_dt.
x = speed_circle * cos(time_circle_corrected);;
482 speed_dt.
y = speed_circle * sin(time_circle_corrected);
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
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_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define FLOAT_VECT3_ZERO(_v)
#define float_rmat_of_eulers
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
#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).
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
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
float state[SIMPLE_KINEMATIC_KALMAN_DIM]
state vector
API to get/set the generic vehicle states.
void tag_tracking_propagate_start()
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 FloatVect3 meas
measured position
#define TAG_TRACKING_MAX_VZ
#define TAG_TRACKING_CAM_POS_Z
#define TAG_TRACKING_CAM_POS_Y
#define TAG_TRACKING_BODY_TO_CAM_THETA
void tag_tracking_propagate()
#define TAG_TRACKING_PREDICT_TIME
#define TAG_TRACKING_Q_SIGMA2
#define TAG_TRACKING_BODY_TO_CAM_PSI
float timeout
timeout for lost flag [sec]
struct FloatVect3 cam_pos
Position of camera in body frame.
#define TAG_TRACKING_BODY_TO_CAM_PHI
bool updated
updated state
#define TAG_TRACKING_COORD_TO_M
static void update_wp(bool report UNUSED)
void tag_tracking_report()
void tag_tracking_parse_target_pos(uint8_t *buf)
static struct SimpleKinematicKalman kalman
static struct tag_tracking tag_track_private
static void update_tag_position(void)
#define TAG_TRACKING_P0_POS
#define TAG_TRACKING_MAX_SPEED
struct FloatRMat body_to_cam
Body to camera rotation.
#define TAG_TRACKING_CAM_POS_X
uint8_t id
ID of detected tag.
#define TAG_TRACKING_MAX_OFFSET
#define TAG_TRACKING_P0_SPEED
void tag_tracking_compute_speed(void)
Control function.
#define TAG_TRACKING_MOVING
#define TAG_TRACKING_DISABLE
#define TAG_TRACKING_RUNNING
#define TAG_TRACKING_FIXED_POS
#define TAG_TRACKING_SEARCHING
#define TAG_TRACKING_LOST
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.