Struct AttitudeController 

pub struct AttitudeController {

    pub ang_vel_body: Vector3<f32>,
    pub actuator_sysid: Vector3<f32>,
    pub sysid_ang_vel_body: Vector3<f32>,
    pub feed_forward_scalar: f32,
    pub throttle_rpy_mix: f32,
    pub throttle_rpy_mix_desired: f32,
    pub attitude_control_max: f32,
    pub dt: f32,
    pub accel_max: Vector3<f32>,
    pub use_sqrt_controller: bool,
    pub p_angle_roll: P,
    pub p_angle_pitch: P,
    pub p_angle_yaw: P,
    pub thrust_error_angle: f32,
    pub attitude_target: Quaternion<f32>,
    pub ang_vel_max: Vector3<f32>,
    pub ang_vel_target: Vector3<f32>,
    pub rate_bf_ff_enabled: bool,
    pub input_tc: f32,
    pub euler_angle_target: Vector3<f32>,
    pub euler_rate_target: Vector3<f32>,
}

The attitude controller works around the concept of the desired attitude, target attitude and measured attitude. The desired attitude is the attitude input into the attitude controller that expresses where the higher level code would like the aircraft to move to. The target attitude is moved to the desired attitude with jerk, acceleration, and velocity limits. The target angular velocities are fed directly into the rate controllers. The angular error between the measured attitude and the target attitude is fed into the angle controller and the output of the angle controller summed at the input of the rate controllers. By feeding the target angular velocity directly into the rate controllers the measured and target attitudes remain very close together.

All input functions below follow the same procedure

1. define the desired attitude the aircraft should attempt to achieve using the input variables
2. using the desired attitude and input variables, define the target angular velocity so that it should move the target attitude towards the desired attitude
3. if _rate_bf_ff_enabled is not being used then make the target attitude and target angular velocities equal to the desired attitude and desired angular velocities.
4. ensure _attitude_target, _euler_angle_target, _euler_rate_target and _ang_vel_target have been defined. This ensures input modes can be changed without discontinuity.
5. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and integrate them into the target attitude. Any errors between the target attitude and the measured attitude are corrected by first correcting the thrust vector until the angle between the target thrust vector measured trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.

Fields

ang_vel_body: Vector3<f32>

The angular velocity (in radians per second) in the body frame.

actuator_sysid: Vector3<f32>

sysid_ang_vel_body: Vector3<f32>

feed_forward_scalar: f32

throttle_rpy_mix: f32

throttle_rpy_mix_desired: f32

attitude_control_max: f32

dt: f32

accel_max: Vector3<f32>

use_sqrt_controller: bool

p_angle_roll: P

p_angle_pitch: P

p_angle_yaw: P

thrust_error_angle: f32

attitude_target: Quaternion<f32>

ang_vel_max: Vector3<f32>

ang_vel_target: Vector3<f32>

rate_bf_ff_enabled: bool

input_tc: f32

euler_angle_target: Vector3<f32>

euler_rate_target: Vector3<f32>

Implementations

impl AttitudeController

pub fn input( &mut self, attitude_desired: Quaternion<f32>, ang_vel_target: Vector3<f32>, attitude_body: Quaternion<f32>, ) -> Quaternion<f32>

Command a Quaternion attitude with feed-forward and smoothing. Returns attitude_desired updated by the integral of the angular velocity

pub fn attitude_control(&mut self, attitude_body: Quaternion<f32>)

Calculate the body frame angular velocities to follow the target attitude. attitude_body represents a quaternion rotation in NED frame to the body

pub fn thrust_heading_rotation_angles( &self, attitude_target: Quaternion<f32>, attitude_body: Quaternion<f32>, ) -> (Quaternion<f32>, Vector3<f32>)

pub fn angular_velocity_target_from_attitude_error( &self, attitude_error_rot_vec_rad: Vector3<f32>, ) -> Vector3<f32>

Calculate the rate target angular velocity using the attitude error rotation vector (in radians)

Trait Implementations

impl Default for AttitudeController

fn default() -> Self

Returns the “default value” for a type.