int
pwm_main(int argc, char *argv[])
{
const char *dev = PWM_OUTPUT_DEVICE_PATH;
unsigned alt_rate = 0;
uint32_t alt_channel_groups = 0;
bool alt_channels_set = false;
bool print_verbose = false;
int ch;
int ret;
char *ep;
uint32_t set_mask = 0;
unsigned group;
unsigned long channels;
unsigned single_ch = 0;
unsigned pwm_value = 0;
if (argc < 1)
usage(NULL);
while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
switch (ch) {
case 'd':
if (NULL == strstr(optarg, "/dev/")) {
warnx("device %s not valid", optarg);
usage(NULL);
}
dev = optarg;
break;
case 'v':
print_verbose = true;
break;
case 'c':
/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
channels = strtoul(optarg, &ep, 0);
while ((single_ch = channels % 10)) {
set_mask |= 1<<(single_ch-1);
channels /= 10;
}
break;
case 'g':
group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32))
usage("bad channel_group value");
alt_channel_groups |= (1 << group);
alt_channels_set = true;
warnx("alt channels set, group: %d", group);
break;
case 'm':
/* Read in mask directly */
set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
break;
case 'a':
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
set_mask |= 1<<i;
}
break;
case 'p':
pwm_value = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad PWM value provided");
break;
case 'r':
alt_rate = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alternative rate provided");
break;
default:
break;
}
}
if (print_verbose && set_mask > 0) {
warnx("Chose channels: ");
printf(" ");
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
if (set_mask & 1<<i)
printf("%d ", i+1);
}
printf("\n");
}
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* get the number of servo channels */
unsigned servo_count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
if (!strcmp(argv[1], "arm")) {
/* tell safety that its ok to disable it with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
if (print_verbose)
warnx("Outputs armed");
exit(0);
} else if (!strcmp(argv[1], "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
if (print_verbose)
warnx("Outputs disarmed");
exit(0);
} else if (!strcmp(argv[1], "rate")) {
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* directly supplied channel mask */
if (set_mask > 0) {
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
exit(0);
} else if (!strcmp(argv[1], "min")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: min PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting min values");
}
exit(0);
} else if (!strcmp(argv[1], "max")) {
int
pwm_main(int argc, char *argv[])
{
const char *dev = PWM_OUTPUT_DEVICE_PATH;
unsigned alt_rate = 0;
uint32_t alt_channel_groups = 0;
bool alt_channels_set = false;
bool print_info = false;
int ch;
int ret;
char *ep;
unsigned group;
int32_t set_mask = -1;
if (argc < 2)
usage(NULL);
while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
switch (ch) {
case 'c':
group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32))
usage("bad channel_group value");
alt_channel_groups |= (1 << group);
alt_channels_set = true;
break;
case 'd':
dev = optarg;
break;
case 'u':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
break;
case 'm':
set_mask = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
break;
case 'v':
print_info = true;
break;
default:
usage(NULL);
}
}
argc -= optind;
argv += optind;
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* directly supplied channel mask */
if (set_mask != -1) {
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (unsigned group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
/* iterate remaining arguments */
unsigned channel = 0;
while (argc--) {
const char *arg = argv[0];
argv++;
if (!strcmp(arg, "arm")) {
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
continue;
}
if (!strcmp(arg, "disarm")) {
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
continue;
}
unsigned pwm_value = strtol(arg, &ep, 0);
if (*ep == '\0') {
ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", channel);
channel++;
continue;
}
usage("unrecognised option");
}
/* print verbose info */
if (print_info) {
/* get the number of servo channels */
unsigned count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
/* print current servo values */
for (unsigned i = 0; i < count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("channel %u: %uus\n", i, spos);
} else {
printf("%u: ERROR\n", i);
}
}
/* print rate groups */
for (unsigned i = 0; i < count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
if (ret != OK)
break;
if (group_mask != 0) {
printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j))
printf(" %u", j);
printf("\n");
}
}
fflush(stdout);
}
exit(0);
}
int
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
lock();
switch (cmd) {
case PWM_SERVO_ARM:
up_pwm_servo_arm(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
case PWM_SERVO_SET_FORCE_SAFETY_ON:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
up_pwm_servo_arm(false);
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_default_rate;
break;
case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate;
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate_channels;
break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_SET(7):
case PWM_SERVO_SET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(3):
case PWM_SERVO_SET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0):
if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
}
break;
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_mode < MODE_6PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(3):
case PWM_SERVO_GET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
break;
case PWM_SERVO_GET_RATEGROUP(0):
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
case PWM_SERVO_GET_RATEGROUP(4):
case PWM_SERVO_GET_RATEGROUP(5):
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET_RATEGROUP(6):
case PWM_SERVO_GET_RATEGROUP(7):
#endif
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
*(unsigned *)arg = 8;
break;
#endif
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
case MODE_4PWM:
*(unsigned *)arg = 4;
break;
case MODE_2PWM:
*(unsigned *)arg = 2;
break;
default:
ret = -EINVAL;
break;
}
break;
case PWM_SERVO_SET_COUNT: {
/* change the number of outputs that are enabled for
* PWM. This is used to change the split between GPIO
* and PWM under control of the flight config
* parameters. Note that this does not allow for
* changing a set of pins to be used for serial on
* FMUv1
*/
switch (arg) {
case 0:
set_mode(MODE_NONE);
break;
case 2:
set_mode(MODE_2PWM);
break;
case 4:
set_mode(MODE_4PWM);
break;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
case 6:
set_mode(MODE_6PWM);
break;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
case 8:
set_mode(MODE_8PWM);
break;
#endif
default:
ret = -EINVAL;
break;
}
break;
}
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)_controls, mixinfo);
if (mixer->check()) {
delete mixer;
_groups_required = 0;
ret = -EINVAL;
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)_controls);
_mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);
}
break;
}
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM;
} else {
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
break;
}
default:
ret = -ENOTTY;
break;
}
unlock();
return ret;
}
int
pwm_main(int argc, char *argv[])
{
const char *dev = PWM_OUTPUT_DEVICE_PATH;
unsigned alt_rate = 0;
uint32_t alt_channel_groups = 0;
bool alt_channels_set = false;
bool print_info = false;
int ch;
int ret;
char *ep;
unsigned group;
int32_t set_mask = -1;
if (argc < 2)
usage(NULL);
while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
switch (ch) {
case 'c':
group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32))
usage("bad channel_group value");
alt_channel_groups |= (1 << group);
alt_channels_set = true;
break;
case 'd':
dev = optarg;
break;
case 'u':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
break;
case 'm':
set_mask = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
break;
case 'v':
print_info = true;
break;
default:
usage(NULL);
}
}
argc -= optind;
argv += optind;
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* directly supplied channel mask */
if (set_mask != -1) {
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (unsigned group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
/* iterate remaining arguments */
unsigned nchannels = 0;
unsigned channel[8] = {0};
while (argc--) {
const char *arg = argv[0];
argv++;
if (!strcmp(arg, "arm")) {
/* tell IO that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
continue;
}
if (!strcmp(arg, "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
continue;
}
unsigned pwm_value = strtol(arg, &ep, 0);
if (*ep == '\0') {
if (nchannels > sizeof(channel) / sizeof(channel[0]))
err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
channel[nchannels] = pwm_value;
nchannels++;
continue;
}
usage("unrecognized option");
}
/* print verbose info */
if (print_info) {
/* get the number of servo channels */
unsigned count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
/* print current servo values */
for (unsigned i = 0; i < count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("channel %u: %uus\n", i, spos);
} else {
printf("%u: ERROR\n", i);
}
}
/* print rate groups */
for (unsigned i = 0; i < count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
if (ret != OK)
break;
if (group_mask != 0) {
printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j))
printf(" %u", j);
printf("\n");
}
}
fflush(stdout);
}
/* perform PWM output */
if (nchannels) {
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
warnx("Press CTRL-C or 'c' to abort.");
while (1) {
for (int i = 0; i < nchannels; i++) {
ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
/* abort on user request */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
close(console);
exit(0);
}
}
/* rate limit to ~ 20 Hz */
usleep(50000);
}
}
exit(0);
}
