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

AC_AttitudeControl: input_thrust_vector with general heading

This commit is contained in:
Leonard Hall 2022-09-06 18:17:56 +09:30 committed by Randy Mackay
parent 64cd5a7401
commit aed694316d
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -653,6 +653,22 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect
attitude_controller_run_quat();
}
// Command a thrust vector and heading rate
void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, HeadingCommand heading)
{
switch (heading.heading_mode) {
case HeadingMode::Rate_Only:
input_thrust_vector_rate_heading(thrust_vector, heading.yaw_rate_cds);
break;
case HeadingMode::Angle_Only:
input_thrust_vector_heading(thrust_vector, heading.yaw_angle_cd, 0.0);
break;
case HeadingMode::Angle_And_Rate:
input_thrust_vector_heading(thrust_vector, heading.yaw_angle_cd, heading.yaw_rate_cds);
break;
}
}
Quaternion AC_AttitudeControl::attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const
{
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};

13
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -552,4 +552,17 @@ public:
float control_monitor_rms_output_pitch_D(void) const;
float control_monitor_rms_output_pitch(void) const;
float control_monitor_rms_output_yaw(void) const;
// structure for angle and/or rate target
enum class HeadingMode {
Angle_Only,
Angle_And_Rate,
Rate_Only
};
struct HeadingCommand {
float yaw_angle_cd;
float yaw_rate_cds;
HeadingMode heading_mode;
};
void input_thrust_vector_heading(const Vector3f& thrust_vector, HeadingCommand heading);
};