From 451faaa059d2897b204d1a9a39d4b006449ab028 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 5 Dec 2022 22:51:43 +1030 Subject: [PATCH] AC_AttitudeControl: Support changing update period --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 18 ++++--- .../AC_AttitudeControl/AC_AttitudeControl.h | 10 ++-- .../AC_AttitudeControl_Heli.cpp | 6 +-- .../AC_AttitudeControl_Heli.h | 11 ++--- .../AC_AttitudeControl_Multi.cpp | 16 +++---- .../AC_AttitudeControl_Multi.h | 2 +- .../AC_AttitudeControl_Multi_6DoF.h | 4 +- .../AC_AttitudeControl_Sub.cpp | 16 +++---- .../AC_AttitudeControl_Sub.h | 2 +- .../AC_AttitudeControl/AC_PosControl.cpp | 47 ++++++++++--------- libraries/AC_AttitudeControl/AC_PosControl.h | 13 +++-- 11 files changed, 80 insertions(+), 65 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 5b013531ce..76b4f15f70 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -1,5 +1,6 @@ #include "AC_AttitudeControl.h" #include +#include extern const AP_HAL::HAL& hal; @@ -195,9 +196,9 @@ void AC_AttitudeControl::reset_rate_controller_I_terms() // reset rate controller I terms smoothly to zero in 0.5 seconds void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() { - get_rate_roll_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC); - get_rate_pitch_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC); - get_rate_yaw_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC); + get_rate_roll_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC); + get_rate_pitch_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC); + get_rate_yaw_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC); } // The attitude controller works around the concept of the desired attitude, target attitude @@ -223,7 +224,7 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() // 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 -// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity +// attitude_desired_quat: is updated on each time_step by the integral of the angular velocity void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) { Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat; @@ -1051,7 +1052,8 @@ float AC_AttitudeControl::get_althold_lean_angle_max_cd() const // Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps float AC_AttitudeControl::max_rate_step_bf_roll() { - float alpha = MIN(get_rate_roll_pid().get_filt_E_alpha(), get_rate_roll_pid().get_filt_D_alpha()); + float dt_average = AP::scheduler().get_filtered_loop_time(); + float alpha = MIN(get_rate_roll_pid().get_filt_E_alpha(dt_average), get_rate_roll_pid().get_filt_D_alpha(dt_average)); 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); @@ -1065,7 +1067,8 @@ float AC_AttitudeControl::max_rate_step_bf_roll() // 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 = MIN(get_rate_pitch_pid().get_filt_E_alpha(), get_rate_pitch_pid().get_filt_D_alpha()); + float dt_average = AP::scheduler().get_filtered_loop_time(); + float alpha = MIN(get_rate_pitch_pid().get_filt_E_alpha(dt_average), get_rate_pitch_pid().get_filt_D_alpha(dt_average)); 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); @@ -1079,7 +1082,8 @@ float AC_AttitudeControl::max_rate_step_bf_pitch() // 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 = MIN(get_rate_yaw_pid().get_filt_E_alpha(), get_rate_yaw_pid().get_filt_D_alpha()); + float dt_average = AP::scheduler().get_filtered_loop_time(); + float alpha = MIN(get_rate_yaw_pid().get_filt_E_alpha(dt_average), get_rate_yaw_pid().get_filt_D_alpha(dt_average)); 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); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index ef4b1615a2..fd7b8af913 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -49,12 +49,10 @@ class AC_AttitudeControl { public: AC_AttitudeControl( AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, - AP_Motors& motors, - float dt) : + AP_Motors& motors) : _p_angle_roll(AC_ATTITUDE_CONTROL_ANGLE_P), _p_angle_pitch(AC_ATTITUDE_CONTROL_ANGLE_P), _p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P), - _dt(dt), _angle_boost(0), _use_sqrt_controller(true), _throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT), @@ -74,6 +72,12 @@ public: // Empty destructor to suppress compiler warning virtual ~AC_AttitudeControl() {} + // set_dt / get_dt - dt is the time since the last time the attitude controllers were updated + // _dt should be set based on the time of the last IMU read used by these controllers + // the attitude controller should run updates for active controllers on each loop to ensure normal operation + void set_dt(float dt) { _dt = dt; } + float get_dt() const { return _dt; } + // pid accessors AC_P& get_angle_roll_p() { return _p_angle_roll; } AC_P& get_angle_pitch_p() { return _p_angle_pitch; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 7d99b4d43e..371b5001bd 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -387,13 +387,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r if (_flags_heli.leaky_i) { _pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); } - float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _motors.limit.roll) + _actuator_sysid.x; + float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _dt, _motors.limit.roll) + _actuator_sysid.x; if (_flags_heli.leaky_i) { _pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); } - float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _motors.limit.pitch) + _actuator_sysid.y; + float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _dt, _motors.limit.pitch) + _actuator_sysid.y; // use pid library to calculate ff float roll_ff = _pid_rate_roll.get_ff(); @@ -449,7 +449,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_ra _pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); } - float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _motors.limit.yaw) + _actuator_sysid.z; + float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _dt, _motors.limit.yaw) + _actuator_sysid.z; // use pid library to calculate ff float vff = _pid_rate_yaw.get_ff()*_feedforward_scalar; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index a802883951..1d6e5bb9c4 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -33,12 +33,11 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { public: AC_AttitudeControl_Heli( AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, - AP_MotorsHeli& motors, - float dt) : - AC_AttitudeControl(ahrs, aparm, motors, dt), - _pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f, dt), - _pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f, dt), - _pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_FF, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATTITUDE_HELI_RATE_Y_FF_FILTER, AC_ATC_HELI_RATE_YAW_FILT_HZ, 0.0f, dt) + AP_MotorsHeli& motors) : + AC_AttitudeControl(ahrs, aparm, motors), + _pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f), + _pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f), + _pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_FF, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATTITUDE_HELI_RATE_Y_FF_FILTER, AC_ATC_HELI_RATE_YAW_FILT_HZ, 0.0f) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index ea2596ddd8..deb26aa6ec 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -235,12 +235,12 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { AP_GROUPEND }; -AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) : - AC_AttitudeControl(ahrs, aparm, motors, dt), +AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors) : + AC_AttitudeControl(ahrs, aparm, motors), _motors_multi(motors), - _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt), - _pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt), - _pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, AC_ATC_MULTI_RATE_YAW_FILT_HZ, 0.0f, dt) + _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ), + _pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ), + _pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, AC_ATC_MULTI_RATE_YAW_FILT_HZ, 0.0f) { AP_Param::setup_object_defaults(this, var_info); } @@ -349,13 +349,13 @@ void AC_AttitudeControl_Multi::rate_controller_run() Vector3f gyro_latest = _ahrs.get_gyro_latest(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x); + _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll) + _actuator_sysid.x); _motors.set_roll_ff(get_rate_roll_pid().get_ff()); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y); + _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch) + _actuator_sysid.y); _motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z); + _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw) + _actuator_sysid.z); _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar); _sysid_ang_vel_body.zero(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 28bccb33cf..8fd11d5b4d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -41,7 +41,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { public: - AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt); + AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors); // empty destructor to suppress compiler warning virtual ~AC_AttitudeControl_Multi() {} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h index d65784dd03..68929acd53 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h @@ -5,8 +5,8 @@ class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi { public: - AC_AttitudeControl_Multi_6DoF(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt): - AC_AttitudeControl_Multi(ahrs,aparm,motors,dt) { + AC_AttitudeControl_Multi_6DoF(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors): + AC_AttitudeControl_Multi(ahrs,aparm,motors) { if (_singleton != nullptr) { AP_HAL::panic("Can only be one AC_AttitudeControl_Multi_6DoF"); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index bd0e6e30e5..a511e1de7d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -259,12 +259,12 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { AP_GROUPEND }; -AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) : - AC_AttitudeControl(ahrs, aparm, motors, dt), +AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors) : + AC_AttitudeControl(ahrs, aparm, motors), _motors_multi(motors), - _pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt), - _pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt), - _pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, 0.0f, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_YAW_FILT_HZ, dt) + _pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ), + _pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ), + _pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, 0.0f, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_YAW_FILT_HZ) { AP_Param::setup_object_defaults(this, var_info); @@ -356,9 +356,9 @@ void AC_AttitudeControl_Sub::rate_controller_run() update_throttle_rpy_mix(); Vector3f gyro_latest = _ahrs.get_gyro_latest(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _motors.limit.roll)); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _motors.limit.pitch)); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _motors.limit.yaw)); + _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll)); + _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch)); + _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw)); control_monitor_update(); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h index b460daf26b..6b7fb3bca4 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h @@ -25,7 +25,7 @@ class AC_AttitudeControl_Sub : public AC_AttitudeControl { public: - AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt); + AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors); // empty destructor to suppress compiler warning virtual ~AC_AttitudeControl_Sub() {} diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 2a78e0ac65..89a0c6b144 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -4,6 +4,7 @@ #include #include // motors library #include +#include extern const AP_HAL::HAL& hal; @@ -300,17 +301,16 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // their values. // AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, - const AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt) : + const AP_Motors& motors, AC_AttitudeControl& attitude_control) : _ahrs(ahrs), _inav(inav), _motors(motors), _attitude_control(attitude_control), - _p_pos_z(POSCONTROL_POS_Z_P, dt), - _pid_vel_z(POSCONTROL_VEL_Z_P, 0.0f, 0.0f, 0.0f, POSCONTROL_VEL_Z_IMAX, POSCONTROL_VEL_Z_FILT_HZ, POSCONTROL_VEL_Z_FILT_D_HZ, dt), - _pid_accel_z(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f, dt), - _p_pos_xy(POSCONTROL_POS_XY_P, dt), - _pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, 0.0f, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, dt), - _dt(dt), + _p_pos_z(POSCONTROL_POS_Z_P), + _pid_vel_z(POSCONTROL_VEL_Z_P, 0.0f, 0.0f, 0.0f, POSCONTROL_VEL_Z_IMAX, POSCONTROL_VEL_Z_FILT_HZ, POSCONTROL_VEL_Z_FILT_D_HZ), + _pid_accel_z(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f), + _p_pos_xy(POSCONTROL_POS_XY_P), + _pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, 0.0f, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ), _vel_max_down_cms(POSCONTROL_SPEED_DOWN), _vel_max_up_cms(POSCONTROL_SPEED_UP), _vel_max_xy_cms(POSCONTROL_SPEED), @@ -458,8 +458,10 @@ void AC_PosControl::relax_velocity_controller_xy() { // decay acceleration and therefore current attitude target to zero // this will be reset by init_xy_controller() if !is_active_xy() - float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC); - _accel_target.xy() *= decay; + if (is_positive(_dt)) { + float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC); + _accel_target.xy() *= decay; + } init_xy_controller(); } @@ -468,7 +470,9 @@ void AC_PosControl::relax_velocity_controller_xy() void AC_PosControl::soften_for_landing_xy() { // decay position error to zero - _pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC)); + if (is_positive(_dt)) { + _pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC)); + } // Prevent I term build up in xy velocity controller. // Note that this flag is reset on each loop in update_xy_controller() @@ -515,7 +519,7 @@ void AC_PosControl::init_xy_controller() init_ekf_xy_reset(); // initialise z_controller time out - _last_update_xy_us = AP_HAL::micros64(); + _last_update_xy_us = AP::ins().get_last_update_usec(); } /// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. @@ -583,10 +587,10 @@ void AC_PosControl::stop_vel_xy_stabilisation() _pid_vel_xy.reset_I(); } -// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times +// is_active_xy - returns true if the xy position controller has bee n run in the previous 5 loop times bool AC_PosControl::is_active_xy() const { - return ((AP_HAL::micros64() - _last_update_xy_us) <= _dt * 5000000.0); + return ((AP::ins().get_last_update_usec() - _last_update_xy_us) <= _dt * 1500000.0); } /// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors. @@ -606,7 +610,7 @@ void AC_PosControl::update_xy_controller() INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } - _last_update_xy_us = AP_HAL::micros64(); + _last_update_xy_us = AP::ins().get_last_update_usec(); float ahrsGndSpdLimit, ahrsControlScaleXY; AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY); @@ -624,7 +628,7 @@ void AC_PosControl::update_xy_controller() // Velocity Controller const Vector2f &curr_vel = _inav.get_velocity_xy_cms(); - Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _limit_vector.xy()); + Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _dt, _limit_vector.xy()); // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise accel_target *= ahrsControlScaleXY; @@ -734,7 +738,7 @@ void AC_PosControl::relax_z_controller(float throttle_setting) // init_z_controller has set the accel PID I term to generate the current throttle set point // Use relax_integrator to decay the throttle set point to throttle_setting - _pid_accel_z.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f, POSCONTROL_RELAX_TC); + _pid_accel_z.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f, _dt, POSCONTROL_RELAX_TC); } /// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude. @@ -775,7 +779,7 @@ void AC_PosControl::init_z_controller() init_ekf_z_reset(); // initialise z_controller time out - _last_update_z_us = AP_HAL::micros64(); + _last_update_z_us = AP::ins().get_last_update_usec(); } /// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. @@ -885,7 +889,6 @@ void AC_PosControl::set_alt_target_with_slew(float pos) /// update_pos_offset_z - updates the vertical offsets used by terrain following void AC_PosControl::update_pos_offset_z(float pos_offset_z) { - postype_t p_offset_z = _pos_offset_z; update_pos_vel_accel(p_offset_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error()); _pos_offset_z = p_offset_z; @@ -901,7 +904,7 @@ void AC_PosControl::update_pos_offset_z(float pos_offset_z) // is_active_z - returns true if the z position controller has been run in the previous 5 loop times bool AC_PosControl::is_active_z() const { - return ((AP_HAL::micros64() - _last_update_z_us) <= _dt * 5000000.0); + return ((AP::ins().get_last_update_usec() - _last_update_z_us) <= _dt * 1500000.0); } /// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors. @@ -921,7 +924,7 @@ void AC_PosControl::update_z_controller() INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } - _last_update_z_us = AP_HAL::micros64(); + _last_update_z_us = AP::ins().get_last_update_usec(); // calculate the target velocity correction float pos_target_zf = _pos_target.z; @@ -937,7 +940,7 @@ void AC_PosControl::update_z_controller() // Velocity Controller const float curr_vel_z = _inav.get_velocity_z_up_cms(); - _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel_z, _motors.limit.throttle_lower, _motors.limit.throttle_upper); + _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel_z, _dt, _motors.limit.throttle_lower, _motors.limit.throttle_upper); _accel_target.z *= AP::ahrs().getControlScaleZ(); // add feed forward component @@ -956,7 +959,7 @@ void AC_PosControl::update_z_controller() if (_vibe_comp_enabled) { thr_out = get_throttle_with_vibration_override(); } else { - thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f; + thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, _dt, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f; thr_out += _pid_accel_z.get_ff() * 0.001f; } thr_out += _motors.get_throttle_hover(); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 2c81ca28df..d69b678738 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -40,9 +40,14 @@ public: /// Constructor AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, - const class AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt); + const class AP_Motors& motors, AC_AttitudeControl& attitude_control); - /// get_dt - gets time delta in seconds for all position controllers + + + /// set_dt / get_dt - dt is the time since the last time the position controllers were updated + /// _dt should be set based on the time of the last IMU read used by these controllers + /// the position controller should run updates for active controllers on each loop to ensure normal operation + void set_dt(float dt) { _dt = dt; } float get_dt() const { return _dt; } /// get_shaping_jerk_xy_cmsss - gets the jerk limit of the xy kinematic path generation in cm/s/s/s @@ -418,7 +423,7 @@ protected: // references to inertial nav and ahrs libraries AP_AHRS_View& _ahrs; const AP_InertialNav& _inav; - const class AP_Motors& _motors; + const class AP_Motors& _motors; AC_AttitudeControl& _attitude_control; // parameters @@ -432,7 +437,7 @@ protected: AC_PID _pid_accel_z; // Z axis acceleration controller to convert desired acceleration to throttle output // internal variables - float _dt; // time difference (in seconds) between calls from the main program + float _dt; // time difference (in seconds) since the last loop time uint64_t _last_update_xy_us; // system time (in microseconds) since last update_xy_controller call uint64_t _last_update_z_us; // system time (in microseconds) since last update_z_controller call float _vel_max_xy_cms; // max horizontal speed in cm/s used for kinematic shaping