Paparazzi UAS  v6.0_unstable-94-gda5b527-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "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:

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

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

#define NAV_FISH_ALPHA   0.6f

Definition at line 85 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_ALPHA_REP   1.f

Definition at line 97 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_ALT   2.0f

Definition at line 125 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_BODY_LENGTH   0.5f

Default parameters.

Definition at line 57 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_D0ALI   1.f

Definition at line 121 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_D0ATT   1.5f

Definition at line 109 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_EW1   0.7f

Definition at line 77 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_EW2   0.f

Definition at line 81 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_FLUCT   0.1f

Definition at line 73 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_LALI   3.f

Definition at line 117 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_LATT   3.f

Definition at line 105 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_LW   (2.5f*NAV_FISH_BODY_LENGTH)

Definition at line 93 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_MAXVELOCITY   0.5f

Definition at line 61 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_MIND2D   1.f

Definition at line 69 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_MINVELOCITY   0.1f

Definition at line 65 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_STRATEGY   0

Definition at line 133 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_TRLALI   2.f

Definition at line 149 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_TRLATT   3.f

Definition at line 141 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_TRYALI   0.25f

Definition at line 145 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_TRYATT   0.2f

Definition at line 137 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_WALL_DISTANCE   10.f

Definition at line 129 of file nav_fish.c.

#define NAV_FISH_YALI   0.15f

Definition at line 113 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_YATT   0.4f

Definition at line 101 of file nav_fish.c.

Referenced by nav_fish_init().

#define NAV_FISH_YW   0.8f

Definition at line 89 of file nav_fish.c.

Referenced by nav_fish_init().

#define PRESCALE   16

Definition at line 483 of file nav_fish.c.

Referenced by nav_fish_velocity_run().

Function Documentation

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

calculates the relative orientation too the wall

Parameters
posENU coordinates
psiangle 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:

static float calculate_new_heading ( void  )
static
static float delta_phi ( float  psi,
float  psi_other 
)
static

calculates difference between two headings

Parameters
psifirst heading
psi_othersecond 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:

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

calculates the distance between two uavs

Parameters
posENU coordinates of the first drone
otherENU 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:

static float distance_to_wall ( struct EnuCoor_f pos)
static

Calculates distance between the uav and wall.

Parameters
posENU 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:

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:

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
posENU coordinates of the current uav
otherENU coordinates of the neighbor uav
psiheading of the current uav
psi_otherheading 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:

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

calculates the attraction effect between two uavs

Parameters
posENU coordinates of the current uav
otherENU coordinates of the neighbor uav
psiheading 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:

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
posENU coordinates of the influenced uav
otherENU coordinates of the influencing uav
psiheading of the influenced uav
psi_otherheading 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:

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.

References f.

Referenced by calculate_new_heading().

+ Here is the caller graph for this function:

static void send_swarm_message ( void  )
inlinestatic
static float sign ( float  x)
static

sign function

Parameters
numberx
Returns
sign of x

Definition at line 232 of file nav_fish.c.

References f.

Referenced by gec_process_msg3(), and viewing_angle().

+ Here is the caller graph for this function:

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
posENU coordinates of the viewer drone
otherENU coordinates of the viewed drone
psiheading of the viewer drone
Returns
viewing angle

Definition at line 301 of file nav_fish.c.

References dir, f, 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

float distance_wall = NAV_FISH_WALL_DISTANCE
static

Definition at line 152 of file nav_fish.c.

Referenced by distance_to_wall().

struct NavFishParams nav_fish_params

Definition at line 153 of file nav_fish.c.

Referenced by nav_fish_init().