Core Position-Control for MC. More...

#include <PositionControl.hpp>

Collaboration diagram for PositionControl:

Public Member Functions
	PositionControl ()=default

	~PositionControl ()=default

void	setPositionGains (const matrix::Vector3f &P)
	Set the position control gains. More...

void	setVelocityGains (const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D)
	Set the velocity control gains. More...

void	setVelocityLimits (const float vel_horizontal, const float vel_up, float vel_down)
	Set the maximum velocity to execute with feed forward and position control. More...

void	setThrustLimits (const float min, const float max)
	Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller. More...

void	setTiltLimit (const float tilt)
	Set the maximum tilt angle in radians the output attitude is allowed to have. More...

void	setHoverThrust (const float thrust)
	Set the maximum tilt angle in radians the output attitude is allowed to have. More...

void	updateState (const PositionControlStates &states)
	Update the current vehicle state. More...

bool	updateSetpoint (const vehicle_local_position_setpoint_s &setpoint)
	Update the desired setpoints. More...

void	updateConstraints (const vehicle_constraints_s &constraints)
	Set constraints that are stricter than the global limits. More...

void	generateThrustYawSetpoint (const float dt)
	Apply P-position and PID-velocity controller that updates the member thrust, yaw- and yawspeed-setpoints. More...

void	resetIntegralXY ()
	Set the integral term in xy to 0. More...

void	resetIntegralZ ()
	Set the integral term in z to 0. More...

const matrix::Vector3f	getVelSp () const
	Get the. More...

void	getLocalPositionSetpoint (vehicle_local_position_setpoint_s &local_position_setpoint) const
	Get the controllers output local position setpoint These setpoints are the ones which were executed on including PID output and feed-forward. More...

void	getAttitudeSetpoint (vehicle_attitude_setpoint_s &attitude_setpoint) const
	Get the controllers output attitude setpoint This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. More...

Private Member Functions
bool	_interfaceMapping ()
	Maps setpoints to internal-setpoints. More...

void	_positionController ()

void	_velocityController (const float &dt)
	applies the P-position-controller More...

void	_setCtrlFlag (bool value)
	applies the PID-velocity-controller More...

Private Attributes
matrix::Vector3f	_gain_pos_p
	Position control proportional gain. More...

matrix::Vector3f	_gain_vel_p
	Velocity control proportional gain. More...

matrix::Vector3f	_gain_vel_i
	Velocity control integral gain. More...

matrix::Vector3f	_gain_vel_d
	Velocity control derivative gain. More...

float	_lim_vel_horizontal {}
	Horizontal velocity limit with feed forward and position control. More...

float	_lim_vel_up {}
	Upwards velocity limit with feed forward and position control. More...

float	_lim_vel_down {}
	Downwards velocity limit with feed forward and position control. More...

float	_lim_thr_min {}
	Minimum collective thrust allowed as output [-1,0] e.g. -0.9. More...

float	_lim_thr_max {}
	Maximum collective thrust allowed as output [-1,0] e.g. -0.1. More...

float	_lim_tilt {}
	Maximum tilt from level the output attitude is allowed to have. More...

float	_hover_thrust {}
	Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation. More...

matrix::Vector3f	_pos
	current position More...

matrix::Vector3f	_vel
	current velocity More...

matrix::Vector3f	_vel_dot
	velocity derivative (replacement for acceleration estimate) More...

matrix::Vector3f	_thr_int
	integral term of the velocity controller More...

float	_yaw {}
	current heading More...

vehicle_constraints_s	_constraints {}
	variable constraints More...

matrix::Vector3f	_pos_sp
	desired position More...

matrix::Vector3f	_vel_sp
	desired velocity More...

matrix::Vector3f	_acc_sp
	desired acceleration More...

matrix::Vector3f	_thr_sp
	desired thrust More...

float	_yaw_sp {}
	desired heading More...

float	_yawspeed_sp {}

bool	_skip_controller {false}
	desired yaw-speed More...

bool	_ctrl_pos [3] = {true, true, true}
	True if the control-loop for position was used. More...

bool	_ctrl_vel [3] = {true, true, true}
	True if the control-loop for velocity was used. More...

Detailed Description

Core Position-Control for MC.

This class contains P-controller for position and PID-controller for velocity. Inputs: vehicle position/velocity/yaw desired set-point position/velocity/thrust/yaw/yaw-speed constraints that are stricter than global limits Output thrust vector and a yaw-setpoint

If there is a position and a velocity set-point present, then the velocity set-point is used as feed-forward. If feed-forward is active, then the velocity component of the P-controller output has priority over the feed-forward component.

A setpoint that is NAN is considered as not set. If there is a position/velocity- and thrust-setpoint present, then the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop.

Definition at line 75 of file PositionControl.hpp.

Constructor & Destructor Documentation

