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/drivers/hil/hil.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 20 ++++++++++++ src/modules/px4iofirmware/mixer.cpp | 4 +-- src/modules/px4iofirmware/protocol.h | 5 +++ src/modules/px4iofirmware/px4io.h | 5 +-- src/modules/px4iofirmware/registers.c | 1 + 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 +- src/modules/uORB/topics/multirotor_motor_limits.h | 9 +++--- src/modules/uavcan/uavcan_main.cpp | 2 +- src/systemcmds/tests/test_mixer.cpp | 8 ++--- 16 files changed, 69 insertions(+), 48 deletions(-) (limited to 'src') diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9d28b48d4..961ec4724 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -402,7 +402,7 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index cb463eb59..3b8c4ee77 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -536,7 +536,7 @@ MK::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7b09a4676..92afc7cd7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -664,7 +664,7 @@ PX4FMU::task_main() } /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3bd2c06a4..33125699f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -88,6 +88,7 @@ #include #include #include +#include #include @@ -288,6 +289,7 @@ private: orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety + orb_advert_t _to_mixer_status; ///< mixer status flags actuator_outputs_s _outputs; ///< mixed outputs servorail_status_s _servorail_status; ///< servorail status @@ -513,6 +515,7 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_servorail(0), _to_safety(0), + _to_mixer_status(0), _outputs{}, _servorail_status{}, _primary_pwm_device(false), @@ -1687,6 +1690,8 @@ PX4IO::io_publish_pwm_outputs() { /* data we are going to fetch */ actuator_outputs_s outputs; + multirotor_motor_limits_s motor_limits; + outputs.timestamp = hrt_absolute_time(); /* get servo values from IO */ @@ -1718,6 +1723,21 @@ PX4IO::io_publish_pwm_outputs() orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } + /* get mixer status flags from IO */ + uint16_t mixer_status; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status,sizeof(mixer_status)/sizeof(uint16_t)); + memcpy(&motor_limits,&mixer_status,sizeof(motor_limits)); + + if (ret != OK) + return ret; + + /* publish mixer status */ + if(_to_mixer_status == 0) { + _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits); + } else { + orb_publish(ORB_ID(multirotor_motor_limits),_to_mixer_status, &motor_limits); + } + return OK; } 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 -#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; 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; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 64e734b00..19aa05970 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -202,7 +202,7 @@ int test_mixer(int argc, char *argv[]) while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -248,7 +248,7 @@ int test_mixer(int argc, char *argv[]) } /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -275,7 +275,7 @@ int test_mixer(int argc, char *argv[]) while (hrt_elapsed_time(&starttime) < 600000) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -311,7 +311,7 @@ int test_mixer(int argc, char *argv[]) while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); -- cgit v1.2.3