#include <RateControl.hpp>

Collaboration diagram for RateControl:

Public Member Functions
	RateControl ()=default

	~RateControl ()=default

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

void	setIntegratorLimit (const matrix::Vector3f &integrator_limit)
	Set the mximum absolute value of the integrator for all axes. More...

void	setDTermCutoff (const float loop_rate, const float cutoff, const bool force)
	Set update frequency and low-pass filter cutoff that is applied to the derivative term. More...

void	setFeedForwardGain (const matrix::Vector3f &FF)
	Set direct rate to torque feed forward gain. More...

void	setSaturationStatus (const MultirotorMixer::saturation_status &status)
	Set saturation status. More...

matrix::Vector3f	update (const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed)
	Run one control loop cycle calculation. More...

void	resetIntegral ()
	Set the integral term to 0 to prevent windup. More...

void	getRateControlStatus (rate_ctrl_status_s &rate_ctrl_status)
	Get status message of controller for logging/debugging. More...

Private Member Functions
void	updateIntegral (matrix::Vector3f &rate_error, const float dt)

Private Attributes
matrix::Vector3f	_gain_p
	rate control proportional gain for all axes x, y, z More...

matrix::Vector3f	_gain_i
	rate control integral gain More...

matrix::Vector3f	_gain_d
	rate control derivative gain More...

matrix::Vector3f	_lim_int
	integrator term maximum absolute value More...

matrix::Vector3f	_gain_ff
	direct rate to torque feed forward gain only useful for helicopters More...

matrix::Vector3f	_rate_prev
	angular rates of previous update More...

matrix::Vector3f	_rate_prev_filtered
	low-pass filtered angular rates of previous update More...

matrix::Vector3f	_rate_int
	integral term of the rate controller More...

math::LowPassFilter2pVector3f	_lp_filters_d {0.f, 0.f}
	low-pass filters for D-term (roll, pitch & yaw) More...

bool	_mixer_saturation_positive [3] {}

bool	_mixer_saturation_negative [3] {}

Detailed Description

Definition at line 48 of file RateControl.hpp.

Constructor & Destructor Documentation

◆ RateControl()

RateControl::RateControl ( )

default

◆ ~RateControl()

RateControl::~RateControl ( )

default

Member Function Documentation

◆ getRateControlStatus()

void RateControl::getRateControlStatus ( rate_ctrl_status_s & rate_ctrl_status )

Get status message of controller for logging/debugging.

Parameters

rate_ctrl_status status message to fill with internal states

Definition at line 128 of file RateControl.cpp.

References rate_ctrl_status_s::pitchspeed_integ, rate_ctrl_status_s::rollspeed_integ, and rate_ctrl_status_s::yawspeed_integ.

Referenced by resetIntegral(), and MulticopterRateControl::Run().

Here is the caller graph for this function:

◆ resetIntegral()

void RateControl::resetIntegral ( )

inline

Set the integral term to 0 to prevent windup.

See also: _rate_int

Definition at line 103 of file RateControl.hpp.

References _rate_int, getRateControlStatus(), updateIntegral(), and matrix::Matrix< Type, M, N >::zero().

Referenced by MulticopterRateControl::Run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setDTermCutoff()

void RateControl::setDTermCutoff	(	const float	loop_rate,
		const float	cutoff,
		const bool	force
	)

Set update frequency and low-pass filter cutoff that is applied to the derivative term.

Parameters

loop_rate	[Hz] rate with which update function is called
cutoff	[Hz] cutoff frequency for the low-pass filter on the dervative term
force	flag to force an expensive update even if the cutoff didn't change

Definition at line 50 of file RateControl.cpp.

References f().

Referenced by MulticopterRateControl::parameters_updated(), MulticopterRateControl::Run(), and setIntegratorLimit().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setFeedForwardGain()

void RateControl::setFeedForwardGain ( const matrix::Vector3f & FF )

inline

Set direct rate to torque feed forward gain.

See also: _gain_ff

Parameters

FF	3D vector of feed forward gains for body x,y,z axis

Definition at line 81 of file RateControl.hpp.

References _gain_ff, dt, setSaturationStatus(), status, and update().

Referenced by MulticopterRateControl::parameters_updated().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setGains()

void RateControl::setGains	(	const matrix::Vector3f &	P,
		const matrix::Vector3f &	I,
		const matrix::Vector3f &	D
	)

Set the rate control gains.

Parameters

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

Definition at line 43 of file RateControl.cpp.

References P.

Referenced by MulticopterRateControl::parameters_updated().

Here is the caller graph for this function:

◆ setIntegratorLimit()

void RateControl::setIntegratorLimit ( const matrix::Vector3f & integrator_limit )

inline

