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
276 float theta =
atan2f(pos->
y, pos->
x);
277 float delta =
dir - theta;
448 if (
nfp.strategy == 2) {
453 if (
nfp.strategy == 1) {
462 if (
nfp.strategy == 0) {
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
uint32_t tow
GPS time of week in ms.
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
float y_ali
alignement intensity
float e_w1
wall interaction's first coefficient
float d0_att
attraction balance distance
float tr_l_ali
alignement distance to trajectory intensity
float min_d2d
minimum distance between two drones
float min_velocity
minimum velocity when facing obstacles
float max_velocity
max velocity allowed
float l_att
attraction distance
float d0_ali
alignement balance distance
float tr_y_att
attraction to trajectory intensity
float y_w
wall interaction intensity
float tr_y_ali
alignement to trajectory intensity
float l_ali
alignement distance
float alpha
random fluctuation reduction to wall
float alpha_rep
intensity of repulsion
float tr_l_att
attraction distance to trajectory intensity
float y_att
attraction intensity
float e_w2
wall interaction's first coefficient
float l_w
wall interaction distance
uint8_t strategy
strategy for choosing focal uav : 0 for closest uav , 1 for most influential uav
float fluct
random fluctuation intensity
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.