45#include "generated/flight_plan.h"
56#ifndef NAV_FISH_BODY_LENGTH
57#define NAV_FISH_BODY_LENGTH 0.5f
60#ifndef NAV_FISH_MAXVELOCITY
61#define NAV_FISH_MAXVELOCITY 0.5f
64#ifndef NAV_FISH_MINVELOCITY
65#define NAV_FISH_MINVELOCITY 0.1f
68#ifndef NAV_FISH_MIND2D
69#define NAV_FISH_MIND2D 1.f
73#define NAV_FISH_FLUCT 0.1f
77#define NAV_FISH_EW1 0.7f
81#define NAV_FISH_EW2 0.f
85#define NAV_FISH_ALPHA 0.6f
89#define NAV_FISH_YW 0.8f
93#define NAV_FISH_LW (2.5f*NAV_FISH_BODY_LENGTH)
96#ifndef NAV_FISH_ALPHA_REP
97#define NAV_FISH_ALPHA_REP 1.f
101#define NAV_FISH_YATT 0.4f
105#define NAV_FISH_LATT 3.f
108#ifndef NAV_FISH_D0ATT
109#define NAV_FISH_D0ATT 1.5f
113#define NAV_FISH_YALI 0.15f
117#define NAV_FISH_LALI 3.f
120#ifndef NAV_FISH_D0ALI
121#define NAV_FISH_D0ALI 1.f
125#define NAV_FISH_ALT 2.0f
128#ifndef NAV_FISH_WALL_DISTANCE
129#define NAV_FISH_WALL_DISTANCE 10.f
132#ifndef NAV_FISH_STRATEGY
133#define NAV_FISH_STRATEGY 0
136#ifndef NAV_FISH_TRYATT
137#define NAV_FISH_TRYATT 0.2f
140#ifndef NAV_FISH_TRLATT
141#define NAV_FISH_TRLATT 3.f
144#ifndef NAV_FISH_TRYALI
145#define NAV_FISH_TRYALI 0.25f
148#ifndef NAV_FISH_TRLALI
149#define NAV_FISH_TRLALI 2.f
155#define nfp nav_fish_params
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));
260 float dist =
distance_wall - sqrtf(((pos->
x - x_home) * (pos->
x - x_home)) + ((pos->
y - y_home) * (pos->
y - y_home)));
275 float dir = M_PI_2 - psi;
276 float theta = atan2f(pos->
y, pos->
x);
277 float delta =
dir - theta;
306 if (fabsf(diff.
x) < 1e-5f) {
309 dir = atanf(diff.
y/diff.
x);
311 return (
sign(diff.
x)*M_PI_2) - psi -
dir;
321 float dp = psi_other - psi;
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)));
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);
360 if(d2d <=
nfp.min_d2d) {
361 if (view > -M_PI_2 && view <= M_PI_2) {
367 float amplifier = 1.f;
368 if (d2d <
nfp.d0_att) {
369 amplifier = expf(
nfp.alpha_rep * ((
nfp.d0_att - d2d) /
nfp.d0_att));
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;
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);
421 float fw = expf(-powf(
nav_fish.r_w /
nfp.l_w, 2.f));
426 nav_fish.step_size = (1.1f - fw) / 2.f;
430 float psi_focal = 0.f;
433 float psi_current = 0.f;
435 float min_distance = 1000000.f;
436 float max_influence = 0.f;
441 if (
ti_acs[ac].ac_id == AC_ID ||
ti_acs[ac].ac_id == 0) {
continue; }
445 id_current =
ti_acs[ac].ac_id;
448 if (
nfp.strategy == 2) {
453 if (
nfp.strategy == 1) {
455 if (influence > max_influence) {
456 max_influence = influence;
458 pos_focal = pos_current;
459 psi_focal = psi_current;
462 if (
nfp.strategy == 0) {
464 if (distance < min_distance) {
465 min_distance = distance;
467 pos_focal = pos_current;
468 psi_focal = psi_current;
473 if (pos_focal != NULL) {
491 if (
counter > 0) {
return true; }
499#if NAV_FISH_SYNC_SEND
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
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).
#define FLOAT_ANGLE_NORMALIZE(_a)
#define VECT3_DIFF(_c, _a, _b)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
#define NAV_FISH_WALL_DISTANCE
float r_w
distance to wall
static float neighbor_alignement(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi, float psi_other)
calculates the alignement effect between two uavs
static float distance_wall
#define NAV_FISH_MAXVELOCITY
#define NAV_FISH_BODY_LENGTH
Default parameters.
float velocity
current velocity
static float distance_drone_to_drone(struct EnuCoor_f *pos, struct EnuCoor_f *other)
calculates the distance between two uavs
float f_att
attraction effect
static float angle_to_wall(struct EnuCoor_f *pos, float psi)
calculates the relative orientation too the wall
#define NAV_FISH_ALPHA_REP
static float distance_to_wall(struct EnuCoor_f *pos)
Calculates distance between the uav and wall.
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
static float neighbor_attraction(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi)
calculates the attraction effect between two uavs
static float viewing_angle(struct EnuCoor_f *pos, struct EnuCoor_f *other, float psi)
calculates a uav's viewing angle on another uav
float theta_w
angle to wall
static float calculate_new_heading(void)
calculates new variation of the heading for the uav based on current state
#define NAV_FISH_MINVELOCITY
float f_w
intensity of wall effect
static float sign(float x)
sign function
static float normal_random_gen(void)
Gaussian random number generator with mean =0 and invariance =1 using Box-Muller method.
float heading
heading command
static void send_swarm_message(void)
static float delta_phi(float psi, float psi_other)
calculates difference between two headings
struct NavFishParams nav_fish_params
void nav_fish_init(void)
initialization of parameters and state variables
float f_ali
alignement effect
bool nav_fish_velocity_run(void)
runs the uav according to fish movement model using velocity control
#define NAV_FISH_STRATEGY
float f_fluct
fluctuation effect
Paparazzi floating point math for geodetic calculations.
vector in East North Up coordinates Units: meters
Horizontal guidance for rotorcrafts.
Rotorcraft navigation functions.
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.