diff options
author | Lorenz Meier <lm@qgroundcontrol.org> | 2014-09-26 14:42:48 +0200 |
---|---|---|
committer | Lorenz Meier <lm@qgroundcontrol.org> | 2014-09-26 14:42:48 +0200 |
commit | ae02b76a5eea03a7b94397b2f6e3e4f35b6a67ed (patch) | |
tree | 87225a59386cca609c3bfed55b7bdf99cc546800 /src/modules/systemlib | |
parent | 549d60cfb6958cf7c4362edc47b84759d2c8605c (diff) | |
parent | 5aafa1b021a80868a3b5aebde13cd54d407e6e41 (diff) | |
download | px4-firmware-ae02b76a5eea03a7b94397b2f6e3e4f35b6a67ed.tar.gz px4-firmware-ae02b76a5eea03a7b94397b2f6e3e4f35b6a67ed.tar.bz2 px4-firmware-ae02b76a5eea03a7b94397b2f6e3e4f35b6a67ed.zip |
Merge pull request #1325 from hsteinhaus/motor_limit_fb2
Notify about multirotor motor limits
Diffstat (limited to 'src/modules/systemlib')
-rw-r--r-- | src/modules/systemlib/mixer/mixer.h | 6 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 22 |
2 files changed, 27 insertions, 1 deletions
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index cdf60febc..17989558e 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -130,6 +130,9 @@ #include <nuttx/config.h> #include "drivers/drv_mixer.h" + +#include <uORB/topics/multirotor_motor_limits.h> + #include "mixer_load.h" /** @@ -531,6 +534,9 @@ private: float _yaw_scale; float _idle_speed; + orb_advert_t _limits_pub; + multirotor_motor_limits_s _limits; + 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 4b22a46d0..57e17b67d 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,7 +36,8 @@ * * Multi-rotor mixers. */ - +#include <uORB/uORB.h> +#include <uORB/topics/multirotor_motor_limits.h> #include <nuttx/config.h> #include <sys/types.h> @@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; + _limits.roll_pitch = false; + _limits.yaw = false; + _limits.throttle_upper = false; + _limits.throttle_lower = false; + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; + _limits.yaw = true; } /* calculate min and max output values */ @@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; } + _limits.roll_pitch = true; } else { /* roll/pitch mixed without limiting, add yaw control */ @@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; + _limits.throttle_upper = true; } else { scale_out = 1.0f; @@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { + if (outputs[i] < _idle_speed) { + _limits.throttle_lower = true; + } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + /* publish/advertise motor limits if running on FMU */ + if (_limits_pub > 0) { + orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); + } else { + _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); + } +#endif return _rotor_count; } |