diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-11-06 19:02:01 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-11-06 19:02:01 -0800 |
commit | 7da799d154f6f3c5f0c1a92cbad4288f83c90650 (patch) | |
tree | aee9e23f60d01b78d18016015a4f9675cb74439d /apps | |
parent | 08d6ade585200e2c6bed73d02ebfe990e6929446 (diff) | |
download | px4-firmware-7da799d154f6f3c5f0c1a92cbad4288f83c90650.tar.gz px4-firmware-7da799d154f6f3c5f0c1a92cbad4288f83c90650.tar.bz2 px4-firmware-7da799d154f6f3c5f0c1a92cbad4288f83c90650.zip |
Scale control signals with thrust for low thrust
Diffstat (limited to 'apps')
-rw-r--r-- | apps/systemlib/mixer/mixer_multirotor.cpp | 22 |
1 files changed, 22 insertions, 0 deletions
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 0e714ed50..a3577245d 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -161,6 +161,28 @@ MultirotorMixer::mix(float *outputs, unsigned space) float max = 0.0f; float fixup_scale; + float min_thrust = 0.05f; + float max_thrust = 1.0f; + float output_factor = 0.0f; + float startpoint_full_control = 0.20f; + float endpoint_full_control = 0.80f; + + if (thrust <= min_thrust) { + output_factor = 0.0f; + } else if (thrust < startpoint_full_control && thrust > min_thrust) { + output_factor = (thrust/max_thrust)/startpoint_full_control; + } else if (thrust >= startpoint_full_control && thrust < endpoint_full_control) { + output_factor = max_thrust; + } else if (thrust >= endpoint_full_control) { + output_factor = max_thrust/(endpoint_full_control-max_thrust); + } + + roll *= output_factor; + pitch *= output_factor; + yaw *= output_factor; + + + /* perform initial mix pass yielding un-bounded outputs */ for (unsigned i = 0; i < _rotor_count; i++) { float tmp = roll * _rotors[i].roll_scale + |