void set_pwm_output(mavlink_actuator_control_target_t *actuator_controls) { if (actuator_controls == nullptr) { // Without valid argument, set all channels to 0 for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { int ret = ::ioctl(_pwm_fd, PWM_SERVO_SET(i), 0); if (ret != OK) { PX4_ERR("PWM_SERVO_SET(%d)", i); } } } else { for (unsigned i = 0; i < sizeof(actuator_controls->controls) / sizeof(actuator_controls->controls[0]); i++) { if (!isnan(actuator_controls->controls[i])) { long unsigned pwm = actuator_controls->controls[i]; int ret = ::ioctl(_pwm_fd, PWM_SERVO_SET(i), pwm); if (ret != OK) { PX4_ERR("PWM_SERVO_SET(%d)", i); } } } } }
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 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 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; }
// inizializzazione servo void unibo_motor_output_init() { int ret = 0; pwm_fd = open("/dev/pwm_output", O_RDWR); if (pwm_fd < 0) { warnx("cannot open fd\n"); exit(1); } // eventuale impostazione PWM_SERVO_SET_UPDATE_RATE, vedi pwm.c ret = ioctl(pwm_fd, PWM_SERVO_ARM, 0); if (ret != OK) { warnx("errore arm\n"); exit(1); } int i; for(i = MOTORS_START; i < MOTORS_NUMBER; i++) { ioctl(pwm_fd, PWM_SERVO_SET(i), 900); } }
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; }
static void mixer_tick(void *arg) { uint16_t *control_values; int control_count; int i; bool should_arm; /* * Start by looking for R/C control inputs. * This updates system_state with any control inputs received. */ mixer_get_rc_input(); /* * Decide which set of inputs we're using. */ if (system_state.mixer_use_fmu) { /* we have recent control data from the FMU */ control_count = PX4IO_OUTPUT_CHANNELS; control_values = &system_state.fmu_channel_data[0]; /* check that we are receiving fresh data from the FMU */ if (!system_state.fmu_data_received) { fmu_input_drops++; /* too many frames without FMU input, time to go to failsafe */ if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { system_state.mixer_use_fmu = false; } } else { fmu_input_drops = 0; system_state.fmu_data_received = false; } } else if (system_state.rc_channels > 0) { /* we have control data from an R/C input */ control_count = system_state.rc_channels; control_values = &system_state.rc_channel_data[0]; } else { /* we have no control input */ control_count = 0; } /* * Tickle each mixer, if we have control data. */ if (control_count > 0) { for (i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { mixer_update(i, control_values, control_count); /* * If we are armed, update the servo output. */ if (system_state.armed) ioctl(mixer_servo_fd, PWM_SERVO_SET(i), mixers[i].current_value); } } /* * Decide whether the servos should be armed right now. */ should_arm = system_state.armed && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ ioctl(mixer_servo_fd, PWM_SERVO_ARM, 0); mixer_servos_armed = true; } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm*/ ioctl(mixer_servo_fd, PWM_SERVO_DISARM, 0); mixer_servos_armed = false; } }
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 esc_calib_main(int argc, char *argv[]) { const char *dev = PWM_OUTPUT_DEVICE_PATH; char *ep; bool channels_selected[MAX_CHANNELS] = {false}; int ch; int ret; char c; if (argc < 2) usage(NULL); while ((ch = getopt(argc, argv, "d:")) != EOF) { switch (ch) { case 'd': dev = optarg; argc--; break; default: usage(NULL); } } if(argc < 1) { usage("no channels provided"); } while (--argc) { const char *arg = argv[argc]; unsigned channel_number = strtol(arg, &ep, 0); if (*ep == '\0') { if (channel_number > MAX_CHANNELS || channel_number <= 0) { err(1, "invalid channel number: %d", channel_number); } else { channels_selected[channel_number-1] = true; } } } /* Wait for confirmation */ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); if (!console) err(1, "failed opening console"); warnx("ATTENTION, please remove or fix props before starting calibration!\n" "\n" "Also press the arming switch first for safety off\n" "\n" "Do you really want to start calibration: y or n?\n"); /* wait for user input */ while (1) { if (read(console, &c, 1) == 1) { if (c == 'y' || c == 'Y') { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); close(console); exit(0); } else if (c == 'n' || c == 'N') { warnx("ESC calibration aborted"); close(console); exit(0); } else { warnx("Unknown input, ESC calibration aborted"); close(console); exit(0); } } /* rate limit to ~ 20 Hz */ usleep(50000); } /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) err(1, "can't open %s", dev); // XXX arming not necessaire at the moment // /* Then arm */ // ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); // if (ret != OK) // err(1, "PWM_SERVO_SET_ARM_OK"); // ret = ioctl(fd, PWM_SERVO_ARM, 0); // if (ret != OK) // err(1, "PWM_SERVO_ARM"); /* Wait for user confirmation */ warnx("Set high PWM\n" "Connect battery now and hit ENTER after the ESCs confirm the first calibration step"); while (1) { /* First set high PWM */ for (unsigned i = 0; i<MAX_CHANNELS; i++) { if(channels_selected[i]) { ret = ioctl(fd, PWM_SERVO_SET(i), 2100); if (ret != OK) err(1, "PWM_SERVO_SET(%d)", i); } } if (read(console, &c, 1) == 1) { if (c == 13) { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); close(console); exit(0); } } /* rate limit to ~ 20 Hz */ usleep(50000); } /* we don't need any more user input */ warnx("Set low PWM, hit ENTER when finished"); while (1) { /* Then set low PWM */ for (unsigned i = 0; i<MAX_CHANNELS; i++) { if(channels_selected[i]) { ret = ioctl(fd, PWM_SERVO_SET(i), 900); if (ret != OK) err(1, "PWM_SERVO_SET(%d)", i); } } if (read(console, &c, 1) == 1) { if (c == 13) { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); close(console); exit(0); } } /* rate limit to ~ 20 Hz */ usleep(50000); } warnx("ESC calibration finished"); close(console); // XXX disarming not necessaire at the moment /* Now disarm again */ // ret = ioctl(fd, PWM_SERVO_DISARM, 0); 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; }
/* params: * - channel: the channel to set. Channels are assigned like this: * - Channel 0 is Servo FIN (Probably left? :D Don't know yet) * - Channel 1 is Servo RIN (See above :D) * - Channel 2 is Motor FIN (forward) * - Channel 3 is Motor RIN (backwards) * - value: Pulse high duration in microseconds. Period is specified in up_nsh.c * as the update_rate part of the pwm_servo_config * (1/update_rate = period, hence 0 = no pulse, full period = high) * * For breaking, set the motor PWMs both to 20000 (high). Not tested yet * though. */ void subhelper_set_pwmchan( uint8_t channel, uint16_t value ) { /* Set servo value via ioctl */ result = ioctl(pwm_fd, PWM_SERVO_SET(channel), value); }
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 esc_calib_main(int argc, char *argv[]) { const char *dev = PWM_OUTPUT0_DEVICE_PATH; char *ep; int ch; int ret; char c; unsigned max_channels = 0; uint32_t set_mask = 0; unsigned long channels; unsigned single_ch = 0; uint16_t pwm_high = PWM_DEFAULT_MAX; uint16_t pwm_low = PWM_DEFAULT_MIN; struct pollfd fds; fds.fd = 0; /* stdin */ fds.events = POLLIN; if (argc < 2) { usage("no channels provided"); return 1; } int arg_consumed = 0; int myoptind = 1; const char *myoptarg = NULL; while ((ch = px4_getopt(argc, argv, "d:c:m:al:h:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': dev = myoptarg; arg_consumed += 2; break; case 'c': /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ channels = strtoul(myoptarg, &ep, 0); while ((single_ch = channels % 10)) { set_mask |= 1 << (single_ch - 1); channels /= 10; } break; case 'm': /* Read in mask directly */ set_mask = strtoul(myoptarg, &ep, 0); if (*ep != '\0') { usage("bad set_mask value"); return 1; } break; case 'a': /* Choose all channels */ for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { set_mask |= 1 << i; } break; case 'l': /* Read in custom low value */ pwm_low = strtoul(myoptarg, &ep, 0); if (*ep != '\0' #if PWM_LOWEST_MIN > 0 || pwm_low < PWM_LOWEST_MIN #endif || pwm_low > PWM_HIGHEST_MIN) { usage("low PWM invalid"); return 1; } break; case 'h': /* Read in custom high value */ pwm_high = strtoul(myoptarg, &ep, 0); if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) { usage("high PWM invalid"); return 1; } break; default: usage(NULL); return 1; } } if (set_mask == 0) { usage("no channels chosen"); return 1; } if (pwm_low > pwm_high) { usage("low pwm is higher than high pwm"); return 1; } /* make sure no other source is publishing control values now */ struct actuator_controls_s actuators; int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); /* clear changed flag */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators); /* wait 50 ms */ usleep(50000); /* now expect nothing changed on that topic */ bool orb_updated; orb_check(act_sub, &orb_updated); if (orb_updated) { PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" "\tmc_att_control stop\n" "\tfw_att_control stop\n"); return 1; } printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" "\n" "Make sure\n" "\t - that the ESCs are not powered\n" "\t - that safety is off (two short blinks)\n" "\t - that the controllers are stopped\n" "\n" "Do you want to start calibration now: y or n?\n"); /* wait for user input */ while (1) { ret = poll(&fds, 1, 0); if (ret > 0) { if (read(0, &c, 1) <= 0) { printf("ESC calibration read error\n"); return 0; } if (c == 'y' || c == 'Y') { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); return 0; } else if (c == 'n' || c == 'N') { printf("ESC calibration aborted\n"); return 0; } else { printf("Unknown input, ESC calibration aborted\n"); return 0; } } /* rate limit to ~ 20 Hz */ usleep(50000); } /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) { PX4_ERR("can't open %s", dev); return 1; } /* get number of channels available on the device */ ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); if (ret != OK) { PX4_ERR("PWM_SERVO_GET_COUNT"); return 1; } /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) { PX4_ERR("PWM_SERVO_SET_ARM_OK"); return 1; } /* tell IO/FMU that the system is armed (it will output values if safety is off) */ ret = ioctl(fd, PWM_SERVO_ARM, 0); if (ret != OK) { PX4_ERR("PWM_SERVO_ARM"); return 1; } printf("Outputs armed"); /* wait for user confirmation */ printf("\nHigh PWM set: %d\n" "\n" "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" "\n", pwm_high); fflush(stdout); while (1) { /* set max PWM */ for (unsigned i = 0; i < max_channels; i++) { if (set_mask & 1 << i) { ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high); if (ret != OK) { PX4_ERR("PWM_SERVO_SET(%d), value: %d", i, pwm_high); goto cleanup; } } } ret = poll(&fds, 1, 0); if (ret > 0) { if (read(0, &c, 1) <= 0) { printf("ESC calibration read error\n"); goto done; } if (c == 13) { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited"); goto done; } } /* rate limit to ~ 20 Hz */ usleep(50000); } printf("Low PWM set: %d\n" "\n" "Hit ENTER when finished\n" "\n", pwm_low); while (1) { /* set disarmed PWM */ for (unsigned i = 0; i < max_channels; i++) { if (set_mask & 1 << i) { ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low); if (ret != OK) { PX4_ERR("PWM_SERVO_SET(%d), value: %d", i, pwm_low); goto cleanup; } } } ret = poll(&fds, 1, 0); if (ret > 0) { if (read(0, &c, 1) <= 0) { printf("ESC calibration read error\n"); goto done; } if (c == 13) { break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); goto done; } } /* rate limit to ~ 20 Hz */ usleep(50000); } /* disarm */ ret = ioctl(fd, PWM_SERVO_DISARM, 0); if (ret != OK) { PX4_ERR("PWM_SERVO_DISARM"); goto cleanup; } printf("Outputs disarmed"); printf("ESC calibration finished\n"); done: close(fd); return 0; cleanup: close(fd); return 1; }