#include "modules/nav/nav_fish.h"
#include "modules/multi/traffic_info.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "state.h"
#include "autopilot.h"
#include "autopilot_guided.h"
#include "subsystems/navigation/waypoints.h"
#include "math/pprz_geodetic_float.h"
#include "generated/flight_plan.h"
#include <math.h>
#include "subsystems/datalink/telemetry.h"
#include <stdio.h>
#include <stdlib.h>

Include dependency graph for nav_fish.c:

Data Structures
struct	NavFish

Macros
#define	NAV_FISH_BODY_LENGTH 0.5f
	Default parameters. More...

#define	NAV_FISH_MAXVELOCITY 0.5f

#define	NAV_FISH_MINVELOCITY 0.1f

#define	NAV_FISH_MIND2D 1.f

#define	NAV_FISH_FLUCT 0.1f

#define	NAV_FISH_EW1 0.7f

#define	NAV_FISH_EW2 0.f

#define	NAV_FISH_ALPHA 0.6f

#define	NAV_FISH_YW 0.8f

#define	NAV_FISH_LW (2.5f*NAV_FISH_BODY_LENGTH)

#define	NAV_FISH_ALPHA_REP 1.f

#define	NAV_FISH_YATT 0.4f

#define	NAV_FISH_LATT 3.f

#define	NAV_FISH_D0ATT 1.5f

#define	NAV_FISH_YALI 0.15f

#define	NAV_FISH_LALI 3.f

#define	NAV_FISH_D0ALI 1.f

#define	NAV_FISH_ALT 2.0f

#define	NAV_FISH_WALL_DISTANCE 10.f

#define	NAV_FISH_STRATEGY 0

#define	NAV_FISH_TRYATT 0.2f

#define	NAV_FISH_TRLATT 3.f

#define	NAV_FISH_TRYALI 0.25f

#define	NAV_FISH_TRLALI 2.f

#define	nfp nav_fish_params

#define	PRESCALE 16

Functions
void	nav_fish_init (void)
	initialization of parameters and state variables More...

static void	send_swarm_message (void)

static float	sign (float x)
	sign function More...

static float	normal_random_gen (void)
	Gaussian random number generator with mean =0 and invariance =1 using Box-Muller method. More...

static float	distance_to_wall (struct EnuCoor_f *pos)
	Calculates distance between the uav and wall. More...

static float	angle_to_wall (struct EnuCoor_f *pos, float psi)
	calculates the relative orientation too the wall More...

static float	distance_drone_to_drone (struct EnuCoor_f pos, struct EnuCoor_f other)
	calculates the distance between two uavs More...

static float	viewing_angle (struct EnuCoor_f pos, struct EnuCoor_f other, float psi)
	calculates a uav's viewing angle on another uav More...

static float	delta_phi (float psi, float psi_other)
	calculates difference between two headings More...

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 More...

static float	neighbor_attraction (struct EnuCoor_f pos, struct EnuCoor_f other, float psi)
	calculates the attraction effect between two uavs More...

static float	neighbor_alignement (struct EnuCoor_f pos, struct EnuCoor_f other, float psi, float psi_other)
	calculates the alignement effect between two uavs More...

static float	calculate_new_heading (void)
	calculates new variation of the heading for the uav based on current state More...

bool	nav_fish_velocity_run (void)
	runs the uav according to fish movement model using velocity control More...

Variables
static float	distance_wall = NAV_FISH_WALL_DISTANCE

struct NavFishParams	nav_fish_params

struct NavFish	nav_fish

Data Structure Documentation

◆ NavFish

struct NavFish

Definition at line 158 of file nav_fish.c.

Data Fields
float	f_ali	alignement effect
float	f_att	attraction effect
float	f_fluct	fluctuation effect
float	f_w	intensity of wall effect
float	f_wall	wall effect
float	heading	heading command
float	r_w	distance to wall
float	step_size	step size
float	theta_w	angle to wall
float	velocity	current velocity

Macro Definition Documentation

◆ NAV_FISH_ALPHA

#define NAV_FISH_ALPHA 0.6f

Definition at line 85 of file nav_fish.c.

◆ NAV_FISH_ALPHA_REP

#define NAV_FISH_ALPHA_REP 1.f

Definition at line 97 of file nav_fish.c.

◆ NAV_FISH_ALT

#define NAV_FISH_ALT 2.0f

Definition at line 125 of file nav_fish.c.

◆ NAV_FISH_BODY_LENGTH

#define NAV_FISH_BODY_LENGTH 0.5f

Default parameters.

Definition at line 57 of file nav_fish.c.

◆ NAV_FISH_D0ALI

#define NAV_FISH_D0ALI 1.f

Definition at line 121 of file nav_fish.c.

◆ NAV_FISH_D0ATT

#define NAV_FISH_D0ATT 1.5f

