AC_AttitudeControl: keep _euler_angle_target up to date with _attitud…

…e_target quaternion in attitude_controller_run_quat

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -249,8 +249,6 @@ void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vec
         _ang_vel_target = ang_vel_target;
     }
 
-    // calculate the attitude target euler angles
-    _attitude_target.to_euler(_euler_angle_target);
 
     // Convert body-frame angular velocity into euler angle derivative of desired attitude
     ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
@@ -324,9 +322,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
     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.to_euler(_euler_angle_target);
-
     // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
     euler_roll_angle += get_roll_trim_rad();
 
@@ -383,9 +378,6 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
     float euler_pitch_rate = radians(euler_pitch_rate_cds * 0.01f);
     float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
 
-    // calculate the attitude target euler angles
-    _attitude_target.to_euler(_euler_angle_target);
-
     if (_rate_bf_ff_enabled) {
         // translate the roll pitch and yaw acceleration limits to the euler axis
         const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
@@ -425,9 +417,6 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
     float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
     float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
 
-    // calculate the attitude target euler angles
-    _attitude_target.to_euler(_euler_angle_target);
-
     if (_rate_bf_ff_enabled) {
         // Compute acceleration-limited body frame rates
         // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
@@ -545,9 +534,6 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
     _attitude_target = _attitude_target * attitude_target_update;
     _attitude_target.normalize();
 
-    // calculate the attitude target euler angles
-    _attitude_target.to_euler(_euler_angle_target);
-
     // Set rate feedforward requests to zero
     _euler_rate_target.zero();
     _ang_vel_target.zero();
@@ -567,9 +553,6 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
         heading_rate = constrain_float(heading_rate, -slew_yaw_max_rads, slew_yaw_max_rads);
     }
 
-    // calculate the attitude target euler angles
-    _attitude_target.to_euler(_euler_angle_target);
-
     // convert thrust vector to a quaternion attitude
     Quaternion thrust_vec_quat = attitude_from_thrust_vector(thrust_vector, 0.0f);
 
@@ -619,9 +602,6 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect
     float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -slew_yaw_max_rads, slew_yaw_max_rads);
     float heading_angle = radians(heading_angle_cd * 0.01f);
 
-    // calculate the attitude target euler angles
-    _attitude_target.to_euler(_euler_angle_target);
-
     // convert thrust vector and heading to a quaternion attitude
     const Quaternion desired_attitude_quat = attitude_from_thrust_vector(thrust_vector, heading_angle);
 
@@ -751,6 +731,9 @@ void AC_AttitudeControl::attitude_controller_run_quat()
     // ensure Quaternion stay normalised
     _attitude_target.normalize();
 
+    // Update euler representation of target
+    _attitude_target.to_euler(_euler_angle_target);
+
     // Record error to handle EKF resets
     _attitude_ang_error = attitude_body.inverse() * _attitude_target;
 }
@@ -952,6 +935,7 @@ void AC_AttitudeControl::reset_yaw_target_and_rate(bool reset_rate)
     Quaternion _attitude_target_update;
     _attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
     _attitude_target = _attitude_target_update * _attitude_target;
+    _attitude_target.to_euler(_euler_angle_target);
 
     if (reset_rate) {
         // set yaw rate to zero

libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp

@@ -37,7 +37,7 @@ void AC_AttitudeControl_TS::relax_attitude_controllers(bool exclude_pitch)
         pitch_rotation.from_axis_angle(Vector3f(0, -1, 0), current_eulers.y);
         _attitude_target = current_attitude * pitch_rotation;
         _attitude_target.normalize();
-        _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
+        _attitude_target.to_euler(_euler_angle_target);
         _attitude_ang_error = current_attitude.inverse() * _attitude_target;
 
         // Initialize the roll and yaw angular rate variables to the current rate
@@ -105,6 +105,7 @@ void AC_AttitudeControl_TS::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool
         bf_yaw_Q.from_axis_angle(Vector3f(-cpitch * body_roll, 0, 0));
     }
     _attitude_target = _attitude_target * bf_roll_Q * bf_yaw_Q;
+    _attitude_target.to_euler(_euler_angle_target);
 
     // _euler_angle_target 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.