Modules
	VtolNavigationFsmMethods

	VtolNavigationDo

	VtolNavigationEnable

Files
file	vtol_follower_control.c
	Control algorithms for the vtol follower.

file	vtol_follower_fsm.c
	FSMs for VTOL path navigation.

file	vtol_follower_priv.h
	Includes for the internal methods.

file	vtolpathfollower.c
	Compute attitude to achieve a path for VTOL aircrafts.

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

Macros
#define	MILLI 1000

#define	MAX_QUEUE_SIZE 4

#define	STACK_SIZE_BYTES 1548

#define	TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL

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 }

enum	vtol_goals { GOAL_LAND_NONE, GOAL_HOLD_POSITION, GOAL_FLY_PATH, GOAL_LAND_HERE, GOAL_LAND_HOME }

enum	vtol_pid { NORTH_VELOCITY, EAST_VELOCITY, DOWN_VELOCITY, NORTH_POSITION, EAST_POSITION, DOWN_POSITION, VTOL_PID_NUM }
	The named set of PIDs used for navigation. More...

Functions
static int32_t	vtol_follower_control_impl (const float *hold_pos_ned, float alt_rate, bool update_status)

int32_t	vtol_follower_control_path (const PathDesiredData pathDesired, struct path_status progress)

int32_t	vtol_follower_control_endpoint (const float *hold_pos_ned)

int32_t	vtol_follower_control_land (const float hold_pos_ned, bool landed)

int32_t	vtol_follower_control_altrate (const float *hold_pos_ned, float alt_adj)

static int32_t	vtol_follower_control_accel (float dT)

static float	vtol_follower_control_altitude (float downCommand)

int32_t	vtol_follower_control_attitude (float dT, const float *att_adj)

static float	loiter_deadband (float input, float threshold, float expoPercent)

bool	vtol_follower_control_loiter (float dT, float hold_pos, float att_adj, float *alt_adj)

void	vtol_follower_control_settings_updated ()

static void	go_enable_fly_path (void)

static void	vtol_fsm_timer_set (int32_t ms)

static bool	vtol_fsm_timer_expired ()

int32_t	vtol_follower_fsm_activate_goal (enum vtol_goals new_goal)

int32_t	vtol_follower_fsm_update ()

static void	vtolPathFollowerTask (void *parameters)

int32_t	VtolPathFollowerStart ()

int32_t	VtolPathFollowerInitialize ()

	MODULE_INITCALL (VtolPathFollowerInitialize, VtolPathFollowerStart)

Variables
static AltitudeHoldSettingsData	altitudeHoldSettings

struct pid	vtol_pids [VTOL_PID_NUM]

static float	vtol_path_m =0

static float	vtol_path_r =0

static float	vtol_end_m =0

static float	vtol_end_r =0

static float	loiter_brakealpha =0.96f

static float	loiter_errordecayalpha =0.88f

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...

VtolPathFollowerSettingsData	vtol_guidanceSettings

float	vtol_dT

VtolPathFollowerSettingsData	vtol_guidanceSettings

float	vtol_dT = 0.050f

static struct pios_thread *	pathfollowerTaskHandle

static struct pios_queue *	queue

static bool	module_enabled = false

struct pid	vtol_pids [VTOL_PID_NUM]

volatile bool	settings_updated = true

Detailed Description

Macro Definition Documentation

#define MAX_QUEUE_SIZE 4

Definition at line 65 of file vtolpathfollower.c.

#define MILLI 1000

Definition at line 102 of file vtol_follower_fsm.c.

#define STACK_SIZE_BYTES 1548

Definition at line 66 of file vtolpathfollower.c.

#define TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL

Definition at line 67 of file vtolpathfollower.c.

Enumeration Type Documentation

enum vtol_fsm_event

Events that can be be injected into the FSM and trigger state changes.

