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); }
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; }
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_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); // 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()); // 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_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 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_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_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); }