diff options
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 16 |
1 files changed, 11 insertions, 5 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f62df54f6..1c4cc1838 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1675,14 +1675,20 @@ PX4IO::io_publish_pwm_outputs() uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); - if (ret != OK) + if (ret != OK){ 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 */ - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < actuator_max; i++){ outputs.output[i] = ctl[i]; + } - outputs.noutputs = _max_actuators; + outputs.noutputs = actuator_max; /* lazily advertise on first publication */ 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("actuators"); + printf("actuators (including S.BUS)"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf("\n"); - printf("servos"); + printf("hardware servo ports"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); |