int pwm_main(int argc, char *argv[]) { const char *dev = PWM_OUTPUT_DEVICE_PATH; unsigned alt_rate = 0; uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_verbose = false; int ch; int ret; char *ep; 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-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { case 'd': if (NULL == strstr(optarg, "/dev/")) { warnx("device %s not valid", optarg); usage(NULL); } dev = optarg; break; case 'v': print_verbose = true; break; case 'c': /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ channels = strtoul(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; warnx("alt channels set, group: %d", group); break; case 'm': /* Read in mask directly */ set_mask = strtoul(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 = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad PWM value provided"); break; case 'r': alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') usage("bad alternative rate provided"); break; default: break; } } 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"); 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) 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) 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) warnx("Outputs disarmed"); exit(0); } else if (!strcmp(argv[1], "rate")) { /* 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)"); } /* directly supplied channel mask */ if (set_mask > 0) { ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); if (ret != OK) err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); } /* assign alternate rate to channel groups */ if (alt_channels_set) { uint32_t mask = 0; for (group = 0; group < 32; group++) { if ((1 << group) & alt_channel_groups) { uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); 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) err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); } exit(0); } else if (!strcmp(argv[1], "min")) { if (set_mask == 0) { usage("no channels set"); } if (pwm_value == 0) usage("no PWM value provided"); 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: min PWM: %d", i+1, pwm_value); } pwm_values.channel_count++; } 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 min values"); } exit(0); } else if (!strcmp(argv[1], "max")) {
int pwm_main(int argc, char *argv[]) { const char *dev = PWM_OUTPUT_DEVICE_PATH; unsigned alt_rate = 0; uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_info = false; int ch; int ret; char *ep; unsigned group; int32_t set_mask = -1; if (argc < 2) usage(NULL); while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) { 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': dev = optarg; break; case 'u': alt_rate = strtol(optarg, &ep, 0); if (*ep != '\0') usage("bad alt_rate value"); break; case 'm': set_mask = strtol(optarg, &ep, 0); if (*ep != '\0') usage("bad set_mask value"); break; case 'v': print_info = true; break; default: usage(NULL); } } argc -= optind; argv += optind; /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) err(1, "can't open %s", dev); /* 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)"); } /* 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"); } /* assign alternate rate to channel groups */ if (alt_channels_set) { uint32_t mask = 0; for (unsigned group = 0; group < 32; group++) { if ((1 << group) & alt_channel_groups) { uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); if (ret != OK) err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); mask |= group_mask; } } ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); if (ret != OK) err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); } /* iterate remaining arguments */ unsigned channel = 0; while (argc--) { const char *arg = argv[0]; argv++; if (!strcmp(arg, "arm")) { ret = ioctl(fd, PWM_SERVO_ARM, 0); if (ret != OK) err(1, "PWM_SERVO_ARM"); continue; } if (!strcmp(arg, "disarm")) { 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') { ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); if (ret != OK) err(1, "PWM_SERVO_SET(%d)", channel); channel++; continue; } usage("unrecognised option"); } /* print verbose info */ if (print_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"); /* print current servo values */ for (unsigned i = 0; i < count; i++) { servo_position_t spos; ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); if (ret == OK) { printf("channel %u: %uus\n", i, spos); } else { printf("%u: ERROR\n", i); } } /* print rate groups */ for (unsigned i = 0; i < count; i++) { uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); 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); printf("\n"); } } fflush(stdout); } exit(0); }
int PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; lock(); switch (cmd) { case PWM_SERVO_ARM: up_pwm_servo_arm(true); break; case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: case PWM_SERVO_SET_FORCE_SAFETY_OFF: case PWM_SERVO_SET_FORCE_SAFETY_ON: // these are no-ops, as no safety switch break; case PWM_SERVO_DISARM: up_pwm_servo_arm(false); break; case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: *(uint32_t *)arg = _pwm_default_rate; break; case PWM_SERVO_SET_UPDATE_RATE: ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); break; case PWM_SERVO_GET_UPDATE_RATE: *(uint32_t *)arg = _pwm_alt_rate; break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; case PWM_SERVO_GET_SELECT_UPDATE_RATE: *(uint32_t *)arg = _pwm_alt_rate_channels; break; case PWM_SERVO_SET_FAILSAFE_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ if (pwm->channel_count > _max_actuators) { ret = -EINVAL; break; } for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MAX) { _failsafe_pwm[i] = PWM_HIGHEST_MAX; } else if (pwm->values[i] < PWM_LOWEST_MIN) { _failsafe_pwm[i] = PWM_LOWEST_MIN; } else { _failsafe_pwm[i] = pwm->values[i]; } } /* * update the counter * this is needed to decide if disarmed PWM output should be turned on or not */ _num_failsafe_set = 0; for (unsigned i = 0; i < _max_actuators; i++) { if (_failsafe_pwm[i] > 0) _num_failsafe_set++; } break; } case PWM_SERVO_GET_FAILSAFE_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < _max_actuators; i++) { pwm->values[i] = _failsafe_pwm[i]; } pwm->channel_count = _max_actuators; break; } case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ if (pwm->channel_count > _max_actuators) { ret = -EINVAL; break; } for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MAX) { _disarmed_pwm[i] = PWM_HIGHEST_MAX; } else if (pwm->values[i] < PWM_LOWEST_MIN) { _disarmed_pwm[i] = PWM_LOWEST_MIN; } else { _disarmed_pwm[i] = pwm->values[i]; } } /* * update the counter * this is needed to decide if disarmed PWM output should be turned on or not */ _num_disarmed_set = 0; for (unsigned i = 0; i < _max_actuators; i++) { if (_disarmed_pwm[i] > 0) _num_disarmed_set++; } break; } case PWM_SERVO_GET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < _max_actuators; i++) { pwm->values[i] = _disarmed_pwm[i]; } pwm->channel_count = _max_actuators; break; } case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ if (pwm->channel_count > _max_actuators) { ret = -EINVAL; break; } for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MIN) { _min_pwm[i] = PWM_HIGHEST_MIN; } else if (pwm->values[i] < PWM_LOWEST_MIN) { _min_pwm[i] = PWM_LOWEST_MIN; } else { _min_pwm[i] = pwm->values[i]; } } break; } case PWM_SERVO_GET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < _max_actuators; i++) { pwm->values[i] = _min_pwm[i]; } pwm->channel_count = _max_actuators; arg = (unsigned long)&pwm; break; } case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ if (pwm->channel_count > _max_actuators) { ret = -EINVAL; break; } for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] < PWM_LOWEST_MAX) { _max_pwm[i] = PWM_LOWEST_MAX; } else if (pwm->values[i] > PWM_HIGHEST_MAX) { _max_pwm[i] = PWM_HIGHEST_MAX; } else { _max_pwm[i] = pwm->values[i]; } } break; } case PWM_SERVO_GET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < _max_actuators; i++) { pwm->values[i] = _max_pwm[i]; } pwm->channel_count = _max_actuators; arg = (unsigned long)&pwm; break; } #ifdef CONFIG_ARCH_BOARD_AEROCORE case PWM_SERVO_SET(7): case PWM_SERVO_SET(6): if (_mode < MODE_8PWM) { ret = -EINVAL; break; } #endif case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ case PWM_SERVO_SET(3): case PWM_SERVO_SET(2): if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): if (arg <= 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { ret = -EINVAL; } break; #ifdef CONFIG_ARCH_BOARD_AEROCORE case PWM_SERVO_GET(7): case PWM_SERVO_GET(6): if (_mode < MODE_8PWM) { ret = -EINVAL; break; } #endif case PWM_SERVO_GET(5): case PWM_SERVO_GET(4): if (_mode < MODE_6PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ case PWM_SERVO_GET(3): case PWM_SERVO_GET(2): if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); break; case PWM_SERVO_GET_RATEGROUP(0): case PWM_SERVO_GET_RATEGROUP(1): case PWM_SERVO_GET_RATEGROUP(2): case PWM_SERVO_GET_RATEGROUP(3): case PWM_SERVO_GET_RATEGROUP(4): case PWM_SERVO_GET_RATEGROUP(5): #ifdef CONFIG_ARCH_BOARD_AEROCORE case PWM_SERVO_GET_RATEGROUP(6): case PWM_SERVO_GET_RATEGROUP(7): #endif *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { #ifdef CONFIG_ARCH_BOARD_AEROCORE case MODE_8PWM: *(unsigned *)arg = 8; break; #endif case MODE_6PWM: *(unsigned *)arg = 6; break; case MODE_4PWM: *(unsigned *)arg = 4; break; case MODE_2PWM: *(unsigned *)arg = 2; break; default: ret = -EINVAL; break; } break; case PWM_SERVO_SET_COUNT: { /* change the number of outputs that are enabled for * PWM. This is used to change the split between GPIO * and PWM under control of the flight config * parameters. Note that this does not allow for * changing a set of pins to be used for serial on * FMUv1 */ switch (arg) { case 0: set_mode(MODE_NONE); break; case 2: set_mode(MODE_2PWM); break; case 4: set_mode(MODE_4PWM); break; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) case 6: set_mode(MODE_6PWM); break; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) case 8: set_mode(MODE_8PWM); break; #endif default: ret = -EINVAL; break; } break; } case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; _groups_required = 0; } break; case MIXERIOCADDSIMPLE: { mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback, (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; _groups_required = 0; ret = -EINVAL; } else { if (_mixers == nullptr) _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); _mixers->add_mixer(mixer); _mixers->groups_required(_groups_required); } break; } case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); if (_mixers == nullptr) { _groups_required = 0; ret = -ENOMEM; } else { ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { debug("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; ret = -EINVAL; } else { _mixers->groups_required(_groups_required); } } break; } default: ret = -ENOTTY; break; } unlock(); return ret; }
int pwm_main(int argc, char *argv[]) { const char *dev = PWM_OUTPUT_DEVICE_PATH; unsigned alt_rate = 0; uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_info = false; int ch; int ret; char *ep; unsigned group; int32_t set_mask = -1; if (argc < 2) usage(NULL); while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) { 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': dev = optarg; break; case 'u': alt_rate = strtol(optarg, &ep, 0); if (*ep != '\0') usage("bad alt_rate value"); break; case 'm': set_mask = strtol(optarg, &ep, 0); if (*ep != '\0') usage("bad set_mask value"); break; case 'v': print_info = true; break; default: usage(NULL); } } argc -= optind; argv += optind; /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) err(1, "can't open %s", dev); /* 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)"); } /* 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"); } /* assign alternate rate to channel groups */ if (alt_channels_set) { uint32_t mask = 0; for (unsigned group = 0; group < 32; group++) { if ((1 << group) & alt_channel_groups) { uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); if (ret != OK) err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); mask |= group_mask; } } ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); if (ret != OK) 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; nchannels++; continue; } usage("unrecognized option"); } /* print verbose info */ if (print_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"); /* print current servo values */ for (unsigned i = 0; i < count; i++) { servo_position_t spos; ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); if (ret == OK) { printf("channel %u: %uus\n", i, spos); } else { printf("%u: ERROR\n", i); } } /* print rate groups */ for (unsigned i = 0; i < count; i++) { uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); 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); printf("\n"); } } fflush(stdout); } /* 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); } } /* rate limit to ~ 20 Hz */ usleep(50000); } } exit(0); }