L1 Nonlinear Guidance Logic. More...

#include <ecl_l1_pos_controller.h>

Collaboration diagram for ECL_L1_Pos_Controller:

Public Member Functions
float	nav_bearing ()
	The current target bearing. More...

float	nav_lateral_acceleration_demand ()
	Get lateral acceleration demand. More...

float	bearing_error ()
	Heading error. More...

float	target_bearing ()
	Bearing from aircraft to current target. More...

float	get_roll_setpoint ()
	Get roll angle setpoint for fixed wing. More...

float	crosstrack_error ()
	Get the current crosstrack error. More...

bool	reached_loiter_target ()
	Returns true if the loiter waypoint has been reached. More...

bool	circle_mode ()
	Returns true if following a circle (loiter) More...

float	switch_distance (float waypoint_switch_radius)
	Get the switch distance. More...

void	navigate_waypoints (const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B, const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed)
	Navigate between two waypoints. More...

void	navigate_loiter (const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector)
	Navigate on an orbit around a loiter waypoint. More...

void	navigate_heading (float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed)
	Navigate on a fixed bearing. More...

void	navigate_level_flight (float current_heading)
	Keep the wings level. More...

void	set_l1_period (float period)
	Set the L1 period. More...

void	set_l1_damping (float damping)
	Set the L1 damping factor. More...

void	set_l1_roll_limit (float roll_lim_rad)
	Set the maximum roll angle output in radians. More...

void	set_roll_slew_rate (float roll_slew_rate)
	Set roll angle slew rate. More...

void	set_dt (float dt)
	Set control loop dt. More...

Private Member Functions
matrix::Vector2f	get_local_planar_vector (const matrix::Vector2f &origin, const matrix::Vector2f &target) const
	Convert a 2D vector from WGS84 to planar coordinates. More...

void	update_roll_setpoint ()
	Update roll angle setpoint. More...

Private Attributes
float	_lateral_accel {0.0f}
	Lateral acceleration setpoint in m/s^2. More...

float	_L1_distance {20.0f}
	L1 lead distance, defined by period and damping. More...

bool	_circle_mode {false}
	flag for loiter mode More...

float	_nav_bearing {0.0f}
	bearing to L1 reference point More...

float	_bearing_error {0.0f}
	bearing error More...

float	_crosstrack_error {0.0f}
	crosstrack error in meters More...

float	_target_bearing {0.0f}
	the heading setpoint More...

float	_L1_period {25.0f}
	L1 tracking period in seconds. More...

float	_L1_damping {0.75f}
	L1 damping ratio. More...

float	_L1_ratio {5.0f}
	L1 ratio for navigation. More...

float	_K_L1 {2.0f}
	L1 control gain for _L1_damping. More...

float	_heading_omega {1.0f}
	Normalized frequency. More...

float	_roll_lim_rad {math::radians(30.0f)}
	maximum roll angle in radians More...

float	_roll_setpoint {0.0f}
	current roll angle setpoint in radians More...

float	_roll_slew_rate {0.0f}
	roll angle setpoint slew rate limit in rad/s More...

float	_dt {0}
	control loop time in seconds More...

Detailed Description

L1 Nonlinear Guidance Logic.

Definition at line 71 of file ecl_l1_pos_controller.h.

Member Function Documentation

◆ bearing_error()

float ECL_L1_Pos_Controller::bearing_error ( )

inline

Heading error.

The heading error is either compared to the current track or to the tangent of the current loiter radius.

Definition at line 94 of file ecl_l1_pos_controller.h.

References _bearing_error.

◆ circle_mode()

bool ECL_L1_Pos_Controller::circle_mode ( )

inline

Returns true if following a circle (loiter)

Definition at line 125 of file ecl_l1_pos_controller.h.

References _circle_mode, navigate_heading(), navigate_level_flight(), navigate_loiter(), navigate_waypoints(), set_l1_damping(), set_l1_period(), and switch_distance().

