mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: limit yaw error in bodyframe roll control
This commit is contained in:
parent
c726db2a82
commit
ce1a082956
@ -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 {
|
||||
|
Loading…
Reference in New Issue
Block a user