multirotor mixer slew rate limiting: naming and fixes

- avoid dividing by zero when calculating max delta output
- better comments when calculating max delta output
- better naming of functions and variables

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

src/drivers/px4fmu/fmu.cpp

@@ -1075,8 +1075,12 @@ PX4FMU::cycle()
 				dt = 0.02f;
 			}
 
-			float slew_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
-			_mixers->update_slew_rate(slew_max);
+			if (_mot_t_max > FLT_EPSILON) {
+				// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
+				// factor 2 is needed because actuator ouputs are in the range [-1,1]
+				float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
+				_mixers->set_max_delta_out_once(delta_out_max);
+			}
 
 			/* do mixing */
 			float outputs[_max_actuators];

src/modules/px4iofirmware/mixer.cpp

@@ -232,10 +232,13 @@ 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(
+		if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
+			// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
+			// factor 2 is needed because actuator ouputs are in the range [-1,1]
+			float delta_out_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);
+			mixer_group.set_max_delta_out_once(delta_out_max);
+		}
 
 		/* mix */
 
@@ -518,10 +521,13 @@ 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(
+	if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
+		// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
+		// factor 2 is needed because actuator ouputs are in the range [-1,1]
+		float delta_out_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);
+		mixer_group.set_max_delta_out_once(delta_out_max);
+	}
 
 	/* mix */
 	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);

src/modules/systemlib/mixer/mixer.h

@@ -187,10 +187,14 @@ public:
 	/**
 	 * @brief      Update slew rate parameter. This tells the multicopter mixer
 	 *             the maximum allowed change of the output values per cycle.
+	 *             The value is only valid for one cycle, in order to have continuous
+	 *             slew rate limiting this function needs to be called before every call
+	 *             to mix().
+	 *
+	 * @param[in]  delta_out_max  Maximum delta output.
 	 *
-	 * @param[in]  slew_rate_max  The maximum slew rate.
 	 */
-	virtual void 			update_slew_rate(float slew_rate_max) {};
+	virtual void 			set_max_delta_out_once(float delta_out_max) {};
 
 
 protected:
@@ -324,10 +328,14 @@ public:
 	/**
 	 * @brief      Update slew rate parameter. This tells the multicopter mixer
 	 *             the maximum allowed change of the output values per cycle.
+	 *             The value is only valid for one cycle, in order to have continuous
+	 *             slew rate limiting this function needs to be called before every call
+	 *             to mix().
+	 *
+	 * @param[in]  delta_out_max  Maximum delta output.
 	 *
-	 * @param[in]  slew_rate_max  The maximum slew rate.
 	 */
-	virtual void 			update_slew_rate(float slew_rate_max);
+	virtual void 			set_max_delta_out_once(float delta_out_max);
 
 private:
 	Mixer				*_first;	/**< linked list of mixers */
@@ -538,10 +546,14 @@ public:
 	/**
 	 * @brief      Update slew rate parameter. This tells the multicopter mixer
 	 *             the maximum allowed change of the output values per cycle.
+	 *             The value is only valid for one cycle, in order to have continuous
+	 *             slew rate limiting this function needs to be called before every call
+	 *             to mix().
+	 *
+	 * @param[in]  delta_out_max  Maximum delta output.
 	 *
-	 * @param[in]  slew_rate_max  The maximum slew rate.
 	 */
-	virtual void		update_slew_rate(float slew_rate_max) {_slew_rate_max = slew_rate_max;}
+	virtual void 			set_max_delta_out_once(float delta_out_max) {_delta_out_max = delta_out_max;}
 
 private:
 	float				_roll_scale;
@@ -549,7 +561,7 @@ private:
 	float				_yaw_scale;
 	float				_idle_speed;
 
-	float 				_slew_rate_max;
+	float 				_delta_out_max;
 
 	orb_advert_t			_limits_pub;
 	multirotor_motor_limits_s 	_limits;

src/modules/systemlib/mixer/mixer_group.cpp

@@ -201,12 +201,12 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 	return ret;
 }
 
-void MixerGroup::update_slew_rate(float slew_rate_max)
+void MixerGroup::set_max_delta_out_once(float delta_out_max)
 {
 	Mixer	*mixer = _first;
 
 	while (mixer != nullptr) {
-		mixer->update_slew_rate(slew_rate_max);
+		mixer->set_max_delta_out_once(delta_out_max);
 		mixer = mixer->_next;
 	}
 }

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -90,7 +90,7 @@ 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),
+	_delta_out_max(0.0f),
 	_limits_pub(),
 	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
 	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
@@ -375,14 +375,14 @@ 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) {
+		if (_delta_out_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;
+			if (delta_out > _delta_out_max) {
+				outputs[i] = _outputs_prev[i] + _delta_out_max;
 
-			} else if (delta_out < -_slew_rate_max) {
-				outputs[i] = _outputs_prev[i] - _slew_rate_max;
+			} else if (delta_out < -_delta_out_max) {
+				outputs[i] = _outputs_prev[i] - _delta_out_max;
 			}
 		}
 
@@ -391,7 +391,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	}
 
 	// 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;
+	_delta_out_max = 0.0f;
 
 	return _rotor_count;
 }