From c2a511d81d2356191ee812e8b4d192d397a3295c Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 27 Sep 2016 14:42:15 +0200 Subject: [PATCH] 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 --- src/drivers/px4fmu/fmu.cpp | 8 ++++-- src/modules/px4iofirmware/mixer.cpp | 22 ++++++++++------ src/modules/systemlib/mixer/mixer.h | 26 ++++++++++++++----- src/modules/systemlib/mixer/mixer_group.cpp | 4 +-- .../systemlib/mixer/mixer_multirotor.cpp | 14 +++++----- 5 files changed, 48 insertions(+), 26 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5fc5a92bd4..0589639fa5 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/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]; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 314a9e7fd8..315d7cf138 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/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( - r_setup_slew_max); - mixer_group.update_slew_rate(slew_max); + 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.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( - r_setup_slew_max); - mixer_group.update_slew_rate(slew_max); + 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.set_max_delta_out_once(delta_out_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 63822ae562..fe93973e75 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/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; diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index b55adeb248..1da681df28 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/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; } } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 3106bfdd86..8cbccbee4f 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/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; }