From ae1faa6de6d6952af73a8a9367625fbf96822fe1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 May 2014 13:45:43 +0200 Subject: MC mixer input limiting implemented. --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) (limited to 'src/modules/systemlib') diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 740a22781..2af9d913d 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -67,6 +67,11 @@ namespace { +float constrain(float val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + /* * These tables automatically generated by multi_tables - do not edit. */ @@ -276,11 +281,11 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl unsigned MultirotorMixer::mix(float *outputs, unsigned space) { - float roll = get_control(0, 0) * _roll_scale; + float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); - float pitch = get_control(0, 1) * _pitch_scale; - float yaw = get_control(0, 2) * _yaw_scale; - float thrust = get_control(0, 3); + float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); + float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); + float thrust = constrain(get_control(0, 3), 0.0f, 1.0f); //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; -- cgit v1.2.3