Referenced by FixedwingPositionControl::calculate_target_airspeed(), and FixedwingPositionControl::control_position().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ crosstrack_error()

float ECL_L1_Pos_Controller::crosstrack_error ( )

inline

Get the current crosstrack error.

Returns: Crosstrack error in meters.

Definition at line 115 of file ecl_l1_pos_controller.h.

References _crosstrack_error.

Referenced by RoverPositionControl::run(), and FixedwingPositionControl::status_publish().

Here is the caller graph for this function:

◆ get_local_planar_vector()

Vector2f ECL_L1_Pos_Controller::get_local_planar_vector	(	const matrix::Vector2f &	origin,
		const matrix::Vector2f &	target
	)		const

private

Convert a 2D vector from WGS84 to planar coordinates.

This converts from latitude and longitude to planar coordinates with (0,0) being at the position of ref and returns a vector in meters towards wp.

Parameters

ref	The reference position in WGS84 coordinates
wp	The point to convert to into the local coordinates, in WGS84 coordinates

Returns: The vector in meters pointing from the reference position to the coordinates

Definition at line 354 of file ecl_l1_pos_controller.cpp.

References CONSTANTS_RADIUS_OF_EARTH, and math::radians().

Referenced by navigate_loiter(), and navigate_waypoints().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_roll_setpoint()

float ECL_L1_Pos_Controller::get_roll_setpoint ( )

inline

Get roll angle setpoint for fixed wing.

Returns: Roll angle (in NED frame)

Definition at line 108 of file ecl_l1_pos_controller.h.

References _roll_setpoint.

Referenced by FixedwingPositionControl::control_landing(), FixedwingPositionControl::control_position(), and FixedwingPositionControl::control_takeoff().

Here is the caller graph for this function:

◆ nav_bearing()

float ECL_L1_Pos_Controller::nav_bearing ( )

inline

The current target bearing.

Returns: bearing angle (-pi..pi, in NED frame)

Definition at line 79 of file ecl_l1_pos_controller.h.

References _nav_bearing, and matrix::wrap_pi().

Referenced by FixedwingPositionControl::control_landing(), FixedwingPositionControl::control_position(), FixedwingPositionControl::control_takeoff(), RoverPositionControl::run(), and FixedwingPositionControl::status_publish().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_lateral_acceleration_demand()

float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand ( )

inline

Get lateral acceleration demand.

Returns: Lateral acceleration in m/s^2

Definition at line 86 of file ecl_l1_pos_controller.h.

References _lateral_accel.

Referenced by RoverPositionControl::control_position().

Here is the caller graph for this function:

◆ navigate_heading()

void ECL_L1_Pos_Controller::navigate_heading	(	float	navigation_heading,
		float	current_heading,
		const matrix::Vector2f &	ground_speed
	)

Navigate on a fixed bearing.

This only holds a certain direction and does not perform cross track correction. Helpful for semi-autonomous modes. Introduced by [2].

Returns: sets _lateral_accel setpoint

Definition at line 300 of file ecl_l1_pos_controller.cpp.

References _bearing_error, _circle_mode, _crosstrack_error, _heading_omega, _L1_distance, _lateral_accel, _nav_bearing, _target_bearing, math::constrain(), f(), M_PI_F, update_roll_setpoint(), and matrix::wrap_pi().

Referenced by circle_mode(), and FixedwingPositionControl::control_landing().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ navigate_level_flight()

void ECL_L1_Pos_Controller::navigate_level_flight ( float current_heading )

Keep the wings level.

This is typically needed for maximum-lift-demand situations, such as takeoff or near stall. Introduced in [2].

Definition at line 337 of file ecl_l1_pos_controller.cpp.

References _bearing_error, _circle_mode, _crosstrack_error, _lateral_accel, _nav_bearing, _target_bearing, and update_roll_setpoint().

Referenced by circle_mode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ navigate_loiter()