Enumerator
FSM_EVENT_AUTO	Fake event to auto transition to the next state
FSM_EVENT_TIMEOUT	The timeout configured expired
FSM_EVENT_HIT_TARGET	The UAV hit the current target
FSM_EVENT_LEFT_TARGET	The UAV left the target
FSM_EVENT_NUM_EVENTS

Definition at line 66 of file vtol_follower_fsm.c.

enum vtol_fsm_state

The states the FSM's can be in. The actual behavior of the states is ultimately determined by the entry function when enabling the state and the static method that is called while staying in that state. In most cases the specific state also sets the nav mode and a default methods will farm it out to the appropriate algorithm.

Enumerator
FSM_STATE_FAULT	Invalid state transition occurred
FSM_STATE_INIT	Starting state, normally auto transitions
FSM_STATE_HOLDING	Holding at current location
FSM_STATE_FLYING_PATH	Flying a path to a destination
FSM_STATE_LANDING	Landing at a destination
FSM_STATE_PRE_RTH_RISE	Rise to 15 m before flying home
FSM_STATE_POST_RTH_HOLD	Hold at home before initiating landing
FSM_STATE_DISARM	Disarm the system after landing
FSM_STATE_UNCHANGED	Fake state to indicate do nothing
FSM_STATE_NUM_STATES

Definition at line 81 of file vtol_follower_fsm.c.

enum vtol_goals

The set of goals the VTOL follower will attempt to achieve this selects the particular FSM that is running. These goals are determined by the input to this module.

Enumerator
GOAL_LAND_NONE	Fly to the home location and land
GOAL_HOLD_POSITION	Hold a location specified by PathDesired
GOAL_FLY_PATH	Fly a path specified by PathDesired
GOAL_LAND_HERE	Land at the current location
GOAL_LAND_HOME	Fly to the home location and land

Definition at line 41 of file vtol_follower_priv.h.

enum vtol_pid

The named set of PIDs used for navigation.

Enumerator
NORTH_VELOCITY
EAST_VELOCITY
DOWN_VELOCITY
NORTH_POSITION
EAST_POSITION
DOWN_POSITION
VTOL_PID_NUM

Definition at line 50 of file vtol_follower_priv.h.

Function Documentation

static void go_enable_fly_path ( void )

static

Definition at line 628 of file vtol_follower_fsm.c.

static float loiter_deadband	(	float	input,
		float	threshold,
		float	expoPercent
	)

static

Definition at line 545 of file vtol_follower_control.c.

MODULE_INITCALL	(	VtolPathFollowerInitialize	,
		VtolPathFollowerStart
	)

static int32_t vtol_follower_control_accel ( float dT )

static

Compute the desired acceleration based on the desired velocity and actual velocity

Definition at line 338 of file vtol_follower_control.c.

static float vtol_follower_control_altitude ( float downCommand )

static

Definition at line 402 of file vtol_follower_control.c.

int32_t vtol_follower_control_altrate	(	const float *	hold_pos_ned,
		float	alt_adj
	)

Control algorithm for loitering– allow control of altitude rate.

Parameters

[in]	hold_pos_ned	The position to control for.
[in]	alt_adj	If 0, holds at altitude in hold_pos_ned. Otherwise, a rate in meters per second to descend.

Definition at line 327 of file vtol_follower_control.c.

int32_t vtol_follower_control_attitude	(	float	dT,
		const float *	att_adj
	)

Compute desired attitude from the desired velocity

Parameters

[in]	dT	the time since last evaluation
[in]	att_adj	an adjustment to the attitude for loiter mode

Takes in NedActual which has the acceleration in the NED frame as the feedback term and then compares the VelocityActual against the VelocityDesired

Definition at line 449 of file vtol_follower_control.c.

int32_t vtol_follower_control_endpoint ( const float * hold_pos_ned )

Control algorithm to stay or approach at a fixed location.

Parameters

[in] ned The position to attempt to hold This method does not attempt any particular path, simply a straight line approach. The calculated velocity to attempt is stored in VelocityDesired.

