From 7761b98832c6dbcbe4495127178337315d37d78d Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Thu, 5 Sep 2024 13:06:32 +0300 Subject: [PATCH] pwm_esc: Support more /dev/pwmX devices according to the device Signed-off-by: Jukka Laitinen --- src/drivers/pwm_esc/pwm_esc.cpp | 145 +++++++++++++++++++------------- 1 file changed, 87 insertions(+), 58 deletions(-) diff --git a/src/drivers/pwm_esc/pwm_esc.cpp b/src/drivers/pwm_esc/pwm_esc.cpp index 28e85cee0fa4..48f2450a98c3 100644 --- a/src/drivers/pwm_esc/pwm_esc.cpp +++ b/src/drivers/pwm_esc/pwm_esc.cpp @@ -68,12 +68,22 @@ #define PWMESC_OUT_PATH "/dev/pwmX"; #endif -#ifndef PWMESC_MAX_DEVICES -#define PWMESC_MAX_DEVICES 2 +/* Number of PWMESC device nodes /dev/pwm0 .. /dev/pwmX */ + +#ifndef PWMESC_N_DEVICES +#define PWMESC_N_DEVICES 1 #endif -#ifndef PWMESC_MAX_CHANNELS -#define PWMESC_MAX_CHANNELS CONFIG_PWM_NCHANNELS +/* Number channels in each device */ + +#ifndef PWMESC_CHANNELS_PER_DEV +#define PWMESC_CHANNELS_PER_DEV {CONFIG_PWM_NCHANNELS} +#endif + +/* Total number of pwm channels (sum of PWMESC_CHANNELS_PER_DEV) */ + +#ifndef PWMESC_N_CHANNELS +#define PWMESC_N_CHANNELS ((PWMESC_N_DEVICES) * (PWMESC_CHANNELS_PER_DEV)) #endif #ifndef PWM_DEFAULT_RATE @@ -150,7 +160,7 @@ class PWMESC : public OutputModuleInterface /** * Don't allow more channels than MAX_ACTUATORS */ - static_assert(PWMESC_MAX_CHANNELS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails"); + static_assert(PWMESC_N_CHANNELS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails"); private: @@ -178,7 +188,7 @@ class PWMESC : public OutputModuleInterface bool _hitl_mode; ///< Hardware-in-the-loop simulation mode - don't publish actuator_outputs - int _pwm_fd[PWMESC_MAX_DEVICES]; + int _pwm_fd[PWMESC_N_DEVICES]; int32_t _pwm_rate{PWM_DEFAULT_RATE}; @@ -240,7 +250,7 @@ PWMESC::PWMESC(bool hitl) : _task(-1), _task_should_exit(false), _perf_update(perf_alloc(PC_ELAPSED, "pwm update")), - _mixing_output(hitl ? "PWM_MAIN" : PARAM_PREFIX, PWMESC_MAX_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, + _mixing_output(hitl ? "PWM_MAIN" : PARAM_PREFIX, PWMESC_N_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true), _hitl_mode(hitl) { @@ -312,27 +322,34 @@ PWMESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne { bool ret = true; struct pwm_info_s pwm; + const unsigned ch_per_dev[PWMESC_N_DEVICES] = PWMESC_CHANNELS_PER_DEV; - memset(&pwm, 0, sizeof(struct pwm_info_s)); - pwm.frequency = _pwm_rate; + if (!_hitl_mode) { + /* When not in hitl, run PWMs */ + unsigned n_out = 0; - for (unsigned i = 0; i < num_outputs; i++) { - // TODO: channel to proper pwm device map. - // this is now just quick hack for one pwm device, direct map of channels + for (int dev = 0; dev < PWMESC_N_DEVICES && _pwm_fd[dev] >= 0; dev++) { + memset(&pwm, 0, sizeof(struct pwm_info_s)); + pwm.frequency = _pwm_rate; - uint16_t pwm_val = outputs[i]; - pwm.channels[i].duty = ((((uint32_t)pwm_val) << 16) / (1000000 / _pwm_rate)); - pwm.channels[i].channel = i + 1; - } + unsigned i; + + for (i = 0; i < ch_per_dev[dev] && n_out < num_outputs; i++) { + uint16_t pwm_val = outputs[n_out++]; + pwm.channels[i].duty = ((((uint32_t)pwm_val) << 16) / (1000000 / _pwm_rate)); + pwm.channels[i].channel = i + 1; + } + if (i < CONFIG_PWM_NCHANNELS) { + pwm.channels[i].channel = -1; + } - if (!_hitl_mode) { - /* When not in hitl, run PWMs */ - if (::ioctl(_pwm_fd[0], PWMIOC_SETCHARACTERISTICS, - (unsigned long)((uintptr_t)&pwm)) < 0) { - PX4_ERR("PWMIOC_SETCHARACTERISTICS failed: %d\n", - errno); - ret = false; + if (::ioctl(_pwm_fd[dev], PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)) < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS for pwm%d failed: %d\n", dev, + errno); + ret = false; + } } } else { @@ -376,34 +393,44 @@ PWMESC::set_defaults(int fd, unsigned long request) { /* Configure PWM to default rate, 1 as duty and start */ + int ret = -1; + const int ch_per_dev[PWMESC_N_DEVICES] = PWMESC_CHANNELS_PER_DEV; struct pwm_info_s pwm; - memset(&pwm, 0, sizeof(struct pwm_info_s)); - pwm.frequency = _pwm_rate; - - for (int j = 0; j < PWMESC_MAX_CHANNELS; j++) { - pwm.channels[j].duty = 1; /* 0 is not allowed duty cycle value */ - pwm.channels[j].channel = j + 1; - } - /* Set the frequency and duty */ + for (int dev = 0; dev < PWMESC_N_DEVICES; dev++) { + memset(&pwm, 0, sizeof(struct pwm_info_s)); + pwm.frequency = _pwm_rate; - int ret = ::ioctl(fd, PWMIOC_SETCHARACTERISTICS, - (unsigned long)((uintptr_t)&pwm)); + int i; - if (ret < 0) { - PX4_ERR("PWMIOC_SETCHARACTERISTICS) failed: %d\n", - errno); + for (i = 0; i < ch_per_dev[dev]; i++) { + pwm.channels[i].duty = 1; /* 0 is not allowed duty cycle value */ + pwm.channels[i].channel = i + 1; + } - } else { + if (i < CONFIG_PWM_NCHANNELS) { + pwm.channels[i].channel = -1; + } - /* Start / stop */ + /* Set the frequency and duty */ - ret = ::ioctl(fd, request, 0); + ret = ::ioctl(fd, PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)); if (ret < 0) { - PX4_ERR("PWMIOC_START/STOP failed: %d\n", errno); - } + PX4_ERR("PWMIOC_SETCHARACTERISTICS failed: %d\n", + errno); + + } else { + + /* Start / stop */ + + ret = ::ioctl(fd, request, 0); + if (ret < 0) { + PX4_ERR("PWMIOC_START/STOP failed: %d\n", errno); + } + } } return ret; @@ -412,37 +439,39 @@ PWMESC::set_defaults(int fd, unsigned long request) int PWMESC::init_pwm_outputs() { - int ret = 0; + int ret = -1; _mixing_output.setIgnoreLockdown(_hitl_mode); - _mixing_output.setMaxNumOutputs(PWMESC_MAX_CHANNELS); + _mixing_output.setMaxNumOutputs(PWMESC_N_CHANNELS); const int update_interval_in_us = math::constrain(1000000 / (_pwm_rate * 2), 500, 100000); _mixing_output.setMaxTopicUpdateRate(update_interval_in_us); /* Open the PWM devnode */ - // TODO: loop through the devices, open all fd:s + // loop through the devices, open all fd:s char pwm_device_name[] = PWMESC_OUT_PATH; - int n_pwm_devices = 1; - for (int i = 0; i < 1 && i < n_pwm_devices; i++) { + for (int i = 0; i < PWMESC_N_DEVICES; i++) { pwm_device_name[sizeof(pwm_device_name) - 2] = '0' + i; _pwm_fd[i] = ::open(pwm_device_name, O_RDONLY); - if (_pwm_fd[i] < 0) { - PX4_ERR("pwm_main: open %s failed: %d\n", pwm_device_name, errno); - ret = -1; - continue; - } + if (_pwm_fd[i] >= 0) { + /* Configure PWM to default rate, 0 pulse and start */ - /* Configure PWM to default rate, 0 pulse and start */ + ret = set_defaults(_pwm_fd[i], PWMIOC_START); - if (set_defaults(_pwm_fd[i], PWMIOC_START) != 0) { - ret = -1; + if (ret != 0) { + PX4_ERR("Init failed for %s errno %d", pwm_device_name, errno); + break; + } } } + if (ret != 0) { + PX4_ERR("PWM init failed"); + } + return ret; } @@ -504,10 +533,10 @@ PWMESC::task_main() /* Configure PWM to default rate, 0 pulse and stop */ - int n_pwm_devices = 1; - - for (int i = 0; i < 1 && i < n_pwm_devices; i++) { - set_defaults(_pwm_fd[i], PWMIOC_STOP); + for (int i = 0; i < PWMESC_N_DEVICES; i++) { + if (_pwm_fd[i] >= 0) { + set_defaults(_pwm_fd[i], PWMIOC_STOP); + } } /* tell the dtor that we are exiting */