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_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_MAX_ACCELERATION   4.0
#define	NAV_HYBRID_SOFT_ACCELERATION   NAV_HYBRID_MAX_ACCELERATION
#define	NAV_HYBRID_MAX_EXPECTED_WIND   5.0f
#define	NAV_HYBRID_NAV_LINE_DIST   50.f
#define	NAV_HYBRID_NAV_CIRCLE_DIST   40.f
#define	NAV_HYBRID_EXT_VISION_SETPOINT_MODE   FALSE
#define	NAV_HYBRID_LIMIT_CIRCLE_RADIUS   FALSE

Functions
static float	max_speed_for_deceleration (float dist_to_wp)
	Calculate max speed when decelerating at MAX capacity a_max distance travelled d = 1/2 a_max t^2 The time in which it does this is: T = V / a_max The maximum speed at which to fly to still allow arriving with zero speed at the waypoint given maximum deceleration is: V = sqrt(2 * a_max * d)
static void	nav_hybrid_goto (struct EnuCoor_f *wp)
	Implement basic nav function for the hybrid case.
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.

Variables
float	nav_hybrid_max_bank = 0.52f
float	nav_max_speed = NAV_MAX_SPEED
float	nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION
float	nav_max_acceleration_sp = NAV_HYBRID_MAX_ACCELERATION
float	nav_hybrid_soft_acceleration = NAV_HYBRID_SOFT_ACCELERATION
float	nav_hybrid_max_acceleration = NAV_HYBRID_MAX_ACCELERATION
float	nav_hybrid_max_expected_wind = NAV_HYBRID_MAX_EXPECTED_WIND
float	nav_hybrid_line_gain = 1.0f
float	nav_hybrid_pos_gain = 1.0f
bool	force_forward = 0.0f
	forward flight for hybrid nav

Detailed Description

Specific navigation functions for hybrid aircraft.

Definition in file nav_rotorcraft_hybrid.c.

Macro Definition Documentation

NAV_HYBRID_EXT_VISION_SETPOINT_MODE

#define NAV_HYBRID_EXT_VISION_SETPOINT_MODE   FALSE

Definition at line 108 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_GOTO_MAX_SPEED

#define NAV_HYBRID_GOTO_MAX_SPEED   NAV_MAX_SPEED

Definition at line 51 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_LIMIT_CIRCLE_RADIUS

#define NAV_HYBRID_LIMIT_CIRCLE_RADIUS   FALSE

Definition at line 113 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_MAX_ACCELERATION

#define NAV_HYBRID_MAX_ACCELERATION   4.0

Definition at line 59 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_MAX_AIRSPEED

#define NAV_HYBRID_MAX_AIRSPEED   15.0f

Definition at line 40 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_MAX_EXPECTED_WIND

#define NAV_HYBRID_MAX_EXPECTED_WIND   5.0f

Definition at line 72 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_NAV_CIRCLE_DIST

#define NAV_HYBRID_NAV_CIRCLE_DIST   40.f

Definition at line 87 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_NAV_LINE_DIST

#define NAV_HYBRID_NAV_LINE_DIST   50.f

Definition at line 83 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_SOFT_ACCELERATION

#define NAV_HYBRID_SOFT_ACCELERATION   NAV_HYBRID_MAX_ACCELERATION

Definition at line 63 of file nav_rotorcraft_hybrid.c.

NAV_HYBRID_SPEED_MARGIN

#define NAV_HYBRID_SPEED_MARGIN   10.0f

Definition at line 44 of file nav_rotorcraft_hybrid.c.

NAV_MAX_SPEED

#define NAV_MAX_SPEED   (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN)

Definition at line 47 of file nav_rotorcraft_hybrid.c.

Function Documentation

max_speed_for_deceleration()

static float max_speed_for_deceleration ( float  dist_to_wp )

static

Calculate max speed when decelerating at MAX capacity a_max distance travelled d = 1/2 a_max t^2 The time in which it does this is: T = V / a_max The maximum speed at which to fly to still allow arriving with zero speed at the waypoint given maximum deceleration is: V = sqrt(2 * a_max * d)

