diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 45492f790a..f9c9b5d487 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -186,6 +186,8 @@ def get_actuator_output_params(yaml_config, output_functions, standard_params = group.get('standard_params', []) extra_function_groups = group.get('extra_function_groups', []) pwm_timer_param = group.get('pwm_timer_param', None) + if 'timer_config_file' in group: + timer_config_file = os.path.join(root_dir, group['timer_config_file']) if timer_config_file is None: raise Exception('trying to generate pwm outputs, but --timer-config not set') timer_groups = get_timer_groups(timer_config_file, verbose) diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt index d3320ca2f0..73df21c30f 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -39,6 +39,8 @@ px4_add_module( px4io.cpp px4io_serial.cpp px4io_uploader.cpp + MODULE_CONFIG + module.yaml DEPENDS arch_px4io_serial circuit_breaker diff --git a/src/drivers/px4io/module.yaml b/src/drivers/px4io/module.yaml new file mode 100644 index 0000000000..9d0ec8fcbd --- /dev/null +++ b/src/drivers/px4io/module.yaml @@ -0,0 +1,29 @@ +module_name: IO PWM Output +actuator_output: + output_groups: + - generator: pwm + param_prefix: PWM_MAIN + channel_labels: ['PWM Main', 'PWM Capture'] + timer_config_file: "boards/px4/io-v2/src/timer_config.cpp" + 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 } + 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: + -1: OneShot + 50: PWM50 + 100: PWM100 + 200: PWM200 + 400: PWM400 + reboot_required: true + diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bb6e52522f..8badca7f46 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -174,6 +174,7 @@ private: void updateDisarmed(); void updateFailsafe(); + void updateTimerRateGroups(); static int checkcrc(int argc, char *argv[]); static int bind(int argc, char *argv[]); @@ -218,6 +219,7 @@ private: hrt_abstime _last_status_publish{0}; bool _param_update_force{true}; ///< force a parameter update + bool _timer_rates_configured{false}; /* advertised topics */ uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; @@ -364,8 +366,12 @@ PX4IO::PX4IO(device::Device *interface) : OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)), _interface(interface) { - _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); + } + + _mixing_output.setLowrateSchedulingInterval(20_ms); } PX4IO::~PX4IO() @@ -555,7 +561,7 @@ void PX4IO::updateFailsafe() pwm_output_values pwm{}; for (unsigned i = 0; i < _max_actuators; i++) { - pwm.values[i] = _mixing_output.failsafeValue(i); + pwm.values[i] = _mixing_output.actualFailsafeValue(i); } io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators); @@ -575,7 +581,9 @@ void PX4IO::Run() perf_count(_interval_perf); // schedule minimal update rate if there are no actuator controls - ScheduleDelayed(20_ms); + if (!_mixing_output.useDynamicMixing()) { + ScheduleDelayed(20_ms); + } /* if we have new control data from the ORB, handle it */ if (_param_sys_hitl.get() <= 0) { @@ -705,10 +713,60 @@ void PX4IO::Run() perf_end(_cycle_perf); } +void PX4IO::updateTimerRateGroups() +{ + if (_timer_rates_configured) { // avoid setting timer rates on each param update + return; + } + + _timer_rates_configured = true; + + int timer = 0; + + uint16_t timer_rates[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {}; + + for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) { + 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); + + if (handle == PARAM_INVALID) { + break; + } + + param_get(handle, &tim_config); + + if (tim_config > 0) { + timer_rates[timer] = tim_config; + + } else if (tim_config == -1) { // OneShot + timer_rates[timer] = 0; + } + + ++timer; + } + + int ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0, timer_rates, timer); + + if (ret != 0) { + PX4_ERR("io_reg_set failed (%i)", ret); + } +} + void PX4IO::update_params() { + if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) { + // sync params to IO + updateTimerRateGroups(); + updateFailsafe(); + updateDisarmed(); + return; + } + // skip update when armed or PWM disabled - if (_mixing_output.armed().armed || _class_instance == -1) { + if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) { return; } @@ -1140,18 +1198,29 @@ int PX4IO::io_get_status() status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i); } - // PWM rates, 0 = low rate, 1 = high rate - const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + if (_mixing_output.useDynamicMixing()) { - const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); - const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + int i = 0; - for (unsigned i = 0; i < _max_actuators; i++) { - if (pwm_rate & (1 << i)) { - status.pwm_rate_hz[i] = pwm_high_rate; + for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) { + // This is a bit different than below, setting the groups, not the channels + status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset); + } - } else { - status.pwm_rate_hz[i] = pwm_low_rate; + } else { + // PWM rates, 0 = low rate, 1 = high rate + const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + + const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); + const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + + for (unsigned i = 0; i < _max_actuators; i++) { + if (pwm_rate & (1 << i)) { + status.pwm_rate_hz[i] = pwm_high_rate; + + } else { + status.pwm_rate_hz[i] = pwm_low_rate; + } } } @@ -1532,6 +1601,12 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case PWM_SERVO_SET_UPDATE_RATE: PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE"); + + if (_mixing_output.useDynamicMixing()) { + ret = -EINVAL; + break; + } + /* set the requested alternate rate */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; @@ -1545,6 +1620,11 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case PWM_SERVO_SET_SELECT_UPDATE_RATE: { PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE"); + if (_mixing_output.useDynamicMixing()) { + ret = -EINVAL; + break; + } + /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); @@ -1579,7 +1659,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0) { + if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { _mixing_output.failsafeValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX); } } @@ -1613,7 +1693,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0) { + if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { _mixing_output.disarmedValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX); } } @@ -1645,7 +1725,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0) { + if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { _mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN); } } @@ -1675,7 +1755,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0) { + if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { _mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX); } } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index ed24534b16..f109592c80 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -461,7 +461,7 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li } } else if (all_disabled) { - _interface.ScheduleOnInterval(300_ms); + _interface.ScheduleOnInterval(_lowrate_schedule_interval); PX4_DEBUG("Scheduling at low rate"); } else { @@ -817,19 +817,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat /* overwrite outputs in case of force_failsafe with _failsafe_value values */ if (_armed.force_failsafe) { for (size_t i = 0; i < _max_num_outputs; i++) { - if (_failsafe_value[i] == UINT16_MAX) { // if set to default, use the one provided by the function - float default_failsafe = NAN; - - if (_functions[i]) { - default_failsafe = _functions[i]->defaultFailsafeValue(_function_assignment[i]); - } - - _current_output_value[i] = output_limit_calc_single(_reverse_output_mask & (1 << i), - _disarmed_value[i], _min_value[i], _max_value[i], default_failsafe); - - } else { - _current_output_value[i] = _failsafe_value[i]; - } + _current_output_value[i] = actualFailsafeValue(i); } } @@ -908,6 +896,32 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output } } +uint16_t +MixingOutput::actualFailsafeValue(int index) +{ + if (!_use_dynamic_mixing) { + return failsafeValue(index); + } + + uint16_t value = 0; + + if (_failsafe_value[index] == UINT16_MAX) { // if set to default, use the one provided by the function + float default_failsafe = NAN; + + if (_functions[index]) { + default_failsafe = _functions[index]->defaultFailsafeValue(_function_assignment[index]); + } + + value = output_limit_calc_single(_reverse_output_mask & (1 << index), + _disarmed_value[index], _min_value[index], _max_value[index], default_failsafe); + + } else { + value = _failsafe_value[index]; + } + + return value; +} + void MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS]) { diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 20ef35a9fd..4ef5f64480 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -54,6 +54,8 @@ #include #include +using namespace time_literals; + /** * @class OutputModuleInterface * Base class for an output module. @@ -187,6 +189,11 @@ public: uint16_t &minValue(int index) { return _min_value[index]; } uint16_t &maxValue(int index) { return _max_value[index]; } + /** + * Returns the actual failsafe value taking into account the assigned function + */ + uint16_t actualFailsafeValue(int index); + /** * Get the motor index that maps from PX4 convention to the configured one * @param index motor index in [0, num_motors-1] @@ -203,6 +210,8 @@ public: const char *paramPrefix() const { return _param_prefix; } + void setLowrateSchedulingInterval(hrt_abstime interval) { _lowrate_schedule_interval = interval; } + protected: void updateParams() override; @@ -330,6 +339,7 @@ private: false; ///< whether or not the output module supports reversible motors (range [-1, 0] for motors) const char *const _param_prefix; ParamHandles _param_handles[MAX_ACTUATORS]; + hrt_abstime _lowrate_schedule_interval{300_ms}; uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 178b06c41d..73c82dae9d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -193,6 +193,10 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */ #define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */ #define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ +#define PX4IO_P_SETUP_PWM_RATE_GROUP0 19 /* Configure timer group 0 update rate in Hz */ +#define PX4IO_P_SETUP_PWM_RATE_GROUP1 20 /* Configure timer group 1 update rate in Hz */ +#define PX4IO_P_SETUP_PWM_RATE_GROUP2 21 /* Configure timer group 2 update rate in Hz */ +#define PX4IO_P_SETUP_PWM_RATE_GROUP3 22 /* Configure timer group 3 update rate in Hz */ #define PX4IO_THERMAL_IGNORE UINT16_MAX #define PX4IO_THERMAL_OFF 0 diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 2d4c589d77..bce448e3c0 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -145,7 +145,8 @@ volatile uint16_t r_page_setup[] = { [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0, [PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE, - [PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0 + [PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0, + [PX4IO_P_SETUP_PWM_RATE_GROUP0 ... PX4IO_P_SETUP_PWM_RATE_GROUP3] = 0 }; #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI) @@ -465,6 +466,33 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_page_setup[offset] = value; break; + case PX4IO_P_SETUP_PWM_RATE_GROUP0: + case PX4IO_P_SETUP_PWM_RATE_GROUP1: + case PX4IO_P_SETUP_PWM_RATE_GROUP2: + case PX4IO_P_SETUP_PWM_RATE_GROUP3: + + /* For PWM constrain to [25,400]Hz + * For Oneshot there is no rate, 0 is therefore used to select Oneshot mode + */ + if (value != 0) { + if (value < 25) { + value = 25; + } + + if (value > 400) { + value = 400; + } + } + + if (up_pwm_servo_set_rate_group_update(offset - PX4IO_P_SETUP_PWM_RATE_GROUP0, value) == OK) { + r_page_setup[offset] = value; + + } else { + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + + break; + default: return -1; } diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 87464c17e8..f93c904e69 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -333,6 +333,10 @@ actuator_output: maxlength: 2 schema: type: string + timer_config_file: + # Only used for 'pwm' generator, specifies + # board-specific timer_config.cpp file + type: string pwm_timer_param: # Only used for 'pwm' generator, per-timer config param type: dict