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;
}