mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: rename thrust_heading_rotation_angles args
This commit is contained in:
parent
ad6e2eeed8
commit
f53242b323
|
@ -593,14 +593,14 @@ void AC_AttitudeControl::attitude_controller_run_quat()
|
||||||
|
|
||||||
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
|
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
|
||||||
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
|
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
|
||||||
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
|
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_target_quat, const Quaternion& attitude_vehicle_quat, Vector3f& attitude_error_vector, float& thrust_error_angle)
|
||||||
{
|
{
|
||||||
Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.
|
Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.
|
||||||
att_to_quat.rotation_matrix(att_to_rot_matrix);
|
attitude_target_quat.rotation_matrix(att_to_rot_matrix);
|
||||||
Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
|
Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
|
||||||
|
|
||||||
Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
|
Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
|
||||||
att_from_quat.rotation_matrix(att_from_rot_matrix);
|
attitude_vehicle_quat.rotation_matrix(att_from_rot_matrix);
|
||||||
Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
|
Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
|
||||||
|
|
||||||
// the dot product is used to calculate the current lean angle for use of external functions
|
// the dot product is used to calculate the current lean angle for use of external functions
|
||||||
|
@ -610,44 +610,44 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
|
||||||
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
|
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
|
||||||
|
|
||||||
// the dot product is used to calculate the angle between the target and desired thrust vectors
|
// the dot product is used to calculate the angle between the target and desired thrust vectors
|
||||||
thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec, -1.0f, 1.0f));
|
thrust_error_angle = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec, -1.0f, 1.0f));
|
||||||
|
|
||||||
// Normalize the thrust rotation vector
|
// Normalize the thrust rotation vector
|
||||||
float thrust_vector_length = thrust_vec_cross.length();
|
float thrust_vector_length = thrust_vec_cross.length();
|
||||||
if (is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)) {
|
if (is_zero(thrust_vector_length) || is_zero(thrust_error_angle)) {
|
||||||
thrust_vec_cross = Vector3f(0, 0, 1);
|
thrust_vec_cross = Vector3f(0, 0, 1);
|
||||||
thrust_vec_dot = 0.0f;
|
thrust_error_angle = 0.0f;
|
||||||
} else {
|
} else {
|
||||||
thrust_vec_cross /= thrust_vector_length;
|
thrust_vec_cross /= thrust_vector_length;
|
||||||
}
|
}
|
||||||
Quaternion thrust_vec_correction_quat;
|
Quaternion thrust_vec_correction_quat;
|
||||||
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);
|
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_error_angle);
|
||||||
|
|
||||||
// Rotate thrust_vec_correction_quat to the att_from frame
|
// Rotate thrust_vec_correction_quat to the att_from frame
|
||||||
thrust_vec_correction_quat = att_from_quat.inverse() * thrust_vec_correction_quat * att_from_quat;
|
thrust_vec_correction_quat = attitude_vehicle_quat.inverse() * thrust_vec_correction_quat * attitude_vehicle_quat;
|
||||||
|
|
||||||
// calculate the remaining rotation required after thrust vector is rotated transformed to the att_from frame
|
// calculate the remaining rotation required after thrust vector is rotated transformed to the att_from frame
|
||||||
Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_from_quat.inverse() * att_to_quat;
|
Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * attitude_vehicle_quat.inverse() * attitude_target_quat;
|
||||||
|
|
||||||
// calculate the angle error in x and y.
|
// calculate the angle error in x and y.
|
||||||
Vector3f rotation;
|
Vector3f rotation;
|
||||||
thrust_vec_correction_quat.to_axis_angle(rotation);
|
thrust_vec_correction_quat.to_axis_angle(rotation);
|
||||||
att_diff_angle.x = rotation.x;
|
attitude_error_vector.x = rotation.x;
|
||||||
att_diff_angle.y = rotation.y;
|
attitude_error_vector.y = rotation.y;
|
||||||
|
|
||||||
// calculate the angle error in z (x and y should be zero here).
|
// calculate the angle error in z (x and y should be zero here).
|
||||||
yaw_vec_correction_quat.to_axis_angle(rotation);
|
yaw_vec_correction_quat.to_axis_angle(rotation);
|
||||||
att_diff_angle.z = rotation.z;
|
attitude_error_vector.z = rotation.z;
|
||||||
|
|
||||||
// Todo: Limit roll an pitch error based on output saturation and maximum error.
|
// Todo: Limit roll an pitch error based on output saturation and maximum error.
|
||||||
|
|
||||||
// Limit Yaw Error based on maximum acceleration - Update to include output saturation and maximum error.
|
// Limit Yaw Error based on maximum acceleration - Update to include output saturation and maximum error.
|
||||||
// Currently the limit is based on the maximum acceleration using the linear part of the SQRT controller.
|
// Currently the limit is based on the maximum acceleration using the linear part of the SQRT controller.
|
||||||
// This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters.
|
// This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters.
|
||||||
if (!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) {
|
if (!is_zero(_p_angle_yaw.kP()) && fabsf(attitude_error_vector.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) {
|
||||||
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP());
|
attitude_error_vector.z = constrain_float(wrap_PI(attitude_error_vector.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP());
|
||||||
yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f, 0.0f, att_diff_angle.z));
|
yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f, 0.0f, attitude_error_vector.z));
|
||||||
att_to_quat = att_from_quat * thrust_vec_correction_quat * yaw_vec_correction_quat;
|
attitude_target_quat = attitude_vehicle_quat * thrust_vec_correction_quat * yaw_vec_correction_quat;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -287,7 +287,7 @@ public:
|
||||||
|
|
||||||
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
|
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
|
||||||
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
|
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
|
||||||
void thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot);
|
void thrust_heading_rotation_angles(Quaternion& attitude_target_quat, const Quaternion& attitude_vehicle_quat, Vector3f& attitude_error_vector, float& thrust_error_angle);
|
||||||
|
|
||||||
// Calculates the body frame angular velocities to follow the target attitude
|
// Calculates the body frame angular velocities to follow the target attitude
|
||||||
void attitude_controller_run_quat();
|
void attitude_controller_run_quat();
|
||||||
|
|
Loading…
Reference in New Issue