fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

pwm info provides more information, some fixes for setting rate/min/max/disarmed

Browse Source
This commit is contained in:
Julian Oes 2013-10-07 16:24:49 +02:00
parent 9ff5217118
commit ea0aa49b54
10 changed files with 396 additions and 294 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/drivers/drv_pwm_output.h
View File

@ -101,38 +101,56 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** get default servo update rate */
#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** set alternate servo update rate */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3)
/** get alternate servo update rate */
#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** get the number of servos in *(unsigned *)arg */
#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3)
#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 5)
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6)
/** check the selected update rates */
#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7)
/** set the 'ARM ok' bit, which activates the safety switch */
#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5)
#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8)
/** clear the 'ARM ok' bit, which deactivates the safety switch */
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9)
/** start DSM bind */
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10)
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
/** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
/** set the PWM value when disarmed - should be no PWM (zero) by default */
#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9)
#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 12)
/** get the PWM value when disarmed */
#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 13)
/** set the minimum PWM value the output will send */
#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10)
#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 14)
/** get the minimum PWM value the output will send */
#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 15)
/** set the maximum PWM value the output will send */
#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11)
#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 16)
/** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 17)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

2
src/drivers/hil/hil.cpp
View File

@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate
break;

2
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK;
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK;
break;

14
src/drivers/px4fmu/fmu.cpp
View File

@ -685,14 +685,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
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_SELECT_UPDATE_RATE:
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(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {

48
src/drivers/px4io/px4io.cpp
View File

@ -958,7 +958,7 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
return io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, vals, len);
}
@ -1684,7 +1684,7 @@ PX4IO::print_status()
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
printf("\nidle values");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
printf("\n");
}
@ -1716,12 +1716,22 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
/* get the default update rate */
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
break;
case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE: {
case PWM_SERVO_GET_UPDATE_RATE:
/* get the alternative update rate */
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
/* blindly clear the PWM update alarm - might be set for some other reason */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
@ -1738,23 +1748,51 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
break;
case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_idle_values(pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_DISARMED_PWM:
ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t));
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
warnx("Set min values");
set_min_values(pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_MIN_PWM:
ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t));
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_max_values(pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_MAX_PWM:
ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t));
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_GET_COUNT:
@ -2448,5 +2486,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'bind' or 'update'");
}

4
src/modules/px4iofirmware/mixer.cpp
View File

@ -307,9 +307,9 @@ mixer_tick(void)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the idle servo outputs. */
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_idle[i]);
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
}

4
src/modules/px4iofirmware/protocol.h
View File

