AC_AttitudeControl: Fix dt update order

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -279,6 +279,8 @@ void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vec
     Vector3f ang_vel_target = attitude_desired_quat * ang_vel_body;
 
     if (_rate_bf_ff_enabled) {
+        // update attitude target
+        update_attitude_target();
         // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
         // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
         // and an exponential decay specified by _input_tc at the end.
@@ -321,6 +323,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
     euler_roll_angle += get_roll_trim_rad();
 
     if (_rate_bf_ff_enabled) {
+        // update attitude target
+        update_attitude_target();
+
         // 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()});
 
@@ -373,6 +378,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
 
     const float slew_yaw_max_rads = get_slew_yaw_max_rads();
     if (_rate_bf_ff_enabled) {
+        // update attitude target
+        update_attitude_target();
+
         // 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()});
 
@@ -428,6 +436,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
     _attitude_target.to_euler(_euler_angle_target);
 
     if (_rate_bf_ff_enabled) {
+        // update attitude target
+        update_attitude_target();
+
         // 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()});
 
@@ -470,6 +481,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
     _attitude_target.to_euler(_euler_angle_target);
 
     if (_rate_bf_ff_enabled) {
+        // update attitude target
+        update_attitude_target();
+
         // Compute acceleration-limited body frame rates
         // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
         // the output rate towards the input rate.
@@ -622,6 +636,9 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
     thrust_vector_rotation_angles(thrust_vec_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);
 
     if (_rate_bf_ff_enabled) {
+        // update attitude target
+        update_attitude_target();
+
         // 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.
         _ang_vel_target.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
@@ -667,6 +684,9 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect
     const Quaternion desired_attitude_quat = attitude_from_thrust_vector(thrust_vector, heading_angle);
 
     if (_rate_bf_ff_enabled) {
+        // update attitude target
+        update_attitude_target();
+
         // calculate the angle error in x and y.
         Vector3f attitude_error;
         float thrust_vector_diff_angle;
@@ -745,6 +765,18 @@ Quaternion AC_AttitudeControl::attitude_from_thrust_vector(Vector3f thrust_vecto
     return thrust_vec_quat*yaw_quat;
 }
 
+// Calculates the body frame angular velocities to follow the target attitude
+void AC_AttitudeControl::update_attitude_target()
+{
+    // rotate target and normalize
+    Quaternion attitude_target_update;
+    attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt});
+    _attitude_target = _attitude_target * attitude_target_update;
+
+    // ensure Quaternion stay normalised
+    _attitude_target.normalize();
+}
+
 // Calculates the body frame angular velocities to follow the target attitude
 void AC_AttitudeControl::attitude_controller_run_quat()
 {
@@ -782,16 +814,6 @@ void AC_AttitudeControl::attitude_controller_run_quat()
         _ang_vel_body += ang_vel_body_feedforward;
     }
 
-    if (_rate_bf_ff_enabled) {
-        // rotate target and normalize
-        Quaternion attitude_target_update;
-        attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt});
-        _attitude_target = _attitude_target * attitude_target_update;
-    }
-
-    // ensure Quaternion stay normalised
-    _attitude_target.normalize();
-
     // Record error to handle EKF resets
     _attitude_ang_error = attitude_body.inverse() * _attitude_target;
 }

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -346,6 +346,9 @@ class AC_AttitudeControl {
     // translates body frame acceleration limits to the euler axis
     Vector3f euler_accel_limit(const Quaternion &att, const Vector3f &euler_accel);
 
+    // Calculates the body frame angular velocities to follow the target attitude
+    void update_attitude_target();
+
     // Calculates the body frame angular velocities to follow the target attitude
     void attitude_controller_run_quat();