slew rate limiting: moved logic to multirotor mixer

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

src/modules/px4iofirmware/mixer.cpp

@@ -72,8 +72,6 @@ static bool should_arm_nothrottle = false;
 static bool should_always_enable_pwm = false;
 static volatile bool in_mixer = false;
 
-static uint16_t outputs_prev[4] = {900, 900, 900, 900};
-
 extern int _sbus_fd;
 
 /* selected control values and count for mixing */
@@ -234,6 +232,11 @@ mixer_tick(void)
 		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
+		// update slew rate value
+		float slew_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
+					 r_setup_slew_max);
+		mixer_group.update_slew_rate(slew_max);
+
 		/* mix */
 
 		/* poor mans mutex */
@@ -245,26 +248,6 @@ mixer_tick(void)
 		pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
 			       r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
-		// test slew rate limiting of motor outputs
-		// other option would be low pass filtering
-		float d_pwm_max = 1000.0f / REG_TO_FLOAT(r_setup_slew_max);	// max allowed delta pwm per second
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (d_pwm_max > 0.0f) {
-				float pwm_diff = r_page_servos[i] - outputs_prev[i];
-
-				if (pwm_diff > d_pwm_max * dt) {
-					r_page_servos[i] = outputs_prev[i] + d_pwm_max * dt;
-
-				} else if (pwm_diff < -d_pwm_max * dt) {
-					// XXX might not need this as we won't lose sync on deccelerating
-					r_page_servos[i] = outputs_prev[i] - d_pwm_max * dt;
-				}
-			}
-
-			outputs_prev[i] = r_page_servos[i];
-		}
-
 		/* clamp unused outputs to zero */
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = 0;
@@ -535,6 +518,11 @@ mixer_set_failsafe()
 	float	outputs[PX4IO_SERVO_COUNT];
 	unsigned mixed;
 
+	// update slew rate value
+	float slew_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
+				 r_setup_slew_max);
+	mixer_group.update_slew_rate(slew_max);
+
 	/* mix */
 	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
 

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;
 }