vtol_follower_fsm.c File Reference

FSMs for VTOL path navigation. More...

#include "openpilot.h"
#include <pios_thread.h>
#include "coordinate_conversions.h"
#include "physical_constants.h"
#include "vtol_follower_priv.h"
#include "pathdesired.h"
#include "positionactual.h"
#include "vtolpathfollowersettings.h"
#include "vtolpathfollowerstatus.h"

Go to the source code of this file.

Data Structures
struct	vtol_fsm_transition
	Structure for the FSM. More...

Macros
#define	MILLI   1000

Enumerations
enum	vtol_fsm_event {   FSM_EVENT_AUTO, FSM_EVENT_TIMEOUT, FSM_EVENT_HIT_TARGET, FSM_EVENT_LEFT_TARGET,   FSM_EVENT_NUM_EVENTS }
	Events that can be be injected into the FSM and trigger state changes. More...
enum	vtol_fsm_state {   FSM_STATE_FAULT, FSM_STATE_INIT, FSM_STATE_HOLDING, FSM_STATE_FLYING_PATH,   FSM_STATE_LANDING, FSM_STATE_PRE_RTH_RISE, FSM_STATE_POST_RTH_HOLD, FSM_STATE_DISARM,   FSM_STATE_UNCHANGED, FSM_STATE_NUM_STATES }

Functions
static void	go_enable_hold_here (void)
static void	go_enable_fly_path (void)
static void	go_enable_rise_here (void)
static void	go_enable_pause_home_10s (void)
static void	go_enable_fly_home (void)
static void	go_enable_land_home (void)
static int32_t	do_hold (void)
static int32_t	do_path (void)
static int32_t	do_requested_path (void)
static int32_t	do_land (void)
static int32_t	do_loiter (void)
static int32_t	do_ph_climb (void)
static void	vtol_fsm_timer_set (int32_t ms)
static bool	vtol_fsm_timer_expired ()
static void	hold_position (float north, float east, float down)
static int	vtol_fsm_enter_state (enum vtol_fsm_state state)
static bool	vtol_fsm_process_event (enum vtol_fsm_event event)
static void	vtol_fsm_process_auto ()
static void	vtol_fsm_fsm_init (const struct vtol_fsm_transition *goal)
static void	vtol_fsm_inject_event (enum vtol_fsm_event event)
static int32_t	vtol_fsm_static ()
static int32_t	do_slow_altitude_change (float descent_rate)
int32_t	vtol_follower_fsm_activate_goal (enum vtol_goals new_goal)
int32_t	vtol_follower_fsm_update ()

Variables
static const float	RTH_ALT_ERROR = 0.8f
	The altitude to come within for RTH */. More...
static const float	RTH_CLIMB_SPEED = 1.0f
	Climb at 1m/s. More...
static const struct vtol_fsm_transition	fsm_hold_position [FSM_STATE_NUM_STATES]
static const struct vtol_fsm_transition	fsm_follow_path [FSM_STATE_NUM_STATES]
static const struct vtol_fsm_transition	fsm_land_home [FSM_STATE_NUM_STATES]
static uint32_t	vtol_fsm_timer_expiration = 0
	Specifies a time since startup in ms that the timeout fires. More...
static const struct vtol_fsm_transition *	current_goal
	The currently selected goal FSM. More...
static enum vtol_fsm_state	curr_state
	The current state within the goal fsm. More...
static float	vtol_hold_position_ned [3]
	The setpoint for position hold relative to home in m. More...
static PathDesiredData	vtol_fsm_path_desired
	The configured path desired. Uses the PathDesired structure. More...

Detailed Description

FSMs for VTOL path navigation.

Author

dRonin, http://dRonin.org/, Copyright (C) 2015

Tau Labs, http://taulabs.org, Copyright (C) 2013-2014

Note

This module contains a set of FSMs that are selected based on the vtol_goals that comes from PathDesired. Some of those goals may be simple single step actions like fly to a location and hold. However, others might be more complex like when landing at home. The switchable FSMs allow easily adjusting the complexity.

The individual vtol_fsm_state do not directly determine the behavior, because there is a lot of redundancy between some of the states. For most common behaviors (fly a path, hold a position) the ultimate behavior is determined by the vtol_nav_mode. When a state is entered the "enable_*" method (VtolNavigationEnable) that is called will configure the navigation mode and the appropriate parameters, as well as configure any timeouts.

While in a state the "do_" methods (VtolNavigationDo) will actually update the control signals to achieve the desired flight. The default method do_default will work in most cases and simply calls the appropriate method based on the vtol_nav_mode.

Definition in file vtol_follower_fsm.c.