aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: support for upgrade to PID object

Browse Source
This commit is contained in:
Leonard Hall 2019-06-27 19:06:12 +09:30 committed by Randy Mackay
parent f913108efd
commit 6f14673f08
1 changed files with 17 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -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;