diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 129c4ca321..b9b91c4460 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -96,10 +96,16 @@ then if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ] then - if ! $OUTPUT_CMD start + if param compare SYS_CTRL_ALLOC 1 then - echo "$OUTPUT_CMD start failed" - tune_control play error + pwm_out start + dshot start + else + if ! $OUTPUT_CMD start + then + echo "$OUTPUT_CMD start failed" + tune_control play error + fi fi fi fi @@ -214,10 +220,10 @@ fi if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ] then - if [ $PWM_OUT != none ] + if [ $PWM_OUT != none -a $PWM_MAIN_RATE != none ] then # Set PWM output frequency. - if [ $PWM_MAIN_RATE != none ] + if ! param compare SYS_CTRL_ALLOC 1 then pwm rate -c ${PWM_OUT} -r ${PWM_MAIN_RATE} fi diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index be3044b2d8..0764de6468 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -33,6 +33,8 @@ #include "DShot.h" +#include + char DShot::_telemetry_device[] {}; px4::atomic_bool DShot::_request_telemetry_init{false}; @@ -43,7 +45,7 @@ DShot::DShot() : _mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE); _mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE); _mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE); - + _mixing_output.setAllFailsafeValues(UINT16_MAX); } DShot::~DShot() @@ -114,39 +116,112 @@ int DShot::task_spawn(int argc, char *argv[]) void DShot::enable_dshot_outputs(const bool enabled) { if (enabled && !_outputs_initialized) { - DShotConfig config = (DShotConfig)_param_dshot_config.get(); + if (_mixing_output.useDynamicMixing()) { - unsigned int dshot_frequency = DSHOT600; + unsigned int dshot_frequency = 0; + uint32_t dshot_frequency_param = 0; - switch (config) { - case DShotConfig::DShot150: - dshot_frequency = DSHOT150; - break; + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + uint32_t channels = io_timer_get_group(timer); - case DShotConfig::DShot300: - dshot_frequency = DSHOT300; - break; + if (channels == 0) { + continue; + } - case DShotConfig::DShot600: - dshot_frequency = DSHOT600; - break; + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - case DShotConfig::DShot1200: - dshot_frequency = DSHOT1200; - break; + int32_t tim_config = 0; + param_t handle = param_find(param_name); + param_get(handle, &tim_config); + unsigned int dshot_frequency_request = 0; - default: - break; - } + if (tim_config == -5) { + dshot_frequency_request = DSHOT150; - int ret = up_dshot_init(_output_mask, dshot_frequency); + } else if (tim_config == -4) { + dshot_frequency_request = DSHOT300; - if (ret < 0) { - PX4_ERR("up_dshot_init failed (%i)", ret); - return; + } else if (tim_config == -3) { + dshot_frequency_request = DSHOT600; + + } else if (tim_config == -2) { + dshot_frequency_request = DSHOT1200; + + } else { + _output_mask &= ~channels; // don't use for dshot + } + + if (dshot_frequency_request != 0) { + if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) { + PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name); + param_set_no_notification(handle, &dshot_frequency_param); + + } else { + dshot_frequency = dshot_frequency_request; + dshot_frequency_param = tim_config; + } + } + } + + int ret = up_dshot_init(_output_mask, dshot_frequency); + + if (ret < 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return; + } + + _output_mask = ret; + + // disable unused functions + for (unsigned i = 0; i < _num_outputs; ++i) { + if (((1 << i) & _output_mask) == 0) { + _mixing_output.disableFunction(i); + } + } + + if (_output_mask == 0) { + // exit the module if no outputs used + request_stop(); + return; + } + + } else { + DShotConfig config = (DShotConfig)_param_dshot_config.get(); + + unsigned int dshot_frequency = DSHOT600; + + switch (config) { + case DShotConfig::DShot150: + dshot_frequency = DSHOT150; + break; + + case DShotConfig::DShot300: + dshot_frequency = DSHOT300; + break; + + case DShotConfig::DShot600: + dshot_frequency = DSHOT600; + break; + + case DShotConfig::DShot1200: + dshot_frequency = DSHOT1200; + break; + + default: + break; + } + + int ret = up_dshot_init(_output_mask, dshot_frequency); + + if (ret < 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return; + } + + _output_mask = ret; } - _output_mask = ret; _outputs_initialized = true; } @@ -414,7 +489,7 @@ void DShot::Run() _mixing_output.update(); // update output status if armed or if mixer is loaded - bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers(); + bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized(); if (_outputs_on != outputs_on) { enable_dshot_outputs(outputs_on); diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt index 017865fdb4..1a4483f3bd 100644 --- a/src/drivers/pwm_out/CMakeLists.txt +++ b/src/drivers/pwm_out/CMakeLists.txt @@ -37,6 +37,8 @@ px4_add_module( SRCS PWMOut.cpp PWMOut.hpp + MODULE_CONFIG + module.yaml DEPENDS arch_io_pins mixer diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 733ad38c9e..f8767c489f 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -56,8 +56,10 @@ PWMOut::PWMOut(int instance, uint8_t output_base) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval")) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + if (!_mixing_output.useDynamicMixing()) { + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + } } PWMOut::~PWMOut() @@ -92,7 +94,13 @@ int PWMOut::init() _mixing_output.setDriverInstance(_class_instance); - _num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE); + if (_mixing_output.useDynamicMixing()) { + _num_outputs = FMU_MAX_ACTUATORS; + + } else { + _num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE); + } + _pwm_mask = ((1u << _num_outputs) - 1) << _output_base; _mixing_output.setMaxNumOutputs(_num_outputs); @@ -132,6 +140,10 @@ int PWMOut::init() */ int PWMOut::set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate) { + if (_mixing_output.useDynamicMixing()) { + return -EINVAL; + } + PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate); for (unsigned pass = 0; pass < 2; pass++) { @@ -264,6 +276,11 @@ int PWMOut::task_spawn(int argc, char *argv[]) return PX4_ERROR; } + // only start one instance with dynamic mixing + if (dev->_mixing_output.useDynamicMixing()) { + break; + } + } else { PX4_ERR("alloc failed"); } @@ -283,44 +300,110 @@ bool PWMOut::update_pwm_out_state(bool on) { if (on && !_pwm_initialized && _pwm_mask != 0) { - // Collect all PWM masks from all instances - uint32_t pwm_mask_new = 0; - // Collect the PWM alt rate channels across all instances - uint32_t pwm_alt_rate_channels_new = 0; + if (_mixing_output.useDynamicMixing()) { + + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + _timer_rates[timer] = -1; + + uint32_t channels = io_timer_get_group(timer); + + if (channels == 0) { + continue; + } + + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + + int32_t tim_config = 0; + param_t handle = param_find(param_name); + param_get(handle, &tim_config); + + if (tim_config > 0) { + _timer_rates[timer] = tim_config; - for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { - if (_objects[i].load()) { + } else if (tim_config == -1) { // OneShot + _timer_rates[timer] = 0; - pwm_mask_new |= _objects[i].load()->get_pwm_mask(); - pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels(); + } else { + _pwm_mask &= ~channels; // don't use for pwm + } + } + + int ret = up_pwm_servo_init(_pwm_mask); + + if (ret < 0) { + PX4_ERR("up_pwm_servo_init failed (%i)", ret); + return false; + } + + _pwm_mask = ret; + + // set the timer rates + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer); + + if (channels == 0) { + continue; + } + + ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]); + + if (ret != 0) { + PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret); + _timer_rates[timer] = -1; + _pwm_mask &= ~channels; + } + } + + _pwm_initialized = true; + + // disable unused functions + for (unsigned i = 0; i < _num_outputs; ++i) { + if (((1 << i) & _pwm_mask) == 0) { + _mixing_output.disableFunction(i); + } } - } - // Initialize the PWM output state for all instances - // this is re-done once per instance, but harmless - int ret = up_pwm_servo_init(pwm_mask_new); + } else { + // Collect all PWM masks from all instances + uint32_t pwm_mask_new = 0; + // Collect the PWM alt rate channels across all instances + uint32_t pwm_alt_rate_channels_new = 0; - if (ret >= 0) { for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { if (_objects[i].load()) { - _objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret); + + pwm_mask_new |= _objects[i].load()->get_pwm_mask(); + pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels(); } } - // Set rate is not affecting non-masked channels, so can be called - // individually - set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate()); + // Initialize the PWM output state for all instances + // this is re-done once per instance, but harmless + int ret = up_pwm_servo_init(pwm_mask_new); - _pwm_initialized = true; - _all_instances_ready.fetch_add(1); + if (ret >= 0) { + for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { + if (_objects[i].load()) { + _objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret); + } + } - } else { - PX4_ERR("up_pwm_servo_init failed (%i)", ret); + // Set rate is not affecting non-masked channels, so can be called + // individually + set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate()); + + _pwm_initialized = true; + _all_instances_ready.fetch_add(1); + + } else { + PX4_ERR("up_pwm_servo_init failed (%i)", ret); + } } } up_pwm_servo_arm(on, _pwm_mask); - return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES; + return _all_instances_ready.load() == PWM_OUT_MAX_INSTANCES || _mixing_output.useDynamicMixing(); } bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -332,7 +415,12 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], /* output to the servos */ if (_pwm_initialized) { - for (size_t i = 0; i < math::min(_num_outputs, num_outputs); i++) { + for (size_t i = 0; i < num_outputs; i++) { + if (!_mixing_output.isFunctionSet(i)) { + // do not run any signal on disabled channels + outputs[i] = 0; + } + if (_pwm_mask & (1 << (i + _output_base))) { up_pwm_servo_set(_output_base + i, outputs[i]); } @@ -362,13 +450,16 @@ void PWMOut::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); - // push backup schedule - ScheduleDelayed(_backup_schedule_interval_us); + if (!_mixing_output.useDynamicMixing()) { + // push backup schedule + ScheduleDelayed(_backup_schedule_interval_us); + } _mixing_output.update(); /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.armed().in_esc_calibration_mode; + bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing() + || _mixing_output.armed().in_esc_calibration_mode; if (_pwm_on != pwm_on) { @@ -384,10 +475,12 @@ void PWMOut::Run() _parameter_update_sub.copy(&pupdate); // update parameters from storage - // update_params(); // do not update PWM params for now (was interfering with VTOL PWM settings) + if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings) + update_params(); + } } - if (_pwm_initialized && _current_update_rate == 0) { + if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) { update_current_rate(); } @@ -401,6 +494,10 @@ void PWMOut::update_params() { updateParams(); + if (_mixing_output.useDynamicMixing()) { + return; + } + int32_t pwm_min_default = PWM_DEFAULT_MIN; int32_t pwm_max_default = PWM_DEFAULT_MAX; int32_t pwm_disarmed_default = 0; @@ -636,7 +733,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { + if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { ret = -EINVAL; break; } @@ -681,7 +778,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { + if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { ret = -EINVAL; break; } @@ -736,7 +833,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { + if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { ret = -EINVAL; break; } @@ -781,7 +878,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { + if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { ret = -EINVAL; break; } @@ -1223,6 +1320,27 @@ int PWMOut::print_status() perf_print_counter(_interval_perf); _mixing_output.printStatus(); + if (_mixing_output.useDynamicMixing() && _pwm_initialized) { + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + if (_timer_rates[timer] >= 0) { + PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]); + uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer); + + if (channels > 0) { + PX4_INFO_RAW(" channels: "); + + for (uint32_t channel = 0; channel < _num_outputs; ++channel) { + if ((1 << channel) & channels) { + PX4_INFO_RAW("%" PRIu32 " ", channel); + } + } + } + + PX4_INFO_RAW("\n"); + } + } + } + return 0; } diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index a3e03d4003..c9a9a42f37 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,8 @@ private: unsigned _pwm_alt_rate{50}; uint32_t _pwm_alt_rate_channels{0}; + int _timer_rates[MAX_IO_TIMERS] {}; + int _current_update_rate{0}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml new file mode 100644 index 0000000000..490554edba --- /dev/null +++ b/src/drivers/pwm_out/module.yaml @@ -0,0 +1,33 @@ +module_name: PWM Output +actuator_output: + output_groups: + - generator: pwm + param_prefix: PWM_FMU + channel_labels: ['PWM Main', 'PWM Capture'] + standard_params: + disarmed: { min: 800, max: 2200, default: 900 } + min: { min: 800, max: 1400, default: 1000 } + max: { min: 1600, max: 2200, default: 2000 } + failsafe: { min: 800, max: 2200 } + extra_function_groups: [ pwm_fmu ] + pwm_timer_param: + description: + short: Output Protocol Configuration for ${label} + long: | + Select which Output Protocol to use for outputs ${label}. + + Custom PWM rates can be used by directly setting any value >0. + type: enum + default: 400 + values: + -5: DShot150 + -4: DShot300 + -3: DShot600 + -2: DShot1200 + -1: OneShot + 50: PWM50 + 100: PWM100 + 200: PWM200 + 400: PWM400 + reboot_required: true +