![]() |
Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#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"
Go to the source code of this file.
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) |
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... | |
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 |
struct tag_tracking |
Definition at line 158 of file tag_tracking.c.
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 |
struct tag_info |
Definition at line 174 of file tag_tracking.c.
Data Fields | ||
---|---|---|
struct SimpleKinematicKalman | kalman | |
struct tag_tracking | tag_track_private | |
struct tag_tracking_public | tag_tracking | |
uint8_t | wp_id |
struct wp_tracking |
Definition at line 181 of file tag_tracking.c.
Data Fields | ||
---|---|---|
uint8_t | tag_id | |
uint8_t | wp_id |
#define TAG_TRACKING_BODY_TO_CAM_PHI 0.f |
Definition at line 80 of file tag_tracking.c.
#define TAG_TRACKING_BODY_TO_CAM_PSI M_PI_2 |
Definition at line 88 of file tag_tracking.c.
#define TAG_TRACKING_BODY_TO_CAM_THETA 0.f |
Definition at line 84 of file tag_tracking.c.
#define TAG_TRACKING_CAM_POS_X 0.f |
Definition at line 92 of file tag_tracking.c.
#define TAG_TRACKING_CAM_POS_Y 0.f |
Definition at line 96 of file tag_tracking.c.
#define TAG_TRACKING_CAM_POS_Z 0.f |
Definition at line 100 of file tag_tracking.c.
Definition at line 104 of file tag_tracking.c.
#define TAG_TRACKING_ID ABI_BROADCAST |
Definition at line 204 of file tag_tracking.c.
#define TAG_TRACKING_KP 0.5f |
Definition at line 136 of file tag_tracking.c.
#define TAG_TRACKING_KPZ 0.2f |
Definition at line 140 of file tag_tracking.c.
#define TAG_TRACKING_MAX_OFFSET 2.0f |
Definition at line 132 of file tag_tracking.c.
#define TAG_TRACKING_MAX_SPEED 4.f |
Definition at line 144 of file tag_tracking.c.
#define TAG_TRACKING_MAX_VZ 2.f |
Definition at line 148 of file tag_tracking.c.
#define TAG_TRACKING_P0_POS 10.f |
Definition at line 116 of file tag_tracking.c.
#define TAG_TRACKING_P0_SPEED 10.f |
Definition at line 120 of file tag_tracking.c.
#define TAG_TRACKING_PREDICT_TIME 1.f |
Definition at line 128 of file tag_tracking.c.
#define TAG_TRACKING_Q_SIGMA2 1.f |
Definition at line 112 of file tag_tracking.c.
#define TAG_TRACKING_R 1.f |
Definition at line 108 of file tag_tracking.c.
#define TAG_TRACKING_TIMEOUT 5.f |
Definition at line 124 of file tag_tracking.c.
#define TAG_UNUSED_ID -1 |
Definition at line 152 of file tag_tracking.c.
|
static |
Definition at line 274 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().
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 499 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.
struct tag_tracking_public* tag_tracking_get | ( | int16_t | tag_id | ) |
Definition at line 211 of file tag_tracking.c.
References dummy, float_quat_identity(), tag_tracking_public::ned_to_tag_quat, tag_tracking_public::status, tag_infos, tag_info::tag_tracking, TAG_TRACKING_ANY, TAG_TRACKING_NB_MAX, TAG_TRACKING_RUNNING, and TAG_TRACKING_SEARCHING.
Referenced by tag_tracking_get_heading().
float tag_tracking_get_heading | ( | int16_t | tag_id | ) |
Definition at line 228 of file tag_tracking.c.
References float_eulers_of_quat(), tag_tracking_public::ned_to_tag_quat, FloatEulers::psi, and tag_tracking_get().
void tag_tracking_init | ( | void | ) |
Definition at line 356 of file tag_tracking.c.
References float_quat_identity(), float_quat_of_eulers(), float_rmat_of_eulers, FLOAT_VECT3_ZERO, tag_tracking::id, tag_tracking_public::kp, tag_tracking_public::kpz, Min, tag_tracking_public::motion_type, tag_tracking_public::predict_time, rot_x_quat, tag_tracking_public::status, wp_tracking::tag_id, tag_infos, tag_track_cb(), tag_track_ev, tag_info::tag_track_private, tag_info::tag_tracking, TAG_TRACKING_BODY_TO_CAM_PHI, TAG_TRACKING_BODY_TO_CAM_PSI, TAG_TRACKING_BODY_TO_CAM_THETA, TAG_TRACKING_CAM_POS_X, TAG_TRACKING_CAM_POS_Y, TAG_TRACKING_CAM_POS_Z, TAG_TRACKING_FIXED_POS, TAG_TRACKING_ID, TAG_TRACKING_KP, TAG_TRACKING_KPZ, TAG_TRACKING_NB_MAX, TAG_TRACKING_PREDICT_TIME, TAG_TRACKING_SEARCHING, tag_tracking_wps_len, TAG_UNUSED_ID, tag_tracking::timeout, tag_tracking::updated, VECT3_ASSIGN, tag_info::wp_id, wp_tracking::wp_id, and wp_track.
void tag_tracking_parse_target_pos | ( | uint8_t * | buf | ) |
Definition at line 307 of file tag_tracking.c.
References tag_tracking::id, tag_tracking::meas, tag_infos, tag_info::tag_track_private, TAG_TRACKING_COORD_TO_M, TAG_TRACKING_NB_MAX, tag_tracking::timeout, update_tag_position(), tag_tracking::updated, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
void tag_tracking_propagate | ( | void | ) |
Definition at line 402 of file tag_tracking.c.
References simple_kinematic_kalman_get_state(), simple_kinematic_kalman_predict(), SIMPLE_KINEMATIC_KALMAN_SPEED_3D, simple_kinematic_kalman_update_speed(), tag_tracking_public::speed, tag_tracking_public::status, tag_infos, tag_track_dt, tag_info::tag_track_private, tag_info::tag_tracking, TAG_TRACKING_FIXED_POS, TAG_TRACKING_LOST, TAG_TRACKING_NB_MAX, tag_tracking_propagate_start_tag(), TAG_TRACKING_RUNNING, TAG_TRACKING_SEARCHING, TAG_TRACKING_TIMEOUT, tag_tracking::timeout, and update_wp().
void tag_tracking_propagate_start | ( | void | ) |
Definition at line 451 of file tag_tracking.c.
References tag_infos, TAG_TRACKING_NB_MAX, and tag_tracking_propagate_start_tag().
|
static |
Definition at line 444 of file tag_tracking.c.
References tag_info::kalman, simple_kinematic_kalman_init(), tag_tracking_public::status, tag_track_dt, tag_info::tag_track_private, tag_info::tag_tracking, 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(), and tag_tracking_propagate_start().
void tag_tracking_report | ( | void | ) |
Definition at line 458 of file tag_tracking.c.
References DefaultChannel, DefaultDevice, ecef_of_ned_point_f(), tag_info::kalman, LlaCoor_f::lat, lla_of_ecef_f(), LlaCoor_f::lon, msg, State::ned_origin_f, tag_tracking_public::pos, SimpleKinematicKalman::state, state, tag_infos, tag_info::tag_track_private, tag_info::tag_tracking, TAG_TRACKING_NB_MAX, TAG_TRACKING_RUNNING, TAG_UNUSED_ID, update_wp(), and tag_tracking::updated.
void tag_tracking_set_kp | ( | float | kp | ) |
Definition at line 633 of file tag_tracking.c.
References tag_tracking_public::kp, tag_infos, tag_info::tag_tracking, tag_tracking_kp, TAG_TRACKING_NB_MAX, tag_tracking_setting_id, and TAG_UNUSED_ID.
void tag_tracking_set_kpz | ( | float | kpz | ) |
Definition at line 643 of file tag_tracking.c.
References tag_tracking_public::kpz, tag_infos, tag_info::tag_tracking, tag_tracking_kpz, TAG_TRACKING_NB_MAX, tag_tracking_setting_id, and TAG_UNUSED_ID.
void tag_tracking_set_motion_type | ( | float | motion_type | ) |
Definition at line 613 of file tag_tracking.c.
References tag_tracking_public::motion_type, tag_infos, tag_info::tag_tracking, tag_tracking_motion_type, TAG_TRACKING_NB_MAX, tag_tracking_setting_id, and TAG_UNUSED_ID.
void tag_tracking_set_predict_time | ( | float | predict_time | ) |
Definition at line 623 of file tag_tracking.c.
References tag_tracking_public::predict_time, tag_infos, tag_info::tag_tracking, TAG_TRACKING_NB_MAX, tag_tracking_predict_time, tag_tracking_setting_id, and TAG_UNUSED_ID.
void tag_tracking_set_setting_id | ( | float | id | ) |
Definition at line 653 of file tag_tracking.c.
|
static |
Definition at line 236 of file tag_tracking.c.
References tag_tracking::body_to_cam, tag_tracking::body_to_cam_quat, tag_tracking_public::body_to_tag_quat, tag_tracking::cam_pos, tag_tracking::cam_to_tag_quat, float_quat_comp(), float_rmat_transp_vmult(), FLOAT_VECT3_ZERO, tag_info::kalman, tag_tracking::meas, tag_tracking_public::ned_to_tag_quat, tag_tracking_public::pos, simple_kinematic_kalman_get_state(), simple_kinematic_kalman_set_state(), simple_kinematic_kalman_update_pos(), tag_tracking_public::speed, stateGetNedToBodyQuat_f(), stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), tag_tracking_public::status, tag_infos, tag_info::tag_track_private, tag_info::tag_tracking, TAG_TRACKING_DISABLE, TAG_TRACKING_RUNNING, and VECT3_ADD.
Referenced by tag_track_cb(), and tag_tracking_parse_target_pos().
Definition at line 325 of file tag_tracking.c.
References ENU_BFP_OF_REAL, ENU_OF_TO_NED, tag_tracking_public::motion_type, tag_tracking_public::pos, tag_tracking_public::predict_time, tag_tracking_public::speed, tag_info::tag_tracking, TAG_TRACKING_MAX_OFFSET, TAG_TRACKING_MOVING, VECT2_SMUL, VECT2_STRIM, VECT3_ADD, waypoint_move_enu_i(), waypoint_set_enu_i(), and tag_info::wp_id.
Referenced by tag_tracking_propagate(), and tag_tracking_report().
struct tag_tracking_public dummy = {0} |
Definition at line 192 of file tag_tracking.c.
Referenced by send_rate(), stmi2c_clear_pending_interrupts(), and tag_tracking_get().
struct FloatQuat rot_x_quat |
Definition at line 192 of file tag_tracking.c.
Referenced by tag_track_cb(), and tag_tracking_init().
struct tag_info tag_infos[TAG_TRACKING_NB_MAX] |
Definition at line 192 of file tag_tracking.c.
Referenced by tag_track_cb(), tag_tracking_compute_speed(), tag_tracking_get(), tag_tracking_init(), tag_tracking_parse_target_pos(), tag_tracking_propagate(), tag_tracking_propagate_start(), tag_tracking_report(), tag_tracking_set_kp(), tag_tracking_set_kpz(), tag_tracking_set_motion_type(), tag_tracking_set_predict_time(), and update_tag_position().
|
static |
Definition at line 155 of file tag_tracking.c.
Referenced by tag_tracking_propagate(), and tag_tracking_propagate_start_tag().
|
static |
Definition at line 207 of file tag_tracking.c.
Referenced by tag_tracking_init().
float tag_tracking_kp |
Definition at line 609 of file tag_tracking.c.
Referenced by tag_tracking_set_kp().
float tag_tracking_kpz |
Definition at line 610 of file tag_tracking.c.
Referenced by tag_tracking_set_kpz().
float tag_tracking_motion_type |
Definition at line 607 of file tag_tracking.c.
Referenced by tag_tracking_set_motion_type().
float tag_tracking_predict_time |
Definition at line 608 of file tag_tracking.c.
Referenced by tag_tracking_set_predict_time().
int tag_tracking_setting_id |
Definition at line 606 of file tag_tracking.c.
Referenced by tag_tracking_set_kp(), tag_tracking_set_kpz(), tag_tracking_set_motion_type(), and tag_tracking_set_predict_time().
const uint8_t tag_tracking_wps_len = 0 |
Definition at line 192 of file tag_tracking.c.
Referenced by tag_tracking_init().
struct wp_tracking wp_track[] = {} |
Definition at line 155 of file tag_tracking.c.
Referenced by tag_tracking_init().