diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index fcd8306097..1dff37e62f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -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, diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index e1c7a9beb3..21a05e99b9 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -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; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 0d52172470..880520e2da 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -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); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 7131cfdf16..b174ab77a1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -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; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index bed354e7d0..c4246d76d1 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -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); } } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 9b7060f327..ed88569c8a 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -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