AC_AttitudeControl: Fix dt update (test only)

lthall · Sep 2, 2024 · 9c4dea2 · 9c4dea2
1 parent 04292a2
commit 9c4dea2
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 4 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -752,6 +752,13 @@ void AC_AttitudeControl::attitude_controller_run_quat()
     Quaternion attitude_body;
     _ahrs.get_quat_body_to_ned(attitude_body);
 
+    if (_rate_bf_ff_enabled) {
+        // rotate target and normalize
+        Quaternion attitude_target_update;
+        attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target_last.x * _dt, _ang_vel_target_last.y * _dt, _ang_vel_target_last.z * _dt});
+        _attitude_target = _attitude_target * attitude_target_update;
+    }
+
     // This vector represents the angular error to rotate the thrust vector using x and y and heading using z
     Vector3f attitude_error;
     thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
@@ -783,10 +790,9 @@ void AC_AttitudeControl::attitude_controller_run_quat()
     }
 
     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;
+        _ang_vel_target_last = _ang_vel_target;
+    } else {
+        _ang_vel_target_last.zero();
     }
 
     // ensure Quaternion stay normalised

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -507,6 +507,7 @@ class AC_AttitudeControl {
     // the attitude controller as an angular velocity vector, in radians per second in
     // the target attitude frame.
     Vector3f            _ang_vel_target;
+    Vector3f            _ang_vel_target_last;
 
     // This represents the angular velocity in radians per second in the body frame, used in the angular
     // velocity controller.