fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 07:23:33 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_pwm_output.h
+28 -10
View File
@@ -101,38 +101,56 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */ /** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) #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 */ /** 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 */ /** 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 */ /** 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 */ /** 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 */ /** 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 */ /** 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 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 */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
/** power up DSM receiver */ /** 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 */ /** 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 */ /** 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 */ /** 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 */ /** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
src/drivers/hil/hil.cpp
+1 -1
View File
@@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
g_hil->set_pwm_rate(arg); g_hil->set_pwm_rate(arg);
break; break;
case PWM_SERVO_SELECT_UPDATE_RATE: case PWM_SERVO_SET_SELECT_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate // HIL always outputs at the alternate (usually faster) rate
break; break;
src/drivers/mkblctrl/mkblctrl.cpp
+1 -1
View File
@@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK; ret = OK;
break; break;
case PWM_SERVO_SELECT_UPDATE_RATE: case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK; ret = OK;
break; break;
src/drivers/px4fmu/fmu.cpp
+13 -1
View File
@@ -685,14 +685,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false); up_pwm_servo_arm(false);
break; break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_default_rate;
break;
case PWM_SERVO_SET_UPDATE_RATE: case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break; 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); ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break; 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(5):
case PWM_SERVO_SET(4): case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) { if (_mode < MODE_6PWM) {
src/drivers/px4io/px4io.cpp
+43 -5
View File
@@ -958,7 +958,7 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
return E2BIG; return E2BIG;
/* copy values to registers in IO */ /* 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(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
printf("\nidle values"); printf("\nidle values");
for (unsigned i = 0; i < _max_actuators; i++) 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"); 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); ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break; 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: case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested alternate rate */ /* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break; 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 */ /* 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); 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; 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: { case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_idle_values(pwm->values, pwm->channel_count); 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; break;
case PWM_SERVO_SET_MIN_PWM: { case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
warnx("Set min values");
set_min_values(pwm->values, pwm->channel_count); 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; break;
case PWM_SERVO_SET_MAX_PWM: { case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_max_values(pwm->values, pwm->channel_count); 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; break;
case PWM_SERVO_GET_COUNT: case PWM_SERVO_GET_COUNT:
@@ -2448,5 +2486,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv); bind(argc, argv);
out: 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'");
} }
src/modules/px4iofirmware/mixer.cpp
+2 -2
View File
@@ -307,9 +307,9 @@ mixer_tick(void)
up_pwm_servo_set(i, r_page_servos[i]); up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) { } 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++) 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]);
} }
} }
src/modules/px4iofirmware/protocol.h
+2 -2
View File
@@ -220,8 +220,8 @@ enum { /* DSM bind states */
/* PWM maximum values for certain ESCs */ /* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM idle values that are active, even when SAFETY_SAFE */ /* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ #define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/** /**
* As-needed mixer data upload. * As-needed mixer data upload.
src/modules/px4iofirmware/px4io.h
+1 -1
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_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_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_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. * Register aliases.
src/modules/px4iofirmware/registers.c
+26 -29
View File
@@ -220,10 +220,10 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100
/** /**
* PAGE 108 * 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 int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) 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 */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) if (*values == 0) {
/* set to default */ /* ignore 0 */
r_page_servo_control_min[offset] = 900; } else if (*values > 1200) {
else if (*values > 1200)
r_page_servo_control_min[offset] = 1200; r_page_servo_control_min[offset] = 1200;
else if (*values < 900) } else if (*values < 900) {
r_page_servo_control_min[offset] = 900; r_page_servo_control_min[offset] = 900;
else } else {
r_page_servo_control_min[offset] = *values; r_page_servo_control_min[offset] = *values;
}
offset++; offset++;
num_values--; num_values--;
@@ -315,16 +314,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) if (*values == 0) {
/* set to default */ /* ignore 0 */
} else if (*values > 2100) {
r_page_servo_control_max[offset] = 2100; r_page_servo_control_max[offset] = 2100;
} else if (*values < 1800) {
else if (*values > 2100)
r_page_servo_control_max[offset] = 2100;
else if (*values < 1800)
r_page_servo_control_max[offset] = 1800; r_page_servo_control_max[offset] = 1800;
else } else {
r_page_servo_control_max[offset] = *values; r_page_servo_control_max[offset] = *values;
}
offset++; offset++;
num_values--; num_values--;
@@ -332,21 +330,20 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
} }
break; break;
case PX4IO_PAGE_IDLE_PWM: case PX4IO_PAGE_DISARMED_PWM:
/* copy channel data */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) if (*values == 0) {
/* set to default */ /* ignore 0 */
r_page_servo_idle[offset] = 0; } else if (*values < 900) {
r_page_servo_disarmed[offset] = 900;
else if (*values < 900) } else if (*values > 2100) {
r_page_servo_idle[offset] = 900; r_page_servo_disarmed[offset] = 2100;
else if (*values > 2100) } else {
r_page_servo_idle[offset] = 2100; r_page_servo_disarmed[offset] = *values;
else }
r_page_servo_idle[offset] = *values;
/* flag the failsafe values as custom */ /* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; 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: case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max); SELECT_PAGE(r_page_servo_control_max);
break; break;
case PX4IO_PAGE_IDLE_PWM: case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_idle); SELECT_PAGE(r_page_servo_disarmed);
break; break;
default: default:
src/systemcmds/pwm/pwm.c
+279 -242
View File
@@ -77,21 +77,23 @@ usage(const char *reason)
" arm Arm output\n" " arm Arm output\n"
" disarm Disarm output\n" " disarm Disarm output\n"
"\n" "\n"
" rate Configure PWM rates\n" " rate Configure PWM rates\n"
" [-c <channel group>] Channel group that should update at the alternate rate\n" " [-g <channel group>] Channel group that should update at the alternate rate\n"
" [-m <chanmask> ] Directly supply channel mask\n" " [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n" " [-a] Configure all outputs\n"
" -r <alt_rate> PWM rate (50 to 400 Hz)\n" " -r <alt_rate> PWM rate (50 to 400 Hz)\n"
"\n" "\n"
" min ... Configure minimum PWM values\n" " min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n" " max ... Configure maximum PWM values\n"
" disarmed ... Configure disarmed PWM values\n" " disarmed ... Configure disarmed 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" " [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n" " -p <pwm value> PWM value\n"
"\n" "\n"
" test ... Directly set PWM values\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" " [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n" " -p <pwm value> PWM value\n"
"\n" "\n"
@@ -123,7 +125,7 @@ pwm_main(int argc, char *argv[])
if (argc < 1) if (argc < 1)
usage(NULL); 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) { switch (ch) {
case 'd': case 'd':
@@ -140,7 +142,7 @@ pwm_main(int argc, char *argv[])
case 'c': case 'c':
/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ /* 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)) { while ((single_ch = channels % 10)) {
@@ -155,11 +157,12 @@ pwm_main(int argc, char *argv[])
usage("bad channel_group value"); usage("bad channel_group value");
alt_channel_groups |= (1 << group); alt_channel_groups |= (1 << group);
alt_channels_set = true; alt_channels_set = true;
warnx("alt channels set, group: %d", group);
break; break;
case 'm': case 'm':
/* Read in mask directly */ /* Read in mask directly */
set_mask = strtol(optarg, &ep, 0); set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0') if (*ep != '\0')
usage("bad set_mask value"); usage("bad set_mask value");
break; break;
@@ -170,12 +173,12 @@ pwm_main(int argc, char *argv[])
} }
break; break;
case 'p': case 'p':
pwm_value = strtol(optarg, &ep, 0); pwm_value = strtoul(optarg, &ep, 0);
if (*ep != '\0') if (*ep != '\0')
usage("bad PWM value provided"); usage("bad PWM value provided");
break; break;
case 'r': case 'r':
alt_rate = strtol(optarg, &ep, 0); alt_rate = strtoul(optarg, &ep, 0);
if (*ep != '\0') if (*ep != '\0')
usage("bad alternative rate provided"); usage("bad alternative rate provided");
break; break;
@@ -205,241 +208,275 @@ pwm_main(int argc, char *argv[])
if (ret != OK) if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT"); 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")) { } else if (!strcmp(argv[1], "disarm")) {
/* tell IO that its ok to disable its safety with the switch */ /* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); 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) if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK"); err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
/* 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);
} }
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; return 0;
} }