AC_AttitudeControl: add thrust vector methods to 6DoF multi

This commit is contained in:
Peter Hall 2021-06-20 17:56:43 +01:00 committed by Andrew Tridgell
parent 72f06201f9
commit 5a1fdf9f78
4 changed files with 28 additions and 4 deletions

View File

@ -538,7 +538,7 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
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)
{
// 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();
}
// 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)
{
// Convert from centidegrees on public interface to radians

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);
// 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);
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds);
virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, 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);}
// Converts thrust vector and heading angle to quaternion rotation in the earth frame

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);
}
// 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)
{
// pitch/forward

View File

@ -31,6 +31,10 @@ public:
// 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;
// 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
*/