From c94755c847ce122e2da727ce58c3171a11896879 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Feb 2015 01:14:29 +0100 Subject: [PATCH 1/4] PX4IO Firmware: Support 16 output channels --- src/modules/px4iofirmware/mixer.cpp | 6 ++++-- src/modules/px4iofirmware/px4io.h | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 66f0969de1..835dfc0f5e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -272,8 +272,9 @@ mixer_tick(void) if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); + } /* set S.BUS1 or S.BUS2 outputs */ @@ -285,8 +286,9 @@ mixer_tick(void) } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); + } /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 93a33490fa..df2633cc5a 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -51,7 +51,8 @@ /* * Constants and limits. */ -#define PX4IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 16 +#define PX4IO_SERVO_HARDWARE_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_CONTROL_GROUPS 4 #define PX4IO_RC_INPUT_CHANNELS 18 From ea5293b6fe977835b6f601b1e17a5d7f5e4ca248 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Feb 2015 01:14:50 +0100 Subject: [PATCH 2/4] PX4IO driver: Support 16 output channels --- src/drivers/px4io/px4io.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f62df54f6c..1c4cc18384 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)); From 649fcd7cc77aa9ebf4595bd706b8f862b67c5b1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Feb 2015 01:18:36 +0100 Subject: [PATCH 3/4] PX4IO Firmware: Fall back to S.BUS1 for S.BUS2 requested --- src/modules/px4iofirmware/sbus.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f05..82701f0b1a 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -163,8 +163,8 @@ sbus1_output(uint16_t *values, uint16_t num_values) void sbus2_output(uint16_t *values, uint16_t num_values) { - char b = 'B'; - write(sbus_fd, &b, 1); + // XXX S.BUS2 is not implemented, fall back to S.BUS1 + sbus1_output(values, num_values); } bool From 6bd94f15a89c8252e1d04350e043c764483647b7 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 12 Mar 2015 17:43:19 +0100 Subject: [PATCH 4/4] PX4IO Firmware: fix 16ch output --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f0c2cfd26d..ae7aec34e4 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -284,7 +284,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_DIRECT_PWM: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ if (*values != PWM_IGNORE_THIS_CHANNEL) {