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 rate step command

Browse Source
This commit is contained in:
Leonard Hall 2024-05-16 23:40:47 +09:30 committed by Peter Barker
parent ff002cba3f
commit ddfccb3098
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -534,6 +534,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds,
_attitude_target.to_euler(_euler_angle_target); _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(_attitude_target, _ang_vel_target, _euler_rate_target); ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
// finally update the attitude target // finally update the attitude target
_ang_vel_body = _ang_vel_target; _ang_vel_body = _ang_vel_target;
} }
@ -619,6 +620,25 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
attitude_controller_run_quat(); attitude_controller_run_quat();
} }
// Command an rate step (i.e change) in body frame rate
// Used to command a step in rate without exciting the orthogonal axis during autotune
// Done as a single thread-safe function to avoid intermediate zero values being seen by the attitude controller
void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd)
{
// 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);
// Set the target angular velocity to be zero to minimize target overshoot after the rate step finishes
_ang_vel_target.zero();
// Convert body-frame angular velocity into euler angle derivative of desired attitude
_euler_rate_target.zero();
Vector3f ang_vel_body {roll_rate_step_bf_cd, pitch_rate_step_bf_cd, yaw_rate_step_bf_cd};
ang_vel_body = ang_vel_body * 0.01f * DEG_TO_RAD;
// finally update the attitude target
_ang_vel_body = ang_vel_body;
}
// Command a thrust vector and heading rate // Command a thrust vector and heading rate
void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw) void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw)
{ {

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -206,6 +206,9 @@ public:
// Command an angular step (i.e change) in body frame angle // Command an angular step (i.e change) in body frame angle
virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd); virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd);
// Command an angular rate step (i.e change) in body frame rate
virtual void input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd);
// Command a thrust vector in the earth frame and a heading angle and/or rate // Command a thrust vector in the earth frame and a heading angle and/or rate
virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true); virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true);
virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds); virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds);