// 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::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; }
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; }
// 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) _att_target_ang_vel_rads.x = _ahrs.get_gyro().x; _att_target_ang_vel_rads.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 - _att_target_ang_vel_rads.z; rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads); _att_target_ang_vel_rads.z += rate_change_rads; } else { _att_target_ang_vel_rads.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)) { _att_target_euler_rad.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll); _att_target_euler_rad.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch); _att_target_euler_rad.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw); } // handle flipping over pitch axis if (_att_target_euler_rad.y > M_PI/2.0f) { _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI); _att_target_euler_rad.y = wrap_PI(M_PI - _att_target_euler_rad.x); _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI); } if (_att_target_euler_rad.y < -M_PI/2.0f) { _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI); _att_target_euler_rad.y = wrap_PI(-M_PI - _att_target_euler_rad.x); _att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI); } // convert body-frame angle errors to body-frame rate targets update_ang_vel_target_from_att_error(); // set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates _ang_vel_target_rads.x = _att_target_ang_vel_rads.x; _ang_vel_target_rads.y = _att_target_ang_vel_rads.y; // add desired target to yaw _ang_vel_target_rads.z += _att_target_ang_vel_rads.z; }
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; }