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

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();

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);