Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_fish.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Adjiri Adam <adam.adjiri@etu.isae-ensma.fr>
3  * Gautier Hattenberger <gautier.hattenberger@enac.fr>
4  *
5  * This file is part of paparazzi
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
36 #include "modules/nav/nav_fish.h"
40 #include "state.h"
41 #include "autopilot.h"
42 #include "autopilot_guided.h"
43 #include "modules/nav/waypoints.h"
45 #include "generated/flight_plan.h"
46 #include <math.h>
48 
49 #include <stdio.h>
50 #include <stdlib.h>
51 
56 #ifndef NAV_FISH_BODY_LENGTH
57 #define NAV_FISH_BODY_LENGTH 0.5f
58 #endif
59 
60 #ifndef NAV_FISH_MAXVELOCITY
61 #define NAV_FISH_MAXVELOCITY 0.5f
62 #endif
63 
64 #ifndef NAV_FISH_MINVELOCITY
65 #define NAV_FISH_MINVELOCITY 0.1f
66 #endif
67 
68 #ifndef NAV_FISH_MIND2D
69 #define NAV_FISH_MIND2D 1.f
70 #endif
71 
72 #ifndef NAV_FISH_FLUCT
73 #define NAV_FISH_FLUCT 0.1f
74 #endif
75 
76 #ifndef NAV_FISH_EW1
77 #define NAV_FISH_EW1 0.7f
78 #endif
79 
80 #ifndef NAV_FISH_EW2
81 #define NAV_FISH_EW2 0.f
82 #endif
83 
84 #ifndef NAV_FISH_ALPHA
85 #define NAV_FISH_ALPHA 0.6f
86 #endif
87 
88 #ifndef NAV_FISH_YW
89 #define NAV_FISH_YW 0.8f
90 #endif
91 
92 #ifndef NAV_FISH_LW
93 #define NAV_FISH_LW (2.5f*NAV_FISH_BODY_LENGTH)
94 #endif
95 
96 #ifndef NAV_FISH_ALPHA_REP
97 #define NAV_FISH_ALPHA_REP 1.f
98 #endif
99 
100 #ifndef NAV_FISH_YATT
101 #define NAV_FISH_YATT 0.4f
102 #endif
103 
104 #ifndef NAV_FISH_LATT
105 #define NAV_FISH_LATT 3.f
106 #endif
107 
108 #ifndef NAV_FISH_D0ATT
109 #define NAV_FISH_D0ATT 1.5f
110 #endif
111 
112 #ifndef NAV_FISH_YALI
113 #define NAV_FISH_YALI 0.15f
114 #endif
115 
116 #ifndef NAV_FISH_LALI
117 #define NAV_FISH_LALI 3.f
118 #endif
119 
120 #ifndef NAV_FISH_D0ALI
121 #define NAV_FISH_D0ALI 1.f
122 #endif
123 
124 #ifndef NAV_FISH_ALT
125 #define NAV_FISH_ALT 2.0f
126 #endif
127 
128 #ifndef NAV_FISH_WALL_DISTANCE
129 #define NAV_FISH_WALL_DISTANCE 10.f
130 #endif
131 
132 #ifndef NAV_FISH_STRATEGY
133 #define NAV_FISH_STRATEGY 0
134 #endif
135 
136 #ifndef NAV_FISH_TRYATT
137 #define NAV_FISH_TRYATT 0.2f
138 #endif
139 
140 #ifndef NAV_FISH_TRLATT
141 #define NAV_FISH_TRLATT 3.f
142 #endif
143 
144 #ifndef NAV_FISH_TRYALI
145 #define NAV_FISH_TRYALI 0.25f
146 #endif
147 
148 #ifndef NAV_FISH_TRLALI
149 #define NAV_FISH_TRLALI 2.f
150 #endif
151 
154 // shortname to params
155 #define nfp nav_fish_params
156 
157 //state data
158 struct NavFish {
159  float heading;
160  float step_size;
161  float r_w;
162  float f_w;
163  float theta_w;
164  float f_fluct;
165  float f_wall;
166  float f_ali;
167  float f_att;
168  float velocity;
169 };
170 
171 struct NavFish nav_fish;
172 
175 void nav_fish_init(void)
176 {
199 
200  nav_fish.heading = 0.f;
202  nav_fish.r_w = 0.f;
203  nav_fish.f_w = 0.f;
204  nav_fish.theta_w = 0.f;
205  nav_fish.f_fluct = 0.f;
206  nav_fish.f_wall = 0.f;
207  nav_fish.f_ali = 0.f;
208  nav_fish.f_att = 0.f;
209  nav_fish.velocity = 0.5f;
210 }
211 
212 
213 static inline void send_swarm_message(void)
214 {
215  DOWNLINK_SEND_SWARM_FISH(DefaultChannel, DefaultDevice,
216  &nav_fish.heading,
218  &nav_fish.r_w,
219  &nav_fish.f_w,
220  &nav_fish.theta_w,
221  &nav_fish.f_fluct,
222  &nav_fish.f_wall,
223  &nav_fish.f_ali,
224  &nav_fish.f_att);
225 }
226 
232 static float sign(float x)
233 {
234  if(x >= 0.f) {
235  return 1.f;
236  } else {
237  return -1.f;
238  }
239 }
240 
244 static float normal_random_gen(void)
245 {
246  float random1 = ((float)rand()) / ((float)(RAND_MAX));
247  float random2 = ((float)rand()) / ((float)(RAND_MAX));
248  return cosf(2.f*M_PI * random1) * sqrtf(-2.f*logf(random2));
249 }
250 
256 static float distance_to_wall(struct EnuCoor_f *pos)
257 {
258  float x_home = waypoint_get_x(WP_HOME);
259  float y_home = waypoint_get_y(WP_HOME);
260  float dist = distance_wall - sqrtf(((pos->x - x_home) * (pos->x - x_home)) + ((pos->y - y_home) * (pos->y - y_home)));
261  if (dist < 0.f) {
262  return 0.f;
263  }
264  return dist;
265 }
266 
273 static float angle_to_wall(struct EnuCoor_f *pos, float psi)
274 {
275  float dir = M_PI_2 - psi;
276  float theta = atan2f(pos->y, pos->x);
277  float delta = dir - theta;
278  FLOAT_ANGLE_NORMALIZE(delta);
279  return delta;
280 }
281 
288 static float distance_drone_to_drone(struct EnuCoor_f *pos, struct EnuCoor_f *other)
289 {
290  struct EnuCoor_f diff;
291  VECT3_DIFF(diff, *other, *pos);
292  return sqrtf(VECT2_NORM2(diff));
293 }
294 
301 static float viewing_angle(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi)
302 {
303  struct EnuCoor_f diff;
304  VECT3_DIFF(diff, *other, *pos);
305  float dir = 0.f;
306  if (fabsf(diff.x) < 1e-5f) {
307  dir = sign(diff.y) * M_PI_2;
308  } else {
309  dir = atanf(diff.y/diff.x);
310  }
311  return (sign(diff.x)*M_PI_2) - psi - dir;
312 }
313 
319 static float delta_phi(float psi, float psi_other)
320 {
321  float dp = psi_other - psi;
323  return dp;
324 }
325 
333 static float neighbor_influence(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi , float psi_other)
334 {
335  float view = viewing_angle(pos, other, psi);
336  float d2d = distance_drone_to_drone(pos, other);
337  float d_phi = delta_phi(psi, psi_other);
338  float tmp_ali = d2d / nfp.l_ali;
339  tmp_ali = nfp.y_ali * sinf(d_phi) * ((d2d + nfp.d0_ali) / nfp.l_ali) * expf(-tmp_ali * tmp_ali);
340  float amplifier = 1.f;
341  if (d2d < nfp.d0_att) {
342  amplifier = expf(nfp.alpha_rep * ((nfp.d0_att - d2d)/(nfp.d0_att)));
343  }
344  float tmp_att = d2d / nfp.l_att;
345  tmp_att = nfp.y_att * (((d2d - nfp.d0_att) / nfp.l_att) / (1.f + tmp_att * tmp_att)) * sinf(view) * amplifier;
346  return fabs(tmp_att + tmp_ali);
347 }
348 
355 static float neighbor_attraction(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi)
356 {
357  float view = viewing_angle(pos, other, psi);
358  float d2d = distance_drone_to_drone(pos, other);
359 
360  if(d2d <= nfp.min_d2d) {
361  if (view > -M_PI_2 && view <= M_PI_2) {
362  nav_fish.velocity = nav_fish.velocity - nfp.min_velocity;
363  } else {
364  nav_fish.velocity = nav_fish.velocity + nfp.min_velocity;
365  }
366  }
367  float amplifier = 1.f;
368  if (d2d < nfp.d0_att) {
369  amplifier = expf(nfp.alpha_rep * ((nfp.d0_att - d2d) / nfp.d0_att));
370  }
371  float tmp_att = d2d / nfp.l_att;
372  tmp_att = nfp.y_att * (((d2d - nfp.d0_att) / nfp.l_att) / (1.f + tmp_att * tmp_att)) * sinf(view) * amplifier;
373  return tmp_att;
374 }
375 
383 static float neighbor_alignement(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi , float psi_other)
384 {
385  float d2d = distance_drone_to_drone(pos, other);
386  float d_phi = delta_phi(psi, psi_other);
387  float tmp_ali = d2d / nfp.l_ali;
388  tmp_ali = nfp.y_ali * sinf(d_phi) * ((d2d + nfp.d0_ali) / nfp.l_ali) * expf(-tmp_ali * tmp_ali);
389  return tmp_ali;
390 }
391 
392 //static float perpendicular_distance(float x, float y, float sx, float sy, float dx, float dy)
393 //{
394 // float agent = (dy * (y - sy) + dx * (x - sx)) / (dx * dx + dy * dy);
395 // float x_new = sx + dx * agent;
396 // float y_new = sy + dy * agent;
397 // return sqrtf((x - x_new) * (x - x_new) + (y - y_new) * (y - y_new));
398 //}
399 
403 static float calculate_new_heading(void)
404 {
405  struct EnuCoor_f *pos = stateGetPositionEnu_f();
406  float psi = stateGetNedToBodyEulers_f()->psi;
407 
408  // compute distance and angle to wall
410  nav_fish.theta_w = angle_to_wall(pos, psi);
411 
412  // trajectory attraction and alignement (not ready)
413  //float dx = x_finish - x_start;
414  //float dy = y_finish - y_start;
415  //float alpha = atan2f(dx, dy) - psi;
416  //float pd = perpendicular_distance(pos->x, pos->y, x_start, y_start, dx, dy);
417  //float beta = (sign(x_finish - pos->x)*M_PI_2) - psi - atanf((y_finish - pos->y)/(x_finish - pos->x)) ;
418  //float trajectory = (nfp.tr_y_ali * sinf(alpha) * expf(nfp.tr_l_ali - pd)) + (nfp.tr_y_att * sinf(beta) * expf((pd - (3 * nfp.tr_l_att)) / (nfp.tr_l_att + pd)));
419 
420  // compute fluctuation and wall reaction
421  float fw = expf(-powf(nav_fish.r_w / nfp.l_w, 2.f));
422  float ow = (-1.0f)*sinf(nav_fish.theta_w) * (1.f + nfp.e_w1 * cosf(nav_fish.theta_w) + nfp.e_w2 * cosf(2.f * nav_fish.theta_w));
423  nav_fish.f_w = fw;
424  nav_fish.f_fluct = nfp.fluct * (1.f - nfp.alpha * fw) * normal_random_gen();
425  nav_fish.f_wall = nfp.y_w * ow * fw;
426  nav_fish.step_size = (1.1f - fw) / 2.f;
427 
428  // compute alignement and attraction with other drones
429  struct EnuCoor_f *pos_focal = NULL;
430  float psi_focal = 0.f;
431  //uint8_t id_focal = 0;
432  struct EnuCoor_f *pos_current = NULL;
433  float psi_current = 0.f;
434  uint8_t id_current = 0;
435  float min_distance = 1000000.f;
436  float max_influence = 0.f;
437  nav_fish.f_ali = 0.f;
438  nav_fish.f_att = 0.f;
439 
440  for (uint8_t ac = 0; ac < NB_ACS; ac++) {
441  if (ti_acs[ac].ac_id == AC_ID || ti_acs[ac].ac_id == 0) { continue; }
442  float delta_t = Max((int)(gps.tow - acInfoGetItow(ti_acs[ac].ac_id)) / 1000., 0.);
443  if (delta_t < 1.f) {
444  // compute if others position is not older than 1s
445  id_current = ti_acs[ac].ac_id;
446  pos_current = acInfoGetPositionEnu_f(id_current);
447  psi_current = acInfoGetCourse(id_current);
448  if (nfp.strategy == 2) {
449  pos_focal = NULL;
450  nav_fish.f_ali = nav_fish.f_ali + neighbor_alignement(pos, pos_current, psi, psi_current);
451  nav_fish.f_att = nav_fish.f_att + neighbor_attraction(pos, pos_current, psi);
452  }
453  if (nfp.strategy == 1) {
454  float influence = neighbor_influence(pos, pos_current ,psi ,psi_current);
455  if (influence > max_influence) {
456  max_influence = influence;
457  //id_focal = id_current;
458  pos_focal = pos_current;
459  psi_focal = psi_current;
460  }
461  }
462  if (nfp.strategy == 0) {
463  float distance = distance_drone_to_drone(pos, pos_current);
464  if (distance < min_distance) {
465  min_distance = distance;
466  //id_focal = id_current;
467  pos_focal = pos_current;
468  psi_focal = psi_current;
469  }
470  }
471  }
472  }
473  if (pos_focal != NULL) {
474  nav_fish.f_ali = neighbor_alignement(pos, pos_focal, psi, psi_focal);
475  nav_fish.f_att = neighbor_attraction(pos, pos_focal, psi);
476  }
477 
478  // compute heading variation
479  float diff_heading = nav_fish.f_fluct + nav_fish.f_wall + nav_fish.f_ali + nav_fish.f_att;
480  return diff_heading;
481 }
482 
483 #define PRESCALE 16
488 {
489  static int counter = PRESCALE;
490  counter = counter - (1 + (int)(nav_fish.f_w * (PRESCALE - 1)));
491  if (counter > 0) { return true; }
492  counter = PRESCALE;
493  float diff_heading = calculate_new_heading();
494  float new_heading = nav_fish.heading + diff_heading;
498  nav_fish.velocity = nfp.max_velocity;
499 #if NAV_FISH_SYNC_SEND
501 #endif
502  return true;
503 }
504 
505 
Core autopilot interface common to all firmwares.
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
Set guided setpoints using flag mask in GUIDED mode.
Autopilot guided mode interface.
#define GUIDED_FLAG_XY_VEL
#define GUIDED_FLAG_XY_BODY
struct GpsState gps
global GPS state
Definition: gps.c:69
uint32_t tow
GPS time of week in ms.
Definition: gps.h:108
uint8_t ac_id
Definition: traffic_info.h:58
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
Definition: traffic_info.h:416
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
Definition: traffic_info.h:440
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
float psi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
float new_heading
Definition: guidance_OA.c:114
uint32_t counter
Definition: ins_flow.c:192
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:97
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:105
#define NAV_FISH_WALL_DISTANCE
Definition: nav_fish.c:129
float r_w
distance to wall
Definition: nav_fish.c:161
static float neighbor_alignement(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi, float psi_other)
calculates the alignement effect between two uavs
Definition: nav_fish.c:383
static float distance_wall
Definition: nav_fish.c:152
#define NAV_FISH_MAXVELOCITY
Definition: nav_fish.c:61
#define NAV_FISH_ALT
Definition: nav_fish.c:125
#define NAV_FISH_BODY_LENGTH
Default parameters.
Definition: nav_fish.c:57
float velocity
current velocity
Definition: nav_fish.c:168
#define NAV_FISH_D0ALI
Definition: nav_fish.c:121
static float distance_drone_to_drone(struct EnuCoor_f *pos, struct EnuCoor_f *other)
calculates the distance between two uavs
Definition: nav_fish.c:288
float step_size
step size
Definition: nav_fish.c:160
struct NavFish nav_fish
Definition: nav_fish.c:171
#define NAV_FISH_MIND2D
Definition: nav_fish.c:69
#define NAV_FISH_YALI
Definition: nav_fish.c:113
#define NAV_FISH_TRYALI
Definition: nav_fish.c:145
#define NAV_FISH_LATT
Definition: nav_fish.c:105
float f_att
attraction effect
Definition: nav_fish.c:167
static float angle_to_wall(struct EnuCoor_f *pos, float psi)
calculates the relative orientation too the wall
Definition: nav_fish.c:273
#define NAV_FISH_EW2
Definition: nav_fish.c:81
#define NAV_FISH_ALPHA_REP
Definition: nav_fish.c:97
#define NAV_FISH_YATT
Definition: nav_fish.c:101
static float distance_to_wall(struct EnuCoor_f *pos)
Calculates distance between the uav and wall.
Definition: nav_fish.c:256
static float neighbor_influence(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi, float psi_other)
calculates the influence of a uav on a neighbor of his
Definition: nav_fish.c:333
static float neighbor_attraction(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi)
calculates the attraction effect between two uavs
Definition: nav_fish.c:355
static float viewing_angle(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi)
calculates a uav's viewing angle on another uav
Definition: nav_fish.c:301
#define NAV_FISH_ALPHA
Definition: nav_fish.c:85
float f_wall
wall effect
Definition: nav_fish.c:165
#define NAV_FISH_EW1
Definition: nav_fish.c:77
#define NAV_FISH_LW
Definition: nav_fish.c:93
#define NAV_FISH_YW
Definition: nav_fish.c:89
float theta_w
angle to wall
Definition: nav_fish.c:163
#define NAV_FISH_TRYATT
Definition: nav_fish.c:137
static float calculate_new_heading(void)
calculates new variation of the heading for the uav based on current state
Definition: nav_fish.c:403
#define NAV_FISH_FLUCT
Definition: nav_fish.c:73
#define NAV_FISH_MINVELOCITY
Definition: nav_fish.c:65
float f_w
intensity of wall effect
Definition: nav_fish.c:162
#define NAV_FISH_LALI
Definition: nav_fish.c:117
#define PRESCALE
Definition: nav_fish.c:483
static float sign(float x)
sign function
Definition: nav_fish.c:232
static float normal_random_gen(void)
Gaussian random number generator with mean =0 and invariance =1 using Box-Muller method.
Definition: nav_fish.c:244
float heading
heading command
Definition: nav_fish.c:159
static void send_swarm_message(void)
Definition: nav_fish.c:213
static float delta_phi(float psi, float psi_other)
calculates difference between two headings
Definition: nav_fish.c:319
struct NavFishParams nav_fish_params
Definition: nav_fish.c:153
void nav_fish_init(void)
initialization of parameters and state variables
Definition: nav_fish.c:175
float f_ali
alignement effect
Definition: nav_fish.c:166
#define NAV_FISH_D0ATT
Definition: nav_fish.c:109
#define NAV_FISH_TRLALI
Definition: nav_fish.c:149
#define nfp
Definition: nav_fish.c:155
#define NAV_FISH_TRLATT
Definition: nav_fish.c:141
bool nav_fish_velocity_run(void)
runs the uav according to fish movement model using velocity control
Definition: nav_fish.c:487
#define NAV_FISH_STRATEGY
Definition: nav_fish.c:133
float f_fluct
fluctuation effect
Definition: nav_fish.c:164
float y_ali
alignement intensity
Definition: nav_fish.h:48
float e_w1
wall interaction's first coefficient
Definition: nav_fish.h:41
float d0_att
attraction balance distance
Definition: nav_fish.h:47
float tr_l_ali
alignement distance to trajectory intensity
Definition: nav_fish.h:53
float min_d2d
minimum distance between two drones
Definition: nav_fish.h:38
float min_velocity
minimum velocity when facing obstacles
Definition: nav_fish.h:37
float max_velocity
max velocity allowed
Definition: nav_fish.h:36
float alt
flight altitude
Definition: nav_fish.h:56
float l_att
attraction distance
Definition: nav_fish.h:46
float d0_ali
alignement balance distance
Definition: nav_fish.h:50
float tr_y_att
attraction to trajectory intensity
Definition: nav_fish.h:54
float y_w
wall interaction intensity
Definition: nav_fish.h:43
float tr_y_ali
alignement to trajectory intensity
Definition: nav_fish.h:52
float l_ali
alignement distance
Definition: nav_fish.h:49
float alpha
random fluctuation reduction to wall
Definition: nav_fish.h:40
float alpha_rep
intensity of repulsion
Definition: nav_fish.h:51
float tr_l_att
attraction distance to trajectory intensity
Definition: nav_fish.h:55
float y_att
attraction intensity
Definition: nav_fish.h:45
float e_w2
wall interaction's first coefficient
Definition: nav_fish.h:42
float l_w
wall interaction distance
Definition: nav_fish.h:44
uint8_t strategy
strategy for choosing focal uav : 0 for closest uav , 1 for most influential uav
Definition: nav_fish.h:57
float fluct
random fluctuation intensity
Definition: nav_fish.h:39
nav fish param structure
Definition: nav_fish.h:35
Paparazzi floating point math for geodetic calculations.
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
Horizontal guidance for rotorcrafts.
Rotorcraft navigation functions.
#define NB_ACS
Definition: rssi.c:38
static const float dir[]
API to get/set the generic vehicle states.
Periodic telemetry system header (includes downlink utility and generated code).
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98