add slew-rate

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<matrix::Vector3f>::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;
+}

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(),

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<matrix::Vector3f> _auto_trim_slew{}; ///< prevents large trim changes
 
 	void updateActuatorControlsStatus(float dt);
 

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) {

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;