From bb9b11657483c7f1c5750c6ddc178fba4bfdfe50 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 21 May 2019 17:13:41 -0600 Subject: [PATCH] AC_AttitudeControl: fix argument order in tailsitter bodyframe roll input methods increase allowed yaw error in tailsitter bodyframe roll modes add combined bodyframe roll method delete old versions of body-frame roll input methods invert mc_controls --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 83 ++++--------------- .../AC_AttitudeControl/AC_AttitudeControl.h | 8 +- 2 files changed, 19 insertions(+), 72 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index d975bf2f1f..444ed1e849 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -344,67 +344,10 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle attitude_controller_run_quat(); } -// Command euler pitch and yaw angles and roll rate (used only by tailsitter quadplanes) -// Multicopter style controls: roll stick is tailsitter bodyframe yaw in hover -void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float euler_yaw_rate_cds, float euler_pitch_cd, float body_roll_cd) -{ - // Convert from centidegrees on public interface to radians - float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f); - float euler_pitch = radians(constrain_float(euler_pitch_cd * 0.01f, -90.0f, 90.0f)); - float body_roll = radians(constrain_float(body_roll_cd * 0.01f, -90.0f, 90.0f)); - - // Compute attitude error - Quaternion attitude_vehicle_quat; - Quaternion error_quat; - attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned()); - error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; - Vector3f att_error; - error_quat.to_axis_angle(att_error); - - // limit yaw error - if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) { - // update heading - _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt); - } - - // init attitude target to desired euler yaw and pitch with zero roll - _attitude_target_quat.from_euler(0, euler_pitch, _attitude_target_euler_angle.z); - - const float cpitch = cosf(euler_pitch); - const float spitch = fabsf(sinf(euler_pitch)); - - // apply body-frame yaw/roll (this is roll/yaw for a tailsitter in forward flight) - // rotate body_roll axis by |sin(pitch angle)| - Quaternion bf_roll_Q; - bf_roll_Q.from_axis_angle(Vector3f(0, 0, spitch * body_roll)); - - // rotate body_yaw axis by cos(pitch angle) - Quaternion bf_yaw_Q; - bf_yaw_Q.from_axis_angle(Vector3f(-cpitch * body_roll, 0, 0)); - _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q; - - // _attitude_target_euler_angle roll and pitch: Note: roll/yaw will be indeterminate when pitch is near +/-90 - // These should be used only for logging target eulers, with the caveat noted above. - // Also note that _attitude_target_quat.from_euler() should only be used in special circumstances - // such as when attitude is specified directly in terms of Euler angles. - // _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll(); - // _attitude_target_euler_angle.y = euler_pitch; - - // 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 attitude error - error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; - error_quat.to_axis_angle(att_error); - - // Compute the angular velocity target from the attitude error - _rate_target_ang_vel = update_ang_vel_target_from_att_error(att_error); -} - -// Command euler pitch and yaw angles and roll rate (used only by tailsitter quadplanes) -// Plane style controls: yaw stick is tailsitter bodyframe yaw in hover -void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float euler_yaw_rate_cds, float euler_pitch_cd, float body_roll_cd) +// Command euler yaw rate and pitch angle with roll angle specified in body frame +// (used only by tailsitter quadplanes) +// If plane_controls is true, swap the effects of roll and yaw as euler pitch approaches 90 degrees +void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_yaw_rate_cds, float euler_pitch_cd, float body_roll_cd) { // Convert from centidegrees on public interface to radians float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f); @@ -423,10 +366,14 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float error_quat.to_axis_angle(att_error); // limit yaw error - if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) { + if (fabsf(att_error.z) < 2*AC_ATTITUDE_THRUST_ERROR_ANGLE) { // update heading - float yaw_rate = euler_yaw_rate * spitch + body_roll * cpitch; - _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt); + if (plane_controls) { + float yaw_rate = euler_yaw_rate * spitch + body_roll * cpitch; + _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt); + } else { + _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt); + } } // init attitude target to desired euler yaw and pitch with zero roll @@ -439,11 +386,15 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float // rotate body_yaw axis by cos(pitch angle) Quaternion bf_yaw_Q; - bf_yaw_Q.from_axis_angle(Vector3f(cpitch, 0, 0), euler_yaw_rate); + if (plane_controls) { + bf_yaw_Q.from_axis_angle(Vector3f(cpitch, 0, 0), euler_yaw_rate); + } else { + bf_yaw_Q.from_axis_angle(Vector3f(-cpitch * body_roll, 0, 0)); + } _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q; // _attitude_target_euler_angle roll and pitch: Note: roll/yaw will be indeterminate when pitch is near +/-90 - // These should be used only for logging target eulers, with the caveat noted above + // These should be used only for logging target eulers, with the caveat noted above. // Also note that _attitude_target_quat.from_euler() should only be used in special circumstances // such as when attitude is specified directly in terms of Euler angles. // _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f02393e74c..2d5479817d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -135,13 +135,9 @@ public: // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing virtual void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw); - // Command euler yaw rate and pitch angle with roll angle specified in body frame with multicopter style controls + // Command euler yaw rate and pitch angle with roll angle specified in body frame // (used only by tailsitter quadplanes) - virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); - - // Command euler yaw rate and pitch angle with roll angle specified in body frame with plane style controls - // (used only by tailsitter quadplanes) - virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); + virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_yaw_rate_cds, float euler_pitch_angle_cd, float euler_roll_angle_cd); // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);