Plane: limit yaw error in bodyframe roll control

This commit is contained in:
Mark Whitehorn 2019-04-27 09:54:43 -06:00 committed by Andrew Tridgell
parent c726db2a82
commit ce1a082956

View File

@ -353,8 +353,19 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float
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));
// new heading
// 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);
@ -384,12 +395,7 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
// Compute attitude error
Quaternion attitude_vehicle_quat;
_ahrs.get_quat_body_to_ned(attitude_vehicle_quat);
Quaternion error_quat;
error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Vector3f att_error;
error_quat.to_axis_angle(att_error);
// Compute the angular velocity target from the attitude error
@ -408,9 +414,20 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float
const float cpitch = cosf(euler_pitch);
const float spitch = fabsf(sinf(euler_pitch));
// new heading
// 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
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);
}
// 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);
@ -437,12 +454,7 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
// Compute attitude error
Quaternion attitude_vehicle_quat;
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
Quaternion error_quat;
error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Vector3f att_error;
error_quat.to_axis_angle(att_error);
// Compute the angular velocity target from the attitude error
@ -897,7 +909,7 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
}
// todo: Add Angular Velocity Limit
// Compute the pitch angular velocity demand from the roll angle error
// Compute the pitch angular velocity demand from the pitch angle error
if (_use_sqrt_controller) {
rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
} else {
@ -905,7 +917,7 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
}
// todo: Add Angular Velocity Limit
// Compute the yaw angular velocity demand from the roll angle error
// Compute the yaw angular velocity demand from the yaw angle error
if (_use_sqrt_controller) {
rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt);
} else {