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);
}
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);
// 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;
// If slew_yaw is enabled, constrain yaw target within get_slew_yaw_rads() of _ahrs.yaw
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.z - _ahrs.yaw), -get_slew_yaw_rads(), get_slew_yaw_rads());
// Update attitude target from constrained angle error
_att_target_euler_rad.z = angle_error + _ahrs.yaw;
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, Vector3f(0.0f,0.0f,0.0f));
// 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);
}
// 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();
}
