Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: use is_positive and is_negative

This commit is contained in:
Randy Mackay 2018-02-02 16:00:07 +09:00
parent b3de52dc59
commit 9aa6415e1c
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -579,7 +579,7 @@ float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc,
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt) float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
{ {
// Acceleration is limited directly to smooth the beginning of the curve. // Acceleration is limited directly to smooth the beginning of the curve.
if (accel_max > 0.0f) { if (is_positive(accel_max)) {
float delta_ang_vel = accel_max * dt; 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 { } else {
@ -638,7 +638,7 @@ Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f eule
float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f); float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
Vector3f rot_accel; Vector3f rot_accel;
if(is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || (euler_accel.x < 0.0f) || (euler_accel.y < 0.0f) || (euler_accel.z < 0.0f)) { 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.x = euler_accel.x;
rot_accel.y = euler_accel.y; rot_accel.y = euler_accel.y;
rot_accel.z = euler_accel.z; rot_accel.z = euler_accel.z;
@ -749,7 +749,7 @@ float AC_AttitudeControl::rate_target_to_motor_roll(float rate_actual_rads, floa
float integrator = get_rate_roll_pid().get_integrator(); float integrator = get_rate_roll_pid().get_integrator();
// Ensure that integrator can only be reduced if the output is saturated // Ensure that integrator can only be reduced if the output is saturated
if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) { if (!_motors.limit.roll_pitch || ((is_positive(integrator) && is_negative(rate_error_rads)) || (is_negative(integrator) && is_positive(rate_error_rads)))) {
integrator = get_rate_roll_pid().get_i(); integrator = get_rate_roll_pid().get_i();
} }
@ -772,7 +772,7 @@ float AC_AttitudeControl::rate_target_to_motor_pitch(float rate_actual_rads, flo
float integrator = get_rate_pitch_pid().get_integrator(); float integrator = get_rate_pitch_pid().get_integrator();
// Ensure that integrator can only be reduced if the output is saturated // Ensure that integrator can only be reduced if the output is saturated
if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) { if (!_motors.limit.roll_pitch || ((is_positive(integrator) && is_negative(rate_error_rads)) || (is_negative(integrator) && is_positive(rate_error_rads)))) {
integrator = get_rate_pitch_pid().get_i(); integrator = get_rate_pitch_pid().get_i();
} }
@ -795,7 +795,7 @@ float AC_AttitudeControl::rate_target_to_motor_yaw(float rate_actual_rads, float
float integrator = get_rate_yaw_pid().get_integrator(); float integrator = get_rate_yaw_pid().get_integrator();
// Ensure that integrator can only be reduced if the output is saturated // Ensure that integrator can only be reduced if the output is saturated
if (!_motors.limit.yaw || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) { if (!_motors.limit.yaw || ((is_positive(integrator) && is_negative(rate_error_rads)) || (is_negative(integrator) && is_positive(rate_error_rads)))) {
integrator = get_rate_yaw_pid().get_i(); integrator = get_rate_yaw_pid().get_i();
} }
@ -872,11 +872,11 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
// Inverse proportional controller with piecewise sqrt sections to constrain second derivative // Inverse proportional controller with piecewise sqrt sections to constrain second derivative
float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float second_ord_lim) float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float second_ord_lim)
{ {
if (second_ord_lim > 0.0f && !is_zero(second_ord_lim) && is_zero(p)) { 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 ((second_ord_lim < 0.0f || is_zero(second_ord_lim)) && !is_zero(p)) { } 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 ((second_ord_lim < 0.0f || is_zero(second_ord_lim)) && is_zero(p)) { } else if ((is_negative(second_ord_lim) || is_zero(second_ord_lim)) && is_zero(p)) {
return 0.0f; return 0.0f;
} }
@ -889,7 +889,7 @@ float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float sec
} else { } else {
float linear_dist = second_ord_lim/sq(p); float linear_dist = second_ord_lim/sq(p);
float overshoot = (linear_dist/2.0f) + sq(first_ord_mag)/(2.0f*second_ord_lim); float overshoot = (linear_dist/2.0f) + sq(first_ord_mag)/(2.0f*second_ord_lim);
if (first_ord_mag > 0){ if (is_positive(first_ord_mag)){
return overshoot; return overshoot;
} else { } else {
return -overshoot; return -overshoot;