PX4 Firmware
PX4 Autopilot Software http://px4.io
AttitudeControl Class Reference

#include <AttitudeControl.hpp>

Collaboration diagram for AttitudeControl:

Public Member Functions

 AttitudeControl ()=default
 
 ~AttitudeControl ()=default
 
void setProportionalGain (const matrix::Vector3f &proportional_gain)
 Set proportional attitude control gain. More...
 
void setRateLimit (const matrix::Vector3f &rate_limit)
 Set hard limit for output rate setpoints. More...
 
matrix::Vector3f update (matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
 Run one control loop cycle calculation. More...
 

Private Attributes

matrix::Vector3f _proportional_gain
 
matrix::Vector3f _rate_limit
 
float _yaw_w = 0.0f
 yaw weight [0,1] to prioritize roll and pitch More...
 

Detailed Description

Definition at line 53 of file AttitudeControl.hpp.

Constructor & Destructor Documentation

◆ AttitudeControl()

AttitudeControl::AttitudeControl ( )
default

◆ ~AttitudeControl()

AttitudeControl::~AttitudeControl ( )
default

Member Function Documentation

◆ setProportionalGain()

void AttitudeControl::setProportionalGain ( const matrix::Vector3f proportional_gain)

Set proportional attitude control gain.

Parameters
proportional_gain3D vector containing gains for roll, pitch, yaw

Definition at line 45 of file AttitudeControl.cpp.

References math::constrain(), and f().

Referenced by MulticopterAttitudeControl::parameters_updated().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setRateLimit()

void AttitudeControl::setRateLimit ( const matrix::Vector3f rate_limit)
inline

Set hard limit for output rate setpoints.

Parameters
rate_limit[rad/s] 3D vector containing limits for roll, pitch, yaw

Definition at line 69 of file AttitudeControl.hpp.

References _rate_limit, and update().

Referenced by MulticopterAttitudeControl::parameters_updated().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ update()

matrix::Vector3f AttitudeControl::update ( matrix::Quatf  q,
matrix::Quatf  qd,
float  yawspeed_feedforward 
)

Run one control loop cycle calculation.

Parameters
qestimation of the current vehicle attitude unit quaternion
qddesired vehicle attitude setpoint
yawspeed_feedforward[rad/s] yaw feed forward angular rate in world frame
Returns
[rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller

Definition at line 56 of file AttitudeControl.cpp.

References math::constrain(), matrix::Quaternion< Type >::dcm_z(), matrix::Matrix< Type, M, N >::emult(), f(), matrix::Quaternion< Type >::imag(), matrix::Quaternion< Type >::inversed(), matrix::Vector< Type, M >::normalize(), and math::signNoZero().

Referenced by MulticopterAttitudeControl::control_attitude(), setRateLimit(), and TEST().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ _proportional_gain

matrix::Vector3f AttitudeControl::_proportional_gain
private

Definition at line 81 of file AttitudeControl.hpp.

◆ _rate_limit

matrix::Vector3f AttitudeControl::_rate_limit
private

Definition at line 82 of file AttitudeControl.hpp.

Referenced by setRateLimit().

◆ _yaw_w

float AttitudeControl::_yaw_w = 0.0f
private

yaw weight [0,1] to prioritize roll and pitch

Definition at line 83 of file AttitudeControl.hpp.


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