Paparazzi UAS  v6.0_unstable-39-g758f8ef-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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"
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 
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
484 
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;
495  FLOAT_ANGLE_NORMALIZE(new_heading);
498  nav_fish.velocity = nfp.max_velocity;
499 #if NAV_FISH_SYNC_SEND
501 #endif
502  return true;
503 }
504 
505 
#define FLOAT_ANGLE_NORMALIZE(_a)
uint8_t ac_id
Definition: sim_ap.c:48
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
Set guided setpoints using flag mask in GUIDED mode.
Periodic telemetry system header (includes downlink utility and generated code).
vector in East North Up coordinates Units: meters
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
Definition: traffic_info.h:416
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
float psi
in radians
Autopilot guided mode interface.
#define GUIDED_FLAG_XY_VEL
#define GUIDED_FLAG_XY_BODY
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
uint8_t ac_id
Definition: traffic_info.h:58
Paparazzi floating point math for geodetic calculations.
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
Definition: traffic_info.h:440
float x
in meters
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:77
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
#define NB_ACS
Definition: rssi.c:38
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:85
static const float dir[]
int32_t counter
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
#define Max(x, y)
Definition: main_fbw.c:53
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
API to get/set the generic vehicle states.
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
Horizontal guidance for rotorcrafts.
float y
in meters
struct GpsState gps
global GPS state
Definition: gps.c:69
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float new_heading
Definition: guidance_OA.c:114