Merge pull request #1819 from PX4/chan16

Support 16 channels on IO via S.BUS

5 files changed, 20 insertions, 11 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 1512566fa..3bd2c06a4 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1693,14 +1693,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) {
@@ -2016,13 +2022,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));
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 66f0969de..835dfc0f5 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 8192c5b50..17699b7c0 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
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index f0c2cfd26..ae7aec34e 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) {
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 9d2849090..14d8ccca2 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