Definition at line 109 of file nav_fish.c.

◆ NAV_FISH_EW1

#define NAV_FISH_EW1 0.7f

Definition at line 77 of file nav_fish.c.

◆ NAV_FISH_EW2

#define NAV_FISH_EW2 0.f

Definition at line 81 of file nav_fish.c.

◆ NAV_FISH_FLUCT

#define NAV_FISH_FLUCT 0.1f

Definition at line 73 of file nav_fish.c.

◆ NAV_FISH_LALI

#define NAV_FISH_LALI 3.f

Definition at line 117 of file nav_fish.c.

◆ NAV_FISH_LATT

#define NAV_FISH_LATT 3.f

Definition at line 105 of file nav_fish.c.

◆ NAV_FISH_LW

#define NAV_FISH_LW (2.5f*NAV_FISH_BODY_LENGTH)

Definition at line 93 of file nav_fish.c.

◆ NAV_FISH_MAXVELOCITY

#define NAV_FISH_MAXVELOCITY 0.5f

Definition at line 61 of file nav_fish.c.

◆ NAV_FISH_MIND2D

#define NAV_FISH_MIND2D 1.f

Definition at line 69 of file nav_fish.c.

◆ NAV_FISH_MINVELOCITY

#define NAV_FISH_MINVELOCITY 0.1f

Definition at line 65 of file nav_fish.c.

◆ NAV_FISH_STRATEGY

#define NAV_FISH_STRATEGY 0

Definition at line 133 of file nav_fish.c.

◆ NAV_FISH_TRLALI

#define NAV_FISH_TRLALI 2.f

Definition at line 149 of file nav_fish.c.

◆ NAV_FISH_TRLATT

#define NAV_FISH_TRLATT 3.f

Definition at line 141 of file nav_fish.c.

◆ NAV_FISH_TRYALI

#define NAV_FISH_TRYALI 0.25f

Definition at line 145 of file nav_fish.c.

◆ NAV_FISH_TRYATT

#define NAV_FISH_TRYATT 0.2f

Definition at line 137 of file nav_fish.c.

◆ NAV_FISH_WALL_DISTANCE

#define NAV_FISH_WALL_DISTANCE 10.f

Definition at line 129 of file nav_fish.c.

◆ NAV_FISH_YALI

#define NAV_FISH_YALI 0.15f

Definition at line 113 of file nav_fish.c.

◆ NAV_FISH_YATT

#define NAV_FISH_YATT 0.4f

Definition at line 101 of file nav_fish.c.

◆ NAV_FISH_YW

#define NAV_FISH_YW 0.8f

Definition at line 89 of file nav_fish.c.

◆ nfp

#define nfp nav_fish_params

Definition at line 155 of file nav_fish.c.

◆ PRESCALE

#define PRESCALE 16

Definition at line 483 of file nav_fish.c.

Function Documentation

◆ angle_to_wall()

static float angle_to_wall	(	struct EnuCoor_f *	pos,
		float	psi
	)

static

calculates the relative orientation too the wall

Parameters

pos	ENU coordinates
psi	angle from north clockwise

Returns: angle to circular wall

Definition at line 273 of file nav_fish.c.

References dir, FLOAT_ANGLE_NORMALIZE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by calculate_new_heading().

Here is the caller graph for this function:

◆ calculate_new_heading()

static float calculate_new_heading ( void )

static

calculates new variation of the heading for the uav based on current state

Returns: heading variation

Definition at line 403 of file nav_fish.c.

References ac_id, acInfo::ac_id, acInfoGetCourse(), acInfoGetItow(), acInfoGetPositionEnu_f(), angle_to_wall(), distance_drone_to_drone(), distance_to_wall(), f, NavFish::f_ali, NavFish::f_att, NavFish::f_fluct, NavFish::f_w, NavFish::f_wall, gps, Max, nav_fish, NB_ACS, neighbor_alignement(), neighbor_attraction(), neighbor_influence(), nfp, normal_random_gen(), FloatEulers::psi, NavFish::r_w, stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), NavFish::step_size, NavFish::theta_w, ti_acs, and GpsState::tow.

Referenced by nav_fish_velocity_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ delta_phi()

static float delta_phi	(	float	psi,
		float	psi_other
	)

static

calculates difference between two headings

Parameters

psi	first heading
psi_other	second heading

Returns: the difference between the two headings

Definition at line 319 of file nav_fish.c.

References FLOAT_ANGLE_NORMALIZE.

Referenced by neighbor_alignement(), and neighbor_influence().

Here is the caller graph for this function:

◆ distance_drone_to_drone()

static float distance_drone_to_drone	(	struct EnuCoor_f *	pos,
		struct EnuCoor_f *	other
	)

static

calculates the distance between two uavs

Parameters

