nav_rotorcraft_base.c File Reference

#include "modules/nav/nav_rotorcraft_base.h"
#include "firmwares/rotorcraft/navigation.h"
#include "generated/flight_plan.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for nav_rotorcraft_base.c:

Go to the source code of this file.

Macros
#define	LINE_START_FUNCTION   {}
	Navigation along a figure O. More...
#define	LINE_STOP_FUNCTION   {}

Functions
static void	nav_stage_init (void)
	Implement basic nav function. More...
static void	nav_goto (struct EnuCoor_f *wp)
static void	nav_route (struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static bool	nav_approaching (struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static void	nav_circle (struct EnuCoor_f *wp_center, float radius)
static void	_nav_oval_init (void)
static void	nav_oval (struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
static void	send_segment (struct transport_tx *trans, struct link_device *dev)
static void	send_circle (struct transport_tx *trans, struct link_device *dev)
static void	send_nav_status (struct transport_tx *trans, struct link_device *dev)
void	nav_rotorcraft_init (void)
	Init and register nav functions. More...

Variables
struct NavBase_t	nav_rotorcraft_base
	Basic Nav struct. More...

Detailed Description

Author

2018 Gautier Hattenberger gauti.nosp@m.er.h.nosp@m.atten.nosp@m.berg.nosp@m.er@en.nosp@m.ac.f.nosp@m.r Basic navigation functions for Rotorcraft

Definition in file nav_rotorcraft_base.c.

Macro Definition Documentation

LINE_START_FUNCTION

#define LINE_START_FUNCTION   {}

Navigation along a figure O.

One side leg is defined by waypoints [p1] and [p2]. The navigation goes through 4 states: OC1 (half circle next to [p1]), OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg). Initial state is the route along the desired segment (OC2).

Definition at line 164 of file nav_rotorcraft_base.c.

LINE_STOP_FUNCTION

#define LINE_STOP_FUNCTION   {}

Definition at line 167 of file nav_rotorcraft_base.c.

Function Documentation

_nav_oval_init()

static void _nav_oval_init ( void  )

static

Definition at line 170 of file nav_rotorcraft_base.c.

References NavOval_t::count, nav_rotorcraft_base, OC2, NavBase_t::oval, and NavOval_t::status.

Referenced by nav_register_oval(), and nav_rotorcraft_init().

Here is the caller graph for this function:

nav_approaching()

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

static

Definition at line 73 of file nav_rotorcraft_base.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_register_goto_wp(), and nav_rotorcraft_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_circle()

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

static

Definition at line 114 of file nav_rotorcraft_base.c.

References NavCircle_t::center, NavBase_t::circle, RotorcraftNavigation::horizontal_mode, Min, nav, NAV_CARROT_DIST, NAV_HORIZONTAL_MODE_CIRCLE, nav_rotorcraft_base, NAV_SETPOINT_MODE_POS, pos_diff, NavCircle_t::qdr, NavCircle_t::radians, NavCircle_t::radius, RotorcraftNavigation::setpoint_mode, stateGetPositionEnu_f(), RotorcraftNavigation::target, VECT2_ASSIGN, VECT2_COPY, VECT2_DIFF, VECT2_SUM, FloatVect2::x, and FloatVect2::y.

Referenced by nav_register_circle(), and nav_rotorcraft_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_goto()

static void nav_goto ( struct EnuCoor_f *  wp )

static

Definition at line 41 of file nav_rotorcraft_base.c.

References NavGoto_t::dist2_to_wp, get_dist2_to_point(), NavBase_t::goto_wp, RotorcraftNavigation::horizontal_mode, nav, NAV_HORIZONTAL_MODE_WAYPOINT, nav_rotorcraft_base, NAV_SETPOINT_MODE_POS, RotorcraftNavigation::setpoint_mode, RotorcraftNavigation::target, NavGoto_t::to, and VECT2_COPY.

Referenced by nav_register_goto_wp(), and nav_rotorcraft_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_oval()

static void nav_oval ( struct EnuCoor_f *  wp1, struct EnuCoor_f *  wp2, float  radius  )

static

Definition at line 176 of file nav_rotorcraft_base.c.

References CARROT, NavOval_t::count, dir, float_vect2_normalize(), InitStage, LINE_START_FUNCTION, LINE_STOP_FUNCTION, nav, RotorcraftNavigation::nav_approaching, RotorcraftNavigation::nav_circle, nav_rotorcraft_base, RotorcraftNavigation::nav_route, NavQdrCloseTo, OC1, OC2, OR12, OR21, NavBase_t::oval, NavOval_t::status, VECT2_DIFF, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by nav_rotorcraft_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_rotorcraft_init()

void nav_rotorcraft_init

(

void

)

Init and register nav functions.

Definition at line 298 of file nav_rotorcraft_base.c.

References _nav_oval_init(), NavBase_t::circle, DEFAULT_CIRCLE_RADIUS, DefaultPeriodic, NavBase_t::goto_wp, NavGoto_t::leg_length, NavGoto_t::leg_progress, nav_approaching(), nav_circle(), nav_goto(), nav_oval(), nav_register_circle(), nav_register_goto_wp(), nav_register_oval(), nav_register_stage_init(), nav_rotorcraft_base, nav_route(), nav_stage_init(), NavCircle_t::radius, register_periodic_telemetry(), and send_nav_status().

Here is the call graph for this function:

nav_route()

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

static

Definition at line 50 of file nav_rotorcraft_base.c.

References NavGoto_t::dist2_to_wp, NavGoto_t::from, get_dist2_to_point(), NavBase_t::goto_wp, RotorcraftNavigation::horizontal_mode, NavGoto_t::leg_length, NavGoto_t::leg_progress, nav, NAV_CARROT_DIST, NAV_HORIZONTAL_MODE_ROUTE, nav_rotorcraft_base, NAV_SETPOINT_MODE_POS, pos_diff, RotorcraftNavigation::setpoint_mode, stateGetPositionEnu_f(), RotorcraftNavigation::target, NavGoto_t::to, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, and FloatVect2::y.

Referenced by nav_register_goto_wp(), and nav_rotorcraft_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_stage_init()

static void nav_stage_init ( void  )

static

Implement basic nav function.

Definition at line 35 of file nav_rotorcraft_base.c.

References NavBase_t::circle, NavBase_t::goto_wp, NavGoto_t::leg_progress, nav_rotorcraft_base, and NavCircle_t::radians.

Referenced by nav_register_stage_init(), and nav_rotorcraft_init().

Here is the caller graph for this function:

send_circle()

static void send_circle ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 271 of file nav_rotorcraft_base.c.

References NavCircle_t::center, NavBase_t::circle, dev, nav_rotorcraft_base, NavCircle_t::radius, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by send_nav_status().

Here is the caller graph for this function:

send_nav_status()

static void send_nav_status ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 279 of file nav_rotorcraft_base.c.

References block_time, dev, RotorcraftNavigation::dist2_to_home, NavGoto_t::dist2_to_wp, NavBase_t::goto_wp, RotorcraftNavigation::horizontal_mode, nav, nav_block, NAV_HORIZONTAL_MODE_CIRCLE, NAV_HORIZONTAL_MODE_ROUTE, nav_rotorcraft_base, nav_stage, send_circle(), send_segment(), and stage_time.

Referenced by nav_rotorcraft_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_segment()

static void send_segment ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 262 of file nav_rotorcraft_base.c.

References dev, NavGoto_t::from, NavBase_t::goto_wp, nav_rotorcraft_base, NavGoto_t::to, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by send_nav_status().

Here is the caller graph for this function:

Variable Documentation

nav_rotorcraft_base

struct NavBase_t nav_rotorcraft_base

Basic Nav struct.

Definition at line 1 of file nav_rotorcraft_base.c.

Referenced by _nav_oval_init(), nav_circle(), nav_goto(), nav_hybrid_circle(), nav_hybrid_goto(), nav_hybrid_route(), nav_oval(), nav_rotorcraft_hybrid_init(), nav_rotorcraft_init(), nav_route(), nav_stage_init(), send_circle(), send_nav_status(), and send_segment().