Takes in PositionActual and compares it to PositionDesired and computes VelocityDesired

Definition at line 301 of file vtol_follower_control.c.

static int32_t vtol_follower_control_impl	(	const float *	hold_pos_ned,
		float	alt_rate,
		bool	update_status
	)

static

Controller to maintain/seek a position and optionally descend.

Parameters

[in]	hold_pos_ned	a position to hold
[in]	alt_rate	if not 0, a requested descent/climb rate that overrides hold_pos_ned
[in]	update_status	does this update path_status, or does somoene else?

Definition at line 191 of file vtol_follower_control.c.

int32_t vtol_follower_control_land	(	const float *	hold_pos_ned,
		bool *	landed
	)

Control algorithm to land at a fixed location

Parameters

[in]	hold_pos_ned	The position to attempt to land over (down ignored)
[out]	landed	True once throttle low and velocity at zero (UNIMPL)

Takes in PositionActual and compares it to the hold position and computes VelocityDesired

Definition at line 314 of file vtol_follower_control.c.

bool vtol_follower_control_loiter	(	float	dT,
		float *	hold_pos,
		float *	att_adj,
		float *	alt_adj
	)

Receives loiter commands and makes appropriate adjustments. The updated position and calculated attitude are used in later stages of the control process.

Parameters

[in]	dT	the time since last evaluation
[in,out]	hold_pos	the hold position
[out]	att_adj	an adjustment to be made to attitude for responsiveness.
[out]	alt_adj	a requested descent (negative for climb) rate

Definition at line 576 of file vtol_follower_control.c.

int32_t vtol_follower_control_path	(	const PathDesiredData *	pathDesired,
		struct path_status *	progress
	)

Compute desired velocity to follow the desired path from the current location.

Parameters

[in]	dT	the time since last evaluation
[in]	pathDesired	the desired path to follow
[out]	progress	the current progress information along that path

Returns: 0 if successful, <0 if an error occurred

The calculated velocity to attempt is stored in VelocityDesired

Definition at line 85 of file vtol_follower_control.c.

void vtol_follower_control_settings_updated ( )

Definition at line 717 of file vtol_follower_control.c.

int32_t vtol_follower_fsm_activate_goal ( enum vtol_goals new_goal )

Activate a new goal behavior. This method will fetch any details about the goal (e.g. location) from the appropriate UAVOs

Parameters

[in] new_goal The type of goal to try to achieve

Definition at line 733 of file vtol_follower_fsm.c.

int32_t vtol_follower_fsm_update ( )

Called periodically to allow the FSM to perform the state specific updates and any state transitions

Definition at line 750 of file vtol_follower_fsm.c.

static bool vtol_fsm_timer_expired ( )

static

Checks if there is a pending timer that has expired.

Returns: True if expired.

Definition at line 244 of file vtol_follower_fsm.c.

static void vtol_fsm_timer_set ( int32_t ms )

static

Sets up an interval timer that can be later checked for expiration.

Parameters

[in] ms Relative time in millseconds. 0 to cancel pending timer, if any

Definition at line 224 of file vtol_follower_fsm.c.

int32_t VtolPathFollowerInitialize ( )

Initialise the module, called on startup

Returns: 0 on success or -1 if initialisation failed

Definition at line 102 of file vtolpathfollower.c.

int32_t VtolPathFollowerStart ( )

Initialise the module, called on startup

Returns: 0 on success or -1 if initialisation failed

Definition at line 83 of file vtolpathfollower.c.

static void vtolPathFollowerTask ( void * parameters )

static

Module thread, should not return.

Definition at line 155 of file vtolpathfollower.c.

Variable Documentation

AltitudeHoldSettingsData altitudeHoldSettings

static

Definition at line 64 of file vtol_follower_control.c.

const struct vtol_fsm_transition fsm_follow_path[FSM_STATE_NUM_STATES]

static

Initial value:

