diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 4161cdb972..e267112796 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -269,10 +269,10 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) : _ahrs(ahrs), _steer_angle_p(AR_ATTCONTROL_STEER_ANG_P), - _steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_IMAX, AR_ATTCONTROL_STEER_RATE_FILT, AR_ATTCONTROL_DT, AR_ATTCONTROL_STEER_RATE_FF), - _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, AR_ATTCONTROL_THR_SPEED_IMAX, AR_ATTCONTROL_THR_SPEED_FILT, AR_ATTCONTROL_DT), - _pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, AR_ATTCONTROL_PITCH_THR_IMAX, AR_ATTCONTROL_PITCH_THR_FILT, AR_ATTCONTROL_DT), - _sailboat_heel_pid(AR_ATTCONTROL_HEEL_SAIL_P, AR_ATTCONTROL_HEEL_SAIL_I, AR_ATTCONTROL_HEEL_SAIL_D, AR_ATTCONTROL_HEEL_SAIL_IMAX, AR_ATTCONTROL_HEEL_SAIL_FILT, AR_ATTCONTROL_DT) + _steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_FF, AR_ATTCONTROL_STEER_RATE_IMAX, 0.0f, AR_ATTCONTROL_STEER_RATE_FILT, 0.0f, AR_ATTCONTROL_DT), + _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, 0.0f, AR_ATTCONTROL_THR_SPEED_IMAX, 0.0f, AR_ATTCONTROL_THR_SPEED_FILT, 0.0f, AR_ATTCONTROL_DT), + _pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, 0.0f, AR_ATTCONTROL_PITCH_THR_IMAX, 0.0f, AR_ATTCONTROL_PITCH_THR_FILT, 0.0f, AR_ATTCONTROL_DT), + _sailboat_heel_pid(AR_ATTCONTROL_HEEL_SAIL_P, AR_ATTCONTROL_HEEL_SAIL_I, AR_ATTCONTROL_HEEL_SAIL_D, 0.0f, AR_ATTCONTROL_HEEL_SAIL_IMAX, 0.0f, AR_ATTCONTROL_HEEL_SAIL_FILT, 0.0f, AR_ATTCONTROL_DT) { AP_Param::setup_object_defaults(this, var_info); } @@ -354,36 +354,13 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l _desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad); } - // Calculate the steering rate error (rad/sec) - // We do this in earth frame to allow for rover leaning over in hard corners - const float rate_error = (_desired_turn_rate - _ahrs.get_yaw_rate_earth()); - // set PID's dt _steer_rate_pid.set_dt(dt); - // record desired rate for logging purposes only - _steer_rate_pid.set_desired_rate(_desired_turn_rate); - - // pass error to PID controller - _steer_rate_pid.set_input_filter_all(rate_error); - - // get feed-forward - const float ff = _steer_rate_pid.get_ff(_desired_turn_rate); - - // get p - const float p = _steer_rate_pid.get_p(); - - // get i unless non-skid-steering rover at low speed or steering output has hit a limit - float i = _steer_rate_pid.get_integrator(); - if ((is_negative(rate_error) && !motor_limit_left) || (is_positive(rate_error) && !motor_limit_right)) { - i = _steer_rate_pid.get_i(); - } - - // get d - const float d = _steer_rate_pid.get_d(); - + float output = _steer_rate_pid.update_all(_desired_turn_rate, _ahrs.get_yaw_rate_earth(), (motor_limit_left || motor_limit_right)); + output += _steer_rate_pid.get_ff(); // constrain and return final output - return (ff + p + i + d); + return output; } // get latest desired turn rate in rad/sec (recorded during calls to get_steering_out_rate) @@ -462,28 +439,6 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor // set PID's dt _throttle_speed_pid.set_dt(dt); - // calculate speed error and pass to PID controller - const float speed_error = desired_speed - speed; - _throttle_speed_pid.set_input_filter_all(speed_error); - - // record desired speed for logging purposes only - _throttle_speed_pid.set_desired_rate(desired_speed); - - // get feed-forward - const float ff = _throttle_speed_pid.get_ff(desired_speed); - - // get p - const float p = _throttle_speed_pid.get_p(); - - // get i unless moving at low speed or motors have hit a limit - float i = _throttle_speed_pid.get_integrator(); - if ((is_negative(speed_error) && !motor_limit_low && !_throttle_limit_low) || (is_positive(speed_error) && !motor_limit_high && !_throttle_limit_high)) { - i = _throttle_speed_pid.get_i(); - } - - // get d - const float d = _throttle_speed_pid.get_d(); - // calculate base throttle (protect against divide by zero) float throttle_base = 0.0f; if (is_positive(cruise_speed) && is_positive(cruise_throttle)) { @@ -491,7 +446,9 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor } // calculate final output - float throttle_out = (ff+p+i+d+throttle_base); + float throttle_out = _throttle_speed_pid.update_all(desired_speed, speed, (_throttle_limit_low || _throttle_limit_high)); + throttle_out += _throttle_speed_pid.get_ff(); + throttle_out += throttle_base; // clear local limit flags used to stop i-term build-up as we stop reversed outputs going to motors _throttle_limit_low = false; @@ -571,38 +528,16 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float } _balance_last_ms = now; - // calculate pitch error - const float pitch_error = desired_pitch - _ahrs.pitch; - // set PID's dt _pitch_to_throttle_pid.set_dt(dt); - // record desired speed for logging purposes only - _pitch_to_throttle_pid.set_desired_rate(desired_pitch); - - // pitch error is given as input to PID contoller - _pitch_to_throttle_pid.set_input_filter_all(pitch_error); - - // get feed-forward - const float ff = _pitch_to_throttle_pid.get_ff(desired_pitch); - - // get p - const float p = _pitch_to_throttle_pid.get_p(); - - // get i unless non-skid-steering rover at low speed or steering output has hit a limit - float i = _pitch_to_throttle_pid.get_integrator(); - if ((is_negative(pitch_error) && !motor_limit_low) || (is_positive(pitch_error) && !motor_limit_high)) { - i = _pitch_to_throttle_pid.get_i(); - } - - // get d - const float d = _pitch_to_throttle_pid.get_d(); - // add feed forward from speed - const float spd_ff = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff; + float output = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff; + output += _pitch_to_throttle_pid.update_all(desired_pitch, _ahrs.pitch, (motor_limit_low || motor_limit_high)); + output += _pitch_to_throttle_pid.get_ff(); // constrain and return final output - return (ff + p + i + d + spd_ff); + return output; } // get latest desired pitch in radians for reporting purposes @@ -631,20 +566,13 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt) } _heel_controller_last_ms = now; - // calculate heel error, we don't care about the sign - const float heel_error = fabsf(_ahrs.roll) - desired_heel; - // set PID's dt _sailboat_heel_pid.set_dt(dt); - // record desired heel angle for logging purposes only - _sailboat_heel_pid.set_desired_rate(desired_heel); - - // heel error is given as input to PID contoller - _sailboat_heel_pid.set_input_filter_all(heel_error); + _sailboat_heel_pid.update_all(desired_heel, fabsf(_ahrs.roll)); // get feed-forward - const float ff = _sailboat_heel_pid.get_ff(desired_heel); + const float ff = _sailboat_heel_pid.get_ff(); // get p float p = _sailboat_heel_pid.get_p(); @@ -654,7 +582,7 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt) } // get i - float i = _sailboat_heel_pid.get_integrator(); + float i = _sailboat_heel_pid.get_i(); // constrain i to only be positive, reset integrator if negative if (!is_positive(i)) { i = 0.0f;