fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 03:41:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PWM command: Fix code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-03-28 19:54:00 +01:00
parent 32e790110e
commit 23655675d3
1 changed files with 260 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/pwm/pwm.c
+260 -107
View File
@@ -68,41 +68,43 @@ __EXPORT int pwm_main(int argc, char *argv[]);
static void static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason != NULL) if (reason != NULL) {
warnx("%s", reason); warnx("%s", reason);
}
errx(1, errx(1,
"usage:\n" "usage:\n"
"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
"\n" "\n"
"arm\t\t\t\tArm output\n" "arm\t\t\t\tArm output\n"
"disarm\t\t\t\tDisarm output\n" "disarm\t\t\t\tDisarm output\n"
"\n" "\n"
"rate ...\t\t\tConfigure PWM rates\n" "rate ...\t\t\tConfigure PWM rates\n"
"\t[-g <channel group>]\t(e.g. 0,1,2)\n" "\t[-g <channel group>]\t(e.g. 0,1,2)\n"
"\t[-m <channel mask> ]\t(e.g. 0xF)\n" "\t[-m <channel mask> ]\t(e.g. 0xF)\n"
"\t[-a]\t\t\tConfigure all outputs\n" "\t[-a]\t\t\tConfigure all outputs\n"
"\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n" "\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
"\n" "\n"
"failsafe ...\t\t\tFailsafe PWM\n" "failsafe ...\t\t\tFailsafe PWM\n"
"disarmed ...\t\t\tDisarmed PWM\n" "disarmed ...\t\t\tDisarmed PWM\n"
"min ...\t\t\t\tMinimum PWM\n" "min ...\t\t\t\tMinimum PWM\n"
"max ...\t\t\t\tMaximum PWM\n" "max ...\t\t\t\tMaximum PWM\n"
"\t[-c <channels>]\t\t(e.g. 1234)\n" "\t[-c <channels>]\t\t(e.g. 1234)\n"
"\t[-m <channel mask> ]\t(e.g. 0xF)\n" "\t[-m <channel mask> ]\t(e.g. 0xF)\n"
"\t[-a]\t\t\tConfigure all outputs\n" "\t[-a]\t\t\tConfigure all outputs\n"
"\t-p <pwm value>\t\tPWM value\n" "\t-p <pwm value>\t\tPWM value\n"
"\n" "\n"
"test ...\t\t\tDirectly set PWM\n" "test ...\t\t\tDirectly set PWM\n"
"\t[-c <channels>]\t\t(e.g. 1234)\n" "\t[-c <channels>]\t\t(e.g. 1234)\n"
"\t[-m <channel mask> ]\t(e.g. 0xF)\n" "\t[-m <channel mask> ]\t(e.g. 0xF)\n"
"\t[-a]\t\t\tConfigure all outputs\n" "\t[-a]\t\t\tConfigure all outputs\n"
"\t-p <pwm value>\t\tPWM value\n" "\t-p <pwm value>\t\tPWM value\n"
"\n" "\n"
"info\t\t\t\tPrint information\n" "info\t\t\t\tPrint information\n"
"\n" "\n"
"\t-v\t\t\tVerbose\n" "\t-v\t\t\tVerbose\n"
"\t-d <dev>\t\t(default " PWM_OUTPUT0_DEVICE_PATH ")\n" "\t-d <dev>\t\t(default " PWM_OUTPUT0_DEVICE_PATH ")\n"
); );
} }
@@ -123,10 +125,11 @@ pwm_main(int argc, char *argv[])
unsigned single_ch = 0; unsigned single_ch = 0;
unsigned pwm_value = 0; unsigned pwm_value = 0;
if (argc < 2) if (argc < 2) {
usage(NULL); usage(NULL);
}
while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { while ((ch = getopt(argc - 1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
switch (ch) { switch (ch) {
case 'd': case 'd':
@@ -134,6 +137,7 @@ pwm_main(int argc, char *argv[])
warnx("device %s not valid", optarg); warnx("device %s not valid", optarg);
usage(NULL); usage(NULL);
} }
dev = optarg; dev = optarg;
break; break;
@@ -147,15 +151,19 @@ pwm_main(int argc, char *argv[])
while ((single_ch = channels % 10)) { while ((single_ch = channels % 10)) {
set_mask |= 1<<(single_ch-1); set_mask |= 1 << (single_ch - 1);
channels /= 10; channels /= 10;
} }
break; break;
case 'g': case 'g':
group = strtoul(optarg, &ep, 0); group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32))
if ((*ep != '\0') || (group >= 32)) {
usage("bad channel_group value"); usage("bad channel_group value");
}
alt_channel_groups |= (1 << group); alt_channel_groups |= (1 << group);
alt_channels_set = true; alt_channels_set = true;
warnx("alt channels set, group: %d", group); warnx("alt channels set, group: %d", group);
@@ -164,25 +172,38 @@ pwm_main(int argc, char *argv[])
case 'm': case 'm':
/* Read in mask directly */ /* Read in mask directly */
set_mask = strtoul(optarg, &ep, 0); set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0')
if (*ep != '\0') {
usage("BAD set_mask VAL"); usage("BAD set_mask VAL");
}
break; break;
case 'a': case 'a':
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) { for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
set_mask |= 1<<i; set_mask |= 1 << i;
} }
break; break;
case 'p': case 'p':
pwm_value = strtoul(optarg, &ep, 0); pwm_value = strtoul(optarg, &ep, 0);
if (*ep != '\0')
if (*ep != '\0') {
usage("BAD PWM VAL"); usage("BAD PWM VAL");
}
break; break;
case 'r': case 'r':
alt_rate = strtoul(optarg, &ep, 0); alt_rate = strtoul(optarg, &ep, 0);
if (*ep != '\0')
if (*ep != '\0') {
usage("BAD rate VAL"); usage("BAD rate VAL");
}
break; break;
default: default:
break; break;
} }
@@ -191,46 +212,64 @@ pwm_main(int argc, char *argv[])
if (print_verbose && set_mask > 0) { if (print_verbose && set_mask > 0) {
warnx("Channels: "); warnx("Channels: ");
printf(" "); printf(" ");
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
if (set_mask & 1<<i) for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
printf("%d ", i+1); if (set_mask & 1 << i) {
printf("%d ", i + 1);
}
} }
printf("\n"); printf("\n");
} }
/* open for ioctl only */ /* open for ioctl only */
int fd = open(dev, 0); int fd = open(dev, 0);
if (fd < 0)
if (fd < 0) {
err(1, "can't open %s", dev); err(1, "can't open %s", dev);
}
/* get the number of servo channels */ /* get the number of servo channels */
unsigned servo_count; unsigned servo_count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_GET_COUNT"); err(1, "PWM_SERVO_GET_COUNT");
}
if (!strcmp(argv[1], "arm")) { if (!strcmp(argv[1], "arm")) {
/* tell safety that its ok to disable it with the switch */ /* tell safety that its ok to disable it with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_SET_ARM_OK"); err(1, "PWM_SERVO_SET_ARM_OK");
}
/* tell IO that the system is armed (it will output values if safety is off) */ /* tell IO that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0); ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
if (print_verbose) if (ret != OK) {
err(1, "PWM_SERVO_ARM");
}
if (print_verbose) {
warnx("Outputs armed"); warnx("Outputs armed");
}
exit(0); exit(0);
} else if (!strcmp(argv[1], "disarm")) { } else if (!strcmp(argv[1], "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */ /* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0); ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
if (print_verbose) if (ret != OK) {
err(1, "PWM_SERVO_DISARM");
}
if (print_verbose) {
warnx("Outputs disarmed"); warnx("Outputs disarmed");
}
exit(0); exit(0);
} else if (!strcmp(argv[1], "rate")) { } else if (!strcmp(argv[1], "rate")) {
@@ -238,15 +277,19 @@ pwm_main(int argc, char *argv[])
/* change alternate PWM rate */ /* change alternate PWM rate */
if (alt_rate > 0) { if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
} }
/* directly supplied channel mask */ /* directly supplied channel mask */
if (set_mask > 0) { if (set_mask > 0) {
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
} }
/* assign alternate rate to channel groups */ /* assign alternate rate to channel groups */
@@ -258,17 +301,22 @@ pwm_main(int argc, char *argv[])
uint32_t group_mask; uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
}
mask |= group_mask; mask |= group_mask;
} }
} }
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask); ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "min")) { } else if (!strcmp(argv[1], "min")) {
@@ -276,34 +324,46 @@ pwm_main(int argc, char *argv[])
if (set_mask == 0) { if (set_mask == 0) {
usage("no channels set"); usage("no channels set");
} }
if (pwm_value == 0)
if (pwm_value == 0) {
usage("no PWM value provided"); usage("no PWM value provided");
}
struct pwm_output_values pwm_values; struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values)); memset(&pwm_values, 0, sizeof(pwm_values));
pwm_values.channel_count = servo_count; pwm_values.channel_count = servo_count;
/* first get current state before modifying it */ /* first get current state before modifying it */
ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) { if (ret != OK) {
errx(ret, "failed get min values"); errx(ret, "failed get min values");
} }
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
pwm_values.values[i] = pwm_value; pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: min PWM: %d", i+1, pwm_value); if (print_verbose) {
warnx("Channel %d: min PWM: %d", i + 1, pwm_value);
}
} }
} }
if (pwm_values.channel_count == 0) { if (pwm_values.channel_count == 0) {
usage("no PWM values added"); usage("no PWM values added");
} else { } else {
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
if (ret != OK) {
errx(ret, "failed setting min values"); errx(ret, "failed setting min values");
}
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "max")) { } else if (!strcmp(argv[1], "max")) {
@@ -311,34 +371,46 @@ pwm_main(int argc, char *argv[])
if (set_mask == 0) { if (set_mask == 0) {
usage("no channels set"); usage("no channels set");
} }
if (pwm_value == 0)
if (pwm_value == 0) {
usage("no PWM value provided"); usage("no PWM value provided");
}
struct pwm_output_values pwm_values; struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values)); memset(&pwm_values, 0, sizeof(pwm_values));
pwm_values.channel_count = servo_count; pwm_values.channel_count = servo_count;
/* first get current state before modifying it */ /* first get current state before modifying it */
ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) { if (ret != OK) {
errx(ret, "failed get max values"); errx(ret, "failed get max values");
} }
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
pwm_values.values[i] = pwm_value; pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: max PWM: %d", i+1, pwm_value); if (print_verbose) {
warnx("Channel %d: max PWM: %d", i + 1, pwm_value);
}
} }
} }
if (pwm_values.channel_count == 0) { if (pwm_values.channel_count == 0) {
usage("no PWM values added"); usage("no PWM values added");
} else { } else {
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
if (ret != OK) {
errx(ret, "failed setting max values"); errx(ret, "failed setting max values");
}
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "disarmed")) { } else if (!strcmp(argv[1], "disarmed")) {
@@ -346,36 +418,46 @@ pwm_main(int argc, char *argv[])
if (set_mask == 0) { if (set_mask == 0) {
usage("no channels set"); usage("no channels set");
} }
if (pwm_value == 0)
if (pwm_value == 0) {
warnx("reading disarmed value of zero, disabling disarmed PWM"); warnx("reading disarmed value of zero, disabling disarmed PWM");
}
struct pwm_output_values pwm_values; struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values)); memset(&pwm_values, 0, sizeof(pwm_values));
pwm_values.channel_count = servo_count; pwm_values.channel_count = servo_count;
/* first get current state before modifying it */ /* first get current state before modifying it */
ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK) { if (ret != OK) {
errx(ret, "failed get disarmed values"); errx(ret, "failed get disarmed values");
} }
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
pwm_values.values[i] = pwm_value; pwm_values.values[i] = pwm_value;
if (print_verbose) { if (print_verbose) {
warnx("chan %d: disarmed PWM: %d", i+1, pwm_value); warnx("chan %d: disarmed PWM: %d", i + 1, pwm_value);
} }
} }
} }
if (pwm_values.channel_count == 0) { if (pwm_values.channel_count == 0) {
usage("no PWM values added"); usage("no PWM values added");
} else { } else {
ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK) { if (ret != OK) {
errx(ret, "failed setting disarmed values"); errx(ret, "failed setting disarmed values");
} }
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "failsafe")) { } else if (!strcmp(argv[1], "failsafe")) {
@@ -383,34 +465,46 @@ pwm_main(int argc, char *argv[])
if (set_mask == 0) { if (set_mask == 0) {
usage("no channels set"); usage("no channels set");
} }
if (pwm_value == 0)
if (pwm_value == 0) {
usage("no PWM provided"); usage("no PWM provided");
}
struct pwm_output_values pwm_values; struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values)); memset(&pwm_values, 0, sizeof(pwm_values));
pwm_values.channel_count = servo_count; pwm_values.channel_count = servo_count;
/* first get current state before modifying it */ /* first get current state before modifying it */
ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK) { if (ret != OK) {
errx(ret, "failed get failsafe values"); errx(ret, "failed get failsafe values");
} }
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
pwm_values.values[i] = pwm_value; pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value); if (print_verbose) {
warnx("Channel %d: failsafe PWM: %d", i + 1, pwm_value);
}
} }
} }
if (pwm_values.channel_count == 0) { if (pwm_values.channel_count == 0) {
usage("no PWM values added"); usage("no PWM values added");
} else { } else {
ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values); ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
if (ret != OK) {
errx(ret, "BAD input VAL"); errx(ret, "BAD input VAL");
}
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "test")) { } else if (!strcmp(argv[1], "test")) {
@@ -418,8 +512,10 @@ pwm_main(int argc, char *argv[])
if (set_mask == 0) { if (set_mask == 0) {
usage("no channels set"); usage("no channels set");
} }
if (pwm_value == 0)
if (pwm_value == 0) {
usage("no PWM provided"); usage("no PWM provided");
}
/* get current servo values */ /* get current servo values */
struct pwm_output_values last_spos; struct pwm_output_values last_spos;
@@ -428,8 +524,10 @@ pwm_main(int argc, char *argv[])
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_GET(%d)", i); err(1, "PWM_SERVO_GET(%d)", i);
}
} }
/* perform PWM output */ /* perform PWM output */
@@ -443,34 +541,43 @@ pwm_main(int argc, char *argv[])
while (1) { while (1) {
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value); ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_SET(%d)", i); err(1, "PWM_SERVO_SET(%d)", i);
}
} }
} }
/* abort on user request */ /* abort on user request */
char c; char c;
ret = poll(&fds, 1, 0); ret = poll(&fds, 1, 0);
if (ret > 0) { if (ret > 0) {
read(0, &c, 1); read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') { if (c == 0x03 || c == 0x63 || c == 'q') {
/* reset output to the last value */ /* reset output to the last value */
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]); ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i); if (ret != OK) {
} err(1, "PWM_SERVO_SET(%d)", i);
} }
}
}
warnx("User abort\n"); warnx("User abort\n");
exit(0); exit(0);
} }
} }
usleep(2000); usleep(2000);
} }
exit(0); exit(0);
@@ -486,8 +593,10 @@ pwm_main(int argc, char *argv[])
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_GET(%d)", i); err(1, "PWM_SERVO_GET(%d)", i);
}
} }
/* perform PWM output */ /* perform PWM output */
@@ -509,36 +618,41 @@ pwm_main(int argc, char *argv[])
unsigned phase_counter = 0; unsigned phase_counter = 0;
unsigned const phase_maxcount = 20; unsigned const phase_maxcount = 20;
for ( unsigned steps_timing_index = 0; for (unsigned steps_timing_index = 0;
steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]); steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
steps_timing_index++ ) { steps_timing_index++) {
warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]); warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
while (1) { while (1) {
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
unsigned val; unsigned val;
if (phase == 0) { if (phase == 0) {
val = idle; val = idle;
} else if (phase == 1) { } else if (phase == 1) {
/* ramp - depending how steep it is this ramp will look instantaneous on the output */ /* ramp - depending how steep it is this ramp will look instantaneous on the output */
val = idle + (full - idle) * (phase_maxcount / (float)phase_counter); val = idle + (full - idle) * (phase_maxcount / (float)phase_counter);
} else { } else {
val = off; val = off;
} }
ret = ioctl(fd, PWM_SERVO_SET(i), val); ret = ioctl(fd, PWM_SERVO_SET(i), val);
if (ret != OK)
if (ret != OK) {
err(1, "PWM_SERVO_SET(%d)", i); err(1, "PWM_SERVO_SET(%d)", i);
}
} }
} }
/* abort on user request */ /* abort on user request */
char c; char c;
ret = poll(&fds, 1, 0); ret = poll(&fds, 1, 0);
if (ret > 0) { if (ret > 0) {
ret = read(0, &c, 1); ret = read(0, &c, 1);
@@ -546,23 +660,29 @@ pwm_main(int argc, char *argv[])
if (ret > 0) { if (ret > 0) {
/* reset output to the last value */ /* reset output to the last value */
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) { if (set_mask & 1 << i) {
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]); ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i); if (ret != OK) {
} err(1, "PWM_SERVO_SET(%d)", i);
} }
}
}
warnx("User abort\n"); warnx("User abort\n");
exit(0); exit(0);
} }
} }
if (phase == 1) { if (phase == 1) {
usleep(steps_timings_us[steps_timing_index] / phase_maxcount); usleep(steps_timings_us[steps_timing_index] / phase_maxcount);
} else if (phase == 0) { } else if (phase == 0) {
usleep(50000); usleep(50000);
} else if (phase == 2) { } else if (phase == 2) {
usleep(50000); usleep(50000);
} else { } else {
break; break;
} }
@@ -575,6 +695,7 @@ pwm_main(int argc, char *argv[])
} }
} }
} }
exit(0); exit(0);
@@ -587,38 +708,51 @@ pwm_main(int argc, char *argv[])
uint32_t info_alt_rate_mask; uint32_t info_alt_rate_mask;
ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate); ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
} }
ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate); ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_UPDATE_RATE"); err(1, "PWM_SERVO_GET_UPDATE_RATE");
} }
ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask); ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE"); err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
} }
struct pwm_output_values failsafe_pwm; struct pwm_output_values failsafe_pwm;
struct pwm_output_values disarmed_pwm; struct pwm_output_values disarmed_pwm;
struct pwm_output_values min_pwm; struct pwm_output_values min_pwm;
struct pwm_output_values max_pwm; struct pwm_output_values max_pwm;
ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm); ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_FAILSAFE_PWM"); err(1, "PWM_SERVO_GET_FAILSAFE_PWM");
} }
ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm); ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_DISARMED_PWM"); err(1, "PWM_SERVO_GET_DISARMED_PWM");
} }
ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm); ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_MIN_PWM"); err(1, "PWM_SERVO_GET_MIN_PWM");
} }
ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm); ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_MAX_PWM"); err(1, "PWM_SERVO_GET_MAX_PWM");
} }
@@ -628,48 +762,62 @@ pwm_main(int argc, char *argv[])
servo_position_t spos; servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("channel %u: %u us", i+1, spos);
if (info_alt_rate_mask & (1<<i)) if (ret == OK) {
printf("channel %u: %u us", i + 1, spos);
if (info_alt_rate_mask & (1 << i)) {
printf(" (alternative rate: %d Hz", info_alt_rate); printf(" (alternative rate: %d Hz", info_alt_rate);
else
} else {
printf(" (default rate: %d Hz", info_default_rate); printf(" (default rate: %d Hz", info_default_rate);
}
printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)", printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)",
failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]); failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]);
printf("\n"); printf("\n");
} else { } else {
printf("%u: ERROR\n", i); printf("%u: ERROR\n", i);
} }
} }
/* print rate groups */ /* print rate groups */
for (unsigned i = 0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
uint32_t group_mask; uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
if (ret != OK)
if (ret != OK) {
break; break;
}
if (group_mask != 0) { if (group_mask != 0) {
printf("channel group %u: channels", i); printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++) for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j)) if (group_mask & (1 << j)) {
printf(" %u", j+1); printf(" %u", j + 1);
}
printf("\n"); printf("\n");
} }
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "forcefail")) { } else if (!strcmp(argv[1], "forcefail")) {
if (argc < 3) { if (argc < 3) {
errx(1, "arg missing [on|off]"); errx(1, "arg missing [on|off]");
} else { } else {
if (!strcmp(argv[2], "on")) { if (!strcmp(argv[2], "on")) {
/* force failsafe */ /* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1); ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1);
} else { } else {
/* force failsafe */ /* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0); ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0);
@@ -679,16 +827,20 @@ pwm_main(int argc, char *argv[])
warnx("FAILED setting forcefail %s", argv[2]); warnx("FAILED setting forcefail %s", argv[2]);
} }
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "terminatefail")) { } else if (!strcmp(argv[1], "terminatefail")) {
if (argc < 3) { if (argc < 3) {
errx(1, "arg missing [on|off]"); errx(1, "arg missing [on|off]");
} else { } else {
if (!strcmp(argv[2], "on")) { if (!strcmp(argv[2], "on")) {
/* force failsafe */ /* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1); ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1);
} else { } else {
/* force failsafe */ /* force failsafe */
ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0); ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0);
@@ -698,6 +850,7 @@ pwm_main(int argc, char *argv[])
warnx("FAILED setting termination failsafe %s", argv[2]); warnx("FAILED setting termination failsafe %s", argv[2]);
} }
} }
exit(0); exit(0);
} }