Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: use quat.to_euler(Vector3f&)

Browse Source
This commit is contained in:
Peter Barker 2023-04-16 09:59:20 +10:00 committed by Peter Barker
parent ce8afaf57b
commit f1eec8482b
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -169,7 +169,7 @@ void AC_AttitudeControl::relax_attitude_controllers()
{ {
// Initialize the attitude variables to the current attitude // Initialize the attitude variables to the current attitude
_ahrs.get_quat_body_to_ned(_attitude_target); _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(); _attitude_ang_error.initialise();
// Initialize the angular rate variables to the current rate // 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 // 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 // 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_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); float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
// calculate the attitude target euler angles // 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) // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad(); 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); float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f);
// calculate the attitude target euler angles // 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) // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad(); 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); float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
// calculate the attitude target euler angles // 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) { if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis // 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); float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
// calculate the attitude target euler angles // 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) { if (_rate_bf_ff_enabled) {
// Compute acceleration-limited body frame rates // 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 // Update the unused targets attitude based on current attitude to condition mode change
_ahrs.get_quat_body_to_ned(_attitude_target); _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 // 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_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
_ang_vel_body = _ang_vel_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; _attitude_target = attitude_body * _attitude_ang_error;
// calculate the attitude target euler angles // 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 // 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_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(); _attitude_target.normalize();
// calculate the attitude target euler angles // 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 // Set rate feedforward requests to zero
_euler_rate_target.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 // 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 // convert thrust vector to a quaternion attitude
Quaternion thrust_vec_quat = attitude_from_thrust_vector(thrust_vector, 0.0f); 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); float heading_angle = radians(heading_angle_cd * 0.01f);
// calculate the attitude target euler angles // 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 // convert thrust vector and heading to a quaternion attitude
const Quaternion desired_attitude_quat = attitude_from_thrust_vector(thrust_vector, heading_angle); 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; _attitude_target = attitude_body * _attitude_ang_error;
// calculate the attitude target euler angles // 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 // Convert a 321-intrinsic euler angle derivative to an angular velocity vector