From 555e96a37a79a381dd6aeacf0e99cf6621df1018 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 10 Apr 2015 17:34:38 +0200 Subject: fixed publication of mixer limit flags --- src/modules/systemlib/mixer/mixer.cpp | 2 +- src/modules/systemlib/mixer/mixer.h | 10 +++--- src/modules/systemlib/mixer/mixer_group.cpp | 4 +-- src/modules/systemlib/mixer/mixer_multirotor.cpp | 39 +++++++++++------------- src/modules/systemlib/mixer/mixer_simple.cpp | 2 +- 5 files changed, 26 insertions(+), 31 deletions(-) (limited to 'src/modules/systemlib/mixer') diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 20b1f18ed..3ab41c5c5 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -151,7 +151,7 @@ NullMixer::NullMixer() : } unsigned -NullMixer::mix(float *outputs, unsigned space) +NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { if (space > 0) { *outputs = 0.0f; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 67ef521b4..119068301 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -174,7 +174,7 @@ public: * @param space The number of available entries in the output array; * @return The number of entries in the output array that were populated. */ - virtual unsigned mix(float *outputs, unsigned space) = 0; + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg) = 0; /** * Analyses the mix configuration and updates a bitmask of groups @@ -250,7 +250,7 @@ public: MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); ~MixerGroup(); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); /** @@ -346,7 +346,7 @@ public: */ static NullMixer *from_text(const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); }; @@ -411,7 +411,7 @@ public: uint16_t mid, uint16_t max); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); /** @@ -515,7 +515,7 @@ public: const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); private: diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 4220b168d..ca5101e65 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -99,13 +99,13 @@ MixerGroup::reset() } unsigned -MixerGroup::mix(float *outputs, unsigned space) +MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg) { Mixer *mixer = _first; unsigned index = 0; while ((mixer != nullptr) && (index < space)) { - index += mixer->mix(outputs + index, space - index); + index += mixer->mix(outputs + index, space - index, status_reg); mixer = mixer->_next; } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b753da5ce..e9a8a87ca 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,10 +36,7 @@ * * Multi-rotor mixers. */ -#include -#include #include - #include #include #include @@ -53,6 +50,8 @@ #include #include +#include + #include "mixer.h" // This file is generated by the multi_tables script which is invoked during the build process @@ -199,7 +198,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } unsigned -MultirotorMixer::mix(float *outputs, unsigned space) +MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { 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)); @@ -210,10 +209,9 @@ 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; + if (status_reg != NULL) { + (*status_reg) = 0; + } /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -226,7 +224,9 @@ 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; + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; + } } /* calculate min and max output values */ @@ -257,7 +257,10 @@ MultirotorMixer::mix(float *outputs, unsigned space) outputs[i] = out; } - _limits.roll_pitch = true; + + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; + } } else { /* roll/pitch mixed without lower side limiting, add yaw control */ @@ -270,7 +273,10 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; - _limits.throttle_upper = true; + + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; + } } else { scale_out = 1.0f; @@ -278,20 +284,9 @@ 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; } diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index c3985b5de..e48bda691 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -265,7 +265,7 @@ out: } unsigned -SimpleMixer::mix(float *outputs, unsigned space) +SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float sum = 0.0f; -- cgit v1.2.3