static int pwm_servo_ioctl(struct file *filep, int cmd, unsigned long arg) { /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: pwm_servos_arm(true); return 0; case PWM_SERVO_DISARM: pwm_servos_arm(false); return 0; } /* channel set? */ if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PWM_SERVO_MAX_CHANNELS))) { /* XXX sanity-check value? */ pwm_channel_set(cmd - PWM_SERVO_SET(0), (servo_position_t)arg); return 0; } /* channel get? */ if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PWM_SERVO_MAX_CHANNELS))) { /* XXX sanity-check value? */ *(servo_position_t *)arg = pwm_channel_get(cmd - PWM_SERVO_GET(0)); return 0; } /* not a recognised value */ return -ENOTTY; }
int PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) { int ret = -ENOTTY; lock(); /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: _next_command.arm_ok = true; ret = 0; break; case PWM_SERVO_DISARM: _next_command.arm_ok = false; ret = 0; break; default: /* channel set? */ if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PX4IO_OUTPUT_CHANNELS))) { /* XXX sanity-check value? */ _next_command.servo_command[cmd - PWM_SERVO_SET(0)] = arg; ret = 0; break; } /* channel get? */ if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) { int channel = cmd - PWM_SERVO_GET(0); /* currently no data for this channel */ if (channel >= _rc_channel_count) { ret = -ERANGE; break; } *(servo_position_t *)arg = _rc_channel[channel]; ret = 0; break; } /* not a recognised value */ ret = -ENOTTY; } unlock(); return ret; }
int test_servo(int argc, char *argv[]) { int fd, result; servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); if (fd < 0) { printf("failed opening /dev/pwm_servo\n"); goto out; } result = read(fd, &data, sizeof(data)); if (result != sizeof(data)) { printf("failed bulk-reading channel values\n"); goto out; } printf("Servo readback, pairs of values should match defaults\n"); unsigned servo_count; result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); if (result != OK) { warnx("PWM_SERVO_GET_COUNT"); return ERROR; } for (unsigned i = 0; i < servo_count; i++) { result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos); if (result < 0) { printf("failed reading channel %u\n", i); goto out; } printf("%u: %u %u\n", i, pos, data[i]); } /* tell safety that its ok to disable it with the switch */ result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (result != OK) warnx("FAIL: PWM_SERVO_SET_ARM_OK"); /* tell output device that the system is armed (it will output values if safety is off) */ result = ioctl(fd, PWM_SERVO_ARM, 0); if (result != OK) warnx("FAIL: PWM_SERVO_ARM"); usleep(5000000); printf("Advancing channel 0 to 1500\n"); result = ioctl(fd, PWM_SERVO_SET(0), 1500); printf("Advancing channel 1 to 1800\n"); result = ioctl(fd, PWM_SERVO_SET(1), 1800); out: return 0; }
int test_servo(int argc, char *argv[]) { int fd, result; servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); if (fd < 0) { printf("failed opening /dev/pwm_servo\n"); goto out; } result = read(fd, &data, sizeof(data)); if (result != sizeof(data)) { printf("failed bulk-reading channel values\n"); goto out; } printf("Servo readback, pairs of values should match defaults\n"); for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos); if (result < 0) { printf("failed reading channel %u\n", i); goto out; } printf("%u: %u %u\n", i, pos, data[i]); } printf("Servos arming at default values\n"); result = ioctl(fd, PWM_SERVO_ARM, 0); usleep(5000000); printf("Advancing channel 0 to 1500\n"); result = ioctl(fd, PWM_SERVO_SET(0), 1500); out: return 0; }
int test_ppm_loopback(int argc, char *argv[]) { int _rc_sub = orb_subscribe(ORB_ID(input_rc)); int servo_fd, result; servo_position_t pos; servo_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR); if (servo_fd < 0) { printf("failed opening /dev/pwm_servo\n"); } printf("Servo readback, pairs of values should match defaults\n"); unsigned servo_count; result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); if (result != OK) { warnx("PWM_SERVO_GET_COUNT"); (void)close(servo_fd); return ERROR; } for (unsigned i = 0; i < servo_count; i++) { result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos); if (result < 0) { printf("failed reading channel %u\n", i); } //printf("%u: %u %u\n", i, pos, data[i]); } // /* tell safety that its ok to disable it with the switch */ // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_SET_ARM_OK"); // tell output device that the system is armed (it will output values if safety is off) // result = ioctl(servo_fd, PWM_SERVO_ARM, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_ARM"); int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); if (result) { (void)close(servo_fd); return ERROR; } else { warnx("channel %d set to %d", i, pwm_values[i]); } } warnx("servo count: %d", servo_count); struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0}; for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { pwm_out.values[i] = pwm_values[i]; //warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]); pwm_out.channel_count++; } result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out); /* give driver 10 ms to propagate */ /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct input_rc_s rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); usleep(100000); /* open PPM input and expect values close to the output values */ bool rc_updated; orb_check(_rc_sub, &rc_updated); if (rc_updated) { orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); // int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); // struct input_rc_s rc; // result = read(ppm_fd, &rc, sizeof(rc)); // if (result != sizeof(rc)) { // warnx("Error reading RC output"); // (void)close(servo_fd); // (void)close(ppm_fd); // return ERROR; // } /* go and check values */ for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { if (abs(rc_input.values[i] - pwm_values[i]) > 10) { warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]); (void)close(servo_fd); return ERROR; } } } else { warnx("failed reading RC input data"); (void)close(servo_fd); return ERROR; } close(servo_fd); warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!"); return 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 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 FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = OK; int channel; switch (cmd) { case PWM_SERVO_ARM: up_pwm_servo_arm(true); break; case PWM_SERVO_DISARM: up_pwm_servo_arm(false); break; case PWM_SERVO_SET(2): case PWM_SERVO_SET(3): if (_mode != MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): if (arg < 2100) { channel = cmd - PWM_SERVO_SET(0); up_pwm_servo_set(channel, arg); } else { ret = -EINVAL; } break; case PWM_SERVO_GET(2): case PWM_SERVO_GET(3): if (_mode != MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): { channel = cmd - PWM_SERVO_SET(0); *(servo_position_t *)arg = up_pwm_servo_get(channel); break; } case MIXERIOCGETOUTPUTCOUNT: if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; } else { *(unsigned *)arg = 2; } break; case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; } break; case MIXERIOCADDSIMPLE: { mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo); if (mixer->check()) { delete mixer; ret = -EINVAL; } else { if (_mixers == nullptr) _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); _mixers->add_mixer(mixer); } break; } case MIXERIOCADDMULTIROTOR: /* XXX not yet supported */ ret = -ENOTTY; break; case MIXERIOCLOADFILE: { const char *path = (const char *)arg; if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; } _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); if (_mixers->load_from_file(path) != 0) { delete _mixers; _mixers = nullptr; ret = -EINVAL; } break; } default: ret = -ENOTTY; break; } 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); }
int ROV_main(int argc, char *argv[]) { /* Initialize actuator struct and set it to zero*/ struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); /* Initialize armed struct and set ini values*/ struct actuator_armed_s armed; armed.armed = true; armed.lockdown = false; /* lock down actuators if required, only in HIL */ /* Advertise actuator and armed topics and publish ini values*/ orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); /* handlers for sensor and attitude (EKF) subscriptions */ int _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); int _bat_stat_sub = orb_subscribe(ORB_ID(battery_status)); orb_set_interval(sensor_sub_fd, 1000); /* Initialize aux variables and abs time variable */ int i ; int ret; char c_key; float rcvalue = 0.0f; hrt_abstime stime; //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct sensor_combined_s raw; /**< sensor values */ struct battery_status_s _bat_stat; /**< battery status */ /* Key pressed event */ struct pollfd fds; fds.fd = 0; /* stdin */ fds.events = POLLIN; // controller tuning parameters float k_ax = 0.1f; // gain for uprighting moment (pitch) float k_ay = 0.2f; // gain for roll compensator float c_ax = 0.5f; // gain for inverse influence on thrust and yaw when vehicle is not aligned in plane (x-axis) float c_ay = c_ax; // gain for inverse influence on thrust and yaw when vehicle is not aligned in plane (y-axis) float k_omz = 1.0f; // yaw rate gain float omz_set = 0.0f; // setpoint yaw rate float k_thrust = 1.0f; // forward thrust gain float thrust_set = 0.0f; // thrust setpoint /* Open PWM device driver*/ int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0); /* Print to console */ printf("Start. Press g after ESCs ready. \n"); /* Main loop */ while (true) { /* start main loop by publishing zero values to actuators to start ESCs */ /* Publish zero */ for (i=0; i<4; i++){ actuators.control[i] = rcvalue; actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); } usleep(10000); /* Check whether key pressed */ ret = poll(&fds, 1, 0); if (ret > 0) { /* If key pressed event occured */ read(0, &c_key, 1); if (c_key == 0x67) { // If "g" key pressed go into key control loop warnx("Start\n"); stime = hrt_absolute_time(); /* Keyboard control. Can be aborted with "c" key */ while (true) { for (unsigned k = 0; k < 4; k++) { actuators.control[k] = 0.0f; } usleep(20000); ret = poll(&fds, 1, 0); fflush(stdin); if (ret > 0) { // If key pressed event occured read(0,&c_key,1); fflush(stdin); switch (c_key){ //User abort case 0x63: //c warnx("User abort\n"); //Stop servos for (i=0; i<4; i++){ actuators.control[i] = 0.0f; } actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); usleep(10000); exit(0); //return break; // Emergency stop case 0x20: // space actuators.control[0] = 0.0f; actuators.control[1] = 0.0f; actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; break; // roll case 0x71: // q actuators.control[0] = -1.0f; break; case 0x65: // e actuators.control[0] = +1.0f; break; // pitch case 0x77: // w actuators.control[1] = -1.0f; break; case 0x73: // s actuators.control[1] = +1.0f; break; // yaw case 0x61: // a actuators.control[2] = -1.0f; break; case 0x64: // d actuators.control[2] = +1.0f; break; // Acc case 0x72: // r actuators.control[3] = +1.0f; break; // Rev. Acc case 0x66: // f actuators.control[3] = -1.0f; break; // gyro & acc stabilized mode // keyboard control centered around j - neutral h,g left, k,l right, one/two row up forward, one row down backward default: switch (c_key){ // neutral position stabilizing case 0x6A: // j thrust_set = 0.0f; omz_set = 0.0f; break; // slow left case 0x68: // h thrust_set = 0.0f; omz_set = -0.4f; break; // hard left case 0x67: // g thrust_set = 0.0f; omz_set = -1.0f; break; // slow right case 0x6B: // k thrust_set = 0.0f; omz_set = 0.4f; break; // hard right case 0x6C: // l thrust_set = 0.0f; omz_set = 1.0f; break; // forward case 0x75: // u thrust_set = 0.3f; omz_set = 0.0f; break; // forward slow left case 0x7A: // z thrust_set = 0.3f; omz_set = -0.4f; break; // forward hard left case 0x74: // t thrust_set = 0.3f; omz_set = -1.0f; break; // forward slow right case 0x69: // i thrust_set = 0.3f; omz_set = 0.4f; break; // forward hard right case 0x6F: // o thrust_set = 0.3f; omz_set = 1.0f; break; // backward case 0x6D: // m thrust_set = -0.3f; omz_set = 0.0f; break; // backward slow left case 0x6E: // n thrust_set = -0.3f; omz_set = -0.4f; break; // backward hard left case 0x62: // b thrust_set = -0.3f; omz_set = -1.0f; break; // backward slow right case 0x2C: // , thrust_set = -0.3f; omz_set = 0.4f; break; // backward hard right case 0x2E: // . thrust_set = -0.3f; omz_set = 1.0f; break; case 0x55: // U thrust gain +.1 k_thrust = k_thrust + 0.1f; printf("thrust gain +.1 - k_thrust = %8.4f",(double)k_thrust); break; case 0x4D: // M thrust gain -.1 k_thrust = k_thrust - 0.1f; printf("thrust gain -.1 - k_thrust = %8.4f",(double)k_thrust); break; case 0x4B: // K yaw rate gain +.1 k_omz = k_omz + 0.1f; printf("yaw rate gain +.1 - k_omz = %8.4f",(double)k_omz); break; case 0x48: // H yaw rate gain -.1 k_omz = k_omz - 0.1f; printf("yaw rate gain -.1 - k_omz = %8.4f",(double)k_omz); break; case 0x5A: // Z pitch stabilizer gain +.1 k_ax = k_ax + 0.1f; printf("pitch stabilizer gain +.1 - k_ax = %8.4f",(double)k_ax); break; case 0x4E: // N pitch stabilizer gain -.1 k_ax = k_ax - 0.1f; printf("pitch stabilizer gain -.1 - k_ax = %8.4f",(double)k_ax); break; case 0x54: // T roll stabilizer gain +.1 k_ay = k_ay + 0.1f; printf("roll stabilizer gain +.1 - k_ay = %8.4f",(double)k_ay); break; case 0x42: // B roll stabilizer gain -.1 k_ay = k_ay - 0.1f; printf("roll stabilizer gain -.1 - k_ay = %8.4f",(double)k_ay); break; case 0x49: // I pitch stabilizer dominance +.025 c_ax = c_ax + 0.025f; printf("pitch stabilizer dominance +.025 - c_ax = %8.4f",(double)c_ax); break; case 0x3B: // ; pitch stabilizer dominance -.025 c_ax = c_ax - 0.025f; printf("pitch stabilizer dominance -.025 - c_ax = %8.4f",(double)c_ax); break; case 0x4F: // O roll stabilizer dominance +.1 c_ay = c_ay + 0.1f; printf("roll stabilizer dominance +.1 - c_ay = %8.4f",(double)c_ay); break; case 0x3A: // : roll stabilizer dominance -.1 c_ay = c_ay - 0.1f; printf("roll stabilizer dominance -.1 - c_ay = %8.4f",(double)c_ay); break; default: printf("Unidentified key pressed: %c",c_key); thrust_set = 0.0f; omz_set = 0.0f; actuators.control[0] = 0.0f; actuators.control[1] = 0.0f; actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; break; } // gyro controlled // copy sensors raw data into local buffer orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); // roll moment when y-axis is not horizontal actuators.control[0] = k_ay * raw.accelerometer_m_s2[1]; // pitch moment when x-axis is not horizontal actuators.control[1] = k_ax * raw.accelerometer_m_s2[0]; // yaw moment when omz not omz_set and y and x axes are in horizontal plane actuators.control[2] = k_omz * (omz_set - raw.gyro1_rad_s[2]) /(1+abs(c_ax*raw.accelerometer_m_s2[0])+abs(c_ay*raw.accelerometer_m_s2[1])); // forward thrust when nose is directed horizontally actuators.control[3] = k_thrust * thrust_set /(1+abs(c_ax*raw.accelerometer_m_s2[0])+abs(c_ay*raw.accelerometer_m_s2[1])); break; // default /* // Emergency stop case 0x70: // p actuators.control[0] = 0.0f; actuators.control[1] = 0.0f; actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; break;*/ } } /* sanity check and publish actuator outputs */ if (isfinite(actuators.control[0]) && isfinite(actuators.control[1]) && isfinite(actuators.control[2]) && isfinite(actuators.control[3])) { actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); } /* copy attitude topic which is produced by attitude estimator */ orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); /* copy battery status into local buffer */ orb_copy(ORB_ID(battery_status), _bat_stat_sub, &_bat_stat); /* Plot to terminal */ if (hrt_absolute_time() - stime > 500000){ /* Plot pwm values */ for (unsigned k = 0; k < 4; k++) { servo_position_t spos; ret = ioctl(fd, PWM_SERVO_GET(k), (unsigned long)&spos); if (ret == OK) { printf("pwm channel %u: %u us\n", k+1, spos); } } /* Print sensor & EKF values */ // printf("Snrs:\n Pres:\t%8.4f\n", // (double)raw.baro_pres_mbar); // // printf("Temp:\t%8.4f\n", // (double)raw.baro_temp_celcius); // printf("Ang:\t%8.4f\t%8.4f\t%8.4f\n", (double)_v_att.roll, (double)_v_att.pitch, (double)_v_att.yaw); printf("Vel:\t%8.4f\t%8.4f\t%8.4f\n", (double)_v_att.rollspeed, (double)_v_att.pitchspeed, (double)_v_att.yawspeed); printf("Acc:\t%8.4f\t%8.4f\t%8.4f\n \n", (double)_v_att.rollacc, (double)_v_att.pitchacc, (double)_v_att.yawacc); printf("ADC 10:\t%8.4f\n", (double)raw.adc_voltage_v[6]); printf("BAT:\t%8.4f\n \n", (double)_bat_stat.voltage_filtered_v); stime = hrt_absolute_time(); } fflush( stdout ); usleep(10000); } } } } return OK; }