@ -220,8 +220,8 @@ enum { /* DSM bind states */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM idle values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.

2
src/modules/px4iofirmware/px4io.h
View File

@ -80,7 +80,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*
* Register aliases.

55
src/modules/px4iofirmware/registers.c
View File

@ -220,10 +220,10 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100
/**
* PAGE 108
*
* idle PWM values for difficult ESCs
* disarmed PWM values for difficult ESCs
*
*/
uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@ -293,16 +293,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_control_min[offset] = 900;
else if (*values > 1200)
if (*values == 0) {
/* ignore 0 */
} else if (*values > 1200) {
r_page_servo_control_min[offset] = 1200;
else if (*values < 900)
} else if (*values < 900) {
r_page_servo_control_min[offset] = 900;
else
} else {
r_page_servo_control_min[offset] = *values;
}
offset++;
num_values--;
@ -315,16 +314,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
if (*values == 0) {
/* ignore 0 */
} else if (*values > 2100) {
r_page_servo_control_max[offset] = 2100;
else if (*values > 2100)
r_page_servo_control_max[offset] = 2100;
else if (*values < 1800)
} else if (*values < 1800) {
r_page_servo_control_max[offset] = 1800;
else
} else {
r_page_servo_control_max[offset] = *values;
}
offset++;
num_values--;
@ -332,21 +330,20 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
case PX4IO_PAGE_IDLE_PWM:
case PX4IO_PAGE_DISARMED_PWM:
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_idle[offset] = 0;
else if (*values < 900)
r_page_servo_idle[offset] = 900;
else if (*values > 2100)
r_page_servo_idle[offset] = 2100;
else
r_page_servo_idle[offset] = *values;
if (*values == 0) {
/* ignore 0 */
} else if (*values < 900) {
r_page_servo_disarmed[offset] = 900;
} else if (*values > 2100) {
r_page_servo_disarmed[offset] = 2100;
} else {
r_page_servo_disarmed[offset] = *values;
}
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
@ -769,8 +766,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_IDLE_PWM:
SELECT_PAGE(r_page_servo_idle);
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
break;
default:

521
src/systemcmds/pwm/pwm.c
View File

@ -77,21 +77,23 @@ usage(const char *reason)
" arm Arm output\n"
" disarm Disarm output\n"
"\n"
" rate Configure PWM rates\n"
" [-c <channel group>] Channel group that should update at the alternate rate\n"
" rate Configure PWM rates\n"
" [-g <channel group>] Channel group that should update at the alternate rate\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n"
" -r <alt_rate> PWM rate (50 to 400 Hz)\n"
"\n"
" min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n"
" disarmed ... Configure disarmed PWM values\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n"
" disarmed ... Configure disarmed PWM values\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n"
"\n"
" test ... Directly set PWM values\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n"
"\n"
@ -123,7 +125,7 @@ pwm_main(int argc, char *argv[])
if (argc < 1)
usage(NULL);
while ((ch = getopt(argc-1, &argv[1], "d:vc:m:ap:r:")) != EOF) {
while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
switch (ch) {
case 'd':
@ -140,7 +142,7 @@ pwm_main(int argc, char *argv[])
case 'c':
/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
channels = strtol(optarg, &ep, 0);
channels = strtoul(optarg, &ep, 0);
while ((single_ch = channels % 10)) {
@ -155,11 +157,12 @@ pwm_main(int argc, char *argv[])
usage("bad channel_group value");
alt_channel_groups |= (1 << group);
alt_channels_set = true;
warnx("alt channels set, group: %d", group);
break;
case 'm':
/* Read in mask directly */
set_mask = strtol(optarg, &ep, 0);
set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
break;
@ -170,12 +173,12 @@ pwm_main(int argc, char *argv[])
}
break;
case 'p':
pwm_value = strtol(optarg, &ep, 0);
pwm_value = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad PWM value provided");
break;
case 'r':
alt_rate = strtol(optarg, &ep, 0);
alt_rate = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alternative rate provided");
break;
@ -205,241 +208,275 @@ pwm_main(int argc, char *argv[])
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
int argi = 1; /* leave away cmd name */
if (!strcmp(argv[1], "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");
while(argi<argc) {
if (print_verbose)
warnx("Outputs armed");
exit(0);
if (!strcmp(argv[argi], "arm")) {
/* tell IO that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
} else if (!strcmp(argv[1], "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
if (print_verbose)
warnx("Outputs disarmed");
exit(0);
} else if (!strcmp(argv[1], "rate")) {
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
if (print_verbose)
warnx("Outputs armed");
exit(0);
} else if (!strcmp(argv[argi], "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
if (print_verbose)
warnx("Outputs disarmed");
exit(0);
}else if (!strcmp(argv[argi], "rate")) {
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
} else {
usage("no alternative rate provided");
}
/* directly supplied channel mask */
if (set_mask > 0) {
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
} else {
usage("no channel/channel groups selected");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
exit(0);
} else if (!strcmp(argv[argi], "min")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: min PWM: %d", i+1, pwm_value);
pwm_values.channel_count++;
}
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
exit(0);
} else if (!strcmp(argv[argi], "max")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: max PWM: %d", i+1, pwm_value);
pwm_values.channel_count++;
}
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
exit(0);
} else if (!strcmp(argv[argi], "disarmed")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: disarmed PWM: %d", i+1, pwm_value);
pwm_values.channel_count++;
}
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting idle values");
}
exit(0);
} else if (!strcmp(argv[argi], "test")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
/* perform PWM output */
/* Open console directly to grab CTRL-C signal */
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
}
/* abort on user request */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
exit(0);
}
}
}
exit(0);
} else if (!strcmp(argv[argi], "info")) {
/* print current servo values */
for (unsigned i = 0; i < servo_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 < servo_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");
}
}
exit(0);
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
argi++;
/* directly supplied channel mask */
if (set_mask > 0) {
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
exit(0);
} else if (!strcmp(argv[1], "min")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: min PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting min values");
}
exit(0);
} else if (!strcmp(argv[1], "max")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: max PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting max values");
}
exit(0);
} else if (!strcmp(argv[1], "disarmed")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: disarmed PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting disarmed values");
}
exit(0);
} else if (!strcmp(argv[1], "test")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
/* perform PWM output */
/* Open console directly to grab CTRL-C signal */
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
}
/* abort on user request */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
exit(0);
}
}
}
exit(0);
} else if (!strcmp(argv[1], "info")) {
printf("device: %s\n", dev);
uint32_t info_default_rate;
uint32_t info_alt_rate;
uint32_t info_alt_rate_mask;
ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
if (ret != OK) {
err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
}
ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
if (ret != OK) {
err(1, "PWM_SERVO_GET_UPDATE_RATE");
}
ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
if (ret != OK) {
err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
}
struct pwm_output_values disarmed_pwm;
struct pwm_output_values min_pwm;
struct pwm_output_values max_pwm;
ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_DISARMED_PWM");
}
ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_MIN_PWM");
}
ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_MAX_PWM");
}
/* print current servo values */
for (unsigned i = 0; i < servo_count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("channel %u: %u us", i+1, spos);
if (info_alt_rate_mask & (1<<i))
printf(" (alternative rate: %d Hz", info_alt_rate);
else
printf(" (default rate: %d Hz", info_default_rate);
printf(" disarmed; %d us, min: %d us, max: %d us)", disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]);
printf("\n");
} else {
printf("%u: ERROR\n", i);
}
}
/* print rate groups */
for (unsigned i = 0; i < servo_count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
if (ret != OK)
break;
if (group_mask != 0) {
printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j))
printf(" %u", j+1);
printf("\n");
}
}
exit(0);
}
usage("specify arm|disarm|set|config|test");
usage("specify arm|disarm|rate|min|max|disarmed|test|info");
return 0;
}