Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
object_tracking.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 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  */
27 
29 #include "subsystems/abi.h"
30 #include "generated/airframe.h"
31 #include "generated/modules.h"
32 
33 // ABI message binding ID
34 #ifndef OBJECT_TRACKING_ID
35 #define OBJECT_TRACKING_ID ABI_BROADCAST
36 #endif
37 
38 // Timout in seconds before entering search mode
39 #ifndef OBJECT_TRACKING_TIMEOUT
40 #define OBJECT_TRACKING_TIMEOUT 3.0f
41 #endif
42 
43 // Turn rate in tracking mode (rad/s)
44 #ifndef OBJECT_TRACKING_RATE
45 #define OBJECT_TRACKING_RATE RadOfDeg(10)
46 #endif
47 
48 // Turn rate in search mode (rad/s)
49 #ifndef OBJECT_TRACKING_SEARCH_RATE
50 #define OBJECT_TRACKING_SEARCH_RATE RadOfDeg(20)
51 #endif
52 
53 // Send debug message
54 #ifndef OBJECT_TRACKING_DEBUG
55 #define OBJECT_TRACKING_DEBUG FALSE
56 #endif
57 
58 #if OBJECT_TRACKING_DEBUG
60 #include "pprzlink/messages.h"
61 #include "mcu_periph/uart.h"
62 #endif
63 
64 float object_tracking_rate; // in rad/s
65 float object_tracking_search_rate; // in rad/s
66 
67 static uint8_t object_frame; // relative or global coordinates
68 static float object_bearing;
69 static float object_height;
70 static float timeout;
71 
73 
74 static const float nav_dt = 1.f / NAV_FREQ;
75 
76 // callback on follow target message
77 static void get_object(uint8_t sender_id __attribute__((unused)),
78  uint32_t id __attribute__((unused)),
79  uint8_t frame, float bearing, float height,
80  float distance __attribute__((unused)))
81 {
83  object_bearing = bearing;
84  object_height = height;
85  timeout = 0.f;
86 }
87 
89 {
92 
93  object_frame = 0; // relative coordinates
94  object_bearing = 0.f;
95  object_height = 0.f;
96  timeout = OBJECT_TRACKING_TIMEOUT; // start in search mode
97 
98  // Bind to camera message
99  AbiBindMsgFOLLOW_TARGET(OBJECT_TRACKING_ID, &object_ev, get_object);
100 }
101 
103 {
105  timeout += nav_dt;
106  // compute expected heading
107  float target_heading = object_bearing;
108  if (!bit_is_set(object_frame, 0)) {
109  target_heading += stateGetNedToBodyEulers_f()->psi; // relative frame
110  }
111  float diff = target_heading - ANGLE_FLOAT_OF_BFP(nav_heading);
112  FLOAT_ANGLE_NORMALIZE(diff);
113  BoundAbs(diff, object_tracking_rate * nav_dt)
115 #if OBJECT_TRACKING_DEBUG
116  float msg[] = {
117  target_heading,
120  timeout
121  };
122  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 4, msg);
123 #endif
124  } else {
126  }
128 }
129 
object_tracking_rate
float object_tracking_rate
max turn rate in control mode in rad/s
Definition: object_tracking.c:64
object_tracking_search_rate
float object_tracking_search_rate
max turn rate in search mode in rad/s
Definition: object_tracking.c:65
ANGLE_FLOAT_OF_BFP
#define ANGLE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:211
FLOAT_ANGLE_NORMALIZE
#define FLOAT_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_float.h:99
abi.h
object_tracking.h
OBJECT_TRACKING_ID
#define OBJECT_TRACKING_ID
Definition: object_tracking.c:35
get_object
static void get_object(uint8_t sender_id, uint32_t id, uint8_t frame, float bearing, float height, float distance)
Definition: object_tracking.c:77
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
uint32_t
unsigned long uint32_t
Definition: types.h:18
object_ev
abi_event object_ev
Definition: object_tracking.c:72
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
timeout
static float timeout
Definition: object_tracking.c:70
NAV_FREQ
#define NAV_FREQ
Definition: navigation.h:43
msg
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
Definition: high_speed_logger_direct_memory.c:134
nav_dt
static const float nav_dt
Definition: object_tracking.c:74
frame
static uint8_t frame[20]
Definition: cc2500_frsky_x.c:163
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
object_tracking_init
void object_tracking_init(void)
init function
Definition: object_tracking.c:88
object_tracking_run
void object_tracking_run(void)
run function
Definition: object_tracking.c:102
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
uint8_t
unsigned char uint8_t
Definition: types.h:14
OBJECT_TRACKING_RATE
#define OBJECT_TRACKING_RATE
Definition: object_tracking.c:45
OBJECT_TRACKING_TIMEOUT
#define OBJECT_TRACKING_TIMEOUT
Definition: object_tracking.c:40
object_bearing
static float object_bearing
Definition: object_tracking.c:68
navigation.h
OBJECT_TRACKING_SEARCH_RATE
#define OBJECT_TRACKING_SEARCH_RATE
Definition: object_tracking.c:50
object_frame
static uint8_t object_frame
Definition: object_tracking.c:67
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
object_height
static float object_height
Definition: object_tracking.c:69
INT32_COURSE_NORMALIZE
#define INT32_COURSE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:131