slew rate limiting: moved logic to multirotor mixer

Signed-off-by: Roman <bapstroman@gmail.com>

src/modules/systemlib/mixer/mixer.h

@@ -184,6 +184,15 @@ public:
 	 */
 	virtual void			groups_required(uint32_t &groups) = 0;
 
+	/**
+	 * @brief      Update slew rate parameter. This tells the multicopter mixer
+	 *             the maximum allowed change of the output values per cycle.
+	 *
+	 * @param[in]  slew_rate_max  The maximum slew rate.
+	 */
+	virtual void 			update_slew_rate(float slew_rate_max) {};
+
+
 protected:
 	/** client-supplied callback used when fetching control values */
 	ControlCallback			_control_cb;
@@ -312,6 +321,14 @@ public:
 	 */
 	int				load_from_buf(const char *buf, unsigned &buflen);
 
+	/**
+	 * @brief      Update slew rate parameter. This tells the multicopter mixer
+	 *             the maximum allowed change of the output values per cycle.
+	 *
+	 * @param[in]  slew_rate_max  The maximum slew rate.
+	 */
+	virtual void 			update_slew_rate(float slew_rate_max);
+
 private:
 	Mixer				*_first;	/**< linked list of mixers */
 
@@ -518,18 +535,30 @@ public:
 	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
 	virtual void			groups_required(uint32_t &groups);
 
+	/**
+	 * @brief      Update slew rate parameter. This tells the multicopter mixer
+	 *             the maximum allowed change of the output values per cycle.
+	 *
+	 * @param[in]  slew_rate_max  The maximum slew rate.
+	 */
+	virtual void		update_slew_rate(float slew_rate_max) {_slew_rate_max = slew_rate_max;}
+
 private:
 	float				_roll_scale;
 	float				_pitch_scale;
 	float				_yaw_scale;
 	float				_idle_speed;
 
+	float 				_slew_rate_max;
+
 	orb_advert_t			_limits_pub;
 	multirotor_motor_limits_s 	_limits;
 
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
 
+	float 				*_outputs_prev = nullptr;
+
 	/* do not allow to copy due to ptr data members */
 	MultirotorMixer(const MultirotorMixer &);
 	MultirotorMixer operator=(const MultirotorMixer &);

src/modules/systemlib/mixer/mixer_group.cpp

@@ -200,3 +200,13 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 	/* nothing more in the buffer for us now */
 	return ret;
 }
+
+void MixerGroup::update_slew_rate(float slew_rate_max)
+{
+	Mixer	*mixer = _first;
+
+	while (mixer != nullptr) {
+		mixer->update_slew_rate(slew_rate_max);
+		mixer = mixer->_next;
+	}
+}

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -90,14 +90,20 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 	_pitch_scale(pitch_scale),
 	_yaw_scale(yaw_scale),
 	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
+	_slew_rate_max(0.0f),
 	_limits_pub(),
 	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
-	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry])
+	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
+	_outputs_prev(new float[_rotor_count])
 {
+	memset(_outputs_prev, _idle_speed, _rotor_count * sizeof(float));
 }
 
 MultirotorMixer::~MultirotorMixer()
 {
+	if (_outputs_prev != nullptr) {
+		delete _outputs_prev;
+	}
 }
 
 MultirotorMixer *
@@ -368,8 +374,25 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 
 		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
 
+		// slew rate limiting
+		if (_slew_rate_max > 0.0f) {
+			float delta_out = outputs[i] - _outputs_prev[i];
+
+			if (delta_out > _slew_rate_max) {
+				outputs[i] = _outputs_prev[i] + _slew_rate_max;
+
+			} else if (delta_out < -_slew_rate_max) {
+				outputs[i] = _outputs_prev[i] - _slew_rate_max;
+			}
+		}
+
+		_outputs_prev[i] = outputs[i];
+
 	}
 
+	// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
+	_slew_rate_max = 0.0f;
+
 	return _rotor_count;
 }