nav_fish.c File Reference

#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 "modules/nav/waypoints.h"
#include "math/pprz_geodetic_float.h"
#include "generated/flight_plan.h"
#include <math.h>
#include "modules/datalink/telemetry.h"
#include <stdio.h>
#include <stdlib.h>

Include dependency graph for nav_fish.c:

Go to the source code of this file.

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 acInfo::ac_id, acInfoGetCourse(), acInfoGetItow(), acInfoGetPositionEnu_f(), angle_to_wall(), distance_drone_to_drone(), distance_to_wall(), NavFish::f_ali, NavFish::f_att, NavFish::f_fluct, NavFish::f_w, NavFish::f_wall, gps, 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, 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

Returns

influence

Definition at line 333 of file nav_fish.c.

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

Referenced by calculate_new_heading().

Here is the call graph for this function:

Here is the caller graph for this function:

normal_random_gen()

static float normal_random_gen ( void  )

static

Gaussian random number generator with mean =0 and invariance =1 using Box-Muller method.

Returns

random number following a normal distribution

Definition at line 244 of file nav_fish.c.

Referenced by calculate_new_heading().

Here is the caller graph for this function:

send_swarm_message()

static void send_swarm_message ( void  )

inlinestatic

Definition at line 213 of file nav_fish.c.

References DefaultChannel, DefaultDevice, NavFish::f_ali, NavFish::f_att, NavFish::f_fluct, NavFish::f_w, NavFish::f_wall, NavFish::heading, nav_fish, NavFish::r_w, NavFish::step_size, and NavFish::theta_w.

Referenced by nav_fish_velocity_run().

Here is the caller graph for this function:

sign()

static float sign ( float  x )

static

sign function

Parameters

number

x

Returns

sign of x

Definition at line 232 of file nav_fish.c.

Referenced by airspeed_uavcan_downlink(), eas_from_dynamic_pressure(), fx_fuselage(), fx_fy_hover(), fx_wing(), fy_wing(), fz_wing(), gec_process_msg3(), ms45xx_i2c_event(), nav_spiral_3D_setup(), and viewing_angle().

Here is the caller graph for this function:

viewing_angle()

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

static

calculates a uav's viewing angle on another uav

Parameters

pos	ENU coordinates of the viewer drone
other	ENU coordinates of the viewed drone
psi	heading of the viewer drone

Returns

viewing angle

Definition at line 301 of file nav_fish.c.

References dir, sign(), VECT3_DIFF, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by neighbor_attraction(), and neighbor_influence().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

distance_wall

float distance_wall = NAV_FISH_WALL_DISTANCE

static

Definition at line 152 of file nav_fish.c.

Referenced by distance_to_wall().

nav_fish

struct NavFish nav_fish

Definition at line 152 of file nav_fish.c.

Referenced by calculate_new_heading(), nav_fish_init(), nav_fish_velocity_run(), neighbor_attraction(), and send_swarm_message().

nav_fish_params

struct NavFishParams nav_fish_params

Definition at line 152 of file nav_fish.c.

Referenced by nav_fish_init().