diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index c79e4f40ab1d04929438aca48234d1f5620906ce..314a9e7fd822195d93beebd1b2c7340ff7c5cc0f 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/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);
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 2f7f1befed84ceef03da446093fce20eb0fd727a..63822ae5624e8a93ec02c316eaba56611430e636 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/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 &);
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 259e1e687c9c187dc6de7df31580c5386b04998a..b55adeb248f9cb1832c239d76df2a7fceb26cf71 100644
--- a/src/modules/systemlib/mixer/mixer_group.cpp
+++ b/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;
+ }
+}
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 607daa12c895176a3e01f69d38ec2b9fde5843fa..6f714b40b5473d3ce8a3a11c6df0d48ec9e4d29f 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/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;
}