void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_rate_rads = radians(euler_roll_rate_cds*0.01f);
float euler_pitch_rate_rads = radians(euler_pitch_rate_cds*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
Vector3f att_error_euler_rad;
// Compute acceleration-limited euler roll rate
if (get_accel_roll_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_euler_rate_rads.x += constrain_float(euler_roll_rate_rads - _att_target_euler_rate_rads.x, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_euler_rate_rads.x = euler_roll_rate_rads;
}
// Compute acceleration-limited euler pitch rate
if (get_accel_pitch_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_euler_rate_rads.y += constrain_float(euler_pitch_rate_rads - _att_target_euler_rate_rads.y, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_euler_rate_rads.y = euler_pitch_rate_rads;
}
// Compute acceleration-limited euler yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
}
// Update the attitude target from the computed euler rates
update_att_target_and_error_roll(_att_target_euler_rate_rads.x, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_and_error_pitch(_att_target_euler_rate_rads.y, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// Apply tilt limit
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
// Convert 321-intrinsic euler angle errors to a body-frame rotation vector
// NOTE: This results in an approximation of the attitude error based on a linearization about the current attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_error_euler_rad, _att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
// Add the angular velocity feedforward, rotated into vehicle frame
Matrix3f Trv;
get_rotation_reference_to_vehicle(Trv);
_ang_vel_target_rads += Trv * _att_target_ang_vel_rads;
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
Vector3f att_error_euler_rad;
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
// Set roll/pitch attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
// Update roll/pitch attitude error.
att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll);
att_error_euler_rad.y = wrap_PI(_att_target_euler_rad.y - _ahrs.pitch);
// Zero the roll and pitch feed-forward rate.
_att_target_euler_rate_rads.x = 0;
_att_target_euler_rate_rads.y = 0;
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
}
// Convert 321-intrinsic euler angle errors to a body-frame rotation vector
// NOTE: This results in an approximation of the attitude error based on a linearization about the current attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_error_euler_rad, _att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
// NOTE: This should be done about the desired attitude instead of about the vehicle attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_target_euler_rate_rads, _att_target_ang_vel_rads);
// NOTE: A rotation of _att_target_ang_vel_rads from desired body frame to estimated body frame is possibly omitted here
// Add the angular velocity feedforward
_ang_vel_target_rads += _att_target_ang_vel_rads;
}
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion attitude_vehicle_quat;
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
// Compute attitude error
Vector3f attitude_error_vector;
thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);
// Compute the angular velocity target from the attitude error
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
_rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;
_rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;
// Add the angular velocity feedforward, rotated into vehicle frame
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
Quaternion attitude_error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Quaternion target_ang_vel_quat = attitude_error_quat.inverse()*attitude_target_ang_vel_quat*attitude_error_quat;
// Correct the thrust vector and smoothly add feedforward and yaw input
if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
_rate_target_ang_vel.z = _ahrs.get_gyro().z;
}else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
float flip_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
_rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar;
_rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-flip_scalar) + _rate_target_ang_vel.z*flip_scalar;
} else {
_rate_target_ang_vel.x += target_ang_vel_quat.q2;
_rate_target_ang_vel.y += target_ang_vel_quat.q3;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
}
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// rotate target and normalize
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
}
}
void AC_AttitudeControl::attitude_controller_run_quat(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads)
{
// Update euler attitude target and angular velocity target
att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
_att_target_ang_vel_rads = att_target_ang_vel_rads;
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion att_vehicle_quat;
att_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
// Compute attitude error
(att_vehicle_quat.inverse()*att_target_quat).to_axis_angle(_att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Add the angular velocity feedforward, rotated into vehicle frame
Matrix3f Trv;
get_rotation_reference_to_vehicle(Trv);
_ang_vel_target_rads += Trv * _att_target_ang_vel_rads;
}
void AC_AttitudeControl::input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads)
{
// Update euler attitude target and angular velocity targets
att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
_att_target_ang_vel_rads = att_target_ang_vel_rads;
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_target_ang_vel_rads, _att_target_euler_rate_rads);
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion att_vehicle_quat;
att_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
// Compute attitude error
(att_vehicle_quat.inverse()*att_target_quat).to_axis_angle(_att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Add the angular velocity feedforward
// NOTE: Rotation of _att_target_ang_vel_rads from desired body frame to estimated body frame is possibly omitted here
_ang_vel_target_rads += _att_target_ang_vel_rads;
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle_rad = radians(euler_yaw_angle_cd*0.01f);
Vector3f att_error_euler_rad;
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
// Set attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.z = euler_yaw_angle_rad;
// Update attitude error.
att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll);
att_error_euler_rad.y = wrap_PI(_att_target_euler_rad.y - _ahrs.pitch);
att_error_euler_rad.z = wrap_PI(_att_target_euler_rad.z - _ahrs.yaw);
// Constrain the yaw angle error
if (slew_yaw) {
att_error_euler_rad.z = constrain_float(att_error_euler_rad.z,-get_slew_yaw_rads(),get_slew_yaw_rads());
}
// Convert 321-intrinsic euler angle errors to a body-frame rotation vector
// NOTE: This results in an approximation of the attitude error based on a linearization about the current attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_error_euler_rad, _att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds)
{
// convert from centidegrees on public interface to radians
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
// store roll, pitch and passthroughs
// NOTE: this abuses yaw_rate_bf_rads
_passthrough_roll = roll_passthrough;
_passthrough_pitch = pitch_passthrough;
_passthrough_yaw = degrees(yaw_rate_bf_rads)*100.0f;
// set rate controller to use pass through
_flags_heli.flybar_passthrough = true;
// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
_attitude_target_ang_vel.x = _ahrs.get_gyro().x;
_attitude_target_ang_vel.y = _ahrs.get_gyro().y;
// accel limit desired yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
float rate_change_rads = yaw_rate_bf_rads - _attitude_target_ang_vel.z;
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
_attitude_target_ang_vel.z += rate_change_rads;
} else {
_attitude_target_ang_vel.z = yaw_rate_bf_rads;
}
integrate_bf_rate_error_to_angle_errors();
_att_error_rot_vec_rad.x = 0;
_att_error_rot_vec_rad.y = 0;
// update our earth-frame angle targets
Vector3f att_error_euler_rad;
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
_attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis
if (_attitude_target_euler_angle.y > M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
if (_attitude_target_euler_angle.y < -M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
// convert body-frame angle errors to body-frame rate targets
_rate_target_ang_vel = update_ang_vel_target_from_att_error(_att_error_rot_vec_rad);
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_rate_target_ang_vel.x = _attitude_target_ang_vel.x;
_rate_target_ang_vel.y = _attitude_target_ang_vel.y;
// add desired target to yaw
_rate_target_ang_vel.z += _attitude_target_ang_vel.z;
_thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
// Sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
Vector3f att_error_euler_rad;
if ((get_accel_roll_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When roll acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler roll-axis
// angular velocity that will cause the euler roll angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_roll_angle_rad-_att_target_euler_rad.x, smoothing_gain, get_accel_roll_max_radss());
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_euler_rate_rads.x = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.x-rate_change_limit_rads, _att_target_euler_rate_rads.x+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_and_error_roll(_att_target_euler_rate_rads.x, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When acceleration limiting and feedforward are not enabled, the target roll euler angle is simply set to the
// input value and the feedforward rate is zeroed.
_att_target_euler_rad.x = euler_roll_angle_rad;
att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll);
_att_target_euler_rate_rads.x = 0;
}
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
if ((get_accel_pitch_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When pitch acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler pitch-axis
// angular velocity that will cause the euler pitch angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_pitch_angle_rad-_att_target_euler_rad.y, smoothing_gain, get_accel_pitch_max_radss());
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_euler_rate_rads.y = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.y-rate_change_limit_rads, _att_target_euler_rate_rads.y+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_and_error_pitch(_att_target_euler_rate_rads.y, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
_att_target_euler_rad.y = euler_pitch_angle_rad;
att_error_euler_rad.y = wrap_PI(_att_target_euler_rad.y - _ahrs.pitch);
_att_target_euler_rate_rads.y = 0;
}
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
}
// Convert 321-intrinsic euler angle errors to a body-frame rotation vector
// NOTE: This results in an approximation of the attitude error based on a linearization about the current attitude
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_error_euler_rad, _att_error_rot_vec_rad);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
if (_rate_bf_ff_enabled) {
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_target_euler_rate_rads, _att_target_ang_vel_rads);
} else {
euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), Vector3f(0,0,_att_target_euler_rate_rads.z), _att_target_ang_vel_rads);
}
// NOTE: Rotation of _att_target_ang_vel_rads from desired body frame to estimated body frame is possibly omitted here
// Add the angular velocity feedforward
_ang_vel_target_rads += _att_target_ang_vel_rads;
}
