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

AC_AttitudeControl: simplify body-frame roll axis swapping

This commit is contained in:
Mark Whitehorn 2019-11-30 15:57:58 -07:00 committed by Andrew Tridgell
parent 14fc6c5446
commit 7e8aecac50
2 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -347,12 +347,12 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
// Command euler yaw rate and pitch angle with roll angle specified in body frame
// (used only by tailsitter quadplanes)
// If plane_controls is true, swap the effects of roll and yaw as euler pitch approaches 90 degrees
void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_yaw_rate_cds, float euler_pitch_cd, float body_roll_cd)
void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float body_roll_cd, float euler_pitch_cd, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
float euler_pitch = radians(constrain_float(euler_pitch_cd * 0.01f, -90.0f, 90.0f));
float body_roll = radians(constrain_float(body_roll_cd * 0.01f, -90.0f, 90.0f));
float euler_pitch = radians(constrain_float(euler_pitch_cd * 0.01f, -90.0f, 90.0f));
float body_roll = radians(-body_roll_cd * 0.01f);
const float cpitch = cosf(euler_pitch);
const float spitch = fabsf(sinf(euler_pitch));
@ -365,16 +365,18 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool pla
Vector3f att_error;
error_quat.to_axis_angle(att_error);
// limit yaw error
if (fabsf(att_error.z) < 2*AC_ATTITUDE_THRUST_ERROR_ANGLE) {
// update heading
if (plane_controls) {
float yaw_rate = euler_yaw_rate * spitch + body_roll * cpitch;
_attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt);
} else {
_attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt);
}
// update heading
float yaw_rate = euler_yaw_rate;
if (plane_controls) {
yaw_rate = (euler_yaw_rate * spitch) + (body_roll * cpitch);
}
// limit yaw error
float yaw_error = fabsf(att_error.z);
float error_ratio = yaw_error / M_PI_2;
if (error_ratio > 1) {
yaw_rate /= (error_ratio * error_ratio);
}
_attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt);
// init attitude target to desired euler yaw and pitch with zero roll
_attitude_target_quat.from_euler(0, euler_pitch, _attitude_target_euler_angle.z);

2
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -137,7 +137,7 @@ public:
// Command euler yaw rate and pitch angle with roll angle specified in body frame
// (used only by tailsitter quadplanes)
virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_yaw_rate_cds, float euler_pitch_angle_cd, float euler_roll_angle_cd);
virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);