void ECL_L1_Pos_Controller::navigate_loiter	(	const matrix::Vector2f &	vector_A,
		const matrix::Vector2f &	vector_curr_position,
		float	radius,
		int8_t	loiter_direction,
		const matrix::Vector2f &	ground_speed_vector
	)

Navigate on an orbit around a loiter waypoint.

This allow orbits smaller than the L1 length, this modification was introduced in [2].

Returns: sets _lateral_accel setpoint

Definition at line 200 of file ecl_l1_pos_controller.cpp.

References _bearing_error, _circle_mode, _crosstrack_error, _K_L1, _L1_damping, _L1_distance, _L1_period, _L1_ratio, _lateral_accel, _nav_bearing, _target_bearing, math::constrain(), f(), FLT_EPSILON, get_bearing_to_next_waypoint(), get_local_planar_vector(), M_PI_F, math::max(), and update_roll_setpoint().

Referenced by circle_mode(), and FixedwingPositionControl::control_position().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ navigate_waypoints()

void ECL_L1_Pos_Controller::navigate_waypoints	(	const matrix::Vector2f &	vector_A,
		const matrix::Vector2f &	vector_B,
		const matrix::Vector2f &	vector_curr_position,
		const matrix::Vector2f &	ground_speed
	)

Navigate between two waypoints.

Calling this function with two waypoints results in the control outputs to fly to the line segment defined by the points and once captured following the line segment. This follows the logic in [1].

Returns: sets _lateral_accel setpoint

Definition at line 72 of file ecl_l1_pos_controller.cpp.

References _bearing_error, _circle_mode, _crosstrack_error, _K_L1, _L1_distance, _L1_ratio, _lateral_accel, _nav_bearing, _target_bearing, math::constrain(), f(), get_bearing_to_next_waypoint(), get_local_planar_vector(), M_PI_F, math::max(), matrix::Vector< Type, M >::normalized(), math::radians(), and update_roll_setpoint().

Referenced by circle_mode(), FixedwingPositionControl::control_landing(), RoverPositionControl::control_position(), FixedwingPositionControl::control_position(), and FixedwingPositionControl::control_takeoff().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reached_loiter_target()

bool ECL_L1_Pos_Controller::reached_loiter_target ( )

inline

Returns true if the loiter waypoint has been reached.

Definition at line 120 of file ecl_l1_pos_controller.h.

References _circle_mode.

◆ set_dt()

void ECL_L1_Pos_Controller::set_dt ( float dt )

inline

Set control loop dt.

The value will be used to apply roll angle setpoint slew rate limiting.

Definition at line 206 of file ecl_l1_pos_controller.h.

References _dt, and dt.

Referenced by FixedwingPositionControl::control_position().

Here is the caller graph for this function:

◆ set_l1_damping()

void ECL_L1_Pos_Controller::set_l1_damping ( float damping )

Set the L1 damping factor.

The original publication recommends a default of sqrt(2) / 2 = 0.707

Definition at line 374 of file ecl_l1_pos_controller.cpp.

References _K_L1, _L1_damping, _L1_period, _L1_ratio, and M_PI_F.

Referenced by circle_mode(), FixedwingPositionControl::init(), and RoverPositionControl::~RoverPositionControl().

Here is the caller graph for this function:

◆ set_l1_period()

void ECL_L1_Pos_Controller::set_l1_period ( float period )

Set the L1 period.

Definition at line 363 of file ecl_l1_pos_controller.cpp.

References _heading_omega, _L1_damping, _L1_period, _L1_ratio, f(), and M_PI_F.

Referenced by circle_mode(), FixedwingPositionControl::init(), and RoverPositionControl::~RoverPositionControl().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_l1_roll_limit()

void ECL_L1_Pos_Controller::set_l1_roll_limit ( float roll_lim_rad )

inline

Set the maximum roll angle output in radians.

Definition at line 196 of file ecl_l1_pos_controller.h.

References _roll_lim_rad.

Referenced by FixedwingPositionControl::init(), and RoverPositionControl::~RoverPositionControl().

Here is the caller graph for this function:

