Skip to content

Commit

Permalink
AC_AttitudeControl: fix argument order in tailsitter bodyframe roll i…
Browse files Browse the repository at this point in the history 
…nput methods

increase allowed yaw error in tailsitter bodyframe roll modes
add combined bodyframe roll method
delete old versions of body-frame roll input methods
invert mc_controls
  • Loading branch information
@kd0aij @tridge
kd0aij authored and tridge committed Dec 17, 2019
1 parent 68dcab3 commit bb9b116
Show file tree
Hide file tree
Showing 2 changed files with 19 additions and 72 deletions.
83 changes: 17 additions & 66 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -344,67 +344,10 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
attitude_controller_run_quat();
}

// Command euler pitch and yaw angles and roll rate (used only by tailsitter quadplanes)
// Multicopter style controls: roll stick is tailsitter bodyframe yaw in hover
void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float euler_yaw_rate_cds, float euler_pitch_cd, float body_roll_cd)
{
// 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));

// Compute attitude error
Quaternion attitude_vehicle_quat;
Quaternion error_quat;
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Vector3f att_error;
error_quat.to_axis_angle(att_error);

// limit yaw error
if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) {
// update heading
_attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_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);

const float cpitch = cosf(euler_pitch);
const float spitch = fabsf(sinf(euler_pitch));

// apply body-frame yaw/roll (this is roll/yaw for a tailsitter in forward flight)
// rotate body_roll axis by |sin(pitch angle)|
Quaternion bf_roll_Q;
bf_roll_Q.from_axis_angle(Vector3f(0, 0, spitch * body_roll));

// rotate body_yaw axis by cos(pitch angle)
Quaternion bf_yaw_Q;
bf_yaw_Q.from_axis_angle(Vector3f(-cpitch * body_roll, 0, 0));
_attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q;

// _attitude_target_euler_angle roll and pitch: Note: roll/yaw will be indeterminate when pitch is near +/-90
// These should be used only for logging target eulers, with the caveat noted above.
// Also note that _attitude_target_quat.from_euler() should only be used in special circumstances
// such as when attitude is specified directly in terms of Euler angles.
// _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll();
// _attitude_target_euler_angle.y = euler_pitch;

// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);

// Compute attitude error
error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
error_quat.to_axis_angle(att_error);

// Compute the angular velocity target from the attitude error
_rate_target_ang_vel = update_ang_vel_target_from_att_error(att_error);
}

// Command euler pitch and yaw angles and roll rate (used only by tailsitter quadplanes)
// Plane style controls: yaw stick is tailsitter bodyframe yaw in hover
void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float euler_yaw_rate_cds, float euler_pitch_cd, float body_roll_cd)
// 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)
{
// Convert from centidegrees on public interface to radians
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
Expand All @@ -423,10 +366,14 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float
error_quat.to_axis_angle(att_error);

// limit yaw error
if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) {
if (fabsf(att_error.z) < 2*AC_ATTITUDE_THRUST_ERROR_ANGLE) {
// update heading
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);
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);
}
}

// init attitude target to desired euler yaw and pitch with zero roll
Expand All @@ -439,11 +386,15 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float

// rotate body_yaw axis by cos(pitch angle)
Quaternion bf_yaw_Q;
bf_yaw_Q.from_axis_angle(Vector3f(cpitch, 0, 0), euler_yaw_rate);
if (plane_controls) {
bf_yaw_Q.from_axis_angle(Vector3f(cpitch, 0, 0), euler_yaw_rate);
} else {
bf_yaw_Q.from_axis_angle(Vector3f(-cpitch * body_roll, 0, 0));
}
_attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q;

// _attitude_target_euler_angle roll and pitch: Note: roll/yaw will be indeterminate when pitch is near +/-90
// These should be used only for logging target eulers, with the caveat noted above
// These should be used only for logging target eulers, with the caveat noted above.
// Also note that _attitude_target_quat.from_euler() should only be used in special circumstances
// such as when attitude is specified directly in terms of Euler angles.
// _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll();
Expand Down
8 changes: 2 additions & 6 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,13 +135,9 @@ class AC_AttitudeControl {
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
virtual 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);

// Command euler yaw rate and pitch angle with roll angle specified in body frame with multicopter style controls
// 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_m(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);

// Command euler yaw rate and pitch angle with roll angle specified in body frame with plane style controls
// (used only by tailsitter quadplanes)
virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
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);

// 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);
Expand Down

0 comments on commit bb9b116

Please sign in to comment.