◆ PositionControl()

PositionControl::PositionControl ( )

default

◆ ~PositionControl()

PositionControl::~PositionControl ( )

default

Member Function Documentation

◆ _interfaceMapping()

bool PositionControl::_interfaceMapping ( )

private

Maps setpoints to internal-setpoints.

Returns: true if mapping succeeded.

Definition at line 128 of file PositionControl.cpp.

References f().

Here is the call graph for this function:

◆ _positionController()

void PositionControl::_positionController ( )

private

Definition at line 231 of file PositionControl.cpp.

References math::constrain(), and ControlMath::constrainXY().

Here is the call graph for this function:

◆ _setCtrlFlag()

void PositionControl::_setCtrlFlag ( bool value )

private

applies the PID-velocity-controller

set control-loop flags (only required for logging)

Definition at line 74 of file PositionControl.cpp.

◆ _velocityController()

void PositionControl::_velocityController ( const float & dt )

private

applies the P-position-controller

Definition at line 247 of file PositionControl.cpp.

References math::constrain(), dt, f(), matrix::Vector< Type, M >::length(), math::min(), and math::sign().

Here is the call graph for this function:

◆ generateThrustYawSetpoint()

void PositionControl::generateThrustYawSetpoint ( const float dt )

Apply P-position and PID-velocity controller that updates the member thrust, yaw- and yawspeed-setpoints.

See also: _thr_sp; _yaw_sp; _yawspeed_sp

Parameters

dt	the delta-time

Definition at line 102 of file PositionControl.cpp.

References FLT_EPSILON.

Referenced by MulticopterPositionControl::Run().

Here is the caller graph for this function:

◆ getAttitudeSetpoint()

void PositionControl::getAttitudeSetpoint ( vehicle_attitude_setpoint_s & attitude_setpoint ) const

Get the controllers output attitude setpoint This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.

It needs to be executed by the attitude controller to achieve velocity and position tracking.

Parameters

attitude_setpoint reference to struct to fill up

Definition at line 381 of file PositionControl.cpp.

References ControlMath::thrustToAttitude(), and vehicle_attitude_setpoint_s::yaw_sp_move_rate.

Referenced by MulticopterPositionControl::Run(), and TEST().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getLocalPositionSetpoint()

void PositionControl::getLocalPositionSetpoint ( vehicle_local_position_setpoint_s & local_position_setpoint ) const

Get the controllers output local position setpoint These setpoints are the ones which were executed on including PID output and feed-forward.

The acceleration or thrust setpoints can be used for attitude control.

Parameters

local_position_setpoint reference to struct to fill up

Definition at line 367 of file PositionControl.cpp.

References vehicle_local_position_setpoint_s::acceleration, vehicle_local_position_setpoint_s::thrust, vehicle_local_position_setpoint_s::vx, vehicle_local_position_setpoint_s::vy, vehicle_local_position_setpoint_s::vz, vehicle_local_position_setpoint_s::x, vehicle_local_position_setpoint_s::y, vehicle_local_position_setpoint_s::yaw, vehicle_local_position_setpoint_s::yawspeed, and vehicle_local_position_setpoint_s::z.

Referenced by MulticopterPositionControl::Run(), and TEST().

Here is the caller graph for this function:

◆ getVelSp()

const matrix::Vector3f PositionControl::getVelSp ( ) const

inline

Get the.

See also: _vel_sp

Returns: The velocity set-point that was executed in the control-loop. Nan if velocity control-loop was skipped.

Definition at line 169 of file PositionControl.hpp.

Referenced by MulticopterPositionControl::Run().

Here is the caller graph for this function:

◆ resetIntegralXY()

void PositionControl::resetIntegralXY ( )

inline

Set the integral term in xy to 0.

See also: _thr_int

Definition at line 156 of file PositionControl.hpp.

Referenced by MulticopterPositionControl::limit_thrust_during_landing(), and MulticopterPositionControl::Run().

Here is the caller graph for this function:

◆ resetIntegralZ()

void PositionControl::resetIntegralZ ( )

inline

Set the integral term in z to 0.

See also: _thr_int

Definition at line 162 of file PositionControl.hpp.

Referenced by MulticopterPositionControl::limit_thrust_during_landing(), and MulticopterPositionControl::Run().

Here is the caller graph for this function:

◆ setHoverThrust()

void PositionControl::setHoverThrust ( const float thrust )

inline

Set the maximum tilt angle in radians the output attitude is allowed to have.

Parameters

thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation

Definition at line 121 of file PositionControl.hpp.

References dt.

Referenced by MulticopterPositionControl::parameters_update().

Here is the caller graph for this function:

◆ setPositionGains()

void PositionControl::setPositionGains ( const matrix::Vector3f & P )

inline

Set the position control gains.

Parameters

P	3D vector of proportional gains for x,y,z axis