= {
        [FSM_STATE_INIT] = {
                .next_state = {
                        [FSM_EVENT_AUTO] = FSM_STATE_FLYING_PATH,
                },
        },
        [FSM_STATE_FLYING_PATH] = {
                .entry_fn = go_enable_fly_path,
                .static_fn = do_requested_path,
                .next_state = {
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_UNCHANGED,
                        [FSM_EVENT_LEFT_TARGET] = FSM_STATE_UNCHANGED,
                },
        },
}

The state machine for following the Path Planner: enable following path segment

Definition at line 149 of file vtol_follower_fsm.c.

const struct vtol_fsm_transition fsm_hold_position[FSM_STATE_NUM_STATES]

static

Initial value:

= {
        [FSM_STATE_INIT] = {
                .next_state = {
                        [FSM_EVENT_AUTO] = FSM_STATE_HOLDING,
                },
        },
        [FSM_STATE_HOLDING] = {
                .entry_fn = go_enable_hold_here,
                .static_fn = do_loiter,
                .next_state = {
                        [FSM_EVENT_HIT_TARGET] = FSM_STATE_UNCHANGED,
                        [FSM_EVENT_LEFT_TARGET] = FSM_STATE_UNCHANGED,
                },
        },
}

The state machine for landing at home does the following: enable holding at the current location

Definition at line 129 of file vtol_follower_fsm.c.

const struct vtol_fsm_transition fsm_land_home[FSM_STATE_NUM_STATES]

static

The state machine for landing at home does the following:

holds where currently at for 10 seconds
ascend to minimum altitude
flies to home at 2 m/s at either current altitude or 15 m above home
holds above home for 10 seconds
descends to ground (This step currently does not complete)
disarms the system

Definition at line 174 of file vtol_follower_fsm.c.

float loiter_brakealpha =0.96f

static

Definition at line 71 of file vtol_follower_control.c.

float loiter_errordecayalpha =0.88f

static

Definition at line 71 of file vtol_follower_control.c.

bool module_enabled = false

static

Definition at line 77 of file vtolpathfollower.c.

struct pios_thread* pathfollowerTaskHandle

static

Definition at line 72 of file vtolpathfollower.c.

struct pios_queue* queue

static

Definition at line 73 of file vtolpathfollower.c.

const float RTH_ALT_ERROR = 0.8f

static

The altitude to come within for RTH */.

Definition at line 62 of file vtol_follower_fsm.c.

const float RTH_CLIMB_SPEED = 1.0f

static

Climb at 1m/s.

Definition at line 63 of file vtol_follower_fsm.c.

volatile bool settings_updated = true

Definition at line 150 of file vtolpathfollower.c.

float vtol_dT

Definition at line 62 of file vtolpathfollower.c.

float vtol_dT = 0.050f

Definition at line 62 of file vtolpathfollower.c.

float vtol_end_m =0

static

Definition at line 68 of file vtol_follower_control.c.

float vtol_end_r =0

static

Definition at line 68 of file vtol_follower_control.c.

uint32_t vtol_fsm_timer_expiration = 0

static

Specifies a time since startup in ms that the timeout fires.

Definition at line 218 of file vtol_follower_fsm.c.

VtolPathFollowerSettingsData vtol_guidanceSettings

Definition at line 61 of file vtolpathfollower.c.

VtolPathFollowerSettingsData vtol_guidanceSettings

Definition at line 61 of file vtolpathfollower.c.

float vtol_path_m =0

static

Definition at line 68 of file vtol_follower_control.c.

float vtol_path_r =0

static

Definition at line 68 of file vtol_follower_control.c.

struct pid vtol_pids[VTOL_PID_NUM]

Definition at line 65 of file vtol_follower_control.c.

struct pid vtol_pids[VTOL_PID_NUM]

Definition at line 65 of file vtol_follower_control.c.

Modules

Files

Data Structures

Macros

Enumerations

Functions

Variables

Detailed Description

Macro Definition Documentation

Enumeration Type Documentation

Function Documentation

Variable Documentation