mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 08:04:07 +08:00
PWM command: Fix code style
This commit is contained in:
Lorenz Meier
parent
32e790110e
commit
23655675d3
@ -68,41 +68,43 @@ __EXPORT int pwm_main(int argc, char *argv[]);
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason != NULL)
|
||||
if (reason != NULL) {
|
||||
warnx("%s", reason);
|
||||
}
|
||||
|
||||
errx(1,
|
||||
"usage:\n"
|
||||
"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
|
||||
"\n"
|
||||
"arm\t\t\t\tArm output\n"
|
||||
"disarm\t\t\t\tDisarm output\n"
|
||||
"\n"
|
||||
"rate ...\t\t\tConfigure PWM rates\n"
|
||||
"\t[-g <channel group>]\t(e.g. 0,1,2)\n"
|
||||
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
|
||||
"\t[-a]\t\t\tConfigure all outputs\n"
|
||||
"\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
|
||||
"\n"
|
||||
"failsafe ...\t\t\tFailsafe PWM\n"
|
||||
"disarmed ...\t\t\tDisarmed PWM\n"
|
||||
"min ...\t\t\t\tMinimum PWM\n"
|
||||
"max ...\t\t\t\tMaximum PWM\n"
|
||||
"\t[-c <channels>]\t\t(e.g. 1234)\n"
|
||||
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
|
||||
"\t[-a]\t\t\tConfigure all outputs\n"
|
||||
"\t-p <pwm value>\t\tPWM value\n"
|
||||
"\n"
|
||||
"test ...\t\t\tDirectly set PWM\n"
|
||||
"\t[-c <channels>]\t\t(e.g. 1234)\n"
|
||||
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
|
||||
"\t[-a]\t\t\tConfigure all outputs\n"
|
||||
"\t-p <pwm value>\t\tPWM value\n"
|
||||
"\n"
|
||||
"info\t\t\t\tPrint information\n"
|
||||
"\n"
|
||||
"\t-v\t\t\tVerbose\n"
|
||||
"\t-d <dev>\t\t(default " PWM_OUTPUT0_DEVICE_PATH ")\n"
|
||||
);
|
||||
"usage:\n"
|
||||
"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
|
||||
"\n"
|
||||
"arm\t\t\t\tArm output\n"
|
||||
"disarm\t\t\t\tDisarm output\n"
|
||||
"\n"
|
||||
"rate ...\t\t\tConfigure PWM rates\n"
|
||||
"\t[-g <channel group>]\t(e.g. 0,1,2)\n"
|
||||
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
|
||||
"\t[-a]\t\t\tConfigure all outputs\n"
|
||||
"\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
|
||||
"\n"
|
||||
"failsafe ...\t\t\tFailsafe PWM\n"
|
||||
"disarmed ...\t\t\tDisarmed PWM\n"
|
||||
"min ...\t\t\t\tMinimum PWM\n"
|
||||
"max ...\t\t\t\tMaximum PWM\n"
|
||||
"\t[-c <channels>]\t\t(e.g. 1234)\n"
|
||||
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
|
||||
"\t[-a]\t\t\tConfigure all outputs\n"
|
||||
"\t-p <pwm value>\t\tPWM value\n"
|
||||
"\n"
|
||||
"test ...\t\t\tDirectly set PWM\n"
|
||||
"\t[-c <channels>]\t\t(e.g. 1234)\n"
|
||||
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
|
||||
"\t[-a]\t\t\tConfigure all outputs\n"
|
||||
"\t-p <pwm value>\t\tPWM value\n"
|
||||
"\n"
|
||||
"info\t\t\t\tPrint information\n"
|
||||
"\n"
|
||||
"\t-v\t\t\tVerbose\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 pwm_value = 0;
|
||||
|
||||
if (argc < 2)
|
||||
if (argc < 2) {
|
||||
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) {
|
||||
|
||||
case 'd':
|
||||
@ -134,6 +137,7 @@ pwm_main(int argc, char *argv[])
|
||||
warnx("device %s not valid", optarg);
|
||||
usage(NULL);
|
||||
}
|
||||
|
||||
dev = optarg;
|
||||
break;
|
||||
|
||||
@ -147,15 +151,19 @@ pwm_main(int argc, char *argv[])
|
||||
|
||||
while ((single_ch = channels % 10)) {
|
||||
|
||||
set_mask |= 1<<(single_ch-1);
|
||||
set_mask |= 1 << (single_ch - 1);
|
||||
channels /= 10;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 'g':
|
||||
group = strtoul(optarg, &ep, 0);
|
||||
if ((*ep != '\0') || (group >= 32))
|
||||
|
||||
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);
|
||||
@ -164,25 +172,38 @@ pwm_main(int argc, char *argv[])
|
||||
case 'm':
|
||||
/* Read in mask directly */
|
||||
set_mask = strtoul(optarg, &ep, 0);
|
||||
if (*ep != '\0')
|
||||
|
||||
if (*ep != '\0') {
|
||||
usage("BAD set_mask VAL");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
|
||||
set_mask |= 1<<i;
|
||||
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')
|
||||
|
||||
if (*ep != '\0') {
|
||||
usage("BAD PWM VAL");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 'r':
|
||||
alt_rate = strtoul(optarg, &ep, 0);
|
||||
if (*ep != '\0')
|
||||
|
||||
if (*ep != '\0') {
|
||||
usage("BAD rate VAL");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -191,46 +212,64 @@ pwm_main(int argc, char *argv[])
|
||||
if (print_verbose && set_mask > 0) {
|
||||
warnx("Channels: ");
|
||||
printf(" ");
|
||||
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
|
||||
if (set_mask & 1<<i)
|
||||
printf("%d ", i+1);
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
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)
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_DISARM");
|
||||
}
|
||||
|
||||
if (print_verbose) {
|
||||
warnx("Outputs disarmed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "rate")) {
|
||||
@ -238,15 +277,19 @@ pwm_main(int argc, char *argv[])
|
||||
/* change alternate PWM rate */
|
||||
if (alt_rate > 0) {
|
||||
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)");
|
||||
}
|
||||
}
|
||||
|
||||
/* directly supplied channel mask */
|
||||
if (set_mask > 0) {
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
/* assign alternate rate to channel groups */
|
||||
@ -258,17 +301,22 @@ pwm_main(int argc, char *argv[])
|
||||
uint32_t 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);
|
||||
}
|
||||
|
||||
mask |= group_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");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "min")) {
|
||||
@ -276,34 +324,46 @@ pwm_main(int argc, char *argv[])
|
||||
if (set_mask == 0) {
|
||||
usage("no channels set");
|
||||
}
|
||||
if (pwm_value == 0)
|
||||
|
||||
if (pwm_value == 0) {
|
||||
usage("no PWM value provided");
|
||||
}
|
||||
|
||||
struct pwm_output_values pwm_values;
|
||||
|
||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||
|
||||
pwm_values.channel_count = servo_count;
|
||||
|
||||
/* first get current state before modifying it */
|
||||
ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values);
|
||||
|
||||
if (ret != OK) {
|
||||
errx(ret, "failed get min values");
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<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);
|
||||
|
||||
if (print_verbose) {
|
||||
warnx("Channel %d: min PWM: %d", i + 1, pwm_value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
if (ret != OK) {
|
||||
errx(ret, "failed setting min values");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "max")) {
|
||||
@ -311,34 +371,46 @@ pwm_main(int argc, char *argv[])
|
||||
if (set_mask == 0) {
|
||||
usage("no channels set");
|
||||
}
|
||||
if (pwm_value == 0)
|
||||
|
||||
if (pwm_value == 0) {
|
||||
usage("no PWM value provided");
|
||||
}
|
||||
|
||||
struct pwm_output_values pwm_values;
|
||||
|
||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||
|
||||
pwm_values.channel_count = servo_count;
|
||||
|
||||
/* first get current state before modifying it */
|
||||
ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&pwm_values);
|
||||
|
||||
if (ret != OK) {
|
||||
errx(ret, "failed get max values");
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<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);
|
||||
|
||||
if (print_verbose) {
|
||||
warnx("Channel %d: max PWM: %d", i + 1, pwm_value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
if (ret != OK) {
|
||||
errx(ret, "failed setting max values");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "disarmed")) {
|
||||
@ -346,36 +418,46 @@ pwm_main(int argc, char *argv[])
|
||||
if (set_mask == 0) {
|
||||
usage("no channels set");
|
||||
}
|
||||
if (pwm_value == 0)
|
||||
|
||||
if (pwm_value == 0) {
|
||||
warnx("reading disarmed value of zero, disabling disarmed PWM");
|
||||
}
|
||||
|
||||
struct pwm_output_values pwm_values;
|
||||
|
||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||
|
||||
pwm_values.channel_count = servo_count;
|
||||
|
||||
/* first get current state before modifying it */
|
||||
ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values);
|
||||
|
||||
if (ret != OK) {
|
||||
errx(ret, "failed get disarmed values");
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
if (set_mask & 1 << i) {
|
||||
pwm_values.values[i] = pwm_value;
|
||||
|
||||
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) {
|
||||
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 disarmed values");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "failsafe")) {
|
||||
@ -383,34 +465,46 @@ pwm_main(int argc, char *argv[])
|
||||
if (set_mask == 0) {
|
||||
usage("no channels set");
|
||||
}
|
||||
if (pwm_value == 0)
|
||||
|
||||
if (pwm_value == 0) {
|
||||
usage("no PWM provided");
|
||||
}
|
||||
|
||||
struct pwm_output_values pwm_values;
|
||||
|
||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||
|
||||
pwm_values.channel_count = servo_count;
|
||||
|
||||
/* first get current state before modifying it */
|
||||
ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
|
||||
|
||||
if (ret != OK) {
|
||||
errx(ret, "failed get failsafe values");
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
if (set_mask & 1 << i) {
|
||||
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) {
|
||||
usage("no PWM values added");
|
||||
|
||||
} else {
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
|
||||
if (ret != OK)
|
||||
|
||||
if (ret != OK) {
|
||||
errx(ret, "BAD input VAL");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "test")) {
|
||||
@ -418,8 +512,10 @@ pwm_main(int argc, char *argv[])
|
||||
if (set_mask == 0) {
|
||||
usage("no channels set");
|
||||
}
|
||||
if (pwm_value == 0)
|
||||
|
||||
if (pwm_value == 0) {
|
||||
usage("no PWM provided");
|
||||
}
|
||||
|
||||
/* get current servo values */
|
||||
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]);
|
||||
if (ret != OK)
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET(%d)", i);
|
||||
}
|
||||
}
|
||||
|
||||
/* perform PWM output */
|
||||
@ -443,34 +541,43 @@ pwm_main(int argc, char *argv[])
|
||||
|
||||
while (1) {
|
||||
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);
|
||||
if (ret != OK)
|
||||
|
||||
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);
|
||||
read(0, &c, 1);
|
||||
|
||||
if (c == 0x03 || c == 0x63 || c == 'q') {
|
||||
/* reset output to the last value */
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
}
|
||||
if (set_mask & 1 << i) {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
warnx("User abort\n");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
usleep(2000);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
|
||||
@ -486,8 +593,10 @@ pwm_main(int argc, char *argv[])
|
||||
for (unsigned i = 0; i < servo_count; 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);
|
||||
}
|
||||
}
|
||||
|
||||
/* perform PWM output */
|
||||
@ -509,36 +618,41 @@ pwm_main(int argc, char *argv[])
|
||||
unsigned phase_counter = 0;
|
||||
unsigned const phase_maxcount = 20;
|
||||
|
||||
for ( unsigned steps_timing_index = 0;
|
||||
steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
|
||||
steps_timing_index++ ) {
|
||||
for (unsigned steps_timing_index = 0;
|
||||
steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
|
||||
steps_timing_index++) {
|
||||
|
||||
warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
|
||||
|
||||
while (1) {
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
if (set_mask & 1 << i) {
|
||||
|
||||
unsigned val;
|
||||
|
||||
if (phase == 0) {
|
||||
val = idle;
|
||||
|
||||
} else if (phase == 1) {
|
||||
/* ramp - depending how steep it is this ramp will look instantaneous on the output */
|
||||
val = idle + (full - idle) * (phase_maxcount / (float)phase_counter);
|
||||
|
||||
} else {
|
||||
val = off;
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), val);
|
||||
if (ret != OK)
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* abort on user request */
|
||||
char c;
|
||||
ret = poll(&fds, 1, 0);
|
||||
|
||||
if (ret > 0) {
|
||||
|
||||
ret = read(0, &c, 1);
|
||||
@ -546,23 +660,29 @@ pwm_main(int argc, char *argv[])
|
||||
if (ret > 0) {
|
||||
/* reset output to the last value */
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
}
|
||||
if (set_mask & 1 << i) {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
warnx("User abort\n");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (phase == 1) {
|
||||
usleep(steps_timings_us[steps_timing_index] / phase_maxcount);
|
||||
|
||||
} else if (phase == 0) {
|
||||
usleep(50000);
|
||||
|
||||
} else if (phase == 2) {
|
||||
usleep(50000);
|
||||
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
@ -575,6 +695,7 @@ pwm_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
|
||||
@ -587,38 +708,51 @@ pwm_main(int argc, char *argv[])
|
||||
uint32_t info_alt_rate_mask;
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET_UPDATE_RATE");
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
|
||||
}
|
||||
|
||||
struct pwm_output_values failsafe_pwm;
|
||||
|
||||
struct pwm_output_values disarmed_pwm;
|
||||
|
||||
struct pwm_output_values min_pwm;
|
||||
|
||||
struct pwm_output_values max_pwm;
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET_FAILSAFE_PWM");
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET_DISARMED_PWM");
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET_MIN_PWM");
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm);
|
||||
|
||||
if (ret != OK) {
|
||||
err(1, "PWM_SERVO_GET_MAX_PWM");
|
||||
}
|
||||
@ -628,48 +762,62 @@ pwm_main(int argc, char *argv[])
|
||||
servo_position_t 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);
|
||||
else
|
||||
|
||||
} else {
|
||||
printf(" (default rate: %d Hz", info_default_rate);
|
||||
}
|
||||
|
||||
|
||||
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");
|
||||
|
||||
} else {
|
||||
printf("%u: ERROR\n", i);
|
||||
}
|
||||
}
|
||||
|
||||
/* print rate groups */
|
||||
for (unsigned i = 0; i < servo_count; i++) {
|
||||
uint32_t group_mask;
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
|
||||
if (ret != OK)
|
||||
|
||||
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+1);
|
||||
if (group_mask & (1 << j)) {
|
||||
printf(" %u", j + 1);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "forcefail")) {
|
||||
|
||||
if (argc < 3) {
|
||||
errx(1, "arg missing [on|off]");
|
||||
|
||||
} else {
|
||||
|
||||
if (!strcmp(argv[2], "on")) {
|
||||
/* force failsafe */
|
||||
ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1);
|
||||
|
||||
} else {
|
||||
/* force failsafe */
|
||||
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]);
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "terminatefail")) {
|
||||
|
||||
if (argc < 3) {
|
||||
errx(1, "arg missing [on|off]");
|
||||
|
||||
} else {
|
||||
|
||||
if (!strcmp(argv[2], "on")) {
|
||||
/* force failsafe */
|
||||
ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1);
|
||||
|
||||
} else {
|
||||
/* force failsafe */
|
||||
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]);
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user