Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_trinity.c File Reference
#include "modules/nav/nav_trinity.h"
#include "firmwares/fixedwing/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
#include "modules/core/abi.h"
+ Include dependency graph for nav_trinity.c:

Go to the source code of this file.

Data Structures

struct  NavTrinity
 

Macros

#define NAV_TRINITY_RECOVER_MAX_TURN   1.5f
 
#define NAV_TRINITY_BORDER_FILTER   20.f
 
#define NAV_TRINITY_LWC_ID   ABI_BROADCAST
 

Enumerations

enum  TrinityStatus {
  TRINITY_ENTER, TRINITY_INSIDE_FIRST, TRINITY_INSIDE, TRINITY_OUTSIDE_START,
  TRINITY_OUTSIDE, TRINITY_RECOVER_START, TRINITY_RECOVER_INSIDE, TRINITY_RECOVER_OUTSIDE
}
 
enum  RotationDir {
  LACE_LEFT, LACE_RIGHT, RSTT_LEFT, RSTT_RIGHT,
  TRINITY_LEFT, TRINITY_RIGHT
}
 

Functions

static float change_rep (float dir)
 
static struct EnuCoor_f process_new_point_trinity (struct EnuCoor_f *position, float alt_sp, float uav_direction)
 
static struct EnuCoor_f process_first_point_trinity (struct EnuCoor_f *position, float alt_sp, float uav_direction)
 
static void update_target_point (struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
 
static void lwc_cb (uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t *data)
 
void nav_trinity_init (void)
 Init function called by modules init. More...
 
void nav_trinity_setup (float init_x, float init_y, float init_z, int turn, float desired_radius, float vx, float vy, float vz)
 Initialized the exploration with a first target point inside the cloud Called from flight plan or with mission parameters. More...
 
bool nav_trinity_run (void)
 Navigation function Called by flight plan or mission run function. More...
 

Variables

static struct NavTrinity nav_trinity
 
static const float nav_dt = 1.f / NAVIGATION_FREQUENCY
 
static abi_event lwc_ev
 

Detailed Description

Author
Titouan Verdu Adaptative trinity pattern for cloud exploration.

Definition in file nav_trinity.c.


Data Structure Documentation

◆ NavTrinity

struct NavTrinity

Definition at line 60 of file nav_trinity.c.

+ Collaboration diagram for NavTrinity:
Data Fields
struct EnuCoor_f actual
struct EnuCoor_f circle
float direction
struct EnuCoor_f estim_border
bool inside_cloud
float last_border_time
float max_recover_radius
struct FloatVect3 pos_incr
float radius
float radius_sign
struct EnuCoor_f recover_circle
float recover_radius
enum RotationDir rotation
enum TrinityStatus status
struct EnuCoor_f target

Macro Definition Documentation

◆ NAV_TRINITY_BORDER_FILTER

#define NAV_TRINITY_BORDER_FILTER   20.f

Definition at line 41 of file nav_trinity.c.

◆ NAV_TRINITY_LWC_ID

#define NAV_TRINITY_LWC_ID   ABI_BROADCAST

Definition at line 178 of file nav_trinity.c.

◆ NAV_TRINITY_RECOVER_MAX_TURN

#define NAV_TRINITY_RECOVER_MAX_TURN   1.5f

Definition at line 37 of file nav_trinity.c.

Enumeration Type Documentation

◆ RotationDir

Enumerator
LACE_LEFT 
LACE_RIGHT 
RSTT_LEFT 
RSTT_RIGHT 
TRINITY_LEFT 
TRINITY_RIGHT 

Definition at line 55 of file nav_trinity.c.

◆ TrinityStatus

Enumerator
TRINITY_ENTER 
TRINITY_INSIDE_FIRST 
TRINITY_INSIDE 
TRINITY_OUTSIDE_START 
TRINITY_OUTSIDE 
TRINITY_RECOVER_START 
TRINITY_RECOVER_INSIDE 
TRINITY_RECOVER_OUTSIDE 

Definition at line 44 of file nav_trinity.c.

Function Documentation

◆ change_rep()

static float change_rep ( float  dir)
static

Definition at line 82 of file nav_trinity.c.

References dir.

Referenced by nav_trinity_run().

+ Here is the caller graph for this function:

◆ lwc_cb()

static void lwc_cb ( uint8_t sender_id  UNUSED,
uint32_t stamp  UNUSED,
int32_t  data_type,
uint32_t  size,
uint8_t data 
)
static

Definition at line 183 of file nav_trinity.c.

References NavTrinity::inside_cloud, and nav_trinity.

Referenced by nav_trinity_init().

+ Here is the caller graph for this function:

◆ nav_trinity_init()

void nav_trinity_init ( void  )

◆ nav_trinity_run()

◆ nav_trinity_setup()

void nav_trinity_setup ( float  init_x,
float  init_y,
float  init_z,
int  turn,
float  desired_radius,
float  vx,
float  vy,
float  vz 
)

Initialized the exploration with a first target point inside the cloud Called from flight plan or with mission parameters.

Definition at line 205 of file nav_trinity.c.

References NavTrinity::actual, NavTrinity::inside_cloud, nav_dt, nav_trinity, NavTrinity::pos_incr, NavTrinity::radius, NavTrinity::radius_sign, NavTrinity::rotation, stateGetPositionEnu_f(), NavTrinity::status, NavTrinity::target, TRINITY_ENTER, TRINITY_LEFT, TRINITY_RIGHT, and VECT3_ASSIGN.

+ Here is the call graph for this function:

◆ process_first_point_trinity()

static struct EnuCoor_f process_first_point_trinity ( struct EnuCoor_f position,
float  alt_sp,
float  uav_direction 
)
static

Definition at line 105 of file nav_trinity.c.

References nav_trinity, process_new_point_trinity(), NavTrinity::rotation, TRINITY_LEFT, and TRINITY_RIGHT.

Referenced by nav_trinity_run().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ process_new_point_trinity()

static struct EnuCoor_f process_new_point_trinity ( struct EnuCoor_f position,
float  alt_sp,
float  uav_direction 
)
static

Definition at line 87 of file nav_trinity.c.

References nav_trinity, NavTrinity::radius, NavTrinity::rotation, TRINITY_RIGHT, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by nav_trinity_run(), and process_first_point_trinity().

+ Here is the caller graph for this function:

◆ update_target_point()

static void update_target_point ( struct EnuCoor_f target,
struct EnuCoor_f border,
float  dt,
float  tau 
)
static

Definition at line 116 of file nav_trinity.c.

References alpha, target, FloatVect2::x, EnuCoor_f::x, FloatVect2::y, and EnuCoor_f::y.

Referenced by nav_trinity_run().

+ Here is the caller graph for this function:

Variable Documentation

◆ lwc_ev

abi_event lwc_ev
static

Definition at line 181 of file nav_trinity.c.

Referenced by nav_trinity_init().

◆ nav_dt

const float nav_dt = 1.f / NAVIGATION_FREQUENCY
static

Definition at line 80 of file nav_trinity.c.

Referenced by nav_trinity_run(), and nav_trinity_setup().

◆ nav_trinity