diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 4 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 5 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 5 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 1 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer.cpp | 2 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer.h | 10 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_group.cpp | 4 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 39 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_simple.cpp | 2 | ||||
-rw-r--r-- | src/modules/uORB/topics/multirotor_motor_limits.h | 9 | ||||
-rw-r--r-- | src/modules/uavcan/uavcan_main.cpp | 2 |
11 files changed, 42 insertions, 41 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 835dfc0f5..6fa26d4ff 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -230,7 +230,7 @@ mixer_tick(void) /* poor mans mutex */ in_mixer = true; - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); in_mixer = false; /* the pwm limit call takes care of out of band errors */ @@ -453,7 +453,7 @@ mixer_set_failsafe() unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 874dc0c39..cbafa3641 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -133,6 +133,11 @@ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ +#define PX4IO_P_STATUS_MIXER_LOWER_LIMIT (1 << 0) /**< at least one actuator output has reached lower limit */ +#define PX4IO_P_STATUS_MIXER_UPPER_LIMIT (1 << 1) /**< at least one actuator output has reached upper limit */ +#define PX4IO_P_STATUS_MIXER_YAW_LIMIT (1 << 2) /**< yaw control is limited because it causes output clipping */ + /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 17699b7c0..8ddf45a12 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -98,8 +98,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) #define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] -#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) +#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) +#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER] #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index ae7aec34e..90e0fb10e 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,6 +90,7 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_MIXER] = 0, }; /** 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 <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; } 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; diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h index 44c51e4d9..589f8a650 100644 --- a/src/modules/uORB/topics/multirotor_motor_limits.h +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -52,11 +52,10 @@ * Motor limits */ struct multirotor_motor_limits_s { - uint8_t roll_pitch : 1; // roll/pitch limit reached - uint8_t yaw : 1; // yaw limit reached - uint8_t throttle_lower : 1; // lower throttle limit reached - uint8_t throttle_upper : 1; // upper throttle limit reached - uint8_t reserved : 4; + uint8_t lower_limit : 1; // at least one actuator command has saturated on the lower limit + uint8_t upper_limit : 1; // at least one actuator command has saturated on the upper limit + uint8_t yaw : 1; // yaw limit reached + uint8_t reserved : 5; // reserved }; /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index b93a95f96..02cde9656 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -421,7 +421,7 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL); new_output = true; } |