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_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);
// 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_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_yaw(_att_target_euler_rate_rads.z, 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 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);
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
}
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_bf_rads = radians(roll_rate_bf_cds*0.01f);
float pitch_rate_bf_rads = radians(pitch_rate_bf_cds*0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
// Compute acceleration-limited body-frame roll rate
if (get_accel_roll_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_ang_vel_rads.x += constrain_float(roll_rate_bf_rads - _att_target_ang_vel_rads.x, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_ang_vel_rads.x = roll_rate_bf_rads;
}
// Compute acceleration-limited body-frame pitch rate
if (get_accel_pitch_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_ang_vel_rads.y += constrain_float(pitch_rate_bf_rads - _att_target_ang_vel_rads.y, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_ang_vel_rads.y = pitch_rate_bf_rads;
}
// Compute acceleration-limited body-frame yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_ang_vel_rads.z += constrain_float(yaw_rate_bf_rads - _att_target_ang_vel_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_ang_vel_rads.z = yaw_rate_bf_rads;
}
// Compute quaternion target attitude
Quaternion att_target_quat;
att_target_quat.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
// Rotate quaternion target attitude using computed rate
att_target_quat.rotate(_att_target_ang_vel_rads*_dt);
att_target_quat.normalize();
// Call attitude controller
attitude_controller_run_quat(att_target_quat, _att_target_ang_vel_rads);
// 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);
}
void AC_AttitudeControl::update_ang_vel_target_from_att_error()
{
// Compute the roll angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) {
_ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x;
}
// Compute the pitch angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) {
_ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y;
}
// Compute the yaw angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) {
_ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z;
}
// Add feedforward term that attempts to ensure that the copter yaws about the reference
// Z axis, rather than the vehicle body Z axis.
// NOTE: This is a small-angle approximation.
_ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z;
_ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z;
}
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();
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_roll(_att_target_euler_rate_rads.x, 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_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_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
_att_target_euler_rad.y = euler_pitch_angle_rad;
_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_yaw(_att_target_euler_rate_rads.z, 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_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
}
// 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(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
} else {
euler_rate_to_ang_vel(_att_target_euler_rad, Vector3f(0,0,_att_target_euler_rate_rads.z), _att_target_ang_vel_rads);
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
}
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad)
{
Vector3f rate_target_ang_vel;
// Compute the roll angular velocity demand from the roll angle error
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
}else{
rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
}
// Compute the pitch angular velocity demand from the roll angle error
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
}else{
rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
}
// Compute the yaw angular velocity demand from the roll angle error
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt);
}else{
rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
}
return rate_target_ang_vel;
}
// Command an angular velocity with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_rads = radians(roll_rate_bf_cds*0.01f);
float pitch_rate_rads = radians(pitch_rate_bf_cds*0.01f);
float yaw_rate_rads = radians(yaw_rate_bf_cds*0.01f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// Compute acceleration-limited euler rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
_attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads, get_accel_roll_max_radss());
_attitude_target_ang_vel.y = input_shaping_ang_vel(_attitude_target_ang_vel.y, pitch_rate_rads, get_accel_pitch_max_radss());
_attitude_target_ang_vel.z = input_shaping_ang_vel(_attitude_target_ang_vel.z, yaw_rate_rads, get_accel_yaw_max_radss());
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
// When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
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 = radians(euler_roll_rate_cds*0.01f);
float euler_pitch_rate = radians(euler_pitch_rate_cds*0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
// the output rate towards the input rate.
_attitude_target_euler_rate.x = input_shaping_ang_vel(_attitude_target_euler_rate.x, euler_roll_rate, euler_accel.x);
_attitude_target_euler_rate.y = input_shaping_ang_vel(_attitude_target_euler_rate.y, euler_pitch_rate, euler_accel.y);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
// Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities.
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + euler_roll_rate*_dt);
_attitude_target_euler_angle.y = constrain_float(_attitude_target_euler_angle.y + euler_pitch_rate*_dt, radians(-85.0f), radians(85.0f));
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + euler_yaw_rate*_dt);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
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, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd*0.01f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);
_attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z, _dt);
if (slew_yaw) {
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
}
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), -get_slew_yaw_rads()*_dt, get_slew_yaw_rads()*_dt);
// Update attitude target from constrained angle error
_attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
} else {
_attitude_target_euler_angle.z = euler_yaw_angle;
}
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain)
{
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt);
_attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt);
_attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_attitude_target_quat = attitude_desired_quat;
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
void AC_AttitudeControl::update_ang_vel_target_from_att_error()
{
// Compute the roll angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) {
_ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x;
}
// Compute the pitch angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) {
_ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y;
}
// Compute the yaw angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) {
_ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z;
}
// Account for precession of desired attitude about the body frame yaw axis
_ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z;
_ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z;
}
