From 2607769470450074cddd73c8c516874c20bfe1df Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 16 Sep 2016 15:56:06 +0200 Subject: [PATCH] slew rate limiting: moved logic to multirotor mixer Signed-off-by: Roman --- src/modules/px4iofirmware/mixer.cpp | 32 ++++++------------- src/modules/systemlib/mixer/mixer.h | 29 +++++++++++++++++ src/modules/systemlib/mixer/mixer_group.cpp | 10 ++++++ .../systemlib/mixer/mixer_multirotor.cpp | 25 ++++++++++++++- 4 files changed, 73 insertions(+), 23 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index c79e4f40ab..314a9e7fd8 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 2f7f1befed..63822ae562 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 259e1e687c..b55adeb248 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 607daa12c8..6f714b40b5 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; }