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

AP_Control: Support changing update period

This commit is contained in:
Leonard Hall 2022-11-30 15:54:18 +10:30 committed by Randy Mackay
parent 78e125c41b
commit 0c55933378
8 changed files with 17 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/APM_Control/AP_PitchController.cpp
View File

@ -155,8 +155,6 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
float rate_y = _ahrs.get_gyro().y;
float old_I = rate_pid.get_i();
rate_pid.set_dt(dt);
bool underspeed = aspeed <= 0.5*float(aparm.airspeed_min);
if (underspeed) {
limit_I = true;
@ -167,7 +165,7 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
//
// note that we run AC_PID in radians so that the normal scaling
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_y * scaler * scaler, limit_I);
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_y * scaler * scaler, dt, limit_I);
if (underspeed) {
// when underspeed we lock the integrator

2
libraries/APM_Control/AP_PitchController.h
View File

@ -56,7 +56,7 @@ private:
AP_Int16 _max_rate_neg;
AP_Float _roll_ff;
float _last_out;
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1};
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1};
float angle_err_deg;
AP_PIDInfo _pid_info;

4
libraries/APM_Control/AP_RollController.cpp
View File

@ -140,8 +140,6 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
float aspeed;
float old_I = rate_pid.get_i();
rate_pid.set_dt(dt);
if (!_ahrs.airspeed_estimate(aspeed)) {
aspeed = 0;
}
@ -155,7 +153,7 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
//
// note that we run AC_PID in radians so that the normal scaling
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_x * scaler * scaler, limit_I);
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_x * scaler * scaler, dt, limit_I);
if (underspeed) {
// when underspeed we lock the integrator

2
libraries/APM_Control/AP_RollController.h
View File

@ -61,7 +61,7 @@ private:
AP_AutoTune *autotune;
bool failed_autotune_alloc;
float _last_out;
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1};
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1};
float angle_err_deg;
AP_PIDInfo _pid_info;

4
libraries/APM_Control/AP_YawController.cpp
View File

@ -279,8 +279,6 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa
float aspeed;
float old_I = rate_pid.get_i();
rate_pid.set_dt(dt);
if (!_ahrs.airspeed_estimate(aspeed)) {
aspeed = 0;
}
@ -294,7 +292,7 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa
//
// note that we run AC_PID in radians so that the normal scaling
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_z * scaler * scaler, limit_I);
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_z * scaler * scaler, dt, limit_I);
if (underspeed) {
// when underspeed we lock the integrator

2
libraries/APM_Control/AP_YawController.h
View File

@ -57,7 +57,7 @@ private:
AP_Float _K_FF;
AP_Int16 _imax;
AP_Int8 _rate_enable;
AC_PID rate_pid{0.04, 0.15, 0, 0.15, 0.666, 3, 0, 12, 0.02, 150, 1};
AC_PID rate_pid{0.04, 0.15, 0, 0.15, 0.666, 3, 0, 12, 150, 1};
uint32_t _last_t;
float _last_out;

28
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -467,10 +467,10 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
AR_AttitudeControl::AR_AttitudeControl() :
_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_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)
_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),
_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),
_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),
_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)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -566,10 +566,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l
_desired_turn_rate = constrain_float(_desired_turn_rate, -turn_rate_max, turn_rate_max);
}
// set PID's dt
_steer_rate_pid.set_dt(dt);
float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), (motor_limit_left || motor_limit_right));
float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), dt, (motor_limit_left || motor_limit_right));
output += _steer_rate_pid.get_ff();
// constrain and return final output
return output;
@ -648,9 +645,6 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor
// acceleration limit desired speed
_desired_speed = get_desired_speed_accel_limited(desired_speed, dt);
// set PID's dt
_throttle_speed_pid.set_dt(dt);
// calculate base throttle (protect against divide by zero)
float throttle_base = 0.0f;
if (is_positive(cruise_speed) && is_positive(cruise_throttle)) {
@ -658,7 +652,7 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor
}
// calculate final output
float throttle_out = _throttle_speed_pid.update_all(_desired_speed, speed, (motor_limit_low || motor_limit_high || _throttle_limit_low || _throttle_limit_high));
float throttle_out = _throttle_speed_pid.update_all(_desired_speed, speed, dt, (motor_limit_low || motor_limit_high || _throttle_limit_low || _throttle_limit_high));
throttle_out += _throttle_speed_pid.get_ff();
throttle_out += throttle_base;
@ -761,15 +755,12 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float
_pitch_limited = false;
}
// set PID's dt
_pitch_to_throttle_pid.set_dt(dt);
// initialise output to feed forward from current pitch angle
const float pitch_rad = AP::ahrs().pitch;
float output = sinf(pitch_rad) * _pitch_to_throttle_ff;
// add regular PID control
output += _pitch_to_throttle_pid.update_all(desired_pitch, pitch_rad, motor_limit);
output += _pitch_to_throttle_pid.update_all(desired_pitch, pitch_rad, dt, motor_limit);
output += _pitch_to_throttle_pid.get_ff();
// update pitch limits for next iteration
@ -821,10 +812,7 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt)
}
_heel_controller_last_ms = now;
// set PID's dt
_sailboat_heel_pid.set_dt(dt);
_sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().roll));
_sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().roll), dt);
// get feed-forward
const float ff = _sailboat_heel_pid.get_ff();

10
libraries/APM_Control/AR_PosControl.cpp
View File

@ -100,8 +100,8 @@ const AP_Param::GroupInfo AR_PosControl::var_info[] = {
AR_PosControl::AR_PosControl(AR_AttitudeControl& atc) :
_atc(atc),
_p_pos(AR_POSCON_POS_P, AR_POSCON_DT),
_pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D, AR_POSCON_DT)
_p_pos(AR_POSCON_POS_P),
_pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -130,10 +130,6 @@ void AR_PosControl::update(float dt)
}
_last_update_ms = AP_HAL::millis();
// update P, PID object's dt
_p_pos.set_dt(dt);
_pid_vel.set_dt(dt);
// calculate position error and convert to desired velocity
_vel_target.zero();
if (_pos_target_valid) {
@ -163,7 +159,7 @@ void AR_PosControl::update(float dt)
// calculate desired acceleration
// To-Do: fixup _limit_vel used below
_accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), _limit_vel);
_accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), dt, _limit_vel);
if (_accel_desired_valid) {
_accel_target += _accel_desired;
}