Set the mximum absolute value of the integrator for all axes.

Parameters

integrator_limit limit value for all axes x, y, z

Definition at line 66 of file RateControl.hpp.

References _lim_int, and setDTermCutoff().

Referenced by MulticopterRateControl::parameters_updated().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setSaturationStatus()

void RateControl::setSaturationStatus ( const MultirotorMixer::saturation_status & status )

Set saturation status.

Parameters

status message from mixer reporting about saturation

Definition at line 59 of file RateControl.cpp.

References MultirotorMixer::saturation_status::flags, MultirotorMixer::saturation_status::pitch_neg, MultirotorMixer::saturation_status::pitch_pos, MultirotorMixer::saturation_status::roll_neg, MultirotorMixer::saturation_status::roll_pos, MultirotorMixer::saturation_status::yaw_neg, and MultirotorMixer::saturation_status::yaw_pos.

Referenced by MulticopterRateControl::Run(), and setFeedForwardGain().

Here is the caller graph for this function:

◆ update()

Vector3f RateControl::update	(	const matrix::Vector3f &	rate,
		const matrix::Vector3f &	rate_sp,
		const float	dt,
		const bool	landed
	)

Run one control loop cycle calculation.

Parameters

rate	estimation of the current vehicle angular rate
rate_sp	desired vehicle angular rate setpoint
dt	desired vehicle angular rate setpoint

Returns: [-1,1] normalized torque vector to apply to the vehicle

Definition at line 69 of file RateControl.cpp.

References matrix::Matrix< Type, M, N >::emult(), and FLT_EPSILON.

Referenced by MulticopterRateControl::Run(), setFeedForwardGain(), and TEST().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateIntegral()

void RateControl::updateIntegral	(	matrix::Vector3f &	rate_error,
		const float	dt
	)

private

Definition at line 96 of file RateControl.cpp.

References math::constrain(), dt, f(), math::max(), math::min(), and math::radians().

Referenced by resetIntegral().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _gain_d

matrix::Vector3f RateControl::_gain_d

private

rate control derivative gain

Definition at line 117 of file RateControl.hpp.

◆ _gain_ff

matrix::Vector3f RateControl::_gain_ff

private

direct rate to torque feed forward gain only useful for helicopters

Definition at line 119 of file RateControl.hpp.

Referenced by setFeedForwardGain().

◆ _gain_i

matrix::Vector3f RateControl::_gain_i

private

rate control integral gain

Definition at line 116 of file RateControl.hpp.

◆ _gain_p

matrix::Vector3f RateControl::_gain_p

private

rate control proportional gain for all axes x, y, z

Definition at line 115 of file RateControl.hpp.

◆ _lim_int

matrix::Vector3f RateControl::_lim_int

private

integrator term maximum absolute value

Definition at line 118 of file RateControl.hpp.

Referenced by setIntegratorLimit().

◆ _lp_filters_d

math::LowPassFilter2pVector3f RateControl::_lp_filters_d {0.f, 0.f}

private

low-pass filters for D-term (roll, pitch & yaw)

Definition at line 125 of file RateControl.hpp.

◆ _mixer_saturation_negative

bool RateControl::_mixer_saturation_negative[3] {}

private

Definition at line 127 of file RateControl.hpp.

◆ _mixer_saturation_positive

bool RateControl::_mixer_saturation_positive[3] {}

private

Definition at line 126 of file RateControl.hpp.

◆ _rate_int

matrix::Vector3f RateControl::_rate_int

private

integral term of the rate controller

Definition at line 124 of file RateControl.hpp.

Referenced by resetIntegral().

◆ _rate_prev

matrix::Vector3f RateControl::_rate_prev

private

angular rates of previous update

Definition at line 122 of file RateControl.hpp.

◆ _rate_prev_filtered

matrix::Vector3f RateControl::_rate_prev_filtered

private

low-pass filtered angular rates of previous update

Definition at line 123 of file RateControl.hpp.

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

src/modules/mc_rate_control/RateControl/RateControl.hpp
src/modules/mc_rate_control/RateControl/RateControl.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RateControl()

◆ ~RateControl()

Member Function Documentation

◆ getRateControlStatus()

◆ resetIntegral()

◆ setDTermCutoff()

◆ setFeedForwardGain()

◆ setGains()

◆ setIntegratorLimit()

◆ setSaturationStatus()

◆ update()

◆ updateIntegral()

Member Data Documentation

◆ _gain_d

◆ _gain_ff

◆ _gain_i

◆ _gain_p

◆ _lim_int

◆ _lp_filters_d

◆ _mixer_saturation_negative

◆ _mixer_saturation_positive

◆ _rate_int

◆ _rate_prev

◆ _rate_prev_filtered