zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Some more interface changes, needs testing and cleanup

sbg
Julian Oes 12 years ago
parent
baa4908054
commit
9ff5217118
2 changed files with 222 additions and 210 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/drivers/px4io/px4io.cpp
  2. 430
      src/systemcmds/pwm/pwm.c

2
src/drivers/px4io/px4io.cpp
Unescape View File

@ -957,8 +957,6 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) @@ -957,8 +957,6 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
/* fail with error */
return E2BIG;
printf("Sending IDLE values\n");
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
}

430
src/systemcmds/pwm/pwm.c
Unescape View File

@ -72,25 +72,33 @@ usage(const char *reason) @@ -72,25 +72,33 @@ usage(const char *reason)
warnx("%s", reason);
errx(1,
"usage:\n"
"pwm [-v] [-d <device>] set|config|arm|disarm|info ...\n"
""
" -v Print verbose information\n"
" -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
"pwm arm|disarm|rate|min|max|disarmed|test|info ...\n"
"\n"
"arm Arm output\n"
"disarm Disarm output\n"
" arm Arm output\n"
" disarm Disarm output\n"
"\n"
"set <channel_value> ... Directly set PWM values\n"
" rate Configure PWM rates\n"
" [-c <channel group>] Channel group that should update at the alternate rate\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n"
" -r <alt_rate> PWM rate (50 to 400 Hz)\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"
" min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n"
" disarmed ... Configure disarmed PWM values\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\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"
" test ... Directly set PWM values\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n"
"\n"
"info Print information about the PWM device\n"
" info Print information about the PWM device\n"
"\n"
" -v Print verbose information\n"
" -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
);
}
@ -106,13 +114,16 @@ pwm_main(int argc, char *argv[]) @@ -106,13 +114,16 @@ pwm_main(int argc, char *argv[])
int ch;
int ret;
char *ep;
int32_t set_mask = -1;
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, argv, "v:d:")) != EOF) {
while ((ch = getopt(argc-1, &argv[1], "d:vc:m:ap:r:")) != EOF) {
switch (ch) {
case 'd':
@ -121,32 +132,82 @@ pwm_main(int argc, char *argv[]) @@ -121,32 +132,82 @@ pwm_main(int argc, char *argv[])
usage(NULL);
}
dev = optarg;
argv+=2;
argc-=2;
break;
case 'v':
print_verbose = true;
argv+=1;
argc-=1;
break;
case 'c':
/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
channels = strtol(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;
break;
case 'm':
/* Read in mask directly */
set_mask = strtol(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 = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad PWM value provided");
break;
case 'r':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alternative rate provided");
break;
default:
break;
}
}
/* get rid of cmd name */
argv+=1;
argc-=1;
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");
int argi = 1; /* leave away cmd name */
for (int argi=0; argi<argc; argi++) {
while(argi<argc) {
if (!strcmp(argv[argi], "arm")) {
/* tell IO that its ok to disable its safety with the switch */
@ -158,7 +219,8 @@ pwm_main(int argc, char *argv[]) @@ -158,7 +219,8 @@ pwm_main(int argc, char *argv[])
if (ret != OK)
err(1, "PWM_SERVO_ARM");
warnx("Outputs armed");
if (print_verbose)
warnx("Outputs armed");
exit(0);
} else if (!strcmp(argv[argi], "disarm")) {
@ -167,233 +229,184 @@ pwm_main(int argc, char *argv[]) @@ -167,233 +229,184 @@ pwm_main(int argc, char *argv[])
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
warnx("Outputs disarmed");
if (print_verbose)
warnx("Outputs disarmed");
exit(0);
} else if (!strcmp(argv[argi], "set")) {
/* iterate remaining arguments */
unsigned nchannels = 0;
unsigned channel[PWM_OUTPUT_MAX_CHANNELS] = {0};
}else if (!strcmp(argv[argi], "rate")) {
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");
/* 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)");
} else {
usage("no alternative rate provided");
}
/* perform PWM output */
if (nchannels) {
/* directly supplied channel mask */
if (set_mask > 0) {
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
} else {
usage("no channel/channel groups selected");
}
/* Open console directly to grab CTRL-C signal */
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
warnx("Press CTRL-C or 'c' to abort.");
for (group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
while (1) {
for (unsigned i = 0; i < nchannels; i++) {
ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
/* abort on user request */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
exit(0);
}
mask |= group_mask;
}
}
} else {
usage("no PWM values supplied");
}
} else if (!strcmp(argv[argi], "config")) {
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
argi++;
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
exit(0);
if (!strcmp(argv[argi], "rate")) {
} else if (!strcmp(argv[argi], "min")) {
while ((ch = getopt(argc, argv, "m:c:")) != EOF) {
switch (ch) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
case 'm':
set_mask = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
break;
argi+=2;
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
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;
argi+=2;
break;
}
}
argi++;
if (argi >= argc)
usage("no alt_rate value supplied");
alt_rate = strtol(argv[argi], &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
break;
/* 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)");
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++;
}
}
/* 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");
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
exit(0);
for (group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
} else if (!strcmp(argv[argi], "max")) {
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
mask |= group_mask;
}
}
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
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: max PWM: %d", i+1, pwm_value);
pwm_values.channel_count++;
}
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
} else if (!strcmp(argv[argi], "min")) {
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
exit(0);
/* 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++;
} else if (!strcmp(argv[argi], "disarmed")) {
continue;
}
usage("unrecognized option");
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
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: disarmed PWM: %d", i+1, pwm_value);
pwm_values.channel_count++;
}
}
} else if (!strcmp(argv[argi], "max")) {
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
/* 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++;
ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
exit(0);
continue;
}
usage("unrecognized option");
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
} else if (!strcmp(argv[argi], "test")) {
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
/* perform PWM output */
} else if (!strcmp(argv[argi], "disarmed")) {
/* Open console directly to grab CTRL-C signal */
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
/* 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++;
warnx("Press CTRL-C or 'c' to abort.");
continue;
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
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");
}
/* abort on user request */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
} else {
usage("specify rate, min, max or disarmed");
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
exit(0);
}
}
}
exit(0);
} else if (!strcmp(argv[argi], "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");
} else if (!strcmp(argv[argi], "info")) {
/* print current servo values */
for (unsigned i = 0; i < count; i++) {
for (unsigned i = 0; i < servo_count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
@ -405,7 +418,7 @@ pwm_main(int argc, char *argv[]) @@ -405,7 +418,7 @@ pwm_main(int argc, char *argv[])
}
/* print rate groups */
for (unsigned i = 0; i < count; i++) {
for (unsigned i = 0; i < servo_count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
@ -419,12 +432,13 @@ pwm_main(int argc, char *argv[]) @@ -419,12 +432,13 @@ pwm_main(int argc, char *argv[])
printf("\n");
}
}
exit(0);
} else {
usage("specify arm|disarm|set|config");
}
argi++;
}
exit(0);
usage("specify arm|disarm|set|config|test");
return 0;
}

Write Preview
Loading…
Cancel
Save