◆ set_roll_slew_rate()

void ECL_L1_Pos_Controller::set_roll_slew_rate ( float roll_slew_rate )

inline

Set roll angle slew rate.

Set to zero to deactivate.

Definition at line 201 of file ecl_l1_pos_controller.h.

References _roll_slew_rate.

Referenced by FixedwingPositionControl::init().

Here is the caller graph for this function:

◆ switch_distance()

float ECL_L1_Pos_Controller::switch_distance ( float waypoint_switch_radius )

Get the switch distance.

This is the distance at which the system will switch to the next waypoint. This depends on the period and damping

Parameters

waypoint_switch_radius The switching radius the waypoint has set.

Definition at line 65 of file ecl_l1_pos_controller.cpp.

References _L1_distance, and math::min().

Referenced by circle_mode(), and FixedwingPositionControl::status_publish().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ target_bearing()

float ECL_L1_Pos_Controller::target_bearing ( )

inline

Bearing from aircraft to current target.

Returns: bearing angle (-pi..pi, in NED frame)

Definition at line 101 of file ecl_l1_pos_controller.h.

References _target_bearing.

Referenced by RoverPositionControl::run(), and FixedwingPositionControl::status_publish().

Here is the caller graph for this function:

◆ update_roll_setpoint()

void ECL_L1_Pos_Controller::update_roll_setpoint ( )

private

Update roll angle setpoint.

This will also apply slew rate limits if set.

Definition at line 49 of file ecl_l1_pos_controller.cpp.

References _dt, _lateral_accel, _roll_lim_rad, _roll_setpoint, _roll_slew_rate, CONSTANTS_ONE_G, math::constrain(), f(), and ISFINITE.

Referenced by navigate_heading(), navigate_level_flight(), navigate_loiter(), and navigate_waypoints().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _bearing_error

float ECL_L1_Pos_Controller::_bearing_error {0.0f}

private

bearing error

Definition at line 214 of file ecl_l1_pos_controller.h.

Referenced by bearing_error(), navigate_heading(), navigate_level_flight(), navigate_loiter(), and navigate_waypoints().

◆ _circle_mode

bool ECL_L1_Pos_Controller::_circle_mode {false}

private

flag for loiter mode

Definition at line 212 of file ecl_l1_pos_controller.h.

Referenced by circle_mode(), navigate_heading(), navigate_level_flight(), navigate_loiter(), navigate_waypoints(), and reached_loiter_target().

◆ _crosstrack_error

float ECL_L1_Pos_Controller::_crosstrack_error {0.0f}

private

crosstrack error in meters

Definition at line 215 of file ecl_l1_pos_controller.h.

Referenced by crosstrack_error(), navigate_heading(), navigate_level_flight(), navigate_loiter(), and navigate_waypoints().

◆ _dt

float ECL_L1_Pos_Controller::_dt {0}

private

control loop time in seconds

Definition at line 227 of file ecl_l1_pos_controller.h.

Referenced by set_dt(), and update_roll_setpoint().

◆ _heading_omega

float ECL_L1_Pos_Controller::_heading_omega {1.0f}

private

Normalized frequency.

Definition at line 222 of file ecl_l1_pos_controller.h.

Referenced by navigate_heading(), and set_l1_period().

◆ _K_L1

float ECL_L1_Pos_Controller::_K_L1 {2.0f}

private

L1 control gain for _L1_damping.

Definition at line 221 of file ecl_l1_pos_controller.h.

Referenced by navigate_loiter(), navigate_waypoints(), and set_l1_damping().

◆ _L1_damping

float ECL_L1_Pos_Controller::_L1_damping {0.75f}

private

L1 damping ratio.

Definition at line 219 of file ecl_l1_pos_controller.h.

Referenced by navigate_loiter(), set_l1_damping(), and set_l1_period().

◆ _L1_distance

float ECL_L1_Pos_Controller::_L1_distance {20.0f}

private

L1 lead distance, defined by period and damping.

