diff --git a/msg/multirotor_motor_limits.msg b/msg/multirotor_motor_limits.msg index 92b8e5a1ff..ba6c8f3de0 100644 --- a/msg/multirotor_motor_limits.msg +++ b/msg/multirotor_motor_limits.msg @@ -1,11 +1,13 @@ uint16 saturation_status # Integer bit mask indicating which axes in the control mixer are saturated -# 0 - True if any motor is saturated at the upper limit -# 1 - True if any motor is saturated at the lower limit -# 2 - True if a positive roll increment will increase motor saturation -# 3 - True if negative roll increment will increase motor saturation -# 4 - True if positive pitch increment will increase motor saturation -# 5 - True if negative pitch increment will increase motor saturation -# 6 - True if positive yaw increment will increase motor saturation -# 7 - True if negative yaw increment will increase motor saturation -# 8 - True if positive thrust increment will increase motor saturation -# 9 - True if negative thrust increment will increase motor saturation + +# 0 - True if the saturation status is valid +# 1 - True if any motor is saturated at the upper limit +# 2 - True if any motor is saturated at the lower limit +# 3 - True if a positive roll increment will increase motor saturation +# 4 - True if negative roll increment will increase motor saturation +# 5 - True if positive pitch increment will increase motor saturation +# 6 - True if negative pitch increment will increase motor saturation +# 7 - True if positive yaw increment will increase motor saturation +# 8 - True if negative yaw increment will increase motor saturation +# 9 - True if positive thrust increment will increase motor saturation +# 10 - True if negative thrust increment will increase motor saturation diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 7e8eb424d9..ea1976b5ad 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -299,9 +299,7 @@ void task_main(int argc, char *argv[]) if (_mixer_group != nullptr) { _outputs.timestamp = hrt_absolute_time(); /* do mixing */ - _outputs.noutputs = _mixer_group->mix(_outputs.output, - actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, - NULL); + _outputs.noutputs = _mixer_group->mix(_outputs.output, actuator_outputs_s::NUM_ACTUATOR_OUTPUTS); /* disable unused ports by setting their output to NaN */ for (size_t i = _outputs.noutputs; i < _outputs.NUM_ACTUATOR_OUTPUTS; i++) { diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 908765ba34..2453610acc 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -539,7 +539,7 @@ MK::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs, NULL); + outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp index ea4a8a91c6..8b843f459c 100644 --- a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp +++ b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp @@ -55,7 +55,6 @@ #include #include - /* * This driver is supposed to run on Snapdragon. It sends actuator_controls (PWM) * to a Pixhawk/Pixfalcon/Pixracer over UART (mavlink) and receives RC input. @@ -415,7 +414,7 @@ void task_main(int argc, char *argv[]) _outputs.timestamp = _controls.timestamp; /* do mixing */ - _outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL); + _outputs.noutputs = _mixer->mix(_outputs.output, 0); /* disable unused ports by setting their output to NaN */ for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 594214d15a..5c596585d2 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -474,7 +474,7 @@ PWMSim::task_main() } /* do mixing */ - num_outputs = _mixers->mix(&outputs.output[0], num_outputs, nullptr); + num_outputs = _mixers->mix(&outputs.output[0], num_outputs); outputs.noutputs = num_outputs; outputs.timestamp = hrt_absolute_time(); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8fd1fdb543..4427364fbf 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -34,80 +34,58 @@ /** * @file fmu.cpp * - * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. + * Driver/configurator for the PX4 FMU */ -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include +#include #include - -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include - -#include +#include +#include +#include #include +#include #include #include - +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include #ifdef HRT_PPM_CHANNEL # include #endif -#include - #define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ -#define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */ -#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY) -#define CYCLE_COUNT 10 /* safety switch must be held for 1 second to activate */ + +static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */ +static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; /* * Define the various LED flash sequences for each system state. */ #define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */ -#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ +#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ #define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */ #define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */ #define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ @@ -127,10 +105,10 @@ enum PortMode { PORT_CAPTURE, }; - #if !defined(BOARD_HAS_PWM) # error "board_config.h needs to define BOARD_HAS_PWM" #endif + class PX4FMU : public device::CDev, public ModuleBase { public: @@ -305,9 +283,8 @@ private: int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); void update_pwm_rev_mask(); - void publish_pwm_outputs(uint16_t *values, size_t numvalues); void update_pwm_out_state(bool on); - void pwm_output_set(unsigned i, unsigned value); + void update_params(); struct GPIOConfig { @@ -550,7 +527,7 @@ PX4FMU::safety_check_button(void) * Debounce the safety button, change state if it has been held for long enough. * */ - bool safety_button_pressed = BUTTON_SAFETY; + bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); /* * Keep pressed for a while to arm. @@ -951,26 +928,6 @@ PX4FMU::update_pwm_trims() } } -void -PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) -{ - actuator_outputs_s outputs = {}; - outputs.noutputs = numvalues; - outputs.timestamp = hrt_absolute_time(); - - for (size_t i = 0; i < _max_actuators; ++i) { - outputs.output[i] = i < numvalues ? (float)values[i] : 0; - } - - if (_outputs_pub == nullptr) { - int instance = _class_instance; - _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); - } -} - int PX4FMU::task_spawn(int argc, char *argv[]) { @@ -1006,7 +963,6 @@ PX4FMU::task_spawn(int argc, char *argv[]) if (!run_as_task) { /* schedule a cycle to start things */ - int ret = work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, nullptr, 0); if (ret < 0) { @@ -1161,14 +1117,6 @@ void PX4FMU::rc_io_invert(bool invert) } #endif -void -PX4FMU::pwm_output_set(unsigned i, unsigned value) -{ - if (_pwm_initialized) { - up_pwm_servo_set(i, value); - } -} - void PX4FMU::update_pwm_out_state(bool on) { @@ -1248,7 +1196,7 @@ PX4FMU::cycle() /* wait for an update */ unsigned n_updates = 0; - int ret = ::poll(_poll_fds, _poll_fds_num, poll_timeout); + int ret = px4_poll(_poll_fds, _poll_fds_num, poll_timeout); /* this would be bad... */ if (ret < 0) { @@ -1261,81 +1209,59 @@ PX4FMU::cycle() } else { perf_begin(_ctl_latency); - /* get controls for required topics */ - unsigned poll_id = 0; + if (_mixers != nullptr) { + /* get controls for required topics */ + unsigned poll_id = 0; - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { - if (i == 0) { - n_updates++; - } + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); - -#if defined(DEBUG_BUILD) - - static int main_out_latency = 0; - static int sum_latency = 0; - static uint64_t last_cycle_time = 0; - - if (i == 0) { - uint64_t now = hrt_absolute_time(); - uint64_t latency = now - _controls[i].timestamp; - - if (latency > main_out_latency) { main_out_latency = latency; } - - sum_latency += latency; - - if ((now - last_cycle_time) >= 1000000) { - last_cycle_time = now; - PX4_DEBUG("pwm max latency: %d, avg: %5.3f", main_out_latency, (double)(sum_latency / 100.0)); - main_out_latency = latency; - sum_latency = 0; + if (_poll_fds[poll_id].revents & POLLIN) { + if (i == 0) { + n_updates++; } + + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } -#endif + poll_id++; } - poll_id++; - } + /* During ESC calibration, we overwrite the throttle value. */ + if (i == 0 && _armed.in_esc_calibration_mode) { - /* During ESC calibration, we overwrite the throttle value. */ - if (i == 0 && _armed.in_esc_calibration_mode) { + /* Set all controls to 0 */ + memset(&_controls[i], 0, sizeof(_controls[i])); - /* Set all controls to 0 */ - memset(&_controls[i], 0, sizeof(_controls[i])); + /* except thrust to maximum. */ + _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - /* except thrust to maximum. */ - _controls[i].control[3] = 1.0f; - - /* Switch off the PWM limit ramp for the calibration. */ - _pwm_limit.state = PWM_LIMIT_STATE_ON; + /* Switch off the PWM limit ramp for the calibration. */ + _pwm_limit.state = PWM_LIMIT_STATE_ON; + } } } } // poll_fds /* run the mixers on every cycle */ { - /* can we mix? */ if (_mixers != nullptr) { - hrt_abstime now = hrt_absolute_time(); - float dt = (now - _time_last_mix) / 1e6f; - _time_last_mix = now; - - if (dt < 0.0001f) { - dt = 0.0001f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - if (_mot_t_max > FLT_EPSILON) { - // maximum value the ouputs of the multirotor mixer are allowed to change in this cycle - // factor 2 is needed because actuator ouputs are in the range [-1,1] - float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; + hrt_abstime now = hrt_absolute_time(); + float dt = (now - _time_last_mix) / 1e6f; + _time_last_mix = now; + + if (dt < 0.0001f) { + dt = 0.0001f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } + + // maximum value the outputs of the multirotor mixer are allowed to change in this cycle + // factor 2 is needed because actuator outputs are in the range [-1,1] + const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; _mixers->set_max_delta_out_once(delta_out_max); } @@ -1345,37 +1271,14 @@ PX4FMU::cycle() /* do mixing */ float outputs[_max_actuators]; - size_t mixed_num_outputs = _mixers->mix(outputs, _num_outputs, NULL); - - /* publish mixer status */ - multirotor_motor_limits_s multirotor_motor_limits = {}; - multirotor_motor_limits.saturation_status = _mixers->get_saturation_status(); - - if (_to_mixer_status == nullptr) { - /* publish limits */ - int instance = _class_instance; - _to_mixer_status = orb_advertise_multi(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits, &instance, - ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits); - - } - - /* disable unused ports by setting their output to NaN */ - for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { - if (i >= mixed_num_outputs) { - outputs[i] = NAN_VALUE; - } - } - - uint16_t pwm_limited[_max_actuators]; + const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs); /* the PWM limit call takes care of out of band errors, NaN and constrains */ + uint16_t pwm_limited[MAX_ACTUATORS]; + pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); - /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ if (_armed.force_failsafe) { for (size_t i = 0; i < mixed_num_outputs; i++) { @@ -1391,19 +1294,43 @@ PX4FMU::cycle() } /* output to the servos */ - for (size_t i = 0; i < mixed_num_outputs; i++) { - pwm_output_set(i, pwm_limited[i]); + if (_pwm_initialized) { + for (size_t i = 0; i < mixed_num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } } /* Trigger all timer's channels in Oneshot mode to fire * the oneshots with updated values. */ - if (n_updates > 0) { up_pwm_update(); } - publish_pwm_outputs(pwm_limited, mixed_num_outputs); + actuator_outputs_s actuator_outputs = {}; + actuator_outputs.timestamp = hrt_absolute_time(); + actuator_outputs.noutputs = mixed_num_outputs; + + // zero unused outputs + for (size_t i = 0; i < mixed_num_outputs; ++i) { + actuator_outputs.output[i] = pwm_limited[i]; + } + + orb_publish_auto(ORB_ID(actuator_outputs), &_outputs_pub, &actuator_outputs, &_class_instance, ORB_PRIO_DEFAULT); + + /* publish mixer status */ + MultirotorMixer::saturation_status saturation_status; + saturation_status.value = _mixers->get_saturation_status(); + + if (saturation_status.flags.valid) { + multirotor_motor_limits_s motor_limits; + motor_limits.timestamp = hrt_absolute_time(); + motor_limits.saturation_status = saturation_status.value; + + orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance, + ORB_PRIO_DEFAULT); + } + perf_end(_ctl_latency); } } @@ -1729,8 +1656,7 @@ PX4FMU::cycle() // we have a new PPM frame. Publish it. rc_updated = true; _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; - fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, - false, false, 0); + fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, false, false, 0); _rc_scan_locked = true; _rc_in.rc_ppm_frame_length = ppm_frame_length; _rc_in.timestamp_last_signal = ppm_last_valid_decode; @@ -1790,10 +1716,8 @@ PX4FMU::cycle() exit_and_cleanup(); } else { - /* schedule next cycle */ work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); - // USEC2TICK(SCHEDULE_INTERVAL - main_out_latency)); } break; @@ -1863,7 +1787,7 @@ PX4FMU::control_callback(uintptr_t handle, control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_index == actuator_controls_s::INDEX_THROTTLE) { /* set the throttle to an invalid value */ - input = NAN_VALUE; + input = NAN; } } @@ -2508,8 +2432,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case MIXERIOCADDSIMPLE: { mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)_controls, mixinfo); + SimpleMixer *mixer = new SimpleMixer(control_callback, (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; @@ -2517,9 +2440,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } else { - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)_controls); + if (_mixers == nullptr) { + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + } _mixers->add_mixer(mixer); _mixers->groups_required(_groups_required); diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt index 2b75220cd8..b24663935c 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( px4io_uploader.cpp px4io_serial.cpp px4io_i2c.cpp - px4io_params.c DEPENDS platforms__common ) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c2635e4664..52b1143596 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1928,12 +1928,6 @@ PX4IO::io_publish_raw_rc() int 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 */ uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); @@ -1942,38 +1936,33 @@ PX4IO::io_publish_pwm_outputs() return ret; } + actuator_outputs_s outputs = {}; + outputs.timestamp = hrt_absolute_time(); + outputs.noutputs = _max_actuators; + /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) { outputs.output[i] = ctl[i]; } - outputs.noutputs = _max_actuators; - - /* lazily advertise on first publication */ - if (_to_outputs == nullptr) { - int instance; - _to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &instance, ORB_PRIO_MAX); - - } else { - orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); - } + int instance; + orb_publish_auto(ORB_ID(actuator_outputs), &_to_outputs, &outputs, &instance, ORB_PRIO_DEFAULT); /* 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)); - motor_limits.saturation_status = mixer_status; + MultirotorMixer::saturation_status saturation_status; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &saturation_status.value, 1); if (ret != OK) { return ret; } /* publish mixer status */ - if (_to_mixer_status == nullptr) { - _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits); + if (saturation_status.flags.valid) { + multirotor_motor_limits_s motor_limits; + motor_limits.timestamp = hrt_absolute_time(); + motor_limits.saturation_status = saturation_status.value; - } else { - orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits); + orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT); } return OK; diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp index 80fb9fe46f..8f8af58f58 100644 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp @@ -402,9 +402,7 @@ void task_main(int argc, char *argv[]) _outputs.timestamp = hrt_absolute_time(); /* do mixing for virtual control group */ - _outputs.noutputs = _mixer->mix(_outputs.output, - _outputs.NUM_ACTUATOR_OUTPUTS, - NULL); + _outputs.noutputs = _mixer->mix(_outputs.output, _outputs.NUM_ACTUATOR_OUTPUTS); //set max, min and disarmed pwm const uint16_t reverse_mask = 0; diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 45630c5216..f046e789ce 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -655,7 +655,7 @@ TAP_ESC::cycle() if (_is_armed && _mixers != nullptr) { /* do mixing */ - num_outputs = _mixers->mix(&_outputs.output[0], num_outputs, NULL); + num_outputs = _mixers->mix(&_outputs.output[0], num_outputs); _outputs.noutputs = num_outputs; _outputs.timestamp = hrt_absolute_time(); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8c1187dc29..31df082c0f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -161,27 +162,12 @@ private: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ - struct multirotor_motor_limits_s _motor_limits; /**< motor limits */ struct mc_att_ctrl_status_s _controller_status; /**< controller status */ struct battery_status_s _battery_status; /**< battery status */ struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */ struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */ - union { - struct { - uint16_t motor_pos : 1; // 0 - true when any motor has saturated in the positive direction - uint16_t motor_neg : 1; // 1 - true when any motor has saturated in the negative direction - uint16_t roll_pos : 1; // 2 - true when a positive roll demand change will increase saturation - uint16_t roll_neg : 1; // 3 - true when a negative roll demand change will increase saturation - uint16_t pitch_pos : 1; // 4 - true when a positive pitch demand change will increase saturation - uint16_t pitch_neg : 1; // 5 - true when a negative pitch demand change will increase saturation - uint16_t yaw_pos : 1; // 6 - true when a positive yaw demand change will increase saturation - uint16_t yaw_neg : 1; // 7 - true when a negative yaw demand change will increase saturation - uint16_t thrust_pos : 1; // 8 - true when a positive thrust demand change will increase saturation - uint16_t thrust_neg : 1; // 9 - true when a negative thrust demand change will increase saturation - } flags; - uint16_t value; - } _saturation_status; + MultirotorMixer::saturation_status _saturation_status{}; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _controller_latency_perf; @@ -415,13 +401,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators{}, _armed{}, _vehicle_status{}, - _motor_limits{}, _controller_status{}, _battery_status{}, _sensor_gyro{}, _sensor_correction{}, - - _saturation_status{}, /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), @@ -784,8 +767,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll() orb_check(_motor_limits_sub, &updated); if (updated) { - orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits); - _saturation_status.value = _motor_limits.saturation_status; + multirotor_motor_limits_s motor_limits = {}; + orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits); + + _saturation_status.value = motor_limits.saturation_status; } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 8659f11d3f..05233ed9bf 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -91,11 +91,7 @@ enum mixer_source { static volatile mixer_source source; -static int mixer_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &control); - +static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control); static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits); static MixerGroup mixer_group(mixer_callback, 0); @@ -107,10 +103,9 @@ int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits) return 0; } - uint16_t mixer_limits = 0; in_mixer = true; - int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &mixer_limits); - *limits = mixer_limits; + int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + *limits = mixer_group.get_saturation_status(); in_mixer = false; return mixcount; @@ -600,7 +595,6 @@ mixer_set_failsafe() } /* mix */ - mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); /* scale to PWM and update the servo outputs as required */ diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 6333f3d95a..7e1acfdfde 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -184,7 +184,7 @@ NullMixer::NullMixer() : } unsigned -NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) +NullMixer::mix(float *outputs, unsigned space) { if (space > 0) { *outputs = 0.0f; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 68efa41997..e14b5107c6 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -131,8 +131,6 @@ #include #include "drivers/drv_mixer.h" -#include - #include "mixer_load.h" /** @@ -174,12 +172,12 @@ 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, uint16_t *status_reg) = 0; + virtual unsigned mix(float *outputs, unsigned space) = 0; /** * Get the saturation status. * - * @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg . + * @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg. */ virtual uint16_t get_saturation_status(void) = 0; @@ -284,7 +282,7 @@ public: MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); ~MixerGroup(); - virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual unsigned mix(float *outputs, unsigned space); virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); @@ -425,7 +423,7 @@ public: */ static NullMixer *from_text(const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual unsigned mix(float *outputs, unsigned space); virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); virtual void set_offset(float trim) {} @@ -490,14 +488,10 @@ public: * @return A new SimpleMixer instance, or nullptr if one could not be * allocated. */ - static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, - uintptr_t cb_handle, - unsigned input, - uint16_t min, - uint16_t mid, - uint16_t max); + static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, + uint16_t mid, uint16_t max); - virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual unsigned mix(float *outputs, unsigned space); virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); @@ -599,12 +593,10 @@ public: * @return A new MultirotorMixer instance, or nullptr * if the text format is bad. */ - static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, - uintptr_t cb_handle, - const char *buf, + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual unsigned mix(float *outputs, unsigned space); virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); @@ -618,7 +610,7 @@ public: * @param[in] delta_out_max Maximum delta output. * */ - virtual void set_max_delta_out_once(float delta_out_max) {_delta_out_max = delta_out_max;} + virtual void set_max_delta_out_once(float delta_out_max) { _delta_out_max = delta_out_max; } unsigned set_trim(float trim) { @@ -632,6 +624,23 @@ public: */ virtual void set_thrust_factor(float val) {_thrust_factor = val;} + union saturation_status { + struct { + uint16_t valid : 1; // 0 - true when the saturation status is used + uint16_t motor_pos : 1; // 1 - true when any motor has saturated in the positive direction + uint16_t motor_neg : 1; // 2 - true when any motor has saturated in the negative direction + uint16_t roll_pos : 1; // 3 - true when a positive roll demand change will increase saturation + uint16_t roll_neg : 1; // 4 - true when a negative roll demand change will increase saturation + uint16_t pitch_pos : 1; // 5 - true when a positive pitch demand change will increase saturation + uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation + uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation + uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation + uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation + uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation + } flags; + uint16_t value; + }; + private: float _roll_scale; float _pitch_scale; @@ -640,27 +649,8 @@ private: float _delta_out_max; float _thrust_factor; - - orb_advert_t _limits_pub; - multirotor_motor_limits_s _limits; - - union { - struct { - uint16_t motor_pos : 1; // 0 - true when any motor has saturated in the positive direction - uint16_t motor_neg : 1; // 1 - true when any motor has saturated in the negative direction - uint16_t roll_pos : 1; // 2 - true when a positive roll demand change will increase saturation - uint16_t roll_neg : 1; // 3 - true when a negative roll demand change will increase saturation - uint16_t pitch_pos : 1; // 4 - true when a positive pitch demand change will increase saturation - uint16_t pitch_neg : 1; // 5 - true when a negative pitch demand change will increase saturation - uint16_t yaw_pos : 1; // 6 - true when a positive yaw demand change will increase saturation - uint16_t yaw_neg : 1; // 7 - true when a negative yaw demand change will increase saturation - uint16_t thrust_pos : 1; // 8 - true when a positive thrust demand change will increase saturation - uint16_t thrust_neg : 1; // 9 - true when a negative thrust demand change will increase saturation - } flags; - uint16_t value; - } _saturation_status; - void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low); + saturation_status _saturation_status; unsigned _rotor_count; const Rotor *_rotors; @@ -729,15 +719,14 @@ public: * @return A new HelicopterMixer instance, or nullptr * if the text format is bad. */ - static HelicopterMixer *from_text(Mixer::ControlCallback control_cb, - uintptr_t cb_handle, - const char *buf, + static HelicopterMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); virtual uint16_t get_saturation_status(void) { return 0; } + unsigned set_trim(float trim) { return 4; diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 9ba34a729b..c90ab2d319 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -103,13 +103,13 @@ MixerGroup::reset() } unsigned -MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg) +MixerGroup::mix(float *outputs, unsigned space) { Mixer *mixer = _first; unsigned index = 0; while ((mixer != nullptr) && (index < space)) { - index += mixer->mix(outputs + index, space - index, status_reg); + index += mixer->mix(outputs + index, space - index); mixer = mixer->_next; } diff --git a/src/modules/systemlib/mixer/mixer_helicopter.cpp b/src/modules/systemlib/mixer/mixer_helicopter.cpp index c3cb33c8e1..3baf911df1 100644 --- a/src/modules/systemlib/mixer/mixer_helicopter.cpp +++ b/src/modules/systemlib/mixer/mixer_helicopter.cpp @@ -234,7 +234,7 @@ HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } unsigned -HelicopterMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) +HelicopterMixer::mix(float *outputs, unsigned space) { /* Find index to use for curves */ float thrust_cmd = get_control(0, 3); diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index ac5afbbb00..2ed5030ab5 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -83,7 +83,6 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ _delta_out_max(0.0f), _thrust_factor(0.0f), - _limits_pub(), _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]), _outputs_prev(new float[_rotor_count]) @@ -178,13 +177,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "6a")) { geometry = MultirotorGeometry::DODECA_BOTTOM_COX; - -#if 0 - - } else if (!strcmp(geomname, "8cw")) { - geometry = MultirotorGeometry::OCTA_COX_WIDE; -#endif - } else if (!strcmp(geomname, "2-")) { geometry = MultirotorGeometry::TWIN_ENGINE; @@ -209,7 +201,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } unsigned -MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) +MultirotorMixer::mix(float *outputs, unsigned space) { /* Summary of mixing strategy: 1) mix roll, pitch and thrust without yaw. @@ -404,22 +396,16 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) // update the saturation status report update_saturation_status(i, clipping_high, clipping_low); - } // this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen _delta_out_max = 0.0f; - // Notify saturation status - if (status_reg != nullptr) { - (*status_reg) = _saturation_status.value; - } - return _rotor_count; } /* - * This function update the control saturation status report using hte following inputs: + * This function update the control saturation status report using the following inputs: * * index: 0 based index identifying the motor that is saturating * clipping_high: true if the motor demand is being limited in the positive direction @@ -438,7 +424,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo } else if (_rotors[index].roll_scale < 0.0f) { // A negative change in roll will increase saturation _saturation_status.flags.roll_neg = true; - } // check if the pitch input is saturating @@ -449,7 +434,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo } else if (_rotors[index].pitch_scale < 0.0f) { // A negative change in pitch will increase saturation _saturation_status.flags.pitch_neg = true; - } // check if the yaw input is saturating @@ -460,7 +444,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo } else if (_rotors[index].yaw_scale < 0.0f) { // A negative change in yaw will increase saturation _saturation_status.flags.yaw_neg = true; - } // A positive change in thrust will increase saturation @@ -479,7 +462,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo } else if (_rotors[index].roll_scale < 0.0f) { // A positive change in roll will increase saturation _saturation_status.flags.roll_pos = true; - } // check if the pitch input is saturating @@ -490,7 +472,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo } else if (_rotors[index].pitch_scale < 0.0f) { // A positive change in pitch will increase saturation _saturation_status.flags.pitch_pos = true; - } // check if the yaw input is saturating @@ -501,13 +482,13 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo } else if (_rotors[index].yaw_scale < 0.0f) { // A positive change in yaw will increase saturation _saturation_status.flags.yaw_pos = true; - } // A negative change in thrust will increase saturation _saturation_status.flags.thrust_neg = true; - } + + _saturation_status.flags.valid = true; } void diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 1f25633028..d4c64650e1 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -286,7 +286,7 @@ out: } unsigned -SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) +SimpleMixer::mix(float *outputs, unsigned space) { float sum = 0.0f; diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 1aa36b414c..e245959de6 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -77,19 +77,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); */ PARAM_DEFINE_INT32(SYS_HITL, 0); -/** - * Set usage of IO board - * - * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. - * - * @boolean - * @min 0 - * @max 1 - * @reboot_required true - * @group System - */ -PARAM_DEFINE_INT32(SYS_USE_IO, 1); - /** * Set restart type * diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 19421c818b..d79ae79fa2 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -909,7 +909,7 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); new_output = true; } diff --git a/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp b/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp index 8446b01fd4..c9867b06d7 100644 --- a/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp +++ b/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp @@ -377,9 +377,7 @@ void task_main(int argc, char *argv[]) if (_mixers != nullptr) { /* do mixing */ - _outputs.noutputs = _mixers->mix(_outputs.output, - 4, - NULL); + _outputs.noutputs = _mixers->mix(_outputs.output, 4); } // Set last throttle for battery calculations diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp index 8890c6d1e9..9d81e28a19 100644 --- a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp +++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp @@ -337,9 +337,7 @@ void task_main(int argc, char *argv[]) int16_t motor_rpms[UART_ESC_MAX_MOTORS]; if (_armed.armed) { - _outputs.noutputs = mixer->mix(&_outputs.output[0], - actuator_controls_s::NUM_ACTUATOR_CONTROLS, - NULL); + _outputs.noutputs = mixer->mix(&_outputs.output[0], actuator_controls_s::NUM_ACTUATOR_CONTROLS); // Make sure we support only up to UART_ESC_MAX_MOTORS motors if (_outputs.noutputs > UART_ESC_MAX_MOTORS) { diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 04f228018f..061615ba4b 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -395,7 +395,7 @@ bool MixerTest::mixerTest() /* mix */ should_prearm = true; - mixed = mixer_group.mix(&outputs[0], output_max, nullptr); + mixed = mixer_group.mix(&outputs[0], output_max); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -436,7 +436,7 @@ bool MixerTest::mixerTest() while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max, nullptr); + mixed = mixer_group.mix(&outputs[0], output_max); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -480,7 +480,7 @@ bool MixerTest::mixerTest() } /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max, nullptr); + mixed = mixer_group.mix(&outputs[0], output_max); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -508,7 +508,7 @@ bool MixerTest::mixerTest() while (hrt_elapsed_time(&starttime) < 600000) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max, nullptr); + mixed = mixer_group.mix(&outputs[0], output_max); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -545,7 +545,7 @@ bool MixerTest::mixerTest() while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max, nullptr); + mixed = mixer_group.mix(&outputs[0], output_max); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);