nav_rover_base.c File Reference

#include "modules/nav/nav_rover_base.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for nav_rover_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_goto (struct EnuCoor_f *wp)
	Implement basic nav function. More...
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_rover_init (void)
	Init and register nav functions. More...

Variables
struct RoverNavBase	nav_rover_base

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 Rovers

Definition in file nav_rover_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 147 of file nav_rover_base.c.

LINE_STOP_FUNCTION

#define LINE_STOP_FUNCTION   {}

Definition at line 150 of file nav_rover_base.c.

Function Documentation

_nav_oval_init()

static void _nav_oval_init ( void  )

static

Definition at line 153 of file nav_rover_base.c.

References RoverNavOval::count, nav_rover_base, OC2, RoverNavBase::oval, and RoverNavOval::status.

Referenced by nav_rover_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 63 of file nav_rover_base.c.

References ARRIVED_AT_WAYPOINT, f, 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_rover_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 103 of file nav_rover_base.c.

References CARROT_DIST, RoverNavCircle::center, RoverNavBase::circle, f, Max, Min, RoverNavigation::mode, nav, NAV_MODE_CIRCLE, nav_rover_base, pos_diff, RoverNavCircle::qdr, RoverNavCircle::radians, RoverNavCircle::radius, stateGetPositionEnu_f(), RoverNavigation::target, VECT2_ASSIGN, VECT2_COPY, VECT2_DIFF, VECT2_SUM, FloatVect2::x, and FloatVect2::y.

Referenced by nav_oval(), and nav_rover_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

Implement basic nav function.

Definition at line 34 of file nav_rover_base.c.

References RoverNavGoto::dist2_to_wp, get_dist2_to_point(), RoverNavBase::goto_wp, RoverNavigation::mode, nav, NAV_MODE_WAYPOINT, nav_rover_base, and RoverNavGoto::to.

Referenced by nav_register_goto_wp(), and nav_rover_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 159 of file nav_rover_base.c.

References ANGLE_BFP_OF_REAL, CARROT, InitStage, INT32_ANGLE_PI, int32_atan2_2(), INT32_DEG_OF_RAD, LINE_START_FUNCTION, LINE_STOP_FUNCTION, nav_approaching_from(), nav_circle(), nav_oval_count, nav_route(), NavQdrCloseTo, OC1, OC2, OR12, OR21, POS_BFP_OF_REAL, waypoints, point::x, and point::y.

Referenced by nav_rover_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_route()

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

static

Definition at line 41 of file nav_rover_base.c.

References CARROT_DIST, RoverNavGoto::dist2_to_wp, f, RoverNavGoto::from, get_dist2_to_point(), RoverNavBase::goto_wp, RoverNavGoto::leg_length, RoverNavGoto::leg_progress, Max, RoverNavigation::mode, nav, NAV_MODE_ROUTE, nav_rover_base, pos_diff, stateGetPositionEnu_f(), RoverNavigation::target, RoverNavGoto::to, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, and FloatVect2::y.

Referenced by nav_oval(), and nav_rover_init().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_rover_init()

void nav_rover_init

(

void

)

Init and register nav functions.

Definition at line 287 of file nav_rover_base.c.

References _nav_oval_init(), RoverNavBase::circle, DEFAULT_CIRCLE_RADIUS, DefaultPeriodic, RoverNavBase::goto_wp, RoverNavGoto::leg_length, RoverNavGoto::leg_progress, nav_approaching(), nav_circle(), nav_goto(), nav_oval(), nav_register_circle(), nav_register_goto_wp(), nav_register_oval(), nav_route(), nav_rover_base, RoverNavCircle::radius, register_periodic_telemetry(), and send_nav_status().

Here is the call graph for this function:

send_circle()

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

static

Definition at line 260 of file nav_rover_base.c.

References RoverNavCircle::center, RoverNavBase::circle, dev, nav_rover_base, RoverNavCircle::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 268 of file nav_rover_base.c.

References block_time, dev, RoverNavigation::dist2_to_home, RoverNavGoto::dist2_to_wp, RoverNavBase::goto_wp, RoverNavigation::mode, nav, nav_block, NAV_MODE_CIRCLE, NAV_MODE_ROUTE, nav_rover_base, nav_stage, send_circle(), send_segment(), and stage_time.

Referenced by nav_rover_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 251 of file nav_rover_base.c.

References dev, RoverNavGoto::from, RoverNavBase::goto_wp, nav_rover_base, RoverNavGoto::to, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by send_nav_status().

Here is the caller graph for this function:

Variable Documentation

nav_rover_base

struct RoverNavBase nav_rover_base

Definition at line 29 of file nav_rover_base.c.

Referenced by _nav_oval_init(), nav_circle(), nav_goto(), nav_route(), nav_rover_init(), send_circle(), send_nav_status(), and send_segment().