AC_AttitudeControl: improve tailsitter bodyframe roll control

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -366,32 +366,21 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll(float eu
     float euler_pitch = radians(euler_pitch_cd*0.01f);
     float body_roll = radians(body_roll_cd*0.01f);
 
-    // back out the body roll to get current euler_yaw
-    Quaternion bf_roll_Q;
-    bf_roll_Q.from_axis_angle(Vector3f(0, 0, -_last_body_roll));
-    Quaternion base_att_Q = _attitude_target_quat * bf_roll_Q;
-
-    // avoid Euler singularities
-    if (_last_euler_pitch > M_PI_4) {
-        base_att_Q.rotate(Vector3f(0,-M_PI_2,0));
-    } else if (_last_euler_pitch < -M_PI_4) {
-        base_att_Q.rotate(Vector3f(0,M_PI_2,0));
-    }
-
-    // current heading
-    float heading = base_att_Q.get_euler_yaw();
-
     // new heading
-    heading = wrap_PI(heading + euler_yaw_rate * _dt);
+    _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, heading);
-    _last_euler_pitch = euler_pitch;
+    _attitude_target_quat.from_euler(0, euler_pitch, _attitude_target_euler_angle.z);
+
+    // 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, fabsf(sinf(euler_pitch)) * body_roll));
 
-    // apply body-frame yaw (this is roll for a tailsitter in forward flight)
-    bf_roll_Q.from_axis_angle(Vector3f(0, 0, body_roll));
-    _last_body_roll = body_roll;
-    _attitude_target_quat = _attitude_target_quat * bf_roll_Q;
+    // rotate body_yaw axis by cos(pitch angle)
+    Quaternion bf_yaw_Q;
+    bf_yaw_Q.from_axis_angle(Vector3f(-cosf(euler_pitch), 0, 0), body_roll);
+    _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q;
 
     // Set rate feedforward requests to zero
     _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -434,11 +434,6 @@ protected:
     // true in inverted flight mode
     bool _inverted_flight;
 
-    // state for input_euler_rate_yaw_euler_angle_pitch_bf_roll()
-    // (would be expensive to compute from _attitude_target_quat)
-    float _last_body_roll;
-    float _last_euler_pitch;
-
 public:
     // log a CTRL message
     void control_monitor_log(void);