forked from Archive/PX4-Autopilot
Changes to pwm systemcmd, basic functionality there, needs polishing
This commit is contained in:
parent
9493c7a45c
commit
baa4908054
|
@ -1748,6 +1748,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
|
||||||
|
|
||||||
case PWM_SERVO_SET_MIN_PWM: {
|
case PWM_SERVO_SET_MIN_PWM: {
|
||||||
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
|
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
|
||||||
|
warnx("Set min values");
|
||||||
set_min_values(pwm->values, pwm->channel_count);
|
set_min_values(pwm->values, pwm->channel_count);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -2381,110 +2382,7 @@ px4io_main(int argc, char *argv[])
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "min")) {
|
|
||||||
|
|
||||||
if (argc < 3) {
|
|
||||||
errx(1, "min command needs at least one channel value (PWM)");
|
|
||||||
}
|
|
||||||
|
|
||||||
int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
|
|
||||||
struct pwm_output_values pwm;
|
|
||||||
|
|
||||||
if (iofd > 0) {
|
|
||||||
|
|
||||||
pwm.channel_count = 0;
|
|
||||||
|
|
||||||
for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
|
|
||||||
{
|
|
||||||
/* set channel to commanline argument or to 900 for non-provided channels */
|
|
||||||
if (argc > i + 2) {
|
|
||||||
pwm.values[i] = atoi(argv[i+2]);
|
|
||||||
if (pwm.values[i] < 900 || pwm.values[i] > 1200) {
|
|
||||||
errx(1, "value out of range of 900 < value < 1200. Aborting.");
|
|
||||||
}
|
|
||||||
pwm.channel_count++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm);
|
|
||||||
|
|
||||||
if (ret != OK)
|
|
||||||
errx(ret, "failed setting min values");
|
|
||||||
} else {
|
|
||||||
errx(1, "not loaded");
|
|
||||||
}
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!strcmp(argv[1], "max")) {
|
|
||||||
|
|
||||||
if (argc < 3) {
|
|
||||||
errx(1, "max command needs at least one channel value (PWM)");
|
|
||||||
}
|
|
||||||
|
|
||||||
int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
|
|
||||||
struct pwm_output_values pwm;
|
|
||||||
|
|
||||||
if (iofd > 0) {
|
|
||||||
|
|
||||||
pwm.channel_count = 0;
|
|
||||||
|
|
||||||
for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
|
|
||||||
{
|
|
||||||
/* set channel to commanline argument or to 2100 for non-provided channels */
|
|
||||||
if (argc > i + 2) {
|
|
||||||
pwm.values[i] = atoi(argv[i+2]);
|
|
||||||
if (pwm.values[i] < 1800 || pwm.values[i] > 2100) {
|
|
||||||
errx(1, "value out of range of 1800 < value < 2100. Aborting.");
|
|
||||||
}
|
|
||||||
pwm.channel_count++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm);
|
|
||||||
|
|
||||||
if (ret != OK)
|
|
||||||
errx(ret, "failed setting max values");
|
|
||||||
} else {
|
|
||||||
errx(1, "not loaded");
|
|
||||||
}
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) {
|
|
||||||
|
|
||||||
if (argc < 3) {
|
|
||||||
errx(1, "max command needs at least one channel value (PWM)");
|
|
||||||
}
|
|
||||||
|
|
||||||
int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
|
|
||||||
struct pwm_output_values pwm;
|
|
||||||
|
|
||||||
if (iofd > 0) {
|
|
||||||
|
|
||||||
pwm.channel_count = 0;
|
|
||||||
|
|
||||||
for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
|
|
||||||
{
|
|
||||||
/* set channel to commanline argument or to 0 for non-provided channels */
|
|
||||||
if (argc > i + 2) {
|
|
||||||
pwm.values[i] = atoi(argv[i+2]);
|
|
||||||
if (pwm.values[i] < 900 || pwm.values[i] > 2100) {
|
|
||||||
errx(1, "value out of range of 900 < value < 2100. Aborting.");
|
|
||||||
}
|
|
||||||
pwm.channel_count++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm);
|
|
||||||
|
|
||||||
if (ret != OK)
|
|
||||||
errx(ret, "failed setting idle values");
|
|
||||||
} else {
|
|
||||||
errx(1, "not loaded");
|
|
||||||
}
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!strcmp(argv[1], "recovery")) {
|
if (!strcmp(argv[1], "recovery")) {
|
||||||
|
|
||||||
|
|
|
@ -45,6 +45,7 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
|
#include <poll.h>
|
||||||
#include <sys/mount.h>
|
#include <sys/mount.h>
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
#include <sys/stat.h>
|
#include <sys/stat.h>
|
||||||
|
@ -71,16 +72,25 @@ usage(const char *reason)
|
||||||
warnx("%s", reason);
|
warnx("%s", reason);
|
||||||
errx(1,
|
errx(1,
|
||||||
"usage:\n"
|
"usage:\n"
|
||||||
"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n"
|
"pwm [-v] [-d <device>] set|config|arm|disarm|info ...\n"
|
||||||
" -v Print information about the PWM device\n"
|
""
|
||||||
" <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
|
" -v Print verbose information\n"
|
||||||
" <alt_rate> PWM update rate for channels in <alt_channel_mask>\n"
|
" -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
|
||||||
" <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n"
|
|
||||||
" arm | disarm Arm or disarm the ouptut\n"
|
|
||||||
" <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
|
|
||||||
" <chanmask> Directly supply alt rate channel mask (debug use only)\n"
|
|
||||||
"\n"
|
"\n"
|
||||||
"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
|
"arm Arm output\n"
|
||||||
|
"disarm Disarm output\n"
|
||||||
|
"\n"
|
||||||
|
"set <channel_value> ... Directly set PWM values\n"
|
||||||
|
"\n"
|
||||||
|
"config rate <alt_rate> Configure PWM rates\n"
|
||||||
|
" [-c <channel group>] Channel group that should update at the alternate rate\n"
|
||||||
|
" [-m <chanmask> ] Directly supply alt rate channel mask\n"
|
||||||
|
"\n"
|
||||||
|
"config min <channel value]> ... Configure minimum PWM values\n"
|
||||||
|
"config max <channel value]> ... Configure maximum PWM values\n"
|
||||||
|
"config disarmed <channel_value> ... Configure disarmed PWM values\n"
|
||||||
|
"\n"
|
||||||
|
"info Print information about the PWM device\n"
|
||||||
);
|
);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -92,57 +102,167 @@ pwm_main(int argc, char *argv[])
|
||||||
unsigned alt_rate = 0;
|
unsigned alt_rate = 0;
|
||||||
uint32_t alt_channel_groups = 0;
|
uint32_t alt_channel_groups = 0;
|
||||||
bool alt_channels_set = false;
|
bool alt_channels_set = false;
|
||||||
bool print_info = false;
|
bool print_verbose = false;
|
||||||
int ch;
|
int ch;
|
||||||
int ret;
|
int ret;
|
||||||
char *ep;
|
char *ep;
|
||||||
unsigned group;
|
|
||||||
int32_t set_mask = -1;
|
int32_t set_mask = -1;
|
||||||
|
unsigned group;
|
||||||
|
|
||||||
if (argc < 2)
|
if (argc < 1)
|
||||||
usage(NULL);
|
usage(NULL);
|
||||||
|
|
||||||
while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
|
while ((ch = getopt(argc, argv, "v:d:")) != EOF) {
|
||||||
switch (ch) {
|
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':
|
case 'd':
|
||||||
|
if (NULL == strstr(optarg, "/dev/")) {
|
||||||
|
warnx("device %s not valid", optarg);
|
||||||
|
usage(NULL);
|
||||||
|
}
|
||||||
dev = optarg;
|
dev = optarg;
|
||||||
|
argv+=2;
|
||||||
|
argc-=2;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'u':
|
case 'v':
|
||||||
alt_rate = strtol(optarg, &ep, 0);
|
print_verbose = true;
|
||||||
if (*ep != '\0')
|
argv+=1;
|
||||||
usage("bad alt_rate value");
|
argc-=1;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* get rid of cmd name */
|
||||||
|
argv+=1;
|
||||||
|
argc-=1;
|
||||||
|
|
||||||
|
/* open for ioctl only */
|
||||||
|
int fd = open(dev, 0);
|
||||||
|
if (fd < 0)
|
||||||
|
err(1, "can't open %s", dev);
|
||||||
|
|
||||||
|
|
||||||
|
for (int argi=0; argi<argc; argi++) {
|
||||||
|
|
||||||
|
if (!strcmp(argv[argi], "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");
|
||||||
|
|
||||||
|
warnx("Outputs armed");
|
||||||
|
exit(0);
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[argi], "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");
|
||||||
|
|
||||||
|
warnx("Outputs disarmed");
|
||||||
|
exit(0);
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[argi], "set")) {
|
||||||
|
|
||||||
|
/* iterate remaining arguments */
|
||||||
|
unsigned nchannels = 0;
|
||||||
|
unsigned channel[PWM_OUTPUT_MAX_CHANNELS] = {0};
|
||||||
|
|
||||||
|
while (argc - argi > 1) {
|
||||||
|
argi++;
|
||||||
|
unsigned pwm_value = strtol(argv[argi], &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]));
|
||||||
|
//XXX check for sane values ?
|
||||||
|
channel[nchannels] = pwm_value;
|
||||||
|
if (print_verbose)
|
||||||
|
warnx("Set channel %d: %d us", nchannels+1, channel[nchannels]);
|
||||||
|
|
||||||
|
nchannels++;
|
||||||
|
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
usage("unrecognized option");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* perform PWM output */
|
||||||
|
if (nchannels) {
|
||||||
|
|
||||||
|
/* Open console directly to grab CTRL-C signal */
|
||||||
|
struct pollfd fds;
|
||||||
|
fds.fd = 0; /* stdin */
|
||||||
|
fds.events = POLLIN;
|
||||||
|
|
||||||
|
warnx("Press CTRL-C or 'c' to abort.");
|
||||||
|
|
||||||
|
while (1) {
|
||||||
|
for (unsigned 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;
|
||||||
|
ret = poll(&fds, 1, 0);
|
||||||
|
if (ret > 0) {
|
||||||
|
|
||||||
|
read(0, &c, 1);
|
||||||
|
if (c == 0x03 || c == 0x63 || c == 'q') {
|
||||||
|
warnx("User abort\n");
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
usage("no PWM values supplied");
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[argi], "config")) {
|
||||||
|
|
||||||
|
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
|
||||||
|
|
||||||
|
argi++;
|
||||||
|
|
||||||
|
if (!strcmp(argv[argi], "rate")) {
|
||||||
|
|
||||||
|
while ((ch = getopt(argc, argv, "m:c:")) != EOF) {
|
||||||
|
switch (ch) {
|
||||||
|
|
||||||
case 'm':
|
case 'm':
|
||||||
set_mask = strtol(optarg, &ep, 0);
|
set_mask = strtol(optarg, &ep, 0);
|
||||||
if (*ep != '\0')
|
if (*ep != '\0')
|
||||||
usage("bad set_mask value");
|
usage("bad set_mask value");
|
||||||
break;
|
break;
|
||||||
|
argi+=2;
|
||||||
|
|
||||||
case 'v':
|
case 'c':
|
||||||
print_info = true;
|
group = strtoul(optarg, &ep, 0);
|
||||||
|
if ((*ep != '\0') || (group >= 32))
|
||||||
|
usage("bad channel_group value");
|
||||||
|
alt_channel_groups |= (1 << group);
|
||||||
|
alt_channels_set = true;
|
||||||
|
argi+=2;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
|
||||||
usage(NULL);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
argc -= optind;
|
argi++;
|
||||||
argv += optind;
|
if (argi >= argc)
|
||||||
|
usage("no alt_rate value supplied");
|
||||||
|
|
||||||
/* open for ioctl only */
|
alt_rate = strtol(argv[argi], &ep, 0);
|
||||||
int fd = open(dev, 0);
|
if (*ep != '\0')
|
||||||
if (fd < 0)
|
usage("bad alt_rate value");
|
||||||
err(1, "can't open %s", dev);
|
break;
|
||||||
|
|
||||||
/* change alternate PWM rate */
|
/* change alternate PWM rate */
|
||||||
if (alt_rate > 0) {
|
if (alt_rate > 0) {
|
||||||
|
@ -162,7 +282,7 @@ pwm_main(int argc, char *argv[])
|
||||||
if (alt_channels_set) {
|
if (alt_channels_set) {
|
||||||
uint32_t mask = 0;
|
uint32_t mask = 0;
|
||||||
|
|
||||||
for (unsigned group = 0; group < 32; group++) {
|
for (group = 0; group < 32; group++) {
|
||||||
if ((1 << group) & alt_channel_groups) {
|
if ((1 << group) & alt_channel_groups) {
|
||||||
uint32_t group_mask;
|
uint32_t group_mask;
|
||||||
|
|
||||||
|
@ -179,45 +299,93 @@ pwm_main(int argc, char *argv[])
|
||||||
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
|
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;
|
} else if (!strcmp(argv[argi], "min")) {
|
||||||
nchannels++;
|
|
||||||
|
/* iterate remaining arguments */
|
||||||
|
while (argc - argi > 1) {
|
||||||
|
argi++;
|
||||||
|
unsigned pwm_value = strtol(argv[argi], &ep, 0);
|
||||||
|
if (*ep == '\0') {
|
||||||
|
if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS)
|
||||||
|
err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS);
|
||||||
|
//XXX check for sane values ?
|
||||||
|
pwm_values.values[pwm_values.channel_count] = pwm_value;
|
||||||
|
pwm_values.channel_count++;
|
||||||
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
usage("unrecognized option");
|
usage("unrecognized option");
|
||||||
}
|
}
|
||||||
|
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 idle values");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[argi], "max")) {
|
||||||
|
|
||||||
|
/* iterate remaining arguments */
|
||||||
|
while (argc - argi > 1) {
|
||||||
|
argi++;
|
||||||
|
unsigned pwm_value = strtol(argv[argi], &ep, 0);
|
||||||
|
if (*ep == '\0') {
|
||||||
|
if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS)
|
||||||
|
err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS);
|
||||||
|
//XXX check for sane values ?
|
||||||
|
pwm_values.values[pwm_values.channel_count] = pwm_value;
|
||||||
|
pwm_values.channel_count++;
|
||||||
|
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
usage("unrecognized option");
|
||||||
|
}
|
||||||
|
if (pwm_values.channel_count == 0) {
|
||||||
|
usage("no PWM values added");
|
||||||
|
} else {
|
||||||
|
|
||||||
|
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
|
||||||
|
if (ret != OK)
|
||||||
|
errx(ret, "failed setting idle values");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[argi], "disarmed")) {
|
||||||
|
|
||||||
|
/* iterate remaining arguments */
|
||||||
|
while (argc - argi > 1) {
|
||||||
|
argi++;
|
||||||
|
unsigned pwm_value = strtol(argv[argi], &ep, 0);
|
||||||
|
if (*ep == '\0') {
|
||||||
|
if (pwm_values.channel_count > PWM_OUTPUT_MAX_CHANNELS)
|
||||||
|
err(1, "too many pwm values (max %d)", PWM_OUTPUT_MAX_CHANNELS);
|
||||||
|
//XXX check for sane values ?
|
||||||
|
pwm_values.values[pwm_values.channel_count] = pwm_value;
|
||||||
|
pwm_values.channel_count++;
|
||||||
|
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
usage("unrecognized option");
|
||||||
|
}
|
||||||
|
if (pwm_values.channel_count == 0) {
|
||||||
|
usage("no PWM values added");
|
||||||
|
} else {
|
||||||
|
|
||||||
|
ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
|
||||||
|
if (ret != OK)
|
||||||
|
errx(ret, "failed setting idle values");
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
usage("specify rate, min, max or disarmed");
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[argi], "info")) {
|
||||||
|
|
||||||
/* print verbose info */
|
|
||||||
if (print_info) {
|
|
||||||
/* get the number of servo channels */
|
/* get the number of servo channels */
|
||||||
unsigned count;
|
unsigned count;
|
||||||
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
|
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
|
||||||
|
@ -251,40 +419,13 @@ pwm_main(int argc, char *argv[])
|
||||||
printf("\n");
|
printf("\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
fflush(stdout);
|
|
||||||
|
} else {
|
||||||
|
usage("specify arm|disarm|set|config");
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 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);
|
exit(0);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
/* rate limit to ~ 20 Hz */
|
|
||||||
usleep(50000);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
Loading…
Reference in New Issue