Merge branch 'px4io-i2c-memory-squeeze' of github.com:PX4/Firmware into px4io-i2c-memory-squeeze

4 files changed, 34 insertions, 10 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 2488f8022..4006f88cf 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -101,6 +101,7 @@ public:
 private:
 	// XXX
 	unsigned		_max_actuators;
+	unsigned		_max_controls;
 	unsigned		_max_rc_input;
 	unsigned		_max_relays;
 	unsigned		_max_transfer;
@@ -277,6 +278,7 @@ PX4IO	*g_dev;
 PX4IO::PX4IO() :
 	I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
 	_max_actuators(0),
+	_max_controls(0),
 	_max_rc_input(0),
 	_max_relays(0),
 	_max_transfer(16),	/* sensible default */
@@ -342,6 +344,7 @@ PX4IO::init()
 
 	/* get some parameters */
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
+	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
 	_max_relays    = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
@@ -637,11 +640,11 @@ PX4IO::io_set_control_state()
 	orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
 	     ORB_ID(actuator_controls_1), _t_actuators, &controls);
 
-	for (unsigned i = 0; i < _max_actuators; i++)
+	for (unsigned i = 0; i < _max_controls; i++)
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
 
 	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
+	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
 }
 
 int
@@ -1219,7 +1222,7 @@ PX4IO::print_status()
 	printf("mapped R/C inputs 0x%04x", mapped_inputs);
 	for (unsigned i = 0; i < _max_rc_input; i++) {
 		if (mapped_inputs & (1 << i))
-			printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i));
+			printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)));
 	}
 	printf("\n");
 	uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT);
@@ -1228,7 +1231,7 @@ PX4IO::print_status()
 		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i));
 	printf("\n");
 
-	/* setup */
+	/* setup and state */
 	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
 	printf("arming 0x%04x%s%s%s%s\n",
@@ -1247,6 +1250,24 @@ PX4IO::print_status()
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS));
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
+	printf("controls");
+	for (unsigned i = 0; i < _max_controls; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
+	printf("\n");
+	for (unsigned i = 0; i < _max_rc_input; i++) {
+		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+		uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
+		printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
+			i,
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
+			io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
+			options,
+			((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
+			((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+	}
 	printf("failsafe");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 6b5164756..21b4edcc3 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -119,7 +119,6 @@ controls_tick() {
 
 	ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
 
-
 	/*
 	 * In some cases we may have received a frame, but input has still
 	 * been lost.
@@ -168,8 +167,8 @@ controls_tick() {
 
 				int16_t scaled = raw;
 
-				/* adjust to zero-relative (-500..500) */
-				scaled -= 1500;
+				/* adjust to zero-relative around center (nominal -500..500) */
+				scaled -= conf[PX4IO_P_RC_CONFIG_CENTER];
 
 				/* scale to fixed-point representation (-10000..10000) */
 				scaled *= 20;
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 77f28cd7a..ed74cb3d3 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -94,9 +94,13 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			debug("AP RX timeout");
 		}
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
+
+		/* XXX this is questionable - vehicle may not make sense for direct control */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
+	} else {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 	}
 
 	source = MIX_FAILSAFE;
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 5fb90b9b0..5ec2f7258 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -165,8 +165,8 @@ volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
  */
 uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
 
-/* valid options excluding ENABLE */
-#define PX4IO_P_RC_CONFIG_OPTIONS_VALID	PX4IO_P_RC_CONFIG_OPTIONS_REVERSE
+/* valid options */
+#define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
 
 /*
  * PAGE 104 uses r_page_servos.