pos	ENU coordinates of the first drone
other	ENU coordinates of the second drone

Returns: the distance between two uavs from their given position

Definition at line 288 of file nav_fish.c.

References VECT2_NORM2, and VECT3_DIFF.

Referenced by calculate_new_heading(), neighbor_alignement(), neighbor_attraction(), and neighbor_influence().

Here is the caller graph for this function:

◆ distance_to_wall()

static float distance_to_wall ( struct EnuCoor_f * pos )

static

Calculates distance between the uav and wall.

Parameters

pos	ENU coordinates

Returns: distance to circular wall

Definition at line 256 of file nav_fish.c.

References distance_wall, f, waypoint_get_x(), waypoint_get_y(), EnuCoor_f::x, and EnuCoor_f::y.

Referenced by calculate_new_heading().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_fish_init()

void nav_fish_init ( void )

initialization of parameters and state variables

nav fish init

Definition at line 175 of file nav_fish.c.

References NavFishParams::alpha, NavFishParams::alpha_rep, NavFishParams::alt, NavFishParams::d0_ali, NavFishParams::d0_att, NavFishParams::e_w1, NavFishParams::e_w2, NavFish::f_ali, NavFish::f_att, NavFish::f_fluct, NavFish::f_w, NavFish::f_wall, NavFishParams::fluct, NavFish::heading, NavFishParams::l_ali, NavFishParams::l_att, NavFishParams::l_w, NavFishParams::max_velocity, NavFishParams::min_d2d, NavFishParams::min_velocity, nav_fish, NAV_FISH_ALPHA, NAV_FISH_ALPHA_REP, NAV_FISH_ALT, NAV_FISH_BODY_LENGTH, NAV_FISH_D0ALI, NAV_FISH_D0ATT, NAV_FISH_EW1, NAV_FISH_EW2, NAV_FISH_FLUCT, NAV_FISH_LALI, NAV_FISH_LATT, NAV_FISH_LW, NAV_FISH_MAXVELOCITY, NAV_FISH_MIND2D, NAV_FISH_MINVELOCITY, nav_fish_params, NAV_FISH_STRATEGY, NAV_FISH_TRLALI, NAV_FISH_TRLATT, NAV_FISH_TRYALI, NAV_FISH_TRYATT, NAV_FISH_YALI, NAV_FISH_YATT, NAV_FISH_YW, NavFish::r_w, NavFish::step_size, NavFishParams::strategy, NavFish::theta_w, NavFishParams::tr_l_ali, NavFishParams::tr_l_att, NavFishParams::tr_y_ali, NavFishParams::tr_y_att, NavFish::velocity, NavFishParams::y_ali, NavFishParams::y_att, and NavFishParams::y_w.

◆ nav_fish_velocity_run()

bool nav_fish_velocity_run ( void )

runs the uav according to fish movement model using velocity control

nav fish velocity run

Returns: true

Definition at line 487 of file nav_fish.c.

References autopilot_guided_update(), calculate_new_heading(), counter, NavFish::f_w, FLOAT_ANGLE_NORMALIZE, GUIDED_FLAG_XY_BODY, GUIDED_FLAG_XY_VEL, NavFish::heading, nav_fish, new_heading, nfp, PRESCALE, send_swarm_message(), and NavFish::velocity.

Here is the call graph for this function:

◆ neighbor_alignement()

static float neighbor_alignement	(	struct EnuCoor_f *	pos,
		struct EnuCoor_f *	other,
		float	psi,
		float	psi_other
	)

static

calculates the alignement effect between two uavs

Parameters

pos	ENU coordinates of the current uav
other	ENU coordinates of the neighbor uav
psi	heading of the current uav
psi_other	heading of the neighbor uav

Returns: alignement

Definition at line 383 of file nav_fish.c.

References delta_phi(), distance_drone_to_drone(), and nfp.

Referenced by calculate_new_heading().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ neighbor_attraction()

static float neighbor_attraction	(	struct EnuCoor_f *	pos,
		struct EnuCoor_f *	other,
		float	psi
	)

static

calculates the attraction effect between two uavs

Parameters

pos	ENU coordinates of the current uav
other	ENU coordinates of the neighbor uav
psi	heading of the current uav

Returns: attraction

Definition at line 355 of file nav_fish.c.

References distance_drone_to_drone(), nav_fish, nfp, NavFish::velocity, and viewing_angle().

Referenced by calculate_new_heading().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ neighbor_influence()

static float neighbor_influence	(	struct EnuCoor_f *	pos,
		struct EnuCoor_f *	other,
		float	psi,
		float	psi_other
	)

static

calculates the influence of a uav on a neighbor of his

Parameters

pos	ENU coordinates of the influenced uav
other	ENU coordinates of the influencing uav
psi	heading of the influenced uav
psi_other	heading of the influencing uav