Skip to content

Commit

Permalink
pwm_esc: Support more /dev/pwmX devices according to the device
Browse files Browse the repository at this point in the history 
Signed-off-by: Jukka Laitinen <[email protected]>
  • Loading branch information
@jlaitine
jlaitine committed Sep 5, 2024
1 parent 1905f08 commit 1b95fe6
Showing 1 changed file with 73 additions and 57 deletions.
130 changes: 73 additions & 57 deletions src/drivers/pwm_esc/pwm_esc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 CONFIG_PWM_NCHANNELS
#endif

#ifndef PWM_DEFAULT_RATE
Expand Down Expand Up @@ -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:

Expand Down Expand Up @@ -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};

Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -312,27 +322,28 @@ 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;

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
if (!_hitl_mode) {
/* When not in hitl, run PWMs */
unsigned n_out = 0;

uint16_t pwm_val = outputs[i];
pwm.channels[i].duty = ((((uint32_t)pwm_val) << 16) / (1000000 / _pwm_rate));
pwm.channels[i].channel = i + 1;
}
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;

for (unsigned 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 (!_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 {
Expand Down Expand Up @@ -376,34 +387,37 @@ 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;
}
for (int i = 0; i < PWMESC_N_DEVICES; i++) {
for (int j = 0; j < ch_per_dev[i]; j++) {
pwm.channels[j].duty = 1; /* 0 is not allowed duty cycle value */
pwm.channels[j].channel = j + 1;
}

/* Set the frequency and duty */
/* Set the frequency and duty */

int ret = ::ioctl(fd, PWMIOC_SETCHARACTERISTICS,
(unsigned long)((uintptr_t)&pwm));
ret = ::ioctl(fd, PWMIOC_SETCHARACTERISTICS,
(unsigned long)((uintptr_t)&pwm));

if (ret < 0) {
PX4_ERR("PWMIOC_SETCHARACTERISTICS) failed: %d\n",
errno);
if (ret < 0) {
PX4_ERR("PWMIOC_SETCHARACTERISTICS failed: %d\n",
errno);

} else {
} else {

/* Start / stop */
/* Start / stop */

ret = ::ioctl(fd, request, 0);
ret = ::ioctl(fd, request, 0);

if (ret < 0) {
PX4_ERR("PWMIOC_START/STOP failed: %d\n", errno);
if (ret < 0) {
PX4_ERR("PWMIOC_START/STOP failed: %d\n", errno);
}
}

}

return ret;
Expand All @@ -412,37 +426,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;
}

Expand Down Expand Up @@ -504,10 +520,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 */
Expand Down

0 comments on commit 1b95fe6

Please sign in to comment.