mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: Formatting Changes
This commit is contained in:
Leonard Hall
Randy Mackay
parent
7d19ba8550
commit
32cc642b2e
|
|
@ -23,7 +23,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
|||
// @Range: 500 18000
|
||||
// @Increment: 100
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS),
|
||||
AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS),
|
||||
|
||||
// 3 was for ACCEL_RP_MAX
|
||||
|
||||
|
|
@ -35,7 +35,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
|||
// @Values: 0:Disabled, 9000:VerySlow, 18000:Slow, 36000:Medium, 54000:Fast
|
||||
// @Increment: 1000
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl, _accel_yaw_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS),
|
||||
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl, _accel_yaw_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS),
|
||||
|
||||
// @Param: RATE_FF_ENAB
|
||||
// @DisplayName: Rate Feedforward Enable
|
||||
|
|
@ -201,8 +201,6 @@ void AC_AttitudeControl::reset_rate_controller_I_terms()
|
|||
// corrected by first correcting the thrust vector until the angle between the target thrust vector measured
|
||||
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
|
||||
|
||||
|
||||
|
||||
// Command a Quaternion attitude with feedforward and smoothing
|
||||
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
|
||||
{
|
||||
|
|
@ -241,9 +239,9 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
|
|||
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
|
||||
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
|
||||
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
|
||||
float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
|
||||
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
|
||||
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
|
||||
|
||||
// calculate the attitude target euler angles
|
||||
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
|
||||
|
|
@ -258,8 +256,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
|
|||
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
|
||||
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
|
||||
// and an exponential decay specified by smoothing_gain at the end.
|
||||
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
|
||||
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
|
||||
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
|
||||
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
|
||||
|
||||
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
|
||||
// the output rate towards the input rate.
|
||||
|
|
@ -275,7 +273,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
|
|||
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
|
||||
_attitude_target_euler_angle.x = euler_roll_angle;
|
||||
_attitude_target_euler_angle.y = euler_pitch_angle;
|
||||
_attitude_target_euler_angle.z += euler_yaw_rate*_dt;
|
||||
_attitude_target_euler_angle.z += euler_yaw_rate * _dt;
|
||||
// Compute quaternion target attitude
|
||||
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
|
||||
|
||||
|
|
@ -292,9 +290,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
|
|||
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
|
||||
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
|
||||
float euler_yaw_angle = radians(euler_yaw_angle_cd*0.01f);
|
||||
float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
|
||||
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
|
||||
float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f);
|
||||
|
||||
// calculate the attitude target euler angles
|
||||
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
|
||||
|
|
@ -309,9 +307,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
|
|||
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
|
||||
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
|
||||
// and an exponential decay specified by _input_tc at the end.
|
||||
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
|
||||
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
|
||||
_attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), _input_tc, euler_accel.z, _attitude_target_euler_rate.z, _dt);
|
||||
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
|
||||
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
|
||||
_attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _attitude_target_euler_angle.z), _input_tc, euler_accel.z, _attitude_target_euler_rate.z, _dt);
|
||||
if (slew_yaw) {
|
||||
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
|
||||
}
|
||||
|
|
@ -328,7 +326,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
|
|||
_attitude_target_euler_angle.y = euler_pitch_angle;
|
||||
if (slew_yaw) {
|
||||
// Compute constrained angle error
|
||||
float angle_error = constrain_float(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), -get_slew_yaw_rads()*_dt, get_slew_yaw_rads()*_dt);
|
||||
float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _attitude_target_euler_angle.z), -get_slew_yaw_rads() * _dt, get_slew_yaw_rads() * _dt);
|
||||
// Update attitude target from constrained angle error
|
||||
_attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
|
||||
} else {
|
||||
|
|
@ -351,9 +349,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
|
|||
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(euler_pitch_cd*0.01f);
|
||||
float body_roll = radians(body_roll_cd*0.01f);
|
||||
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
|
||||
float euler_pitch = radians(euler_pitch_cd * 0.01f);
|
||||
float body_roll = radians(body_roll_cd * 0.01f);
|
||||
|
||||
// new heading
|
||||
_attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt);
|
||||
|
|
@ -397,9 +395,9 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float
|
|||
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)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
|
||||
float euler_pitch = radians(euler_pitch_cd*0.01f);
|
||||
float body_roll = radians(body_roll_cd*0.01f);
|
||||
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
|
||||
float euler_pitch = radians(euler_pitch_cd * 0.01f);
|
||||
float body_roll = radians(body_roll_cd * 0.01f);
|
||||
|
||||
const float cpitch = cosf(euler_pitch);
|
||||
const float spitch = fabsf(sinf(euler_pitch));
|
||||
|
|
@ -446,9 +444,9 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float
|
|||
void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float euler_roll_rate = radians(euler_roll_rate_cds*0.01f);
|
||||
float euler_pitch_rate = radians(euler_pitch_rate_cds*0.01f);
|
||||
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
|
||||
float euler_roll_rate = radians(euler_roll_rate_cds * 0.01f);
|
||||
float euler_pitch_rate = radians(euler_pitch_rate_cds * 0.01f);
|
||||
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
|
||||
|
||||
// calculate the attitude target euler angles
|
||||
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
|
||||
|
|
@ -468,9 +466,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
|
|||
} else {
|
||||
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
|
||||
// Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities.
|
||||
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + euler_roll_rate*_dt);
|
||||
_attitude_target_euler_angle.y = constrain_float(_attitude_target_euler_angle.y + euler_pitch_rate*_dt, radians(-85.0f), radians(85.0f));
|
||||
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + euler_yaw_rate*_dt);
|
||||
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + euler_roll_rate * _dt);
|
||||
_attitude_target_euler_angle.y = constrain_float(_attitude_target_euler_angle.y + euler_pitch_rate * _dt, radians(-85.0f), radians(85.0f));
|
||||
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt);
|
||||
|
||||
// Set rate feedforward requests to zero
|
||||
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
|
||||
|
|
@ -488,9 +486,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
|
|||
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float roll_rate_rads = radians(roll_rate_bf_cds*0.01f);
|
||||
float pitch_rate_rads = radians(pitch_rate_bf_cds*0.01f);
|
||||
float yaw_rate_rads = radians(yaw_rate_bf_cds*0.01f);
|
||||
float roll_rate_rads = radians(roll_rate_bf_cds * 0.01f);
|
||||
float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
|
||||
float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
|
||||
|
||||
// calculate the attitude target euler angles
|
||||
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
|
||||
|
|
@ -525,9 +523,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
|
|||
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float roll_rate_rads = radians(roll_rate_bf_cds*0.01f);
|
||||
float pitch_rate_rads = radians(pitch_rate_bf_cds*0.01f);
|
||||
float yaw_rate_rads = radians(yaw_rate_bf_cds*0.01f);
|
||||
float roll_rate_rads = radians(roll_rate_bf_cds * 0.01f);
|
||||
float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
|
||||
float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
|
||||
|
||||
// Compute acceleration-limited body frame rates
|
||||
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
|
||||
|
|
@ -548,14 +546,14 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds,
|
|||
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float roll_rate_rads = radians(roll_rate_bf_cds*0.01f);
|
||||
float pitch_rate_rads = radians(pitch_rate_bf_cds*0.01f);
|
||||
float yaw_rate_rads = radians(yaw_rate_bf_cds*0.01f);
|
||||
float roll_rate_rads = radians(roll_rate_bf_cds * 0.01f);
|
||||
float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
|
||||
float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
|
||||
|
||||
// Update attitude error
|
||||
Vector3f gyro_latest = _ahrs.get_gyro_latest();
|
||||
Quaternion attitude_ang_error_update_quat;
|
||||
attitude_ang_error_update_quat.from_axis_angle(Vector3f((_attitude_target_ang_vel.x-gyro_latest.x) * _dt, (_attitude_target_ang_vel.y-gyro_latest.y) * _dt, (_attitude_target_ang_vel.z-gyro_latest.z) * _dt));
|
||||
attitude_ang_error_update_quat.from_axis_angle(Vector3f((_attitude_target_ang_vel.x - gyro_latest.x) * _dt, (_attitude_target_ang_vel.y - gyro_latest.y) * _dt, (_attitude_target_ang_vel.z - gyro_latest.z) * _dt));
|
||||
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
|
||||
|
||||
// Compute acceleration-limited body frame rates
|
||||
|
|
@ -570,7 +568,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
|
|||
_ahrs.get_quat_body_to_ned(attitude_vehicle_quat);
|
||||
|
||||
// Update the unused targets attitude based on current attitude to condition mode change
|
||||
_attitude_target_quat = attitude_vehicle_quat*_attitude_ang_error;
|
||||
_attitude_target_quat = attitude_vehicle_quat * _attitude_ang_error;
|
||||
|
||||
// calculate the attitude target euler angles
|
||||
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
|
||||
|
|
@ -593,9 +591,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
|
|||
void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd)
|
||||
{
|
||||
// Convert from centidegrees on public interface to radians
|
||||
float roll_step_rads = radians(roll_angle_step_bf_cd*0.01f);
|
||||
float pitch_step_rads = radians(pitch_angle_step_bf_cd*0.01f);
|
||||
float yaw_step_rads = radians(yaw_angle_step_bf_cd*0.01f);
|
||||
float roll_step_rads = radians(roll_angle_step_bf_cd * 0.01f);
|
||||
float pitch_step_rads = radians(pitch_angle_step_bf_cd * 0.01f);
|
||||
float yaw_step_rads = radians(yaw_angle_step_bf_cd * 0.01f);
|
||||
|
||||
// rotate attitude target by desired step
|
||||
Quaternion attitude_target_update_quat;
|
||||
|
|
@ -630,25 +628,25 @@ void AC_AttitudeControl::attitude_controller_run_quat()
|
|||
|
||||
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
|
||||
// todo: this should probably be a matrix that couples yaw as well.
|
||||
_rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI/4, M_PI/4) * _ahrs.get_gyro().z;
|
||||
_rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI/4, M_PI/4) * _ahrs.get_gyro().z;
|
||||
_rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;
|
||||
_rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;
|
||||
|
||||
ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
|
||||
|
||||
// Add the angular velocity feedforward, rotated into vehicle frame
|
||||
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
|
||||
Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
|
||||
Quaternion desired_ang_vel_quat = to_to_from_quat.inverse()*attitude_target_ang_vel_quat*to_to_from_quat;
|
||||
Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;
|
||||
|
||||
// Correct the thrust vector and smoothly add feedforward and yaw input
|
||||
if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
|
||||
if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
|
||||
_rate_target_ang_vel.z = _ahrs.get_gyro().z;
|
||||
}else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
|
||||
float feedforward_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
|
||||
_rate_target_ang_vel.x += desired_ang_vel_quat.q2*feedforward_scalar;
|
||||
_rate_target_ang_vel.y += desired_ang_vel_quat.q3*feedforward_scalar;
|
||||
} else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
|
||||
float feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE);
|
||||
_rate_target_ang_vel.x += desired_ang_vel_quat.q2 * feedforward_scalar;
|
||||
_rate_target_ang_vel.y += desired_ang_vel_quat.q3 * feedforward_scalar;
|
||||
_rate_target_ang_vel.z += desired_ang_vel_quat.q4;
|
||||
_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-feedforward_scalar) + _rate_target_ang_vel.z*feedforward_scalar;
|
||||
_rate_target_ang_vel.z = _ahrs.get_gyro().z * (1.0 - feedforward_scalar) + _rate_target_ang_vel.z * feedforward_scalar;
|
||||
} else {
|
||||
_rate_target_ang_vel.x += desired_ang_vel_quat.q2;
|
||||
_rate_target_ang_vel.y += desired_ang_vel_quat.q3;
|
||||
|
|
@ -676,34 +674,34 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
|
|||
{
|
||||
Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.
|
||||
att_to_quat.rotation_matrix(att_to_rot_matrix);
|
||||
Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
|
||||
Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
|
||||
|
||||
Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
|
||||
att_from_quat.rotation_matrix(att_from_rot_matrix);
|
||||
Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
|
||||
Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
|
||||
|
||||
// the cross product of the desired and target thrust vector defines the rotation vector
|
||||
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
|
||||
|
||||
// the dot product is used to calculate the angle between the target and desired thrust vectors
|
||||
thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));
|
||||
thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec, -1.0f, 1.0f));
|
||||
|
||||
// Normalize the thrust rotation vector
|
||||
float thrust_vector_length = thrust_vec_cross.length();
|
||||
if(is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)){
|
||||
thrust_vec_cross = Vector3f(0,0,1);
|
||||
if (is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)) {
|
||||
thrust_vec_cross = Vector3f(0, 0, 1);
|
||||
thrust_vec_dot = 0.0f;
|
||||
}else{
|
||||
} else {
|
||||
thrust_vec_cross /= thrust_vector_length;
|
||||
}
|
||||
Quaternion thrust_vec_correction_quat;
|
||||
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);
|
||||
|
||||
// Rotate thrust_vec_correction_quat to the att_from frame
|
||||
thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;
|
||||
thrust_vec_correction_quat = att_from_quat.inverse() * thrust_vec_correction_quat * att_from_quat;
|
||||
|
||||
// calculate the remaining rotation required after thrust vector is rotated transformed to the att_from frame
|
||||
Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;
|
||||
Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_from_quat.inverse() * att_to_quat;
|
||||
|
||||
// calculate the angle error in x and y.
|
||||
Vector3f rotation;
|
||||
|
|
@ -720,10 +718,10 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
|
|||
// Limit Yaw Error based on maximum acceleration - Update to include output saturation and maximum error.
|
||||
// Currently the limit is based on the maximum acceleration using the linear part of the SQRT controller.
|
||||
// This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters.
|
||||
if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
|
||||
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
|
||||
yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
|
||||
att_to_quat = att_from_quat*thrust_vec_correction_quat*yaw_vec_correction_quat;
|
||||
if (!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) {
|
||||
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP());
|
||||
yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f, 0.0f, att_diff_angle.z));
|
||||
att_to_quat = att_from_quat * thrust_vec_correction_quat * yaw_vec_correction_quat;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -744,7 +742,7 @@ float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desi
|
|||
// Acceleration is limited directly to smooth the beginning of the curve.
|
||||
if (is_positive(accel_max)) {
|
||||
float delta_ang_vel = accel_max * dt;
|
||||
return constrain_float(desired_ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel);
|
||||
return constrain_float(desired_ang_vel, target_ang_vel - delta_ang_vel, target_ang_vel + delta_ang_vel);
|
||||
} else {
|
||||
return desired_ang_vel;
|
||||
}
|
||||
|
|
@ -759,15 +757,15 @@ void AC_AttitudeControl::input_shaping_rate_predictor(const Vector2f &error_angl
|
|||
target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _input_tc, get_accel_roll_max_radss(), target_ang_vel.x, dt);
|
||||
target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _input_tc, get_accel_pitch_max_radss(), target_ang_vel.y, dt);
|
||||
} else {
|
||||
target_ang_vel.x = _p_angle_roll.get_p(wrap_PI(error_angle.x));
|
||||
target_ang_vel.y = _p_angle_pitch.get_p(wrap_PI(error_angle.y));
|
||||
target_ang_vel.x = _p_angle_roll.get_p(wrap_PI(error_angle.x));
|
||||
target_ang_vel.y = _p_angle_pitch.get_p(wrap_PI(error_angle.y));
|
||||
}
|
||||
// Limit the angular velocity correction
|
||||
Vector3f ang_vel(target_ang_vel.x, target_ang_vel.y, 0.0f);
|
||||
ang_vel_limit(ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), 0.0f);
|
||||
|
||||
target_ang_vel.x = ang_vel.x;
|
||||
target_ang_vel.y = ang_vel.y;
|
||||
target_ang_vel.x = ang_vel.x;
|
||||
target_ang_vel.y = ang_vel.y;
|
||||
}
|
||||
|
||||
// limits angular velocity
|
||||
|
|
@ -781,7 +779,7 @@ void AC_AttitudeControl::ang_vel_limit(Vector3f& euler_rad, float ang_vel_roll_m
|
|||
euler_rad.y = constrain_float(euler_rad.y, -ang_vel_pitch_max, ang_vel_pitch_max);
|
||||
}
|
||||
} else {
|
||||
Vector2f thrust_vector_ang_vel(euler_rad.x/ang_vel_roll_max, euler_rad.y/ang_vel_pitch_max);
|
||||
Vector2f thrust_vector_ang_vel(euler_rad.x / ang_vel_roll_max, euler_rad.y / ang_vel_pitch_max);
|
||||
float thrust_vector_length = thrust_vector_ang_vel.length();
|
||||
if (thrust_vector_length > 1.0f) {
|
||||
euler_rad.x = thrust_vector_ang_vel.x * ang_vel_roll_max / thrust_vector_length;
|
||||
|
|
@ -801,14 +799,14 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const
|
|||
float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
|
||||
|
||||
Vector3f rot_accel;
|
||||
if(is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || is_negative(euler_accel.x) || is_negative(euler_accel.y) || is_negative(euler_accel.z)) {
|
||||
if (is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || is_negative(euler_accel.x) || is_negative(euler_accel.y) || is_negative(euler_accel.z)) {
|
||||
rot_accel.x = euler_accel.x;
|
||||
rot_accel.y = euler_accel.y;
|
||||
rot_accel.z = euler_accel.z;
|
||||
} else {
|
||||
rot_accel.x = euler_accel.x;
|
||||
rot_accel.y = MIN(euler_accel.y/cos_phi, euler_accel.z/sin_phi);
|
||||
rot_accel.z = MIN(MIN(euler_accel.x/sin_theta, euler_accel.y/sin_phi), euler_accel.z/cos_phi);
|
||||
rot_accel.y = MIN(euler_accel.y / cos_phi, euler_accel.z / sin_phi);
|
||||
rot_accel.z = MIN(MIN(euler_accel.x / sin_theta, euler_accel.y / sin_phi), euler_accel.z / cos_phi);
|
||||
}
|
||||
return rot_accel;
|
||||
}
|
||||
|
|
@ -816,10 +814,10 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const
|
|||
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
|
||||
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
|
||||
{
|
||||
float yaw_shift = radians(yaw_shift_cd*0.01f);
|
||||
float yaw_shift = radians(yaw_shift_cd * 0.01f);
|
||||
Quaternion _attitude_target_update_quat;
|
||||
_attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
|
||||
_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
|
||||
_attitude_target_quat = _attitude_target_update_quat * _attitude_target_quat;
|
||||
}
|
||||
|
||||
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
|
||||
|
|
@ -845,7 +843,7 @@ void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const
|
|||
float cos_phi = cosf(euler_rad.x);
|
||||
|
||||
ang_vel_rads.x = euler_rate_rads.x - sin_theta * euler_rate_rads.z;
|
||||
ang_vel_rads.y = cos_phi * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z;
|
||||
ang_vel_rads.y = cos_phi * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z;
|
||||
ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z;
|
||||
}
|
||||
|
||||
|
|
@ -863,8 +861,8 @@ bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const
|
|||
return false;
|
||||
}
|
||||
|
||||
euler_rate_rads.x = ang_vel_rads.x + sin_phi * (sin_theta/cos_theta) * ang_vel_rads.y + cos_phi * (sin_theta/cos_theta) * ang_vel_rads.z;
|
||||
euler_rate_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z;
|
||||
euler_rate_rads.x = ang_vel_rads.x + sin_phi * (sin_theta / cos_theta) * ang_vel_rads.y + cos_phi * (sin_theta / cos_theta) * ang_vel_rads.z;
|
||||
euler_rate_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z;
|
||||
euler_rate_rads.z = (sin_phi / cos_theta) * ang_vel_rads.y + (cos_phi / cos_theta) * ang_vel_rads.z;
|
||||
return true;
|
||||
}
|
||||
|
|
@ -875,24 +873,24 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
|
|||
Vector3f rate_target_ang_vel;
|
||||
// Compute the roll angular velocity demand from the roll angle error
|
||||
if (_use_sqrt_controller) {
|
||||
rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
|
||||
}else{
|
||||
rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
|
||||
} else {
|
||||
rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
|
||||
}
|
||||
// todo: Add Angular Velocity Limit
|
||||
|
||||
// Compute the pitch angular velocity demand from the roll 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{
|
||||
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 {
|
||||
rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
|
||||
}
|
||||
// todo: Add Angular Velocity Limit
|
||||
|
||||
// Compute the yaw angular velocity demand from the roll 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{
|
||||
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 {
|
||||
rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
|
||||
}
|
||||
// todo: Add Angular Velocity Limit
|
||||
|
|
@ -1002,30 +1000,30 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
|
|||
float correction_rate;
|
||||
if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
|
||||
// second order limit is zero or negative.
|
||||
correction_rate = error*p;
|
||||
correction_rate = error * p;
|
||||
} else if (is_zero(p)) {
|
||||
// P term is zero but we have a second order limit.
|
||||
if (is_positive(error)) {
|
||||
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error));
|
||||
correction_rate = safe_sqrt(2.0f * second_ord_lim * (error));
|
||||
} else if (is_negative(error)) {
|
||||
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error));
|
||||
correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error));
|
||||
} else {
|
||||
correction_rate = 0.0f;
|
||||
}
|
||||
} else {
|
||||
// Both the P and second order limit have been defined.
|
||||
float linear_dist = second_ord_lim/sq(p);
|
||||
float linear_dist = second_ord_lim / sq(p);
|
||||
if (error > linear_dist) {
|
||||
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
|
||||
correction_rate = safe_sqrt(2.0f * second_ord_lim * (error - (linear_dist / 2.0f)));
|
||||
} else if (error < -linear_dist) {
|
||||
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
|
||||
correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error - (linear_dist / 2.0f)));
|
||||
} else {
|
||||
correction_rate = error*p;
|
||||
correction_rate = error * p;
|
||||
}
|
||||
}
|
||||
if (!is_zero(dt)) {
|
||||
// this ensures we do not get small oscillations by over shooting the error correction in the last time step.
|
||||
return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
|
||||
return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
|
||||
} else {
|
||||
return correction_rate;
|
||||
}
|
||||
|
|
@ -1035,23 +1033,23 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
|
|||
float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float second_ord_lim)
|
||||
{
|
||||
if (is_positive(second_ord_lim) && !is_zero(second_ord_lim) && is_zero(p)) {
|
||||
return (first_ord_mag*first_ord_mag)/(2.0f*second_ord_lim);
|
||||
return (first_ord_mag * first_ord_mag) / (2.0f * second_ord_lim);
|
||||
} else if ((is_negative(second_ord_lim) || is_zero(second_ord_lim)) && !is_zero(p)) {
|
||||
return first_ord_mag/p;
|
||||
return first_ord_mag / p;
|
||||
} else if ((is_negative(second_ord_lim) || is_zero(second_ord_lim)) && is_zero(p)) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
|
||||
float linear_velocity = second_ord_lim/p;
|
||||
float linear_velocity = second_ord_lim / p;
|
||||
|
||||
if (fabsf(first_ord_mag) < linear_velocity) {
|
||||
// if our current velocity is below the cross-over point we use a linear function
|
||||
return first_ord_mag/p;
|
||||
return first_ord_mag / p;
|
||||
} else {
|
||||
float linear_dist = second_ord_lim/sq(p);
|
||||
float overshoot = (linear_dist*0.5f) + sq(first_ord_mag)/(2.0f*second_ord_lim);
|
||||
if (is_positive(first_ord_mag)){
|
||||
float linear_dist = second_ord_lim / sq(p);
|
||||
float overshoot = (linear_dist * 0.5f) + sq(first_ord_mag) / (2.0f * second_ord_lim);
|
||||
if (is_positive(first_ord_mag)) {
|
||||
return overshoot;
|
||||
} else {
|
||||
return -overshoot;
|
||||
|
|
@ -1063,30 +1061,30 @@ float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float sec
|
|||
float AC_AttitudeControl::max_rate_step_bf_roll()
|
||||
{
|
||||
float alpha = get_rate_roll_pid().get_filt_alpha();
|
||||
float alpha_remaining = 1-alpha;
|
||||
float alpha_remaining = 1 - alpha;
|
||||
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
|
||||
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
|
||||
return 2.0f*throttle_hover*AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_roll_pid().kD())/_dt + get_rate_roll_pid().kP());
|
||||
return 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_roll_pid().kD()) / _dt + get_rate_roll_pid().kP());
|
||||
}
|
||||
|
||||
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
|
||||
float AC_AttitudeControl::max_rate_step_bf_pitch()
|
||||
{
|
||||
float alpha = get_rate_pitch_pid().get_filt_alpha();
|
||||
float alpha_remaining = 1-alpha;
|
||||
float alpha_remaining = 1 - alpha;
|
||||
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
|
||||
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
|
||||
return 2.0f*throttle_hover*AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid().kD())/_dt + get_rate_pitch_pid().kP());
|
||||
return 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_pitch_pid().kD()) / _dt + get_rate_pitch_pid().kP());
|
||||
}
|
||||
|
||||
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
|
||||
float AC_AttitudeControl::max_rate_step_bf_yaw()
|
||||
{
|
||||
float alpha = get_rate_yaw_pid().get_filt_alpha();
|
||||
float alpha_remaining = 1-alpha;
|
||||
float alpha_remaining = 1 - alpha;
|
||||
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
|
||||
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
|
||||
return 2.0f*throttle_hover*AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_yaw_pid().kD())/_dt + get_rate_yaw_pid().kP());
|
||||
return 2.0f * throttle_hover * AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_yaw_pid().kD()) / _dt + get_rate_yaw_pid().kP());
|
||||
}
|
||||
|
||||
bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix,
|
||||
|
|
|
|||
|
|
@ -76,7 +76,7 @@ public:
|
|||
|
||||
// get the roll acceleration limit in centidegrees/s/s or radians/s/s
|
||||
float get_accel_roll_max() const { return _accel_roll_max; }
|
||||
float get_accel_roll_max_radss() const { return radians(_accel_roll_max*0.01f); }
|
||||
float get_accel_roll_max_radss() const { return radians(_accel_roll_max * 0.01f); }
|
||||
|
||||
// Sets the roll acceleration limit in centidegrees/s/s
|
||||
void set_accel_roll_max(float accel_roll_max) { _accel_roll_max = accel_roll_max; }
|
||||
|
|
@ -86,7 +86,7 @@ public:
|
|||
|
||||
// get the pitch acceleration limit in centidegrees/s/s or radians/s/s
|
||||
float get_accel_pitch_max() const { return _accel_pitch_max; }
|
||||
float get_accel_pitch_max_radss() const { return radians(_accel_pitch_max*0.01f); }
|
||||
float get_accel_pitch_max_radss() const { return radians(_accel_pitch_max * 0.01f); }
|
||||
|
||||
// Sets the pitch acceleration limit in centidegrees/s/s
|
||||
void set_accel_pitch_max(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; }
|
||||
|
|
@ -96,7 +96,7 @@ public:
|
|||
|
||||
// get the yaw acceleration limit in centidegrees/s/s or radians/s/s
|
||||
float get_accel_yaw_max() const { return _accel_yaw_max; }
|
||||
float get_accel_yaw_max_radss() const { return radians(_accel_yaw_max*0.01f); }
|
||||
float get_accel_yaw_max_radss() const { return radians(_accel_yaw_max * 0.01f); }
|
||||
|
||||
// Sets the yaw acceleration limit in centidegrees/s/s
|
||||
void set_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; }
|
||||
|
|
@ -117,7 +117,7 @@ public:
|
|||
void set_attitude_target_to_current_attitude() { _ahrs.get_quat_body_to_ned(_attitude_target_quat); }
|
||||
|
||||
// Sets yaw target to vehicle heading
|
||||
void set_yaw_target_to_current_heading() { shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)*100.0f); }
|
||||
void set_yaw_target_to_current_heading() { shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z) * 100.0f); }
|
||||
|
||||
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
|
||||
void shift_ef_yaw_target(float yaw_shift_cd);
|
||||
|
|
@ -175,19 +175,19 @@ public:
|
|||
// attitude controller's target attitude.
|
||||
// **NOTE** Using vector3f*deg(100) is more efficient than deg(vector3f)*100 or deg(vector3d*100) because it gives the
|
||||
// same result with the fewest multiplications. Even though it may look like a bug, it is intentional. See issue 4895.
|
||||
Vector3f get_att_target_euler_cd() const { return _attitude_target_euler_angle*degrees(100.0f); }
|
||||
Vector3f get_att_target_euler_cd() const { return _attitude_target_euler_angle * degrees(100.0f); }
|
||||
|
||||
// Return the angle between the target thrust vector and the current thrust vector.
|
||||
float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); }
|
||||
|
||||
// Set x-axis angular velocity in centidegrees/s
|
||||
void rate_bf_roll_target(float rate_cds) { _rate_target_ang_vel.x = radians(rate_cds*0.01f); }
|
||||
void rate_bf_roll_target(float rate_cds) { _rate_target_ang_vel.x = radians(rate_cds * 0.01f); }
|
||||
|
||||
// Set y-axis angular velocity in centidegrees/s
|
||||
void rate_bf_pitch_target(float rate_cds) { _rate_target_ang_vel.y = radians(rate_cds*0.01f); }
|
||||
void rate_bf_pitch_target(float rate_cds) { _rate_target_ang_vel.y = radians(rate_cds * 0.01f); }
|
||||
|
||||
// Set z-axis angular velocity in centidegrees/s
|
||||
void rate_bf_yaw_target(float rate_cds) { _rate_target_ang_vel.z = radians(rate_cds*0.01f); }
|
||||
void rate_bf_yaw_target(float rate_cds) { _rate_target_ang_vel.z = radians(rate_cds * 0.01f); }
|
||||
|
||||
// Return roll rate step size in radians/s that results in maximum output after 4 time steps
|
||||
float max_rate_step_bf_roll();
|
||||
|
|
@ -199,13 +199,13 @@ public:
|
|||
float max_rate_step_bf_yaw();
|
||||
|
||||
// Return roll step size in radians that results in maximum output after 4 time steps
|
||||
float max_angle_step_bf_roll() { return max_rate_step_bf_roll()/_p_angle_roll.kP(); }
|
||||
float max_angle_step_bf_roll() { return max_rate_step_bf_roll() / _p_angle_roll.kP(); }
|
||||
|
||||
// Return pitch step size in radians that results in maximum output after 4 time steps
|
||||
float max_angle_step_bf_pitch() { return max_rate_step_bf_pitch()/_p_angle_pitch.kP(); }
|
||||
float max_angle_step_bf_pitch() { return max_rate_step_bf_pitch() / _p_angle_pitch.kP(); }
|
||||
|
||||
// Return yaw step size in radians that results in maximum output after 4 time steps
|
||||
float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw()/_p_angle_yaw.kP(); }
|
||||
float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw() / _p_angle_yaw.kP(); }
|
||||
|
||||
// Return angular velocity in radians used in the angular velocity controller
|
||||
Vector3f rate_bf_targets() const { return _rate_target_ang_vel; }
|
||||
|
|
@ -325,7 +325,7 @@ protected:
|
|||
virtual float get_roll_trim_rad() { return 0;}
|
||||
|
||||
// Return the yaw slew rate limit in radians/s
|
||||
float get_slew_yaw_rads() { return radians(_slew_yaw*0.01f); }
|
||||
float get_slew_yaw_rads() { return radians(_slew_yaw * 0.01f); }
|
||||
|
||||
// Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
|
||||
AP_Float _slew_yaw;
|
||||
|
|
|
|||
|
|
@ -188,8 +188,8 @@ void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
|
|||
return;
|
||||
}
|
||||
|
||||
float althold_lean_angle_max = acosf(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
|
||||
_althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max);
|
||||
float althold_lean_angle_max = acosf(constrain_float(_throttle_in / (AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
|
||||
_althold_lean_angle_max = _althold_lean_angle_max + (_dt / (_dt + _angle_limit_tc)) * (althold_lean_angle_max - _althold_lean_angle_max);
|
||||
}
|
||||
|
||||
void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
|
||||
|
|
@ -200,7 +200,7 @@ void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_an
|
|||
if (apply_angle_boost) {
|
||||
// Apply angle boost
|
||||
throttle_in = get_throttle_boosted(throttle_in);
|
||||
}else{
|
||||
} else {
|
||||
// Clear angle_boost for logging purposes
|
||||
_angle_boost = 0.0f;
|
||||
}
|
||||
|
|
@ -220,11 +220,11 @@ float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in)
|
|||
// inverted_factor reduces from 1 to 0 for tilt angles between 60 and 90 degrees
|
||||
|
||||
float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll();
|
||||
float inverted_factor = constrain_float(2.0f*cos_tilt, 0.0f, 1.0f);
|
||||
float boost_factor = 1.0f/constrain_float(cos_tilt, 0.5f, 1.0f);
|
||||
float inverted_factor = constrain_float(2.0f * cos_tilt, 0.0f, 1.0f);
|
||||
float boost_factor = 1.0f / constrain_float(cos_tilt, 0.5f, 1.0f);
|
||||
|
||||
float throttle_out = throttle_in*inverted_factor*boost_factor;
|
||||
_angle_boost = constrain_float(throttle_out - throttle_in,-1.0f,1.0f);
|
||||
float throttle_out = throttle_in * inverted_factor * boost_factor;
|
||||
_angle_boost = constrain_float(throttle_out - throttle_in, -1.0f, 1.0f);
|
||||
return throttle_out;
|
||||
}
|
||||
|
||||
|
|
@ -233,7 +233,7 @@ float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in)
|
|||
float AC_AttitudeControl_Multi::get_throttle_avg_max(float throttle_in)
|
||||
{
|
||||
throttle_in = constrain_float(throttle_in, 0.0f, 1.0f);
|
||||
return MAX(throttle_in, throttle_in*MAX(0.0f,1.0f-_throttle_rpy_mix)+_motors.get_throttle_hover()*_throttle_rpy_mix);
|
||||
return MAX(throttle_in, throttle_in * MAX(0.0f, 1.0f - _throttle_rpy_mix) + _motors.get_throttle_hover() * _throttle_rpy_mix);
|
||||
}
|
||||
|
||||
// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
|
||||
|
|
@ -242,10 +242,10 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
|
|||
// slew _throttle_rpy_mix to _throttle_rpy_mix_desired
|
||||
if (_throttle_rpy_mix < _throttle_rpy_mix_desired) {
|
||||
// increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
|
||||
_throttle_rpy_mix += MIN(2.0f*_dt, _throttle_rpy_mix_desired-_throttle_rpy_mix);
|
||||
_throttle_rpy_mix += MIN(2.0f * _dt, _throttle_rpy_mix_desired - _throttle_rpy_mix);
|
||||
} else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) {
|
||||
// reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
|
||||
_throttle_rpy_mix -= MIN(0.5f*_dt, _throttle_rpy_mix-_throttle_rpy_mix_desired);
|
||||
_throttle_rpy_mix -= MIN(0.5f * _dt, _throttle_rpy_mix - _throttle_rpy_mix_desired);
|
||||
}
|
||||
_throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -70,7 +70,7 @@ public:
|
|||
float get_throttle_mix(void) const override { return _throttle_rpy_mix; }
|
||||
|
||||
// are we producing min throttle?
|
||||
bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); }
|
||||
bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f * _thr_mix_min); }
|
||||
|
||||
// run lowest level body-frame rate controller and send outputs to the motors
|
||||
void rate_controller_run() override;
|
||||
|
|
|
|||
|
|
@ -176,7 +176,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
|
|||
// @Range: 0 45
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max, 0.0f),
|
||||
AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max, 0.0f),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
@ -229,7 +229,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& ina
|
|||
/// z-axis position controller
|
||||
///
|
||||
|
||||
|
||||
/// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
|
||||
void AC_PosControl::set_dt(float delta_sec)
|
||||
{
|
||||
|
|
@ -250,7 +249,7 @@ void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
|
|||
// ensure speed_down is always negative
|
||||
speed_down = -fabsf(speed_down);
|
||||
|
||||
if ((fabsf(_speed_down_cms-speed_down) > 1.0f) || (fabsf(_speed_up_cms-speed_up) > 1.0f)) {
|
||||
if ((fabsf(_speed_down_cms - speed_down) > 1.0f) || (fabsf(_speed_up_cms - speed_up) > 1.0f)) {
|
||||
_speed_down_cms = speed_down;
|
||||
_speed_up_cms = speed_up;
|
||||
_flags.recalc_leash_z = true;
|
||||
|
|
@ -261,7 +260,7 @@ void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
|
|||
/// set_max_accel_z - set the maximum vertical acceleration in cm/s/s
|
||||
void AC_PosControl::set_max_accel_z(float accel_cmss)
|
||||
{
|
||||
if (fabsf(_accel_z_cms-accel_cmss) > 1.0f) {
|
||||
if (fabsf(_accel_z_cms - accel_cmss) > 1.0f) {
|
||||
_accel_z_cms = accel_cmss;
|
||||
_flags.recalc_leash_z = true;
|
||||
calc_leash_length_z();
|
||||
|
|
@ -274,17 +273,17 @@ void AC_PosControl::set_max_accel_z(float accel_cmss)
|
|||
/// target will also be stopped if the motors hit their limits or leash length is exceeded
|
||||
void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
|
||||
{
|
||||
float alt_change = alt_cm-_pos_target.z;
|
||||
float alt_change = alt_cm - _pos_target.z;
|
||||
|
||||
// do not use z-axis desired velocity feed forward
|
||||
_flags.use_desvel_ff_z = false;
|
||||
|
||||
// adjust desired alt if motors have not hit their limits
|
||||
if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) {
|
||||
if ((alt_change < 0 && !_motors.limit.throttle_lower) || (alt_change > 0 && !_motors.limit.throttle_upper)) {
|
||||
if (!is_zero(dt)) {
|
||||
float climb_rate_cms = constrain_float(alt_change/dt, _speed_down_cms, _speed_up_cms);
|
||||
_pos_target.z += climb_rate_cms*dt;
|
||||
_vel_desired.z = climb_rate_cms; // recorded for reporting purposes
|
||||
float climb_rate_cms = constrain_float(alt_change / dt, _speed_down_cms, _speed_up_cms);
|
||||
_pos_target.z += climb_rate_cms * dt;
|
||||
_vel_desired.z = climb_rate_cms; // recorded for reporting purposes
|
||||
}
|
||||
} else {
|
||||
// recorded for reporting purposes
|
||||
|
|
@ -293,10 +292,9 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
|
|||
|
||||
// do not let target get too far from current altitude
|
||||
float curr_alt = _inav.get_altitude();
|
||||
_pos_target.z = constrain_float(_pos_target.z,curr_alt-_leash_down_z,curr_alt+_leash_up_z);
|
||||
_pos_target.z = constrain_float(_pos_target.z, curr_alt - _leash_down_z, curr_alt + _leash_up_z);
|
||||
}
|
||||
|
||||
|
||||
/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
|
||||
/// should be called continuously (with dt set to be the expected time between calls)
|
||||
/// actual position target will be moved no faster than the speed_down and speed_up
|
||||
|
|
@ -305,7 +303,7 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
|
|||
{
|
||||
// adjust desired alt if motors have not hit their limits
|
||||
// To-Do: add check of _limit.pos_down?
|
||||
if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
|
||||
if ((climb_rate_cms < 0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms > 0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
|
||||
_pos_target.z += climb_rate_cms * dt;
|
||||
}
|
||||
|
||||
|
|
@ -335,18 +333,18 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa
|
|||
// jerk_z is calculated to reach full acceleration in 1000ms.
|
||||
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
|
||||
|
||||
float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));
|
||||
float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f * fabsf(_vel_desired.z - climb_rate_cms) * jerk_z));
|
||||
|
||||
_accel_last_z_cms += jerk_z * dt;
|
||||
_accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);
|
||||
|
||||
float vel_change_limit = _accel_last_z_cms * dt;
|
||||
_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
|
||||
_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z - vel_change_limit, _vel_desired.z + vel_change_limit);
|
||||
_flags.use_desvel_ff_z = true;
|
||||
|
||||
// adjust desired alt if motors have not hit their limits
|
||||
// To-Do: add check of _limit.pos_down?
|
||||
if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
|
||||
if ((_vel_desired.z < 0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z > 0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
|
||||
_pos_target.z += _vel_desired.z * dt;
|
||||
}
|
||||
}
|
||||
|
|
@ -382,7 +380,7 @@ void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
|
|||
_accel_last_z_cms = 0.0f;
|
||||
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
|
||||
_flags.reset_accel_to_throttle = true;
|
||||
_pid_accel_z.set_integrator((throttle_setting-_motors.get_throttle_hover())*1000.0f);
|
||||
_pid_accel_z.set_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f);
|
||||
}
|
||||
|
||||
// get_alt_error - returns altitude error in cm
|
||||
|
|
@ -406,7 +404,7 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
|
|||
const float curr_pos_z = _inav.get_altitude();
|
||||
float curr_vel_z = _inav.get_velocity_z();
|
||||
|
||||
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt
|
||||
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt
|
||||
float linear_velocity; // the velocity we swap between linear and sqrt
|
||||
|
||||
// if position controller is active add current velocity error to avoid sudden jump in acceleration
|
||||
|
|
@ -424,17 +422,17 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
|
|||
}
|
||||
|
||||
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
|
||||
linear_velocity = _accel_z_cms/_p_pos_z.kP();
|
||||
linear_velocity = _accel_z_cms / _p_pos_z.kP();
|
||||
|
||||
if (fabsf(curr_vel_z) < linear_velocity) {
|
||||
// if our current velocity is below the cross-over point we use a linear function
|
||||
stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP();
|
||||
stopping_point.z = curr_pos_z + curr_vel_z / _p_pos_z.kP();
|
||||
} else {
|
||||
linear_distance = _accel_z_cms/(2.0f*_p_pos_z.kP()*_p_pos_z.kP());
|
||||
if (curr_vel_z > 0){
|
||||
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
|
||||
linear_distance = _accel_z_cms / (2.0f * _p_pos_z.kP() * _p_pos_z.kP());
|
||||
if (curr_vel_z > 0) {
|
||||
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z * curr_vel_z / (2.0f * _accel_z_cms));
|
||||
} else {
|
||||
stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
|
||||
stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z * curr_vel_z / (2.0f * _accel_z_cms));
|
||||
}
|
||||
}
|
||||
stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX);
|
||||
|
|
@ -451,7 +449,7 @@ void AC_PosControl::init_takeoff()
|
|||
freeze_ff_z();
|
||||
|
||||
// shift difference between last motor out and hover throttle into accelerometer I
|
||||
_pid_accel_z.set_integrator((_motors.get_throttle()-_motors.get_throttle_hover())*1000.0f);
|
||||
_pid_accel_z.set_integrator((_motors.get_throttle() - _motors.get_throttle_hover()) * 1000.0f);
|
||||
|
||||
// initialise ekf reset handler
|
||||
init_ekf_z_reset();
|
||||
|
|
@ -555,12 +553,12 @@ void AC_PosControl::run_z_controller()
|
|||
|
||||
// feed forward desired acceleration calculation
|
||||
if (_dt > 0.0f) {
|
||||
if (!_flags.freeze_ff_z) {
|
||||
_accel_desired.z = (_vel_target.z - _vel_last.z)/_dt;
|
||||
if (!_flags.freeze_ff_z) {
|
||||
_accel_desired.z = (_vel_target.z - _vel_last.z) / _dt;
|
||||
} else {
|
||||
// stop the feed forward being calculated during a known discontinuity
|
||||
_flags.freeze_ff_z = false;
|
||||
}
|
||||
// stop the feed forward being calculated during a known discontinuity
|
||||
_flags.freeze_ff_z = false;
|
||||
}
|
||||
} else {
|
||||
_accel_desired.z = 0.0f;
|
||||
}
|
||||
|
|
@ -583,10 +581,9 @@ void AC_PosControl::run_z_controller()
|
|||
|
||||
_accel_target.z += _accel_desired.z;
|
||||
|
||||
|
||||
// the following section calculates a desired throttle needed to achieve the acceleration target
|
||||
float z_accel_meas; // actual acceleration
|
||||
float p,i,d; // used to capture pid values for logging
|
||||
float p, i, d; // used to capture pid values for logging
|
||||
|
||||
// Calculate Earth Frame Z acceleration
|
||||
z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
|
||||
|
|
@ -618,14 +615,14 @@ void AC_PosControl::run_z_controller()
|
|||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
// To-Do: should this be replaced with limits check from attitude_controller?
|
||||
if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) {
|
||||
if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i > 0 && _accel_error.z < 0) || (i < 0 && _accel_error.z > 0)) {
|
||||
i = _pid_accel_z.get_i();
|
||||
}
|
||||
|
||||
// get d term
|
||||
d = _pid_accel_z.get_d();
|
||||
|
||||
float thr_out = (p+i+d)*0.001f +_motors.get_throttle_hover();
|
||||
float thr_out = (p + i + d) * 0.001f + _motors.get_throttle_hover();
|
||||
|
||||
// send throttle to attitude controller with angle boost
|
||||
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
|
||||
|
|
@ -638,7 +635,7 @@ void AC_PosControl::run_z_controller()
|
|||
/// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s
|
||||
void AC_PosControl::set_max_accel_xy(float accel_cmss)
|
||||
{
|
||||
if (fabsf(_accel_cms-accel_cmss) > 1.0f) {
|
||||
if (fabsf(_accel_cms - accel_cmss) > 1.0f) {
|
||||
_accel_cms = accel_cmss;
|
||||
_flags.recalc_leash_xy = true;
|
||||
calc_leash_length_xy();
|
||||
|
|
@ -648,7 +645,7 @@ void AC_PosControl::set_max_accel_xy(float accel_cmss)
|
|||
/// set_max_speed_xy - set the maximum horizontal speed maximum in cm/s
|
||||
void AC_PosControl::set_max_speed_xy(float speed_cms)
|
||||
{
|
||||
if (fabsf(_speed_cms-speed_cms) > 1.0f) {
|
||||
if (fabsf(_speed_cms - speed_cms) > 1.0f) {
|
||||
_speed_cms = speed_cms;
|
||||
_flags.recalc_leash_xy = true;
|
||||
calc_leash_length_xy();
|
||||
|
|
@ -703,7 +700,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
|
|||
Vector3f curr_vel = _inav.get_velocity();
|
||||
float linear_distance; // the distance at which we swap from a linear to sqrt response
|
||||
float linear_velocity; // the velocity above which we swap from a linear to sqrt response
|
||||
float stopping_dist; // the distance within the vehicle can stop
|
||||
float stopping_dist; // the distance within the vehicle can stop
|
||||
float kP = _p_pos_xy.kP();
|
||||
|
||||
// add velocity error to current velocity
|
||||
|
|
@ -723,14 +720,14 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
|
|||
}
|
||||
|
||||
// calculate point at which velocity switches from linear to sqrt
|
||||
linear_velocity = _accel_cms/kP;
|
||||
linear_velocity = _accel_cms / kP;
|
||||
|
||||
// calculate distance within which we can stop
|
||||
if (vel_total < linear_velocity) {
|
||||
stopping_dist = vel_total/kP;
|
||||
stopping_dist = vel_total / kP;
|
||||
} else {
|
||||
linear_distance = _accel_cms/(2.0f*kP*kP);
|
||||
stopping_dist = linear_distance + (vel_total*vel_total)/(2.0f*_accel_cms);
|
||||
linear_distance = _accel_cms / (2.0f * kP * kP);
|
||||
stopping_dist = linear_distance + (vel_total * vel_total) / (2.0f * _accel_cms);
|
||||
}
|
||||
|
||||
// constrain stopping distance
|
||||
|
|
@ -783,7 +780,7 @@ void AC_PosControl::init_xy_controller()
|
|||
// initialise I terms from lean angles
|
||||
_pid_vel_xy.reset_filter();
|
||||
lean_angles_to_accel(_accel_target.x, _accel_target.y);
|
||||
_pid_vel_xy.set_integrator(_accel_target-_accel_desired);
|
||||
_pid_vel_xy.set_integrator(_accel_target - _accel_desired);
|
||||
|
||||
// flag reset required in rate to accel step
|
||||
_flags.reset_desired_vel_to_pos = true;
|
||||
|
|
@ -882,7 +879,7 @@ void AC_PosControl::init_vel_controller_xyz()
|
|||
set_desired_velocity(curr_vel);
|
||||
|
||||
// set vehicle acceleration to zero
|
||||
set_desired_accel_xy(0.0f,0.0f);
|
||||
set_desired_accel_xy(0.0f, 0.0f);
|
||||
|
||||
// initialise ekf reset handlers
|
||||
init_ekf_xy_reset();
|
||||
|
|
@ -921,7 +918,6 @@ void AC_PosControl::update_vel_controller_xy()
|
|||
_last_update_xy_us = now_us;
|
||||
}
|
||||
|
||||
|
||||
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
|
||||
/// velocity targets should we set using set_desired_velocity_xyz() method
|
||||
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
|
||||
|
|
@ -978,7 +974,7 @@ void AC_PosControl::desired_accel_to_vel(float nav_dt)
|
|||
void AC_PosControl::desired_vel_to_pos(float nav_dt)
|
||||
{
|
||||
// range check nav_dt
|
||||
if( nav_dt < 0 ) {
|
||||
if (nav_dt < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -1008,7 +1004,7 @@ void AC_PosControl::run_xy_controller(float dt)
|
|||
if (kP <= 0.0f) {
|
||||
_vel_target.x = 0.0f;
|
||||
_vel_target.y = 0.0f;
|
||||
}else{
|
||||
} else {
|
||||
// calculate distance error
|
||||
_pos_error.x = _pos_target.x - curr_pos.x;
|
||||
_pos_error.y = _pos_target.y - curr_pos.y;
|
||||
|
|
@ -1016,8 +1012,7 @@ void AC_PosControl::run_xy_controller(float dt)
|
|||
// Constrain _pos_error and target position
|
||||
// Constrain the maximum length of _vel_target to the maximum position correction velocity
|
||||
// TODO: replace the leash length with a user definable maximum position correction
|
||||
if (limit_vector_length(_pos_error.x, _pos_error.y, _leash))
|
||||
{
|
||||
if (limit_vector_length(_pos_error.x, _pos_error.y, _leash)) {
|
||||
_pos_target.x = curr_pos.x + _pos_error.x;
|
||||
_pos_target.y = curr_pos.y + _pos_error.y;
|
||||
}
|
||||
|
|
@ -1068,13 +1063,13 @@ void AC_PosControl::run_xy_controller(float dt)
|
|||
accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
|
||||
|
||||
// reset accel to current desired acceleration
|
||||
if (_flags.reset_accel_to_lean_xy) {
|
||||
_accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));
|
||||
_flags.reset_accel_to_lean_xy = false;
|
||||
}
|
||||
if (_flags.reset_accel_to_lean_xy) {
|
||||
_accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));
|
||||
_flags.reset_accel_to_lean_xy = false;
|
||||
}
|
||||
|
||||
// filter correction acceleration
|
||||
_accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
|
||||
_accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f * ekfNavVelGainScaler));
|
||||
_accel_target_filter.apply(accel_target, dt);
|
||||
|
||||
// pass the correction acceleration to the target acceleration output
|
||||
|
|
@ -1103,13 +1098,13 @@ void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss,
|
|||
|
||||
// rotate accelerations into body forward-right frame
|
||||
// todo: this should probably be based on the desired heading not the current heading
|
||||
accel_forward = accel_x_cmss*_ahrs.cos_yaw() + accel_y_cmss*_ahrs.sin_yaw();
|
||||
accel_right = -accel_x_cmss*_ahrs.sin_yaw() + accel_y_cmss*_ahrs.cos_yaw();
|
||||
accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
|
||||
accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();
|
||||
|
||||
// update angle targets that will be passed to stabilize controller
|
||||
pitch_target = atanf(-accel_forward/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
|
||||
float cos_pitch_target = cosf(pitch_target*M_PI/18000.0f);
|
||||
roll_target = atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
|
||||
pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
|
||||
float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
|
||||
roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
|
||||
}
|
||||
|
||||
// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
|
||||
|
|
@ -1117,8 +1112,8 @@ void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cms
|
|||
{
|
||||
// rotate our roll, pitch angles into lat/lon frame
|
||||
// todo: this should probably be based on the desired attitude not the current attitude
|
||||
accel_x_cmss = (GRAVITY_MSS * 100) * (-_ahrs.cos_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() - _ahrs.sin_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f);
|
||||
accel_y_cmss = (GRAVITY_MSS * 100) * (-_ahrs.sin_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() + _ahrs.cos_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f);
|
||||
accel_x_cmss = (GRAVITY_MSS * 100) * (-_ahrs.cos_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() - _ahrs.sin_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll() * _ahrs.cos_pitch(), 0.5f);
|
||||
accel_y_cmss = (GRAVITY_MSS * 100) * (-_ahrs.sin_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() + _ahrs.cos_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll() * _ahrs.cos_pitch(), 0.5f);
|
||||
}
|
||||
|
||||
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
|
||||
|
|
@ -1137,16 +1132,16 @@ float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float k
|
|||
}
|
||||
|
||||
// calculate leash length
|
||||
if(speed_cms <= accel_cms / kP) {
|
||||
if (speed_cms <= accel_cms / kP) {
|
||||
// linear leash length based on speed close in
|
||||
leash_length = speed_cms / kP;
|
||||
}else{
|
||||
} else {
|
||||
// leash length grows at sqrt of speed further out
|
||||
leash_length = (accel_cms / (2.0f*kP*kP)) + (speed_cms*speed_cms / (2.0f*accel_cms));
|
||||
leash_length = (accel_cms / (2.0f * kP * kP)) + (speed_cms * speed_cms / (2.0f * accel_cms));
|
||||
}
|
||||
|
||||
// ensure leash is at least 1m long
|
||||
if( leash_length < POSCONTROL_LEASH_LENGTH_MIN ) {
|
||||
if (leash_length < POSCONTROL_LEASH_LENGTH_MIN) {
|
||||
leash_length = POSCONTROL_LEASH_LENGTH_MIN;
|
||||
}
|
||||
|
||||
|
|
@ -1191,7 +1186,6 @@ void AC_PosControl::check_for_ekf_z_reset()
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
/// limit vector to a given length, returns true if vector was limited
|
||||
bool AC_PosControl::limit_vector_length(float& vector_x, float& vector_y, float max_length)
|
||||
{
|
||||
|
|
@ -1204,21 +1198,20 @@ bool AC_PosControl::limit_vector_length(float& vector_x, float& vector_y, float
|
|||
return false;
|
||||
}
|
||||
|
||||
|
||||
/// Proportional controller with piecewise sqrt sections to constrain second derivative
|
||||
Vector3f AC_PosControl::sqrt_controller(const Vector3f& error, float p, float second_ord_lim)
|
||||
{
|
||||
if (second_ord_lim < 0.0f || is_zero(second_ord_lim) || is_zero(p)) {
|
||||
return Vector3f(error.x*p, error.y*p, error.z);
|
||||
return Vector3f(error.x * p, error.y * p, error.z);
|
||||
}
|
||||
|
||||
float linear_dist = second_ord_lim/sq(p);
|
||||
float linear_dist = second_ord_lim / sq(p);
|
||||
float error_length = norm(error.x, error.y);
|
||||
if (error_length > linear_dist) {
|
||||
float first_order_scale = safe_sqrt(2.0f*second_ord_lim*(error_length-(linear_dist * 0.5f)))/error_length;
|
||||
return Vector3f(error.x*first_order_scale, error.y*first_order_scale, error.z);
|
||||
float first_order_scale = safe_sqrt(2.0f * second_ord_lim * (error_length - (linear_dist * 0.5f))) / error_length;
|
||||
return Vector3f(error.x * first_order_scale, error.y * first_order_scale, error.z);
|
||||
} else {
|
||||
return Vector3f(error.x*p, error.y*p, error.z);
|
||||
return Vector3f(error.x * p, error.y * p, error.z);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -128,7 +128,7 @@ public:
|
|||
|
||||
/// get_alt_error - returns altitude error in cm
|
||||
float get_alt_error() const;
|
||||
|
||||
|
||||
// returns horizontal error in cm
|
||||
float get_horizontal_error() const;
|
||||
|
||||
|
|
@ -177,7 +177,7 @@ public:
|
|||
/// set_limit_accel_xy - mark that accel has been limited
|
||||
/// this prevents integrator buildup
|
||||
void set_limit_accel_xy(void) { _limit.accel_xy = true; }
|
||||
|
||||
|
||||
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
|
||||
/// should be called whenever the speed, acceleration or position kP is modified
|
||||
void calc_leash_length_xy();
|
||||
|
|
@ -257,7 +257,7 @@ public:
|
|||
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
|
||||
/// throttle targets will be sent directly to the motors
|
||||
void update_vel_controller_xy();
|
||||
|
||||
|
||||
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
|
||||
/// velocity targets should we set using set_desired_velocity_xyz() method
|
||||
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
|
||||
|
|
|
|||
Loading…
Reference in New Issue