dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

PX4IO driver: Support 16 output channels

This commit is contained in:
Lorenz Meier 2015-02-21 01:14:50 +01:00
parent c94755c847
commit ea5293b6fe
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1675,14 +1675,20 @@ PX4IO::io_publish_pwm_outputs()
uint16_t ctl[_max_actuators]; uint16_t ctl[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
if (ret != OK) if (ret != OK){
return ret; return ret;
}
unsigned maxouts = sizeof(outputs.output) / sizeof(outputs.output[0]);
unsigned actuator_max = (_max_actuators > maxouts) ? maxouts : _max_actuators;
/* convert from register format to float */ /* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++) for (unsigned i = 0; i < actuator_max; i++){
outputs.output[i] = ctl[i]; outputs.output[i] = ctl[i];
}
outputs.noutputs = _max_actuators; outputs.noutputs = actuator_max;
/* lazily advertise on first publication */ /* lazily advertise on first publication */
if (_to_outputs == 0) { if (_to_outputs == 0) {
@ -1998,13 +2004,13 @@ PX4IO::print_status(bool extended_status)
printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
} }
printf("actuators"); printf("actuators (including S.BUS)");
for (unsigned i = 0; i < _max_actuators; i++) for (unsigned i = 0; i < _max_actuators; i++)
printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
printf("\n"); printf("\n");
printf("servos"); printf("hardware servo ports");
for (unsigned i = 0; i < _max_actuators; i++) for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));