#include <ManualSmoothingXY.hpp>

Inheritance diagram for ManualSmoothingXY:

Collaboration diagram for ManualSmoothingXY:

Public Types
enum	Intention { Intention::brake, Intention::acceleration, Intention::deceleration, Intention::direction_change }

Public Member Functions
	ManualSmoothingXY (ModuleParams *parent, const matrix::Vector2f &vel)

	~ManualSmoothingXY ()=default

void	updateMaxVelocity (const float &vel_max)
	Maximum velocity is required to detect user intention. More...

void	smoothVelocity (matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp, const float dt)
	Smoothing of velocity setpoint horizontally based on flight direction. More...

Intention	getIntention ()
	Get user intention. More...

void	overwriteHoverAcceleration (float acc)
	Overwrite methods: Needed if different parameter values than default required. More...

void	overwriteMaxAcceleration (float acc)

void	overwriteDecelerationMin (float dec)

void	overwriteJerkMax (float jerk)

void	overwriteJerkMin (float jerk)

Private Member Functions
void	_updateAcceleration (matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp, const float dt)
	Sets velocity change limits (=acceleration). More...

Intention	_getIntention (const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp)
	Gets user intention. More...

void	_setStateAcceleration (const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, const float dt)
	Set acceleration depending on Intention. More...

void	_velocitySlewRate (matrix::Vector2f &vel_sp, const float dt)
	Limits the velocity setpoint change. More...

matrix::Vector2f	_getWorldToHeadingFrame (const matrix::Vector2f &vec, const float &yaw)
	Rotate vector from local frame into heading frame. More...

matrix::Vector2f	_getHeadingToWorldFrame (const matrix::Vector2f &vec, const float &yaw)
	Rotate vector from heading frame to local frame. More...

Private Attributes
Intention	_intention {Intention::acceleration}
	user intention More...

float	_acc_state_dependent
	velocity change limit that depends on Intention More...

float	_jerk_state_dependent
	acceleration change limit during brake More...

float	_vel_max {10.0f}
	maximum horizontal speed allowed More...

matrix::Vector2f	_vel_sp_prev
	previous velocity setpoint More...

Detailed Description

Definition at line 53 of file ManualSmoothingXY.hpp.

Member Enumeration Documentation

◆ Intention

enum ManualSmoothingXY::Intention

strong

Enumerator
brake
acceleration
deceleration
direction_change

Definition at line 84 of file ManualSmoothingXY.hpp.

Constructor & Destructor Documentation

◆ ManualSmoothingXY()

ManualSmoothingXY::ManualSmoothingXY	(	ModuleParams *	parent,
		const matrix::Vector2f &	vel
	)

Definition at line 45 of file ManualSmoothingXY.cpp.

References _acc_state_dependent, and _jerk_state_dependent.

◆ ~ManualSmoothingXY()

ManualSmoothingXY::~ManualSmoothingXY ( )

default

Member Function Documentation

◆ _getHeadingToWorldFrame()

Vector2f ManualSmoothingXY::_getHeadingToWorldFrame	(	const matrix::Vector2f &	vec,
		const float &	yaw
	)

private

Rotate vector from heading frame to local frame.

Parameters

vec	is an arbitrary vector in heading frame
yaw	is the vehicle yaw

Definition at line 222 of file ManualSmoothingXY.cpp.

References matrix::Quaternion< Type >::conjugate(), and f().

Referenced by overwriteJerkMin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _getIntention()

ManualSmoothingXY::Intention ManualSmoothingXY::_getIntention	(	const matrix::Vector2f &	vel_sp,
		const matrix::Vector2f &	vel,
		const float &	yaw,
		const float &	yawrate_sp
	)

private

Gets user intention.

The intention is deduced from desired velocity setpoint.

Parameters

vel_sp	is desired velocity setpoint before slewrate.
vel	is vehicle velocity in xy-direction.
yaw	is vehicle yaw
yawrate_sp	is the desired yaw-speed

Definition at line 74 of file ManualSmoothingXY.cpp.

References _getWorldToHeadingFrame(), _vel_max, _vel_sp_prev, acceleration, brake, deceleration, direction_change, f(), FLT_EPSILON, matrix::Vector< Type, M >::length(), and matrix::Vector< Type, M >::normalize().

Referenced by _updateAcceleration(), and overwriteJerkMin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _getWorldToHeadingFrame()

Vector2f ManualSmoothingXY::_getWorldToHeadingFrame	(	const matrix::Vector2f &	vec,
		const float &	yaw
	)

private

Rotate vector from local frame into heading frame.

Parameters

vec	is an arbitrary vector in local frame
yaw	is the vehicle heading

Definition at line 214 of file ManualSmoothingXY.cpp.

References matrix::Quaternion< Type >::conjugate_inversed(), and f().

Referenced by _getIntention(), and overwriteJerkMin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _setStateAcceleration()

void ManualSmoothingXY::_setStateAcceleration	(	const matrix::Vector2f &	vel_sp,
		const matrix::Vector2f &	vel,
		const Intention &	intention,
		const float	dt
	)

private

Set acceleration depending on Intention.

Parameters

vel_sp	is desired velocity septoint in xy-direction
vel	is vehicle velociy in xy-direction
intention	is the user intention during flight
dt	is delta-time

Definition at line 122 of file ManualSmoothingXY.cpp.

References _acc_state_dependent, _intention, _jerk_state_dependent, _vel_max, _vel_sp_prev, acceleration, brake, deceleration, direction_change, FLT_EPSILON, matrix::Vector< Type, M >::length(), and math::min().

Referenced by _updateAcceleration(), and overwriteJerkMin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _updateAcceleration()

void ManualSmoothingXY::_updateAcceleration	(	matrix::Vector2f &	vel_sp,
		const matrix::Vector2f &	vel,
		const float &	yaw,
		const float &	yawrate_sp,
		const float	dt
	)

private

Sets velocity change limits (=acceleration).

Depending on the user intention, the acceleration differs.

Parameters

vel_sp	is desired velocity setpoint before slewrate.
vel	is current velocity in horizontal direction
yaw	is vehicle yaw
yawrate_sp	is desired yawspeed
dt	is delta-time

Definition at line 63 of file ManualSmoothingXY.cpp.

References _getIntention(), and _setStateAcceleration().

Referenced by overwriteJerkMin(), and smoothVelocity().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _velocitySlewRate()

void ManualSmoothingXY::_velocitySlewRate	(	matrix::Vector2f &	vel_sp,
		const float	dt
	)

private

Limits the velocity setpoint change.

Parameters

vel_sp	that gets limited based on acceleration and previous velocity setpoint
dt	is delta-time

Definition at line 199 of file ManualSmoothingXY.cpp.

References _acc_state_dependent, _vel_sp_prev, FLT_EPSILON, and matrix::Vector< Type, M >::normalized().

Referenced by overwriteJerkMin(), and smoothVelocity().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getIntention()

Intention ManualSmoothingXY::getIntention ( )

inline

Get user intention.

See also: Intention

Definition at line 94 of file ManualSmoothingXY.hpp.

References _intention.

◆ overwriteDecelerationMin()

void ManualSmoothingXY::overwriteDecelerationMin ( float dec )

inline

Definition at line 102 of file ManualSmoothingXY.hpp.

◆ overwriteHoverAcceleration()

void ManualSmoothingXY::overwriteHoverAcceleration ( float acc )

inline

Overwrite methods: Needed if different parameter values than default required.

Definition at line 100 of file ManualSmoothingXY.hpp.

◆ overwriteJerkMax()

void ManualSmoothingXY::overwriteJerkMax ( float jerk )

inline

Definition at line 103 of file ManualSmoothingXY.hpp.

◆ overwriteJerkMin()

void ManualSmoothingXY::overwriteJerkMin ( float jerk )

