diff options
Diffstat (limited to 'src/modules/systemlib/mixer/mixer_multirotor.cpp')
| -rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 39 |
1 files changed, 17 insertions, 22 deletions
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 <uORB/uORB.h> -#include <uORB/topics/multirotor_motor_limits.h> #include <nuttx/config.h> - #include <sys/types.h> #include <stdint.h> #include <stdbool.h> @@ -53,6 +50,8 @@ #include <unistd.h> #include <math.h> +#include <px4iofirmware/protocol.h> + #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; } |