diff --git a/src/lib/slew_rate/SlewRate.hpp b/src/lib/slew_rate/SlewRate.hpp index 363f2d2cc7..49fd443a28 100644 --- a/src/lib/slew_rate/SlewRate.hpp +++ b/src/lib/slew_rate/SlewRate.hpp @@ -90,3 +90,14 @@ protected: Type _slew_rate{}; ///< maximum rate of change for the value Type _value{}; ///< state to keep last value of the slew rate }; + +template<> +inline matrix::Vector3f SlewRate::update(const matrix::Vector3f new_value, const float deltatime) +{ + // Limit the rate of change of the value + const matrix::Vector3f dvalue_desired = new_value - _value; + const matrix::Vector3f dvalue_max = _slew_rate * deltatime; + const matrix::Vector3f dvalue = matrix::constrain(dvalue_desired, -dvalue_max, dvalue_max); + _value += dvalue; + return _value; +} diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 8ed4070932..1ff70f353f 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -54,6 +54,7 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) : parameters_update(); _rate_ctrl_status_pub.advertise(); + _auto_trim_slew.setSlewRate(Vector3f(0.01f, 0.01f, 0.01f)); } FixedwingRateControl::~FixedwingRateControl() @@ -324,6 +325,9 @@ void FixedwingRateControl::Run() /* bi-linear interpolation over airspeed for actuator trim scheduling */ Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get()); + _auto_trim_slew.update(_auto_trim.getTrim(), dt).print(); + trim += _auto_trim_slew.getState(); + if (airspeed < _param_fw_airspd_trim.get()) { trim(0) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(), diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index d58a10cb7f..2c478c3db8 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -212,6 +212,7 @@ private: RateControl _rate_control; ///< class for rate control calculations FwAutoTrim _auto_trim{this}; + SlewRate _auto_trim_slew{}; ///< prevents large trim changes void updateActuatorControlsStatus(float dt); diff --git a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp b/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp index 03c8df8ffe..20195d78e1 100644 --- a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp +++ b/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp @@ -107,8 +107,8 @@ void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint && !_landed && (dt > 0.001f) && (dt < 0.1f) && torque.isAllFinite() - && _calibrated_airspeed_m_s >= _param_fw_airspd_min.get() - && _calibrated_airspeed_m_s <= _param_fw_airspd_max.get() + // && _calibrated_airspeed_m_s >= _param_fw_airspd_min.get() + // && _calibrated_airspeed_m_s <= _param_fw_airspd_max.get() && _cos_tilt > cosf(math::radians(_kTiltMaxDeg)); if (run_auto_trim) { diff --git a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp b/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp index 0ccb9fb563..979edbdbd5 100644 --- a/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp +++ b/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp @@ -63,6 +63,8 @@ public: void update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); int print_status(); + const matrix::Vector3f &getTrim() const { return _trim_validated; } + protected: void updateParams() override;