diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-14 13:45:43 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-14 13:45:43 +0200 |
commit | ae1faa6de6d6952af73a8a9367625fbf96822fe1 (patch) | |
tree | 62ab9608c404703d8a1ecd284ca48501549e5707 | |
parent | b60964eb9c1e24b14c0cbf4527bf6b7e4bb5fd40 (diff) | |
download | px4-firmware-ae1faa6de6d6952af73a8a9367625fbf96822fe1.tar.gz px4-firmware-ae1faa6de6d6952af73a8a9367625fbf96822fe1.tar.bz2 px4-firmware-ae1faa6de6d6952af73a8a9367625fbf96822fe1.zip |
MC mixer input limiting implemented.
-rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 |
1 files changed, 9 insertions, 4 deletions
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; |