mission_rotorcraft_nav.c File Reference

mission navigation for rotorcrafts More...

#include <stdio.h>
#include "modules/mission/mission_common.h"
#include "autopilot.h"
#include "firmwares/rotorcraft/navigation.h"
#include "generated/flight_plan.h"

Include dependency graph for mission_rotorcraft_nav.c:

Go to the source code of this file.

Macros
#define	BUFFER_ZONE_DIST   10

Functions
bool	mission_point_of_lla (struct EnuCoor_f *point, struct LlaCoor_i *lla)
	Utility function: converts lla (int) to local point (float) More...
bool	mission_element_convert (struct _mission_element *el)
	Convert mission element's points format if needed. More...
static bool	mission_nav_wp (struct _mission_element *el)
	Navigation function to a single waypoint. More...
static bool	mission_nav_circle (struct _mission_element *el)
	Navigation function on a circle. More...
static bool	mission_nav_segment (struct _mission_element *el)
	Navigation function along a segment. More...
static bool	mission_nav_path (struct _mission_element *el)
	Navigation function along a path. More...
static bool	mission_nav_custom (struct _mission_custom *custom, bool init)
	Call custom navigation function. More...
int	mission_run ()
	Run mission. More...

Variables
static const float	dt_navigation = 1.0 / ((float)NAV_FREQ)
struct EnuCoor_i	last_mission_wp = { 0., 0., 0. }

Detailed Description

mission navigation for rotorcrafts

Implement specific navigation routines for the mission control of a rotorcraft

Definition in file mission_rotorcraft_nav.c.

Macro Definition Documentation

#define BUFFER_ZONE_DIST   10

Definition at line 38 of file mission_rotorcraft_nav.c.

Referenced by mission_point_of_lla().

Function Documentation

bool mission_element_convert

(

struct _mission_element *

el

)

Convert mission element's points format if needed.

Parameters

el	pointer to the mission element

Returns

return TRUE if conversion is succesful, FALSE otherwise

Definition at line 79 of file mission_rotorcraft_nav.c.

References _mission_element::element, ENU_BFP_OF_REAL, MissionCircle, MissionPath, MissionSegment, MissionWP, and _mission_element::type.

static bool mission_nav_circle ( struct _mission_element *  el )

inlinestatic

Navigation function on a circle.

Definition at line 141 of file mission_rotorcraft_nav.c.

References _mission_element::duration, _mission_element::element, _mission::element_time, horizontal_mode, HORIZONTAL_MODE_CIRCLE, mission, nav_circle(), NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, and EnuCoor_i::z.

Referenced by mission_run().

Here is the call graph for this function:

Here is the caller graph for this function:

static bool mission_nav_custom ( struct _mission_custom *  custom, bool  init  )

inlinestatic

Call custom navigation function.

Definition at line 225 of file mission_rotorcraft_nav.c.

References _mission_registered::cb, _mission_custom::nb, _mission_custom::params, and _mission_custom::reg.

Referenced by mission_run().

Here is the caller graph for this function:

static bool mission_nav_path ( struct _mission_element *  el )

inlinestatic

Navigation function along a path.

Definition at line 187 of file mission_rotorcraft_nav.c.

References CARROT, _mission_element::duration, _mission_element::element, horizontal_mode, HORIZONTAL_MODE_ROUTE, mission_nav_wp(), nav_approaching_from(), nav_check_wp_time(), nav_route(), NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, POS_FLOAT_OF_BFP, and EnuCoor_i::z.

Referenced by mission_run().

Here is the call graph for this function:

Here is the caller graph for this function:

static bool mission_nav_segment ( struct _mission_element *  el )

inlinestatic

Navigation function along a segment.

Definition at line 161 of file mission_rotorcraft_nav.c.

References CARROT, _mission_element::duration, _mission_element::element, horizontal_mode, HORIZONTAL_MODE_ROUTE, nav_approaching_from(), nav_check_wp_time(), nav_route(), NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, POS_FLOAT_OF_BFP, and EnuCoor_i::z.

Referenced by mission_run().

Here is the call graph for this function:

Here is the caller graph for this function:

static bool mission_nav_wp ( struct _mission_element *  el )

inlinestatic

Navigation function to a single waypoint.

Definition at line 117 of file mission_rotorcraft_nav.c.

References CARROT, _mission_element::duration, _mission_element::element, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, nav_approaching_from(), nav_check_wp_time(), navigation_target, NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, POS_FLOAT_OF_BFP, VECT3_COPY, and EnuCoor_i::z.

Referenced by mission_nav_path(), and mission_run().

Here is the call graph for this function:

Here is the caller graph for this function:

bool mission_point_of_lla	(	struct EnuCoor_f *	point,
		struct LlaCoor_i *	lla
	)

Utility function: converts lla (int) to local point (float)

Get the ENU component of LLA mission point This function is firmware specific.

TODO: don't convert to float, either use double or do completely in fixed point

Definition at line 41 of file mission_rotorcraft_nav.c.

References LlaCoor_i::alt, BUFFER_ZONE_DIST, enu_of_lla_point_i(), float_vect2_norm(), LtpDef_i::hmsl, LtpDef_i::lla, State::ned_initialized_i, State::ned_origin_i, state, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, VECT3_SMUL, waypoint_get_x(), waypoint_get_y(), FloatVect2::x, FloatVect2::y, and EnuCoor_f::z.

Referenced by mission_parse_CIRCLE_LLA(), mission_parse_GOTO_WP_LLA(), mission_parse_PATH_LLA(), and mission_parse_SEGMENT_LLA().

Here is the call graph for this function:

Here is the caller graph for this function:

int mission_run

(

void

)

Run mission.

This function should be implemented into a dedicated file since navigation functions are different for different firmwares

Currently, this function should be called from the flight plan

Returns

return TRUE when the mission is running, FALSE when it is finished

Definition at line 230 of file mission_rotorcraft_nav.c.

References _mission::current_idx, dt_navigation, _mission_element::element, _mission::element_time, mission, MISSION_ELEMENT_NB, mission_get(), mission_nav_circle(), mission_nav_custom(), mission_nav_path(), mission_nav_segment(), mission_nav_wp(), MissionCircle, MissionCustom, MissionPath, MissionSegment, MissionWP, and _mission_element::type.

Here is the call graph for this function:

Variable Documentation

const float dt_navigation = 1.0 / ((float)NAV_FREQ)

static

Definition at line 110 of file mission_rotorcraft_nav.c.

Referenced by mission_run().

struct EnuCoor_i last_mission_wp = { 0., 0., 0. }

Definition at line 113 of file mission_rotorcraft_nav.c.