Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
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
158struct 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
211
212
226
232static float sign(float x)
233{
234 if(x >= 0.f) {
235 return 1.f;
236 } else {
237 return -1.f;
238 }
239}
240
244static 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
256static float distance_to_wall(struct EnuCoor_f *pos)
257{
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
273static 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;
279 return delta;
280}
281
288static 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
301static 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
319static float delta_phi(float psi, float psi_other)
320{
321 float dp = psi_other - psi;
323 return dp;
324}
325
333static 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
355static 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
383static 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
403static 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;
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
448 if (nfp.strategy == 2) {
449 pos_focal = NULL;
452 }
453 if (nfp.strategy == 1) {
455 if (influence > max_influence) {
457 //id_focal = id_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;
469 }
470 }
471 }
472 }
473 if (pos_focal != NULL) {
476 }
477
478 // compute heading variation
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; }
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:74
uint32_t tow
GPS time of week in ms.
Definition gps.h:109
uint8_t ac_id
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
struct acInfo ti_acs[NB_ACS]
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
float psi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
#define VECT3_DIFF(_c, _a, _b)
#define VECT2_NORM2(_v)
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
float new_heading
uint32_t counter
Definition ins_flow.c:187
uint16_t foo
Definition main_demo5.c:58
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.