Paparazzi UAS  v7.0_unstable
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 "modules/core/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 / NAVIGATION_FREQUENCY;
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 - nav.heading;
112  FLOAT_ANGLE_NORMALIZE(diff);
113  BoundAbs(diff, object_tracking_rate * nav_dt)
114  nav.heading += diff;
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 
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
static uint8_t frame[20]
float psi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
#define OBJECT_TRACKING_SEARCH_RATE
static float timeout
void object_tracking_init(void)
init function
#define OBJECT_TRACKING_ID
float object_tracking_search_rate
max turn rate in search mode in rad/s
#define OBJECT_TRACKING_TIMEOUT
float object_tracking_rate
max turn rate in control mode in rad/s
#define OBJECT_TRACKING_RATE
static uint8_t object_frame
static float object_height
abi_event object_ev
void object_tracking_run(void)
run function
static const float nav_dt
static void get_object(uint8_t sender_id, uint32_t id, uint8_t frame, float bearing, float height, float distance)
static float object_bearing
struct RotorcraftNavigation nav
Definition: navigation.c:51
Rotorcraft navigation functions.
float heading
heading setpoint (in radians)
Definition: navigation.h:133
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition: navigation.h:156
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98