Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ctc.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2017 Hector Garcia de Marina
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, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 *
21 */
22
23#include <math.h>
24#include <std.h>
25#include <stdio.h>
26
28//#include "modules/datalink/datalink.h" // dl_buffer
32#include "autopilot.h"
33
35#ifndef CTC_MAX_AC
36#define CTC_MAX_AC 4
37#endif
38
39#if PERIODIC_TELEMETRY
40static void send_ctc(struct transport_tx *trans, struct link_device *dev)
41{
42 pprz_msg_send_CTC(trans, dev, AC_ID, 6 * CTC_MAX_AC, &(tableNei[0][0]));
43}
44
51#endif // PERIODIC TELEMETRY
52
53// Control
55#ifndef CTC_GAIN_K1
56#define CTC_GAIN_K1 0.001
57#endif
58#ifndef CTC_GAIN_K2
59#define CTC_GAIN_K2 0.001
60#endif
61#ifndef CTC_GAIN_ALPHA
62#define CTC_GAIN_ALPHA 0.01
63#endif
65#ifndef CTC_TIMEOUT
66#define CTC_TIMEOUT 1500
67#endif
69#ifndef CTC_OMEGA
70#define CTC_OMEGA 0.25
71#endif
73#ifndef CTC_TIME_BROAD
74#define CTC_TIME_BROAD 100
75#endif
76
78 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, CTC_OMEGA, CTC_TIME_BROAD
79 };
80
85
87
92
93bool ctc_gogo = false;
94bool ctc_first_time = true;
96
97void ctc_init(void)
98{
99 for (int i = 0; i < CTC_MAX_AC; i++) {
100 tableNei[i][0] = -1;
101 }
102
103#if PERIODIC_TELEMETRY
106#endif
107}
108
120
132
133bool collective_tracking_point(float x, float y)
134{
139
141
142 return true;
143}
144
146{
148 struct EnuCoor_f *v = stateGetSpeedEnu_f();
150 float vx = v->x;
151 float vy = v->y;
152 float px = p->x;
153 float py = p->y;
154
155 ctc_control.vx = vx;
156 ctc_control.vy = vy;
157 ctc_control.px = px;
158 ctc_control.py = py;
159
164
165
166 float u_vel = 0;
167 float u_spa = 0;
168
169 if (ctc_first_time) {
171 ctc_first_time = false;
172 }
173
175 float dt = (now - before) / 1000.0;
176 before = now;
177
178 int num_neighbors = 0;
179
180 int num_neighbors = 0;
181
182 for (int i = 0; i < CTC_MAX_AC; i++) {
183 if (tableNei[i][0] != -1) {
187 } else {
188 tableNei[i][5] = (uint16_t)timeout;
190 float vx_nei = tableNei[i][1] / 100.0;
191 float vy_nei = tableNei[i][2] / 100.0;
192 float px_nei = tableNei[i][3] / 100.0;
193 float py_nei = tableNei[i][4] / 100.0;
194
199 }
200 }
201 }
202 else{
203
204 int num_neighbors = 0;
205
206 for (int i = 0; i < CTC_MAX_AC; i++) {
207 if (tableNei[i][0] != -1) {
211 } else {
212 tableNei[i][5] = (uint16_t)timeout;
214 float vx_nei = tableNei[i][1] / 100.0;
215 float vy_nei = tableNei[i][2] / 100.0;
216 float px_nei = tableNei[i][3] / 100.0;
217 float py_nei = tableNei[i][4] / 100.0;
218
223 }
224 }
225 }
226 }
227
228 if (num_neighbors != 0) {
233
238
239 if(num_neighbors != 0){
244
249
251 if(distance_target_ref < 0.1)
256
259
262 u_vel = -ctc_control.k1*(-error_v_x*vy + error_v_y*vx);
263
264
265 float error_ref_x = px - ctc_control.ref_px;
266 float error_ref_y = py - ctc_control.ref_py;
268 }
269
270 float u = u_vel + u_spa;
271
274 -atanf(u * (sqrtf(vx * vx + vy * vy)) / 9.8 / cosf(att->theta));
276
278 }
279
283 }
284}
285
286void ctc_send_info_to_nei(void)
287{
288 struct pprzlink_msg msg;
289
290 for (int i = 0; i < CTC_MAX_AC; i++)
291 if (tableNei[i][0] != -1) {
292 msg.trans = &(DefaultChannel).trans_tx;
293 msg.dev = &(DefaultDevice).device;
294 msg.sender_id = AC_ID;
295 msg.receiver_id = tableNei[i][0];
296 msg.component_id = 0;
298 }
299}
300
302{
303 uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(buf);
304 if (ac_id == AC_ID) {
306 for (int i = 0; i < CTC_MAX_AC; i++)
307 if (tableNei[i][0] == -1) {
308 tableNei[i][0] = (int16_t)nei_id;
309 return;
310 }
311 }
312}
313
315{
317 if (ac_id == AC_ID)
318 for (int i = 0; i < CTC_MAX_AC; i++) {
319 tableNei[i][0] = -1;
320 }
321
322 // Reset the control variables as well
331 ctc_control.vx = 0;
332 ctc_control.vy = 0;
333 ctc_control.px = 0;
334 ctc_control.py = 0;
337
338 // We force again 2 seconds of waiting before the algorithm starts, so all the aircraft have transmitted the necessary information to their neighbors
339 ctc_gogo = false;
340}
341
343{
345 for (int i = 0; i < CTC_MAX_AC; i++)
346 if (tableNei[i][0] == sender_id) {
348 tableNei[i][1] = (int16_t)(DL_CTC_INFO_TO_NEI_vx(buf) * 100);
349 tableNei[i][2] = (int16_t)(DL_CTC_INFO_TO_NEI_vy(buf) * 100);
350 tableNei[i][3] = (int16_t)(DL_CTC_INFO_TO_NEI_px(buf) * 100);
351 tableNei[i][4] = (int16_t)(DL_CTC_INFO_TO_NEI_py(buf) * 100);
352 break;
353 }
354}
355
357{
362}
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition autopilot.c:222
Core autopilot interface common to all firmwares.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
float y
Definition common_nav.h:41
float x
Definition common_nav.h:40
bool collective_tracking_vehicle()
Definition ctc.c:109
float moving_target_vy
Definition ctc.c:91
static void send_ctc_control(struct transport_tx *trans, struct link_device *dev)
Definition ctc.c:45
uint32_t last_transmision
Definition ctc.c:83
#define CTC_TIME_BROAD
Definition ctc.c:74
uint32_t time_init_table
Definition ctc.c:86
bool ctc_first_time
Definition ctc.c:94
void ctc_init(void)
Definition ctc.c:97
bool ctc_gogo
Definition ctc.c:93
void collective_tracking_control()
Definition ctc.c:145
bool collective_tracking_waypoint(uint8_t wp)
Definition ctc.c:121
#define CTC_GAIN_ALPHA
Definition ctc.c:62
float moving_target_py
Definition ctc.c:89
float moving_target_vx
Definition ctc.c:90
static void send_ctc(struct transport_tx *trans, struct link_device *dev)
Definition ctc.c:40
ctc_con ctc_control
Definition ctc.c:77
#define CTC_OMEGA
Definition ctc.c:70
uint32_t starting_time
Definition ctc.c:95
float moving_target_px
Definition ctc.c:88
#define CTC_GAIN_K1
Definition ctc.c:56
#define CTC_TIMEOUT
Definition ctc.c:66
uint32_t before
Definition ctc.c:84
#define CTC_GAIN_K2
Definition ctc.c:59
#define CTC_MAX_AC
Definition ctc.c:36
uint32_t last_info[CTC_MAX_AC]
Definition ctc.c:82
int16_t tableNei[CTC_MAX_AC][6]
Definition ctc.c:81
bool collective_tracking_point(float x, float y)
Definition ctc.c:133
Collective Tracking Control.
float v_centroid_x
Definition ctc.h:40
float ref_px
Definition ctc.h:50
float target_py
Definition ctc.h:43
float p_centroid_x
Definition ctc.h:38
float v_centroid_y
Definition ctc.h:41
float target_vx
Definition ctc.h:44
void parse_ctc_NeiInfoTable(uint8_t *buf)
void parse_ctc_TargetInfo(uint8_t *buf)
float k1
Definition ctc.h:34
float vx
Definition ctc.h:46
float k2
Definition ctc.h:35
float omega
Definition ctc.h:52
float alpha
Definition ctc.h:36
void ctc_send_info_to_nei(void)
uint16_t timeout
Definition ctc.h:37
void parse_ctc_RegTable(uint8_t *buf)
float ref_py
Definition ctc.h:51
uint16_t time_broad
Definition ctc.h:53
float p_centroid_y
Definition ctc.h:39
float target_vy
Definition ctc.h:45
float py
Definition ctc.h:49
float px
Definition ctc.h:48
float target_px
Definition ctc.h:42
void parse_ctc_CleanTable(uint8_t *buf)
float vy
Definition ctc.h:47
Definition ctc.h:33
uint8_t lateral_mode
#define LATERAL_MODE_ROLL
#define AP_MODE_AUTO2
Fixed wing horizontal control.
float theta
in radians
euler angles
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1306
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1058
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
static float timeout
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
float h_ctl_roll_max_setpoint
float h_ctl_roll_setpoint
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.