dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Simple mixer: add output slew rate 

This adds the option to limit the rate of change (slew rate) of an output that's mixed by a simple mixer.
To enable it, a positive number has to be added at the end (6th number) of the output scaler line of the mixer,
specifying the min rise time of this output.
E.g. O:      10000  10000      0 -10000  10000 20000 for a rise time of 2s, resp. a max slew rate of 0.5s^-1.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2020-08-28 11:07:47 +02:00 committed by Lorenz Meier
parent 98cff94702
commit 7c727edc3f
8 changed files with 85 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/mixer/MixerBase/Mixer.hpp
View File

@ -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 */

8
src/lib/mixer/MixerGroup.cpp
View File

@ -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);
}
}

2
src/lib/mixer/MixerGroup.hpp
View File

@ -165,6 +165,8 @@ public:
unsigned get_multirotor_count();
void set_dt_once(float dt);
private:
List<Mixer *> _mixers; /**< linked list of mixers */
};

47
src/lib/mixer/SimpleMixer/SimpleMixer.cpp
View File

@ -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;
}

7
src/lib/mixer/SimpleMixer/SimpleMixer.hpp
View File

@ -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;
};

20
src/lib/mixer_module/mixer_module.cpp
View File

@ -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 */

6
src/lib/mixer_module/mixer_module.hpp
View File

@ -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};

6
src/modules/px4iofirmware/mixer.cpp
View File

@ -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));