Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: Allow diabling of slew limit

This commit is contained in:
Leonard Hall 2022-05-14 13:25:34 +09:30 committed by Randy Mackay
parent 5e528ba7a1
commit 0d863aa736
4 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -551,10 +551,15 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
} }
// 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) void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw)
{ {
// Convert from centidegrees on public interface to radians // Convert from centidegrees on public interface to radians
const float heading_rate = radians(heading_rate_cds * 0.01f); float heading_rate = radians(heading_rate_cds * 0.01f);
// a zero _angle_vel_yaw_max means that setting is disabled
const float slew_yaw_max_rads = get_slew_yaw_max_rads();
if (slew_yaw) {
heading_rate = constrain_float(heading_rate, -slew_yaw_max_rads, slew_yaw_max_rads);
}
// 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.x, _euler_angle_target.y, _euler_angle_target.z);
@ -580,7 +585,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt); _ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt);
// Limit the angular velocity // Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), get_slew_yaw_max_rads()); ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
} else { } else {
Quaternion yaw_quat; Quaternion yaw_quat;
yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate * _dt}); yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate * _dt});

2
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -184,7 +184,7 @@ public:
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 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); 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);
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);} void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);}

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp
View File

@ -49,7 +49,7 @@ void AC_AttitudeControl_Multi_6DoF::input_euler_angle_roll_pitch_yaw(float euler
} }
// Command a thrust vector and heading rate // Command a thrust vector and heading rate
void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds) void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw)
{ {
// convert thrust vector to a roll and pitch angles // convert thrust vector to a roll and pitch angles
// this negates the advantage of using thrust vector control, but works just fine // this negates the advantage of using thrust vector control, but works just fine

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
View File

@ -33,7 +33,7 @@ public:
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override; void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;
// 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
void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds) override; void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true) override;
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) override; void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) override;
/* /*