inline

Definition at line 104 of file ManualSmoothingXY.hpp.

References _getHeadingToWorldFrame(), _getIntention(), _getWorldToHeadingFrame(), _setStateAcceleration(), _updateAcceleration(), and _velocitySlewRate().

Here is the call graph for this function:

◆ overwriteMaxAcceleration()

void ManualSmoothingXY::overwriteMaxAcceleration ( float acc )

inline

Definition at line 101 of file ManualSmoothingXY.hpp.

◆ smoothVelocity()

void ManualSmoothingXY::smoothVelocity	(	matrix::Vector2f &	vel_sp,
		const matrix::Vector2f &	vel,
		const float &	yaw,
		const float &	yawrate_sp,
		const float	dt
	)

Smoothing of velocity setpoint horizontally based on flight direction.

Parameters

vel_sp	velocity setpoint in xy
dt	time delta in seconds

Definition at line 54 of file ManualSmoothingXY.cpp.

References _updateAcceleration(), _vel_sp_prev, and _velocitySlewRate().

Referenced by FlightTaskManualPositionSmooth::_updateSetpoints(), and updateMaxVelocity().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateMaxVelocity()

void ManualSmoothingXY::updateMaxVelocity ( const float & vel_max )

inline

Maximum velocity is required to detect user intention.

Maximum velocity depends on flight-task. In order to deduce user intention from velocity, the maximum allowed velocity has to be updated.

Parameters

vel_max corresponds to vehicle constraint

Definition at line 66 of file ManualSmoothingXY.hpp.

References _vel_max, dt, and smoothVelocity().

Referenced by FlightTaskManualPositionSmooth::_updateSetpoints().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _acc_state_dependent

float ManualSmoothingXY::_acc_state_dependent

private

velocity change limit that depends on Intention

Definition at line 162 of file ManualSmoothingXY.hpp.

Referenced by _setStateAcceleration(), _velocitySlewRate(), and ManualSmoothingXY().

◆ _intention

Intention ManualSmoothingXY::_intention {Intention::acceleration}

private

user intention

Definition at line 161 of file ManualSmoothingXY.hpp.

Referenced by _setStateAcceleration(), and getIntention().

◆ _jerk_state_dependent

float ManualSmoothingXY::_jerk_state_dependent

private

acceleration change limit during brake

Definition at line 163 of file ManualSmoothingXY.hpp.

Referenced by _setStateAcceleration(), and ManualSmoothingXY().

◆ _vel_max

float ManualSmoothingXY::_vel_max {10.0f}

private

maximum horizontal speed allowed

Definition at line 164 of file ManualSmoothingXY.hpp.

Referenced by _getIntention(), _setStateAcceleration(), and updateMaxVelocity().

◆ _vel_sp_prev

matrix::Vector2f ManualSmoothingXY::_vel_sp_prev

private

previous velocity setpoint

Definition at line 165 of file ManualSmoothingXY.hpp.

Referenced by _getIntention(), _setStateAcceleration(), _velocitySlewRate(), and smoothVelocity().

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

src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.hpp
src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.cpp

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ Intention

Constructor & Destructor Documentation

◆ ManualSmoothingXY()

◆ ~ManualSmoothingXY()

Member Function Documentation

◆ _getHeadingToWorldFrame()

◆ _getIntention()

◆ _getWorldToHeadingFrame()

◆ _setStateAcceleration()

◆ _updateAcceleration()

◆ _velocitySlewRate()

◆ getIntention()

◆ overwriteDecelerationMin()

◆ overwriteHoverAcceleration()

◆ overwriteJerkMax()

◆ overwriteJerkMin()

◆ overwriteMaxAcceleration()

◆ smoothVelocity()

◆ updateMaxVelocity()

Member Data Documentation

◆ _acc_state_dependent

◆ _intention

◆ _jerk_state_dependent

◆ _vel_max

◆ _vel_sp_prev