nav_rotorcraft_hybrid.c File Reference

Specific navigation functions for hybrid aircraft. More...

#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#include "math/pprz_isa.h"
#include "generated/flight_plan.h"

Include dependency graph for nav_rotorcraft_hybrid.c:

Go to the source code of this file.

Macros
#define	NAV_HYBRID_MAX_BANK   0.52f
#define	NAV_HYBRID_MAX_AIRSPEED   15.0f
#define	NAV_HYBRID_SPEED_MARGIN   10.0f
#define	NAV_MAX_SPEED   (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN)
#define	NAV_HYBRID_GOTO_MAX_SPEED   NAV_MAX_SPEED
#define	NAV_HYBRID_MAX_DECELERATION   1.0
#define	NAV_HYBRID_NAV_LINE_DIST   50.f
#define	NAV_HYBRID_NAV_CIRCLE_DIST   40.f

Functions
static void	nav_hybrid_goto (struct EnuCoor_f *wp)
	Implement basic nav function for the hybrid case. More...
static void	nav_hybrid_route (struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static bool	nav_hybrid_approaching (struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static void	nav_hybrid_circle (struct EnuCoor_f *wp_center, float radius)
void	nav_rotorcraft_hybrid_init (void)
	Init and register nav functions. More...

Variables
float	nav_max_speed = NAV_MAX_SPEED
float	nav_goto_max_speed = NAV_HYBRID_GOTO_MAX_SPEED
float	nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION
float	nav_hybrid_line_gain = 1.0f
float	nav_hybrid_pos_gain = 1.0
bool	force_forward = 0
	forward flight for hybrid nav More...

Detailed Description

Specific navigation functions for hybrid aircraft.

Definition in file nav_rotorcraft_hybrid.c.

Macro Definition Documentation

NAV_HYBRID_GOTO_MAX_SPEED

#define NAV_HYBRID_GOTO_MAX_SPEED   NAV_MAX_SPEED

Definition at line 49 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_MAX_AIRSPEED

#define NAV_HYBRID_MAX_AIRSPEED   15.0f

Definition at line 38 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_MAX_BANK

#define NAV_HYBRID_MAX_BANK   0.52f

Definition at line 33 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_MAX_DECELERATION

#define NAV_HYBRID_MAX_DECELERATION   1.0

Definition at line 55 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_NAV_CIRCLE_DIST

#define NAV_HYBRID_NAV_CIRCLE_DIST   40.f

Definition at line 71 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_NAV_LINE_DIST

#define NAV_HYBRID_NAV_LINE_DIST   50.f

Definition at line 67 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_SPEED_MARGIN

#define NAV_HYBRID_SPEED_MARGIN   10.0f

Definition at line 42 of file nav_rotorcraft_hybrid.c.

NAV_MAX_SPEED

#define NAV_MAX_SPEED   (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN)

Definition at line 45 of file nav_rotorcraft_hybrid.c.

Function Documentation

nav_hybrid_approaching()

static bool nav_hybrid_approaching ( struct EnuCoor_f *  wp, struct EnuCoor_f *  from, float  approaching_time  )

static

Definition at line 173 of file nav_rotorcraft_hybrid.c.

References ARRIVED_AT_WAYPOINT, float_vect2_norm(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, and FloatVect2::y.

Referenced by nav_rotorcraft_hybrid_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_hybrid_circle()

static void nav_hybrid_circle ( struct EnuCoor_f *  wp_center, float  radius  )

static

Definition at line 212 of file nav_rotorcraft_hybrid.c.

References alpha, NavCircle_t::center, NavBase_t::circle, float_vect2_norm(), float_vect2_normalize(), force_forward, RotorcraftNavigation::horizontal_mode, nav, NAV_HORIZONTAL_MODE_CIRCLE, NAV_HYBRID_MAX_BANK, NAV_HYBRID_NAV_CIRCLE_DIST, nav_hybrid_pos_gain, nav_max_speed, nav_rotorcraft_base, NAV_SETPOINT_MODE_SPEED, pos_diff, PPRZ_ISA_GRAVITY, NavCircle_t::qdr, NavCircle_t::radians, NavCircle_t::radius, RotorcraftNavigation::setpoint_mode, RotorcraftNavigation::speed, stateGetPositionEnu_f(), RotorcraftNavigation::target, VECT2_COPY, VECT2_DIFF, VECT2_SMUL, FloatVect2::x, EnuCoor_f::x, FloatVect2::y, and EnuCoor_f::y.

Referenced by nav_rotorcraft_hybrid_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_hybrid_goto()

static void nav_hybrid_goto ( struct EnuCoor_f *  wp )

static

Implement basic nav function for the hybrid case.

Definition at line 87 of file nav_rotorcraft_hybrid.c.

References NavGoto_t::dist2_to_wp, float_vect2_bound_in_2d(), float_vect2_norm(), force_forward, get_dist2_to_point(), NavBase_t::goto_wp, RotorcraftNavigation::horizontal_mode, Min, nav, nav_goto_max_speed, NAV_HORIZONTAL_MODE_WAYPOINT, nav_hybrid_pos_gain, nav_max_deceleration_sp, nav_max_speed, nav_rotorcraft_base, NAV_SETPOINT_MODE_SPEED, RotorcraftNavigation::setpoint_mode, RotorcraftNavigation::speed, speed_sp, stateGetPositionEnu_f(), RotorcraftNavigation::target, NavGoto_t::to, VECT2_COPY, VECT2_DIFF, and VECT2_SMUL.

Referenced by nav_rotorcraft_hybrid_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_hybrid_route()

static void nav_hybrid_route ( struct EnuCoor_f *  wp_start, struct EnuCoor_f *  wp_end  )

static

Definition at line 124 of file nav_rotorcraft_hybrid.c.

References direction, NavGoto_t::dist2_to_wp, float_vect2_norm(), force_forward, NavGoto_t::from, get_dist2_to_point(), NavBase_t::goto_wp, RotorcraftNavigation::horizontal_mode, nav, NAV_HORIZONTAL_MODE_ROUTE, nav_hybrid_line_gain, NAV_HYBRID_NAV_LINE_DIST, nav_hybrid_pos_gain, nav_max_speed, nav_rotorcraft_base, NAV_SETPOINT_MODE_SPEED, pos_diff, RotorcraftNavigation::setpoint_mode, RotorcraftNavigation::speed, stateGetPositionEnu_f(), RotorcraftNavigation::target, NavGoto_t::to, VECT2_ADD, VECT2_ASSIGN, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, and FloatVect2::y.

Referenced by nav_rotorcraft_hybrid_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_rotorcraft_hybrid_init()

void nav_rotorcraft_hybrid_init

(

void

)

Init and register nav functions.

For hybrid vehicle nav Init should be called after the normal rotorcraft nav_init as we are reusing some of the functions and overwritting others

Definition at line 277 of file nav_rotorcraft_hybrid.c.

References NavBase_t::circle, DEFAULT_CIRCLE_RADIUS, NavBase_t::goto_wp, NavGoto_t::leg_length, NavGoto_t::leg_progress, nav_hybrid_approaching(), nav_hybrid_circle(), nav_hybrid_goto(), nav_hybrid_route(), nav_register_circle(), nav_register_goto_wp(), nav_rotorcraft_base, and NavCircle_t::radius.

Here is the call graph for this function:

Variable Documentation

force_forward

bool force_forward = 0

forward flight for hybrid nav

Definition at line 81 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_circle(), nav_hybrid_goto(), and nav_hybrid_route().

nav_goto_max_speed

float nav_goto_max_speed = NAV_HYBRID_GOTO_MAX_SPEED

Definition at line 52 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_goto().

nav_hybrid_line_gain

float nav_hybrid_line_gain = 1.0f

Definition at line 63 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_route().

nav_hybrid_pos_gain

float nav_hybrid_pos_gain = 1.0

Definition at line 77 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_circle(), nav_hybrid_goto(), and nav_hybrid_route().

nav_max_deceleration_sp

float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION

Definition at line 58 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_goto().

nav_max_speed

float nav_max_speed = NAV_MAX_SPEED

Definition at line 46 of file nav_rotorcraft_hybrid.c.

Referenced by init_controller(), nav_hybrid_circle(), nav_hybrid_goto(), and nav_hybrid_route().