Definition at line 86 of file PositionControl.hpp.

References math::max(), math::min(), and P.

Referenced by MulticopterPositionControl::parameters_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setThrustLimits()

void PositionControl::setThrustLimits	(	const float	min,
		const float	max
	)

Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller.

Parameters

min	minimum thrust e.g. 0.1 or 0
max	maximum thrust e.g. 0.9 or 1

Definition at line 60 of file PositionControl.cpp.

References matrix::max(), and matrix::min().

Referenced by MulticopterPositionControl::parameters_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setTiltLimit()

void PositionControl::setTiltLimit ( const float tilt )

inline

Set the maximum tilt angle in radians the output attitude is allowed to have.

Parameters

tilt	angle from level orientation in radians

Definition at line 115 of file PositionControl.hpp.

Referenced by MulticopterPositionControl::parameters_update().

Here is the caller graph for this function:

◆ setVelocityGains()

void PositionControl::setVelocityGains	(	const matrix::Vector3f &	P,
		const matrix::Vector3f &	I,
		const matrix::Vector3f &	D
	)

Set the velocity control gains.

Parameters

P	3D vector of proportional gains for x,y,z axis
I	3D vector of integral gains
D	3D vector of derivative gains

Definition at line 46 of file PositionControl.cpp.

References P.

Referenced by MulticopterPositionControl::parameters_update().

Here is the caller graph for this function:

◆ setVelocityLimits()

void PositionControl::setVelocityLimits	(	const float	vel_horizontal,
		const float	vel_up,
		float	vel_down
	)

Set the maximum velocity to execute with feed forward and position control.

Parameters

vel_horizontal	horizontal velocity limit
vel_up	upwards velocity limit
vel_down	downwards velocity limit

Definition at line 53 of file PositionControl.cpp.

Referenced by MulticopterPositionControl::parameters_update().

Here is the caller graph for this function:

◆ updateConstraints()

void PositionControl::updateConstraints ( const vehicle_constraints_s & constraints )

Set constraints that are stricter than the global limits.

Parameters

constraints a PositionControl structure with supported constraints

Definition at line 342 of file PositionControl.cpp.

References vehicle_constraints_s::speed_down, vehicle_constraints_s::speed_up, vehicle_constraints_s::speed_xy, and vehicle_constraints_s::tilt.

Referenced by MulticopterPositionControl::Run().

Here is the caller graph for this function:

◆ updateSetpoint()

bool PositionControl::updateSetpoint ( const vehicle_local_position_setpoint_s & setpoint )

Update the desired setpoints.

Parameters

setpoint a vehicle_local_position_setpoint_s structure

Returns: true if setpoint has updated correctly

Definition at line 81 of file PositionControl.cpp.

References vehicle_local_position_setpoint_s::acceleration, vehicle_local_position_setpoint_s::thrust, vehicle_local_position_setpoint_s::vx, vehicle_local_position_setpoint_s::vy, vehicle_local_position_setpoint_s::vz, vehicle_local_position_setpoint_s::x, vehicle_local_position_setpoint_s::y, vehicle_local_position_setpoint_s::yaw, vehicle_local_position_setpoint_s::yawspeed, and vehicle_local_position_setpoint_s::z.

Referenced by MulticopterPositionControl::Run().

Here is the caller graph for this function:

◆ updateState()

void PositionControl::updateState ( const PositionControlStates & states )

Update the current vehicle state.

Parameters

PositionControlStates structure

Definition at line 66 of file PositionControl.cpp.

References PositionControlStates::acceleration, PositionControlStates::position, PositionControlStates::velocity, and PositionControlStates::yaw.

Referenced by MulticopterPositionControl::Run().

Here is the caller graph for this function:

Member Data Documentation

◆ _acc_sp

matrix::Vector3f PositionControl::_acc_sp

private

desired acceleration

Definition at line 240 of file PositionControl.hpp.

◆ _constraints

vehicle_constraints_s PositionControl::_constraints {}

private

variable constraints

Definition at line 235 of file PositionControl.hpp.

◆ _ctrl_pos

bool PositionControl::_ctrl_pos[3] = {true, true, true}

private

True if the control-loop for position was used.

Definition at line 246 of file PositionControl.hpp.

◆ _ctrl_vel

bool PositionControl::_ctrl_vel[3] = {true, true, true}

private

True if the control-loop for velocity was used.

Definition at line 247 of file PositionControl.hpp.

◆ _gain_pos_p

matrix::Vector3f PositionControl::_gain_pos_p

private

Position control proportional gain.

Definition at line 213 of file PositionControl.hpp.

◆ _gain_vel_d

matrix::Vector3f PositionControl::_gain_vel_d

private

Velocity control derivative gain.

Definition at line 216 of file PositionControl.hpp.

◆ _gain_vel_i