Definition at line 214 of file nav_rotorcraft_hybrid.c.

References foo, Min, nav_max_deceleration_sp, and nav_max_speed.

Referenced by nav_hybrid_goto(), and nav_hybrid_route().

Here is the caller graph for this function:

nav_hybrid_approaching()

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

static

Definition at line 222 of file nav_rotorcraft_hybrid.c.

References ARRIVED_AT_WAYPOINT, float_vect2_norm(), foo, stateGetPositionEnu_f(), stateGetSpeedEnu_f(), VECT2_DIFF, VECT2_SMUL, and VECT2_SUM.

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 261 of file nav_rotorcraft_hybrid.c.

References alpha, NavCircle_t::center, NavBase_t::circle, float_vect2_norm(), float_vect2_normalize(), foo, force_forward, RotorcraftNavigation::horizontal_mode, nav, NAV_HORIZONTAL_MODE_CIRCLE, nav_hybrid_max_acceleration, nav_hybrid_max_bank, nav_hybrid_max_expected_wind, NAV_HYBRID_NAV_CIRCLE_DIST, nav_hybrid_pos_gain, nav_hybrid_soft_acceleration, nav_max_acceleration_sp, 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 121 of file nav_rotorcraft_hybrid.c.

References NavGoto_t::dist2_to_wp, float_vect2_bound_in_2d(), float_vect2_norm(), foo, force_forward, get_dist2_to_point(), NavBase_t::goto_wp, RotorcraftNavigation::horizontal_mode, max_speed_for_deceleration(), nav, NAV_HORIZONTAL_MODE_WAYPOINT, nav_hybrid_pos_gain, nav_hybrid_soft_acceleration, nav_max_acceleration_sp, nav_max_speed, nav_rotorcraft_base, NAV_SETPOINT_MODE_POS, 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 157 of file nav_rotorcraft_hybrid.c.

References direction, NavGoto_t::dist2_to_wp, float_vect2_norm(), foo, force_forward, NavGoto_t::from, get_dist2_to_point(), NavBase_t::goto_wp, RotorcraftNavigation::horizontal_mode, max_speed_for_deceleration(), 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 330 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.0f

forward flight for hybrid nav

Definition at line 103 of file nav_rotorcraft_hybrid.c.

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

nav_hybrid_line_gain

float nav_hybrid_line_gain = 1.0f

Definition at line 79 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_route().

nav_hybrid_max_acceleration

float nav_hybrid_max_acceleration = NAV_HYBRID_MAX_ACCELERATION

Definition at line 69 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_circle().

nav_hybrid_max_bank

float nav_hybrid_max_bank = 0.52f

Definition at line 33 of file nav_rotorcraft_hybrid.c.

Referenced by init_controller(), and nav_hybrid_circle().

nav_hybrid_max_expected_wind

float nav_hybrid_max_expected_wind = NAV_HYBRID_MAX_EXPECTED_WIND

Definition at line 74 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_circle().

nav_hybrid_pos_gain

float nav_hybrid_pos_gain = 1.0f

Definition at line 95 of file nav_rotorcraft_hybrid.c.

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

nav_hybrid_soft_acceleration

float nav_hybrid_soft_acceleration = NAV_HYBRID_SOFT_ACCELERATION

Definition at line 68 of file nav_rotorcraft_hybrid.c.

Referenced by nav_hybrid_circle(), and nav_hybrid_goto().

nav_max_acceleration_sp

float nav_max_acceleration_sp = NAV_HYBRID_MAX_ACCELERATION

Definition at line 67 of file nav_rotorcraft_hybrid.c.

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

nav_max_deceleration_sp

float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION

Definition at line 66 of file nav_rotorcraft_hybrid.c.

Referenced by max_speed_for_deceleration(), and rotwing_state_periodic().

nav_max_speed

float nav_max_speed = NAV_MAX_SPEED

Definition at line 48 of file nav_rotorcraft_hybrid.c.

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