diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-28 19:54:00 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-28 19:54:00 +0100 |
commit | 23655675d3f71daf74e64859c8ef519e8d49eab0 (patch) | |
tree | 6a42341db0ba7fbd3777883a4ee42e77c9152fd4 /src/systemcmds/pwm | |
parent | 32e790110e766c0f6fbd5c439d15f2649df2c542 (diff) | |
download | px4-firmware-23655675d3f71daf74e64859c8ef519e8d49eab0.tar.gz px4-firmware-23655675d3f71daf74e64859c8ef519e8d49eab0.tar.bz2 px4-firmware-23655675d3f71daf74e64859c8ef519e8d49eab0.zip |
PWM command: Fix code style
Diffstat (limited to 'src/systemcmds/pwm')
-rw-r--r-- | src/systemcmds/pwm/pwm.c | 361 |
1 files changed, 257 insertions, 104 deletions
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 620b0a6f6..6bb9f235c 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -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) + + if (ret != OK) { err(1, "PWM_SERVO_ARM"); + } - if (print_verbose) + 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) + + if (ret != OK) { err(1, "PWM_SERVO_DISARM"); + } - if (print_verbose) + 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); + printf("channel %u: %u us", i + 1, spos); - if (info_alt_rate_mask & (1<<i)) + 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); } |