matrix::Vector3f PositionControl::_gain_vel_i

private

Velocity control integral gain.

Definition at line 215 of file PositionControl.hpp.

◆ _gain_vel_p

matrix::Vector3f PositionControl::_gain_vel_p

private

Velocity control proportional gain.

Definition at line 214 of file PositionControl.hpp.

◆ _hover_thrust

float PositionControl::_hover_thrust {}

private

Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation.

Definition at line 226 of file PositionControl.hpp.

◆ _lim_thr_max

float PositionControl::_lim_thr_max {}

private

Maximum collective thrust allowed as output [-1,0] e.g. -0.1.

Definition at line 223 of file PositionControl.hpp.

◆ _lim_thr_min

float PositionControl::_lim_thr_min {}

private

Minimum collective thrust allowed as output [-1,0] e.g. -0.9.

Definition at line 222 of file PositionControl.hpp.

◆ _lim_tilt

float PositionControl::_lim_tilt {}

private

Maximum tilt from level the output attitude is allowed to have.

Definition at line 224 of file PositionControl.hpp.

◆ _lim_vel_down

float PositionControl::_lim_vel_down {}

private

Downwards velocity limit with feed forward and position control.

Definition at line 221 of file PositionControl.hpp.

◆ _lim_vel_horizontal

float PositionControl::_lim_vel_horizontal {}

private

Horizontal velocity limit with feed forward and position control.

Definition at line 219 of file PositionControl.hpp.

◆ _lim_vel_up

float PositionControl::_lim_vel_up {}

private

Upwards velocity limit with feed forward and position control.

Definition at line 220 of file PositionControl.hpp.

◆ _pos

matrix::Vector3f PositionControl::_pos

private

current position

Definition at line 229 of file PositionControl.hpp.

◆ _pos_sp

matrix::Vector3f PositionControl::_pos_sp

private

desired position

Definition at line 238 of file PositionControl.hpp.

◆ _skip_controller

bool PositionControl::_skip_controller {false}

private

desired yaw-speed

skips position/velocity controller. true for stabilized mode

Definition at line 245 of file PositionControl.hpp.

◆ _thr_int

matrix::Vector3f PositionControl::_thr_int

private

integral term of the velocity controller

Definition at line 232 of file PositionControl.hpp.

◆ _thr_sp

matrix::Vector3f PositionControl::_thr_sp

private

desired thrust

Definition at line 241 of file PositionControl.hpp.

◆ _vel

matrix::Vector3f PositionControl::_vel

private

current velocity

Definition at line 230 of file PositionControl.hpp.

◆ _vel_dot

matrix::Vector3f PositionControl::_vel_dot

private

velocity derivative (replacement for acceleration estimate)

Definition at line 231 of file PositionControl.hpp.

◆ _vel_sp

matrix::Vector3f PositionControl::_vel_sp

private

desired velocity

Definition at line 239 of file PositionControl.hpp.

◆ _yaw

float PositionControl::_yaw {}

private

current heading

Definition at line 233 of file PositionControl.hpp.

◆ _yaw_sp

float PositionControl::_yaw_sp {}

private

desired heading

Definition at line 242 of file PositionControl.hpp.

◆ _yawspeed_sp

float PositionControl::_yawspeed_sp {}

private

Definition at line 243 of file PositionControl.hpp.

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

src/modules/mc_pos_control/PositionControl/PositionControl.hpp
src/modules/mc_pos_control/PositionControl/PositionControl.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PositionControl()

◆ ~PositionControl()

Member Function Documentation

◆ _interfaceMapping()

◆ _positionController()

◆ _setCtrlFlag()

◆ _velocityController()

◆ generateThrustYawSetpoint()

◆ getAttitudeSetpoint()

◆ getLocalPositionSetpoint()

◆ getVelSp()

◆ resetIntegralXY()

◆ resetIntegralZ()

◆ setHoverThrust()

◆ setPositionGains()

◆ setThrustLimits()

◆ setTiltLimit()

◆ setVelocityGains()

◆ setVelocityLimits()

◆ updateConstraints()

◆ updateSetpoint()

◆ updateState()

Member Data Documentation

◆ _acc_sp

◆ _constraints

◆ _ctrl_pos

◆ _ctrl_vel

◆ _gain_pos_p

◆ _gain_vel_d

◆ _gain_vel_i

◆ _gain_vel_p

◆ _hover_thrust

◆ _lim_thr_max

◆ _lim_thr_min

◆ _lim_tilt

◆ _lim_vel_down

◆ _lim_vel_horizontal

◆ _lim_vel_up

◆ _pos

◆ _pos_sp

◆ _skip_controller

◆ _thr_int

◆ _thr_sp

◆ _vel

◆ _vel_dot

◆ _vel_sp

◆ _yaw

◆ _yaw_sp

◆ _yawspeed_sp