Definition at line 211 of file ecl_l1_pos_controller.h.

Referenced by navigate_heading(), navigate_loiter(), navigate_waypoints(), and switch_distance().

◆ _L1_period

float ECL_L1_Pos_Controller::_L1_period {25.0f}

private

L1 tracking period in seconds.

Definition at line 218 of file ecl_l1_pos_controller.h.

Referenced by navigate_loiter(), set_l1_damping(), and set_l1_period().

◆ _L1_ratio

float ECL_L1_Pos_Controller::_L1_ratio {5.0f}

private

L1 ratio for navigation.

Definition at line 220 of file ecl_l1_pos_controller.h.

Referenced by navigate_loiter(), navigate_waypoints(), set_l1_damping(), and set_l1_period().

◆ _lateral_accel

float ECL_L1_Pos_Controller::_lateral_accel {0.0f}

private

Lateral acceleration setpoint in m/s^2.

Definition at line 210 of file ecl_l1_pos_controller.h.

Referenced by nav_lateral_acceleration_demand(), navigate_heading(), navigate_level_flight(), navigate_loiter(), navigate_waypoints(), and update_roll_setpoint().

◆ _nav_bearing

float ECL_L1_Pos_Controller::_nav_bearing {0.0f}

private

bearing to L1 reference point

Definition at line 213 of file ecl_l1_pos_controller.h.

Referenced by nav_bearing(), navigate_heading(), navigate_level_flight(), navigate_loiter(), and navigate_waypoints().

◆ _roll_lim_rad

float ECL_L1_Pos_Controller::_roll_lim_rad {math::radians(30.0f)}

private

maximum roll angle in radians

Definition at line 224 of file ecl_l1_pos_controller.h.

Referenced by set_l1_roll_limit(), and update_roll_setpoint().

◆ _roll_setpoint

float ECL_L1_Pos_Controller::_roll_setpoint {0.0f}

private

current roll angle setpoint in radians

Definition at line 225 of file ecl_l1_pos_controller.h.

Referenced by get_roll_setpoint(), and update_roll_setpoint().

◆ _roll_slew_rate

float ECL_L1_Pos_Controller::_roll_slew_rate {0.0f}

private

roll angle setpoint slew rate limit in rad/s

Definition at line 226 of file ecl_l1_pos_controller.h.

Referenced by set_roll_slew_rate(), and update_roll_setpoint().

◆ _target_bearing

float ECL_L1_Pos_Controller::_target_bearing {0.0f}

private

the heading setpoint

Definition at line 216 of file ecl_l1_pos_controller.h.

Referenced by navigate_heading(), navigate_level_flight(), navigate_loiter(), navigate_waypoints(), and target_bearing().

The documentation for this class was generated from the following files:

src/lib/ecl/l1/ecl_l1_pos_controller.h
src/lib/ecl/l1/ecl_l1_pos_controller.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Function Documentation

◆ bearing_error()

◆ circle_mode()

◆ crosstrack_error()

◆ get_local_planar_vector()

◆ get_roll_setpoint()

◆ nav_bearing()

◆ nav_lateral_acceleration_demand()

◆ navigate_heading()

◆ navigate_level_flight()

◆ navigate_loiter()

◆ navigate_waypoints()

◆ reached_loiter_target()

◆ set_dt()

◆ set_l1_damping()

◆ set_l1_period()

◆ set_l1_roll_limit()

◆ set_roll_slew_rate()

◆ switch_distance()

◆ target_bearing()

◆ update_roll_setpoint()

Member Data Documentation

◆ _bearing_error

◆ _circle_mode

◆ _crosstrack_error

◆ _dt

◆ _heading_omega

◆ _K_L1

◆ _L1_damping

◆ _L1_distance

◆ _L1_period

◆ _L1_ratio

◆ _lateral_accel

◆ _nav_bearing

◆ _roll_lim_rad

◆ _roll_setpoint

◆ _roll_slew_rate

◆ _target_bearing