From d9a7e528b056556112c74d13a86f30bdab88f635 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 15:44:56 +0200 Subject: Multirotor mixer: idle_speed (aka deadband) fixed --- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 12 ++++-------- 2 files changed, 5 insertions(+), 9 deletions(-) (limited to 'src/modules/systemlib/mixer') diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 1c889a811..fa9d28b74 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -516,7 +516,7 @@ private: float _roll_scale; float _pitch_scale; float _yaw_scale; - float _deadband; + float _idle_speed; unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 8568f9e60..1ca0a21e9 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -176,12 +176,12 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, float roll_scale, float pitch_scale, float yaw_scale, - float deadband) : + float idle_speed) : Mixer(control_cb, cb_handle), _roll_scale(roll_scale), _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), - _deadband(-1.0f + deadband), /* shift to output range here to avoid runtime calculation */ + _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ _rotor_count(_config_rotor_count[geometry]), _rotors(_config_index[geometry]) { @@ -338,15 +338,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) scale_out = 1.0f; } + /* scale outputs to range _idle_speed..1 */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = -1.0f + (outputs[i] * 2 * scale_out); + outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out); } - /* ensure outputs are out of the deadband */ - for (unsigned i = 0; i < _rotor_count; i++) - if (outputs[i] < _deadband) - outputs[i] = _deadband; - return _rotor_count; } -- cgit v1.2.3