Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
27 #include "modules/multi/ctc/ctc.h"
28 #include "subsystems/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
40 static 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 
45 static void send_ctc_control(struct transport_tx *trans, struct link_device *dev)
46 {
47  pprz_msg_send_CTC_CONTROL(trans, dev, AC_ID, &ctc_control.v_centroid_x, &ctc_control.v_centroid_y,
50 }
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
64 
65 #ifndef CTC_TIMEOUT
66 #define CTC_TIMEOUT 1500
67 #endif
68 
69 #ifndef CTC_OMEGA
70 #define CTC_OMEGA 0.25
71 #endif
72 
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 
88 float moving_target_px = 0;
89 float moving_target_py = 0;
90 float moving_target_vx = 0;
91 float moving_target_vy = 0;
92 
93 bool ctc_gogo = false;
94 bool ctc_first_time = true;
96 
97 void 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 
110 {
111  ctc_control.target_px = moving_target_px;
112  ctc_control.target_py = moving_target_py;
113  ctc_control.target_vx = moving_target_vx;
114  ctc_control.target_vy = moving_target_vy;
115 
117 
118  return true;
119 }
120 
122 {
123  ctc_control.target_px = waypoints[wp].x;
124  ctc_control.target_py = waypoints[wp].y;
125  ctc_control.target_vx = 0;
126  ctc_control.target_vy = 0;
127 
129 
130  return true;
131 }
132 
133 bool collective_tracking_point(float x, float y)
134 {
135  ctc_control.target_px = x;
136  ctc_control.target_py = y;
137  ctc_control.target_vx = 0;
138  ctc_control.target_vy = 0;
139 
141 
142  return true;
143 }
144 
146 {
147  struct FloatEulers *att = stateGetNedToBodyEulers_f();
148  struct EnuCoor_f *v = stateGetSpeedEnu_f();
149  struct EnuCoor_f *p = stateGetPositionEnu_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 
160  ctc_control.v_centroid_x = vx;
161  ctc_control.v_centroid_y = vy;
162  ctc_control.p_centroid_x = px;
163  ctc_control.p_centroid_y = py;
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 
174  uint32_t now = get_sys_time_msec();
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) {
184  uint32_t timeout = now - last_info[i];
185  if (timeout > ctc_control.timeout) {
186  tableNei[i][5] = ctc_control.timeout;
187  } else {
188  tableNei[i][5] = (uint16_t)timeout;
189  num_neighbors++;
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 
195  ctc_control.v_centroid_x += vx_nei;
196  ctc_control.v_centroid_y += vy_nei;
197  ctc_control.p_centroid_x += px_nei;
198  ctc_control.p_centroid_y += py_nei;
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) {
208  uint32_t timeout = now - last_info[i];
209  if (timeout > ctc_control.timeout) {
210  tableNei[i][5] = ctc_control.timeout;
211  } else {
212  tableNei[i][5] = (uint16_t)timeout;
213  num_neighbors++;
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 
219  ctc_control.v_centroid_x += vx_nei;
220  ctc_control.v_centroid_y += vy_nei;
221  ctc_control.p_centroid_x += px_nei;
222  ctc_control.p_centroid_y += py_nei;
223  }
224  }
225  }
226  }
227 
228  if (num_neighbors != 0) {
229  ctc_control.v_centroid_x /= (num_neighbors + 1);
230  ctc_control.v_centroid_y /= (num_neighbors + 1);
231  ctc_control.p_centroid_x /= (num_neighbors + 1);
232  ctc_control.p_centroid_y /= (num_neighbors + 1);
233 
234  float error_target_x = ctc_control.target_px - ctc_control.p_centroid_x;
235  float error_target_y = ctc_control.target_py - ctc_control.p_centroid_y;
236  float error_target_ref_x = ctc_control.target_px - ctc_control.ref_px;
237  float error_target_ref_y = ctc_control.target_py - ctc_control.ref_py;
238 
239  if(num_neighbors != 0){
240  ctc_control.v_centroid_x /= (num_neighbors + 1);
241  ctc_control.v_centroid_y /= (num_neighbors + 1);
242  ctc_control.p_centroid_x /= (num_neighbors + 1);
243  ctc_control.p_centroid_y /= (num_neighbors + 1);
244 
245  float error_target_x = ctc_control.target_px - ctc_control.p_centroid_x;
246  float error_target_y = ctc_control.target_py - ctc_control.p_centroid_y;
247  float error_target_ref_x = ctc_control.target_px - ctc_control.ref_px;
248  float error_target_ref_y = ctc_control.target_py - ctc_control.ref_py;
249 
250  float distance_target_ref = sqrtf(error_target_ref_x*error_target_ref_x + error_target_ref_y*error_target_ref_y);
251  if(distance_target_ref < 0.1)
252  distance_target_ref = 0.1;
253  float aux = (1-expf(-ctc_control.alpha*distance_target_ref)) / distance_target_ref;
254  float v_ref_x = ctc_control.target_vx + aux*(error_target_x);
255  float v_ref_y = ctc_control.target_vy + aux*(error_target_y);
256 
257  ctc_control.ref_px += v_ref_x*dt;
258  ctc_control.ref_py += v_ref_y*dt;
259 
260  float error_v_x = ctc_control.v_centroid_x - v_ref_x;
261  float error_v_y = ctc_control.v_centroid_y - v_ref_y;
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;
267  u_spa = ctc_control.omega*(1 + ctc_control.k2*(error_ref_x*vx + error_ref_y*vy));
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 
280  if (((now - last_transmision) > ctc_control.time_broad) && (autopilot_get_mode() == AP_MODE_AUTO2)) {
282  last_transmision = now;
283  }
284 }
285 
286 void 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;
297  pprzlink_msg_send_CTC_INFO_TO_NEI(&msg, &ctc_control.vx, &ctc_control.vy, &ctc_control.px, &ctc_control.py);
298  }
299 }
300 
301 void parse_ctc_RegTable(void)
302 {
303  uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(dl_buffer);
304  if (ac_id == AC_ID) {
305  uint8_t nei_id = DL_CTC_REG_TABLE_nei_id(dl_buffer);
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 
314 void parse_ctc_CleanTable(void)
315 {
316  uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(dl_buffer);
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
323  ctc_control.p_centroid_x = 0;
324  ctc_control.p_centroid_y = 0;
325  ctc_control.v_centroid_x = 0;
326  ctc_control.v_centroid_y = 0;
327  ctc_control.target_px = 0;
328  ctc_control.target_py = 0;
329  ctc_control.target_vx = 0;
330  ctc_control.target_vy = 0;
331  ctc_control.vx = 0;
332  ctc_control.vy = 0;
333  ctc_control.px = 0;
334  ctc_control.py = 0;
335  ctc_control.ref_px = 0;
336  ctc_control.ref_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 
342 void parse_ctc_NeiInfoTable(void)
343 {
344  int16_t sender_id = (int16_t)(SenderIdOfPprzMsg(dl_buffer));
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(dl_buffer) * 100);
349  tableNei[i][2] = (int16_t)(DL_CTC_INFO_TO_NEI_vy(dl_buffer) * 100);
350  tableNei[i][3] = (int16_t)(DL_CTC_INFO_TO_NEI_px(dl_buffer) * 100);
351  tableNei[i][4] = (int16_t)(DL_CTC_INFO_TO_NEI_py(dl_buffer) * 100);
352  break;
353  }
354 }
355 
356 void parse_ctc_TargetInfo(void)
357 {
358  moving_target_px = DL_CTC_INFO_FROM_TARGET_px(dl_buffer);
359  moving_target_py = DL_CTC_INFO_FROM_TARGET_py(dl_buffer);
360  moving_target_vx = DL_CTC_INFO_FROM_TARGET_vx(dl_buffer);
361  moving_target_vy = DL_CTC_INFO_FROM_TARGET_vy(dl_buffer);
362 }
float py
Definition: ctc.h:49
#define CTC_TIMEOUT
Definition: ctc.c:66
uint32_t before
Definition: ctc.c:84
unsigned short uint16_t
Definition: types.h:16
uint8_t ac_id
Definition: sim_ap.c:45
static float timeout
float k2
Definition: ctc.h:35
float k1
Definition: ctc.h:34
float x
Definition: common_nav.h:40
float omega
Definition: ctc.h:52
uint8_t lateral_mode
bool collective_tracking_vehicle()
Definition: ctc.c:109
float vy
Definition: ctc.h:47
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
float v_centroid_y
Definition: ctc.h:41
Periodic telemetry system header (includes downlink utility and generated code).
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
int16_t tableNei[CTC_MAX_AC][6]
Definition: ctc.c:81
vector in East North Up coordinates Units: meters
uint32_t starting_time
Definition: ctc.c:95
float moving_target_py
Definition: ctc.c:89
void ctc_init(void)
Definition: ctc.c:97
uint16_t timeout
Definition: ctc.h:37
bool collective_tracking_point(float x, float y)
Definition: ctc.c:133
float target_px
Definition: ctc.h:42
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define CTC_GAIN_ALPHA
Definition: ctc.c:62
#define CTC_GAIN_K2
Definition: ctc.c:59
uint32_t last_transmision
Definition: ctc.c:83
void ctc_send_info_to_nei(void)
euler angles
Fixed wing horizontal control.
float ref_py
Definition: ctc.h:51
ctc_con ctc_control
Definition: ctc.c:77
float theta
in radians
float y
Definition: common_nav.h:41
static void send_ctc(struct transport_tx *trans, struct link_device *dev)
Definition: ctc.c:40
#define AP_MODE_AUTO2
Definition: ctc.h:33
float p_centroid_y
Definition: ctc.h:39
float target_vy
Definition: ctc.h:45
float x
in meters
float p_centroid_x
Definition: ctc.h:38
Collective Tracking Control.
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
uint32_t last_info[CTC_MAX_AC]
Definition: ctc.c:82
float moving_target_px
Definition: ctc.c:88
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
float target_py
Definition: ctc.h:43
void parse_ctc_TargetInfo(void)
float h_ctl_roll_max_setpoint
float target_vx
Definition: ctc.h:44
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
Core autopilot interface common to all firmwares.
uint32_t time_init_table
Definition: ctc.c:86
float moving_target_vy
Definition: ctc.c:91
unsigned char uint8_t
Definition: types.h:14
#define LATERAL_MODE_ROLL
#define CTC_MAX_AC
Definition: ctc.c:36
float h_ctl_roll_setpoint
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float vx
Definition: ctc.h:46
#define CTC_GAIN_K1
Definition: ctc.c:56
void parse_ctc_CleanTable(void)
void parse_ctc_NeiInfoTable(void)
static void send_ctc_control(struct transport_tx *trans, struct link_device *dev)
Definition: ctc.c:45
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
void parse_ctc_RegTable(void)
static float p[2][2]
void collective_tracking_control()
Definition: ctc.c:145
bool ctc_gogo
Definition: ctc.c:93
uint16_t time_broad
Definition: ctc.h:53
bool collective_tracking_waypoint(uint8_t wp)
Definition: ctc.c:121
float v_centroid_x
Definition: ctc.h:40
float moving_target_vx
Definition: ctc.c:90
bool ctc_first_time
Definition: ctc.c:94
float px
Definition: ctc.h:48
float y
in meters
float alpha
Definition: ctc.h:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
float ref_px
Definition: ctc.h:50
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
#define CTC_TIME_BROAD
Definition: ctc.c:74
#define CTC_OMEGA
Definition: ctc.c:70