diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index e5653cecfa..606d61c0dd 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -73,6 +73,16 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, /// z-axis position controller /// + +/// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) +void AC_PosControl::set_dt(float delta_sec) +{ + _dt = delta_sec; + + // update rate controller's d filter + _pid_alt_accel.set_d_lpf_alpha(POSCONTROOL_ACCEL_Z_DTERM_FILTER, _dt); +} + /// set_speed_z - sets maximum climb and descent rates /// To-Do: call this in the main code as part of flight mode initialisation /// calc_leash_length_z should be called afterwards diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index fbdd35cc2c..6823be3da5 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -36,6 +36,8 @@ #define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 200ms (0.2 seconds) +#define POSCONTROOL_ACCEL_Z_DTERM_FILTER 20 // Z axis accel controller's D term filter (in hz) + class AC_PosControl { public: @@ -51,7 +53,8 @@ public: /// /// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) - void set_dt(float delta_sec) { _dt = delta_sec; } + /// updates z axis accel controller's D term filter + void set_dt(float delta_sec); float get_dt() const { return _dt; } ///