From f1eec8482b09369bc512e43285933bf2d01a0cb4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 16 Apr 2023 09:59:20 +1000 Subject: [PATCH] AC_AttitudeControl: use quat.to_euler(Vector3f&) --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index e1dfc24312..a64a277d88 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -169,7 +169,7 @@ void AC_AttitudeControl::relax_attitude_controllers() { // Initialize the attitude variables to the current attitude _ahrs.get_quat_body_to_ned(_attitude_target); - _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.initialise(); // Initialize the angular rate variables to the current rate @@ -250,7 +250,7 @@ void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vec } // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _attitude_target.to_euler(_euler_angle_target); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target); @@ -274,7 +274,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f); // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _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(); @@ -325,7 +325,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f); // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _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(); @@ -384,7 +384,7 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f); // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _attitude_target.to_euler(_euler_angle_target); if (_rate_bf_ff_enabled) { // translate the roll pitch and yaw acceleration limits to the euler axis @@ -426,7 +426,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f); // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _attitude_target.to_euler(_euler_angle_target); if (_rate_bf_ff_enabled) { // Compute acceleration-limited body frame rates @@ -471,7 +471,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, // Update the unused targets attitude based on current attitude to condition mode change _ahrs.get_quat_body_to_ned(_attitude_target); - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _attitude_target.to_euler(_euler_angle_target); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target); _ang_vel_body = _ang_vel_target; @@ -516,7 +516,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, _attitude_target = attitude_body * _attitude_ang_error; // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _attitude_target.to_euler(_euler_angle_target); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target); @@ -546,7 +546,7 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste _attitude_target.normalize(); // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _attitude_target.to_euler(_euler_angle_target); // Set rate feedforward requests to zero _euler_rate_target.zero(); @@ -568,7 +568,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust } // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _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); @@ -620,7 +620,7 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect float heading_angle = radians(heading_angle_cd * 0.01f); // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _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); @@ -968,7 +968,7 @@ void AC_AttitudeControl::inertial_frame_reset() _attitude_target = attitude_body * _attitude_ang_error; // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); + _attitude_target.to_euler(_euler_angle_target); } // Convert a 321-intrinsic euler angle derivative to an angular velocity vector