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

AC_AttitudeControl: add thrust vector methods to 6DoF multi

Browse Source
This commit is contained in:
Peter Hall 2021-06-20 17:56:43 +01:00 committed by Randy Mackay
parent a699b8b331
commit 716ceb93c5
4 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -538,7 +538,7 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
attitude_controller_run_quat(); attitude_controller_run_quat();
} }
// Command a Quaternion attitude with feedforward and smoothing // 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)
{ {
// Convert from centidegrees on public interface to radians // Convert from centidegrees on public interface to radians
@ -586,7 +586,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
attitude_controller_run_quat(); attitude_controller_run_quat();
} }
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing // Command a thrust vector, heading and heading rate
void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
{ {
// Convert from centidegrees on public interface to radians // Convert from centidegrees on public interface to radians

4
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -167,8 +167,8 @@ 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
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);
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);}
// Converts thrust vector and heading angle to quaternion rotation in the earth frame // Converts thrust vector and heading angle to quaternion rotation in the earth frame

20
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp
View File

@ -48,6 +48,26 @@ void AC_AttitudeControl_Multi_6DoF::input_euler_angle_roll_pitch_yaw(float euler
AC_AttitudeControl_Multi::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw); AC_AttitudeControl_Multi::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw);
} }
// 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)
{
// convert thrust vector to a roll and pitch angles
// this negates the advantage of using thrust vector control, but works just fine
Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
input_euler_angle_roll_pitch_euler_rate_yaw(degrees(angle_target.x) * 100.0f, degrees(angle_target.y) * 100.0f, heading_rate_cds);
}
// Command a thrust vector, heading and heading rate
void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
{
// convert thrust vector to a roll and pitch angles
Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
// note that we are throwing away heading rate here
input_euler_angle_roll_pitch_yaw(degrees(angle_target.x) * 100.0f, degrees(angle_target.y) * 100.0f, heading_angle_cd, true);
}
void AC_AttitudeControl_Multi_6DoF::set_forward_lateral(float &euler_pitch_angle_cd, float &euler_roll_angle_cd) void AC_AttitudeControl_Multi_6DoF::set_forward_lateral(float &euler_pitch_angle_cd, float &euler_roll_angle_cd)
{ {
// pitch/forward // pitch/forward

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
View File

@ -31,6 +31,10 @@ public:
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
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
void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds) override;
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) override;
/* /*
all other input functions should zero thrust vectoring and behave as a normal copter all other input functions should zero thrust vectoring and behave as a normal copter
*/ */