diff --git a/src/lib/mixer/MixerBase/Mixer.hpp b/src/lib/mixer/MixerBase/Mixer.hpp index f08e3d3eda..e90290e143 100644 --- a/src/lib/mixer/MixerBase/Mixer.hpp +++ b/src/lib/mixer/MixerBase/Mixer.hpp @@ -229,6 +229,8 @@ public: virtual unsigned get_multirotor_count() { return 0; } + virtual void set_dt_once(float dt) {} + protected: /** client-supplied callback used when fetching control values */ diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index c9575301d9..b48f5cddee 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -243,3 +243,11 @@ void MixerGroup::set_max_delta_out_once(float delta_out_max) mixer->set_max_delta_out_once(delta_out_max); } } + +void +MixerGroup::set_dt_once(float dt) +{ + for (auto mixer : _mixers) { + mixer->set_dt_once(dt); + } +} diff --git a/src/lib/mixer/MixerGroup.hpp b/src/lib/mixer/MixerGroup.hpp index a2859e270e..f5b7fedb50 100644 --- a/src/lib/mixer/MixerGroup.hpp +++ b/src/lib/mixer/MixerGroup.hpp @@ -165,6 +165,8 @@ public: unsigned get_multirotor_count(); + void set_dt_once(float dt); + private: List _mixers; /**< linked list of mixers */ }; diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.cpp b/src/lib/mixer/SimpleMixer/SimpleMixer.cpp index 50413607d2..270b77ffcf 100644 --- a/src/lib/mixer/SimpleMixer/SimpleMixer.cpp +++ b/src/lib/mixer/SimpleMixer/SimpleMixer.cpp @@ -70,11 +70,17 @@ unsigned SimpleMixer::get_trim(float *trim) return 1; } +void +SimpleMixer::set_dt_once(float dt) +{ + _dt = dt; +} + int -SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time) { int ret; - int s[5]; + int s[6]; int n = -1; buf = findtag(buf, buflen, 'O'); @@ -84,12 +90,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler return -1; } - if ((ret = sscanf(buf, "O: %d %d %d %d %d %n", - &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) { + if ((ret = sscanf(buf, "O: %d %d %d %d %d %d %n", + &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &n)) < 5) { debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); return -1; } + // set slew rate limit to 0 if no 6th number is specified in mixer file + if (ret == 5) { + s[5] = 0; + } + buf = skipline(buf, buflen); if (buf == nullptr) { @@ -102,6 +113,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler scaler.offset = s[2] / 10000.0f; scaler.min_output = s[3] / 10000.0f; scaler.max_output = s[4] / 10000.0f; + slew_rate_rise_time = s[5] / 10000.0f; return 0; } @@ -193,16 +205,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c if (next_tag == 'S') { /* No output scalers specified. Use default values. * Corresponds to: - * O: 10000 10000 0 -10000 10000 + * O: 10000 10000 0 -10000 10000 0 */ mixinfo->output_scaler.negative_scale = 1.0f; mixinfo->output_scaler.positive_scale = 1.0f; mixinfo->output_scaler.offset = 0.f; mixinfo->output_scaler.min_output = -1.0f; mixinfo->output_scaler.max_output = 1.0f; + mixinfo->slew_rate_rise_time = 0.0f; } else { - if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) { + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler, mixinfo->slew_rate_rise_time)) { debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); goto out; } @@ -262,6 +275,28 @@ SimpleMixer::mix(float *outputs, unsigned space) } *outputs = scale(_pinfo->output_scaler, sum); + + if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) { + + // factor 2 is needed because actuator outputs are in the range [-1,1] + const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time; + + float delta_out = *outputs - _output_prev; + + if (delta_out > output_delta_max) { + *outputs = _output_prev + output_delta_max; + + } else if (delta_out < -output_delta_max) { + *outputs = _output_prev - output_delta_max; + } + + } + + // this will force the caller of the mixer to always supply dt values, otherwise no slew rate limiting will happen + _dt = 0.f; + + _output_prev = *outputs; + return 1; } diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.hpp b/src/lib/mixer/SimpleMixer/SimpleMixer.hpp index 40b5622ccc..d2811e6d59 100644 --- a/src/lib/mixer/SimpleMixer/SimpleMixer.hpp +++ b/src/lib/mixer/SimpleMixer/SimpleMixer.hpp @@ -57,6 +57,7 @@ struct mixer_control_s { struct mixer_simple_s { uint8_t control_count; /**< number of inputs */ mixer_scaler_s output_scaler; /**< scaling for the output */ + float slew_rate_rise_time{0.0f}; /**< output max rise time (slew rate limit)*/ mixer_control_s controls[]; /**< actual size of the array is set by control_count */ }; @@ -119,6 +120,7 @@ public: unsigned set_trim(float trim) override; unsigned get_trim(float *trim) override; + void set_dt_once(float dt) override; private: @@ -139,10 +141,13 @@ private: */ static int scale_check(struct mixer_scaler_s &scaler); - static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time); static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index); + float _output_prev{0.f}; + float _dt{0.f}; + mixer_simple_s *_pinfo; }; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index be8a1524a5..addd525e99 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -239,11 +239,11 @@ void MixingOutput::unregister() } } -void MixingOutput::updateOutputSlewrate() +void MixingOutput::updateOutputSlewrateMultirotorMixer() { const hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _time_last_mix) / 1e6f, 0.0001f, 0.02f); - _time_last_mix = now; + const float dt = math::constrain((now - _time_last_dt_update_multicopter) / 1e6f, 0.0001f, 0.02f); + _time_last_dt_update_multicopter = now; // maximum value the outputs of the multirotor mixer are allowed to change in this cycle // factor 2 is needed because actuator outputs are in the range [-1,1] @@ -251,6 +251,16 @@ void MixingOutput::updateOutputSlewrate() _mixers->set_max_delta_out_once(delta_out_max); } +void MixingOutput::updateOutputSlewrateSimplemixer() +{ + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _time_last_dt_update_simple_mixer) / 1e6f, 0.0001f, 0.02f); + _time_last_dt_update_simple_mixer = now; + + // set dt for slew rate limiter in SimpleMixer (is reset internally after usig it, so needs to be set on every update) + _mixers->set_dt_once(dt); +} + unsigned MixingOutput::motorTest() { @@ -355,9 +365,11 @@ bool MixingOutput::update() } if (_param_mot_slew_max.get() > FLT_EPSILON) { - updateOutputSlewrate(); + updateOutputSlewrateMultirotorMixer(); } + updateOutputSlewrateSimplemixer(); // update dt for output slew rate in simple mixer + unsigned n_updates = 0; /* get controls for required topics */ diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 7766fbc37c..834e9f4b16 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -191,7 +191,8 @@ private: unsigned motorTest(); - void updateOutputSlewrate(); + void updateOutputSlewrateMultirotorMixer(); + void updateOutputSlewrateSimplemixer(); void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs); void publishMixerStatus(const actuator_outputs_s &actuator_outputs); void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs); @@ -244,7 +245,8 @@ private: actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; actuator_armed_s _armed{}; - hrt_abstime _time_last_mix{0}; + hrt_abstime _time_last_dt_update_multicopter{0}; + hrt_abstime _time_last_dt_update_simple_mixer{0}; unsigned _max_topic_update_interval_us{0}; ///< max _control_subs topic update interval (0=unlimited) bool _throttle_armed{false}; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 96ce0846a4..43e35efded 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -304,6 +304,9 @@ mixer_tick() mixer_group.set_max_delta_out_once(delta_out_max); } + /* set dt to be used in simple mixer for slew rate limiting */ + mixer_group.set_dt_once(dt); + /* update parameter for mc thrust model if it updated */ if (update_mc_thrust_param) { mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); @@ -657,6 +660,9 @@ mixer_set_failsafe() mixer_group.set_max_delta_out_once(delta_out_max); } + /* set dt to be used in simple mixer for slew rate limiting */ + mixer_group.set_dt_once(dt); + /* update parameter for mc thrust model if it updated */ if (update_mc_thrust_param) { mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));