Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
tag_tracking.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
29 #ifndef TAG_TRACKING_H
30 #define TAG_TRACKING_H
31 
32 #include "std.h"
35 
36 #ifndef TAG_TRACKING_NB_MAX
37 #define TAG_TRACKING_NB_MAX 5
38 #endif
39 
40 
41 #define TAG_TRACKING_ANY -2
42 
43 // Searching status
44 #define TAG_TRACKING_SEARCHING 0
45 #define TAG_TRACKING_RUNNING 1
46 #define TAG_TRACKING_LOST 2
47 #define TAG_TRACKING_DISABLE 3 // don't run kalman filter, update pos from measures
48 
49 // Type of tag motion
50 // If fixed, the speed correction is forced to zero
51 #define TAG_TRACKING_FIXED_POS 0
52 #define TAG_TRACKING_MOVING 1
53 
55  struct FloatVect3 pos;
56  struct FloatVect3 speed;
58  struct FloatQuat ned_to_tag_quat;
61  float predict_time;
62  struct NedCoor_f speed_cmd;
63  float kp;
64  float kpz;
65 };
66 
67 
68 // settings
69 extern int tag_tracking_setting_id;
70 extern float tag_tracking_motion_type;
71 extern float tag_tracking_predict_time;
72 extern float tag_tracking_kp;
73 extern float tag_tracking_kpz;
74 void tag_tracking_set_setting_id(float id);
75 void tag_tracking_set_motion_type(float motion_type);
76 void tag_tracking_set_predict_time(float predict_time);
77 void tag_tracking_set_kp(float kp);
78 void tag_tracking_set_kpz(float kpz);
79 
80 extern struct tag_tracking_public* tag_tracking_get(int16_t tag_id);
83 extern void tag_tracking_init(void);
84 extern void tag_tracking_propagate(void);
85 extern void tag_tracking_propagate_start(void);
86 extern void tag_tracking_report(void);
87 extern void tag_tracking_parse_target_pos(uint8_t *buf);
88 extern void tag_tracking_compute_speed(void);
89 extern bool tag_tracking_set_tracker_id(int16_t tag_id, uint8_t wp_id);
90 extern float tag_tracking_get_heading(int16_t tag_id);
91 
92 #endif // TAG_TRACKING_H
93 
Roation quaternion.
Paparazzi floating point algebra.
Paparazzi floating point math for geodetic calculations.
vector in North East Down coordinates Units: meters
void tag_tracking_set_kpz(float kpz)
Definition: tag_tracking.c:665
struct FloatQuat ned_to_tag_quat
estimated attitude in NED frame
Definition: tag_tracking.h:58
int tag_tracking_setting_id
Definition: tag_tracking.c:628
bool tag_tracking_set_tracker_id(int16_t tag_id, uint8_t wp_id)
Definition: tag_tracking.c:528
float kpz
vertical tracking command gain
Definition: tag_tracking.h:64
void tag_tracking_set_kp(float kp)
Definition: tag_tracking.c:655
struct tag_tracking_public * tag_tracking_get(int16_t tag_id)
Definition: tag_tracking.c:215
float tag_tracking_predict_time
Definition: tag_tracking.c:630
uint8_t tag_tracking_get_motion_type(int16_t tag_id)
Definition: tag_tracking.c:237
struct FloatVect3 pos
estimated position
Definition: tag_tracking.h:55
float tag_tracking_motion_type
Definition: tag_tracking.c:629
void tag_tracking_set_setting_id(float id)
Definition: tag_tracking.c:675
uint8_t status
tracking status flag
Definition: tag_tracking.h:59
void tag_tracking_propagate_start(void)
Definition: tag_tracking.c:461
void tag_tracking_parse_target_pos(uint8_t *buf)
Definition: tag_tracking.c:321
float predict_time
prediction time for WP tag
Definition: tag_tracking.h:61
void tag_tracking_set_predict_time(float predict_time)
Definition: tag_tracking.c:645
float tag_tracking_kpz
Definition: tag_tracking.c:632
struct NedCoor_f speed_cmd
speed command to track the tag position
Definition: tag_tracking.h:62
float tag_tracking_kp
Definition: tag_tracking.c:631
void tag_tracking_propagate(void)
Definition: tag_tracking.c:412
uint8_t tag_tracking_get_status(int16_t tag_id)
Definition: tag_tracking.c:232
struct FloatQuat body_to_tag_quat
estimated attitude in body frame
Definition: tag_tracking.h:57
void tag_tracking_set_motion_type(float motion_type)
Definition: tag_tracking.c:635
void tag_tracking_init(void)
Definition: tag_tracking.c:366
uint8_t motion_type
type of tag motion
Definition: tag_tracking.h:60
void tag_tracking_compute_speed(void)
Control function.
Definition: tag_tracking.c:509
float tag_tracking_get_heading(int16_t tag_id)
Definition: tag_tracking.c:242
struct FloatVect3 speed
estimated speed
Definition: tag_tracking.h:56
void tag_tracking_report(void)
Definition: tag_tracking.c:468
float kp
horizontal tracking command gain
Definition: tag_tracking.h:63
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98