1 files changed, 267 insertions, 263 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 65e8fa4b6..3aecd9b69 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -58,7 +58,6 @@
 #include <arch/board/board.h>
 
 #include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
@@ -85,16 +84,20 @@
 #include <debug.h>
 
 #include <mavlink/mavlink_log.h>
-#include "uploader.h"
 #include <modules/px4iofirmware/protocol.h>
 
+#include "uploader.h"
+
+extern device::Device *PX4IO_i2c_interface() weak_function;
+extern device::Device *PX4IO_serial_interface() weak_function;
+
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
-class PX4IO : public device::I2C
+class PX4IO : public device::CDev
 {
 public:
-	PX4IO();
+	PX4IO(device::Device *interface);
 	virtual ~PX4IO();
 
 	virtual int		init();
@@ -105,8 +108,8 @@ public:
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param rate    The rate in Hz actuator outpus are sent to IO.
-	*      Min 10 Hz, max 400 Hz
+	* @param rate		The rate in Hz actuator outpus are sent to IO.
+	* 			Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
 
@@ -121,8 +124,8 @@ public:
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param len Number of channels, could up to 8
+	 * @param vals		Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param len		Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
@@ -157,7 +160,10 @@ public:
 	};
 
 private:
+	device::Device		*_interface;
+
 	// XXX
+	unsigned		_hardware;
 	unsigned		_max_actuators;
 	unsigned		_max_controls;
 	unsigned		_max_rc_input;
@@ -274,7 +280,7 @@ private:
 	 * @param offset	Register offset to start writing at.
 	 * @param values	Pointer to array of values to write.
 	 * @param num_values	The number of values to write.
-	 * @return		Zero if all values were successfully written.
+	 * @return		OK if all values were successfully written.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
 
@@ -284,7 +290,7 @@ private:
 	 * @param page		Register page to write to.
 	 * @param offset	Register offset to write to.
 	 * @param value		Value to write.
-	 * @return		Zero if the value was written successfully.
+	 * @return		OK if the value was written successfully.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
 
@@ -295,7 +301,7 @@ private:
 	 * @param offset	Register offset to start reading from.
 	 * @param values	Pointer to array where values should be stored.
 	 * @param num_values	The number of values to read.
-	 * @return		Zero if all values were successfully read.
+	 * @return		OK if all values were successfully read.
 	 */
 	int			io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
 
@@ -352,8 +358,10 @@ PX4IO	*g_dev;
 
 }
 
-PX4IO::PX4IO() :
-	I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+PX4IO::PX4IO(device::Device *interface) :
+	CDev("px4io", PX4IO_DEVICE_PATH),
+	_interface(interface),
+	_hardware(0),
 	_max_actuators(0),
 	_max_controls(0),
 	_max_rc_input(0),
@@ -407,6 +415,9 @@ PX4IO::~PX4IO()
 	if (_task != -1)
 		task_delete(_task);
 
+	if (_interface != nullptr)
+		delete _interface;
+
 	g_dev = nullptr;
 }
 
@@ -418,65 +429,30 @@ PX4IO::init()
 	ASSERT(_task == -1);
 
 	/* do regular cdev init */
-	ret = I2C::init();
+	ret = CDev::init();
 	if (ret != OK)
 		return ret;
 
-	/*
-	 * Enable a couple of retries for operations to IO.
-	 *
-	 * Register read/write operations are intentionally idempotent
-	 * so this is safe as designed.
-	 */
-	_retries = 2;
-
-	/* get IO's last seen FMU state */
-	int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	if (val == _io_reg_get_error) {
-		mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE");
-	}
-	uint16_t arming = val;
-
-	/* get basic software version */
-	/* basic configuration */
-	usleep(5000);
-
-	unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
-	unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION);
-
-	if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) {
-		mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n",
-			proto_version,
-			PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC);
-
-		mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware.");
-		log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC);
-		return 1;
-	}
-
-	if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) {
-		mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n",
-			proto_version,
-			PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC);
-
-		mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware.");
-		log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC);
-		return 1;
-	}
-
 	/* get some parameters */
+	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+	if (protocol != PX4IO_PROTOCOL_VERSION) {
+		log("protocol/firmware mismatch");
+		mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
+		return -1;
+	}
+	_hardware      = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
 	_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_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);
 	if ((_max_actuators < 1) || (_max_actuators > 255) ||
-	    (_max_relays < 1)    || (_max_relays > 255)    ||
-	    (_max_transfer < 16)   || (_max_transfer > 255)    ||
+	    (_max_relays > 32)   ||
+	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
 
-		log("failed getting parameters from PX4IO");
-		mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
+		log("config read error");
+		mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
 		return -1;
 	}
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
@@ -489,21 +465,19 @@ PX4IO::init()
 	 * in this case.
 	 */
 
-	printf("arming 0x%04x%s%s%s%s\n",
-		arming,
-		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)    ? " FAILSAFE CUSTOM" : ""));
+	uint16_t reg;
 
+	/* get IO's last seen FMU state */
+	ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
+	if (ret != OK)
+		return ret;
 
 	/*
 	 * in-air restart is only tried if the IO board reports it is
 	 * already armed, and has been configured for in-air restart
 	 */
-	if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
-	    (arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
+	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
 	    	log("INAIR RESTART RECOVERY (needs commander app running)");
@@ -618,14 +592,14 @@ PX4IO::init()
 	}
 
 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
 		return -errno;
 	}
 
-	mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version);
+	mavlink_log_info(_mavlink_fd, "[IO] init ok");
 
 	return OK;
 }
@@ -662,6 +636,14 @@ PX4IO::task_main()
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
 	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
 
+	if ((_t_actuators < 0) ||
+		(_t_actuator_armed < 0) ||
+		(_t_vehicle_control_mode < 0) ||
+		(_t_param < 0)) {
+		log("subscription(s) failed");
+		goto out;
+	}
+
 	/* poll descriptor */
 	pollfd fds[4];
 	fds[0].fd = _t_actuators;
@@ -777,6 +759,7 @@ PX4IO::task_main()
 
 	unlock();
 
+out:
 	debug("exiting");
 
 	/* clean up the alternate device node */
@@ -1124,38 +1107,38 @@ int
 PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 {
 	uint32_t channel_count;
-	int	ret = OK;
+	int	ret;
 
 	/* we don't have the status bits, so input_source has to be set elsewhere */
 	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
 	
+	static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
+
 	/*
-	 * XXX Because the channel count and channel data are fetched
-	 *     separately, there is a risk of a race between the two
-	 *     that could leave us with channel data and a count that 
-	 *     are out of sync.
-	 *     Fixing this would require a guarantee of atomicity from
-	 *     IO, and a single fetch for both count and channels.
+	 * Read the channel count and the first 9 channels.
 	 *
-	 * XXX Since IO has the input calibration info, we ought to be
-	 *     able to get the pre-fixed-up controls directly.
-	 *
-	 * XXX can we do this more cheaply? If we knew we had DMA, it would
-	 *     almost certainly be better to just get all the inputs...
+	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	channel_count =  io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
-	if (channel_count == _io_reg_get_error)
-		return -EIO;
-	if (channel_count > RC_INPUT_MAX_CHANNELS)
-		channel_count = RC_INPUT_MAX_CHANNELS;
-	input_rc.channel_count = channel_count;
+	input_rc.timestamp = hrt_absolute_time();
+	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
+	if (ret != OK)
+		return ret;
 
-	if (channel_count > 0) {
-		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
-		if (ret == OK)
-			input_rc.timestamp = hrt_absolute_time();
+	/*
+	 * Get the channel count any any extra channels. This is no more expensive than reading the
+	 * channel count once.
+	 */
+	channel_count = regs[0];
+	if (channel_count > 9) {
+		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
+		if (ret != OK)
+			return ret;
 	}
 
+	input_rc.channel_count = channel_count;
+	memcpy(input_rc.values, &regs[prolog], channel_count * 2);
+
 	return ret;
 }
 
@@ -1285,25 +1268,12 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
 		return -EINVAL;
 	}
 
-	/* set up the transfer */
-	uint8_t 	addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
-
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_NORESTART;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
-
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_set: error %d", ret);
-	return ret;
+	int ret =  _interface->write((page << 8) | offset, (void *)values, num_values);
+	if (ret != num_values) {
+		debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret);
+		return -1;
+	}
+	return OK;
 }
 
 int
@@ -1315,25 +1285,18 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
 int
 PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
 {
-	/* set up the transfer */
-	uint8_t		addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
-
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_READ;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
+	/* range check the transfer */
+	if (num_values > ((_max_transfer) / sizeof(*values))) {
+		debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+		return -EINVAL;
+	}
 
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_get: data error %d", ret);
-	return ret;
+	int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
+	if (ret != num_values) {
+		debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
+		return -1;
+	}
+	return OK;
 }
 
 uint32_t
@@ -1341,7 +1304,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset)
 {
 	uint16_t value;
 
-	if (io_reg_get(page, offset, &value, 1))
+	if (io_reg_get(page, offset, &value, 1) != OK)
 		return _io_reg_get_error;
 
 	return value;
@@ -1354,8 +1317,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 	uint16_t value;
 
 	ret = io_reg_get(page, offset, &value, 1);
-
-	if (ret)
+	if (ret != OK)
 		return ret;
 	value &= ~clearbits;
 	value |= setbits;
@@ -1427,9 +1389,9 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u software %u bootloader %u buffer %uB\n",
+	printf("protocol %u hardware %u bootloader %u buffer %uB\n",
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
@@ -1460,7 +1422,7 @@ PX4IO::print_status()
 		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE)  ? " FAILSAFE" : ""));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
+	printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
 		alarms,
 		((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
@@ -1468,18 +1430,26 @@ PX4IO::print_status()
 		((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
+		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT)  ? " VSERVO_FAULT" : ""));
 	/* now clear alarms */
 	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
 
-	printf("vbatt %u ibatt %u vbatt scale %u\n",
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
-	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
-	printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
-	       (double)_battery_amp_per_volt,
-	       (double)_battery_amp_bias,
-	       (double)_battery_mamphour_total);
+	if (_hardware == 1) {
+		printf("vbatt mV %u ibatt mV %u vbatt scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+		printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
+		       (double)_battery_amp_per_volt,
+		       (double)_battery_amp_bias,
+		       (double)_battery_mamphour_total);
+	} else if (_hardware == 2) {
+		printf("vservo %u mV vservo scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE));
+		printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
+	}
 	printf("actuators");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@@ -1655,9 +1625,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 
 		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
 
-		uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
-
-		*(uint32_t *)arg = value;
+		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+		if (*(uint32_t *)arg == _io_reg_get_error)
+			ret = -EIO;
 		break;
 	}
 
@@ -1665,7 +1635,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		uint32_t bits = (1 << _max_relays) - 1;
 		/* don't touch relay1 if it's controlling RX vcc */
 		if (_dsm_vcc_ctl)
-			bits &= ~PX4IO_RELAY1;
+			bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
 		break;
 	}
@@ -1673,7 +1643,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 	case GPIO_SET:
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
 			ret = -EINVAL;
 		else
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
@@ -1682,7 +1652,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 	case GPIO_CLEAR:
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
 			ret = -EINVAL;
 		else
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
@@ -1779,8 +1749,8 @@ int
 PX4IO::set_update_rate(int rate)
 {
 	int interval_ms = 1000 / rate;
-	if (interval_ms < 5) {
-		interval_ms = 5;
+	if (interval_ms < 3) {
+		interval_ms = 3;
 		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
 	}
 
@@ -1805,14 +1775,47 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 namespace
 {
 
+device::Device *
+get_interface()
+{
+	device::Device *interface = nullptr;
+
+#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
+	/* try for a serial interface */
+	if (PX4IO_serial_interface != nullptr)
+		interface = PX4IO_serial_interface();
+	if (interface != nullptr)
+		goto got;
+#endif
+
+	/* try for an I2C interface if we haven't got a serial one */
+	if (PX4IO_i2c_interface != nullptr)
+		interface = PX4IO_i2c_interface();
+	if (interface != nullptr)
+		goto got;
+
+	errx(1, "cannot alloc interface");
+
+got:
+	if (interface->init() != OK) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
+
+	return interface;
+}
+
 void
 start(int argc, char *argv[])
 {
 	if (g_dev != nullptr)
 		errx(1, "already loaded");
 
+	/* allocate the interface */
+	device::Device *interface = get_interface();
+
 	/* create the driver - it will set g_dev */
-	(void)new PX4IO();
+	(void)new PX4IO(interface);
 
 	if (g_dev == nullptr)
 		errx(1, "driver alloc failed");
@@ -1969,8 +1972,19 @@ monitor(void)
 	}
 }
 
+void
+if_test(unsigned mode)
+{
+	device::Device *interface = get_interface();
+
+	int result = interface->ioctl(1, mode); /* XXX magic numbers */
+	delete interface;
+
+	errx(0, "test returned %d", result);
 }
 
+} /* namespace */
+
 int
 px4io_main(int argc, char *argv[])
 {
@@ -1981,28 +1995,87 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start"))
 		start(argc - 1, argv + 1);
 
-	if (!strcmp(argv[1], "limit")) {
+	if (!strcmp(argv[1], "update")) {
 
 		if (g_dev != nullptr) {
+			printf("[px4io] loaded, detaching first\n");
+			/* stop the driver */
+			delete g_dev;
+		}
 
-			if ((argc > 2)) {
-				g_dev->set_update_rate(atoi(argv[2]));
-			} else {
-				errx(1, "missing argument (50 - 200 Hz)");
-				return 1;
-			}
+		PX4IO_Uploader *up;
+		const char *fn[5];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+
+		} else {
+			fn[0] = "/fs/microsd/px4io.bin";
+			fn[1] =	"/etc/px4io.bin";
+			fn[2] = "/fs/microsd/px4io2.bin";
+			fn[3] =	"/etc/px4io2.bin";
+			fn[4] =	nullptr;
+		}
+
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+
+		case -ENOENT:
+			errx(1, "PX4IO firmware file not found");
+
+		case -EEXIST:
+		case -EIO:
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
+		case -EINVAL:
+			errx(1, "verify failed - retry the update");
+
+		case -ETIMEDOUT:
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
+		default:
+			errx(1, "unexpected error %d", ret);
+		}
+
+		return ret;
+	}
+
+	if (!strcmp(argv[1], "iftest")) {
+		if (g_dev != nullptr)
+			errx(1, "can't iftest when started");
+
+		if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
+	}
+
+	/* commands below here require a started driver */
+
+	if (g_dev == nullptr)
+		errx(1, "not started");
+
+	if (!strcmp(argv[1], "limit")) {
+
+		if ((argc > 2)) {
+			g_dev->set_update_rate(atoi(argv[2]));
+		} else {
+			errx(1, "missing argument (50 - 400 Hz)");
+			return 1;
 		}
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "current")) {
-		if (g_dev != nullptr) {
-			if ((argc > 3)) {
-				g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
-			} else {
-				errx(1, "missing argument (apm_per_volt, amp_offset)");
-				return 1;
-			}
+		if ((argc > 3)) {
+			g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
+		} else {
+			errx(1, "missing argument (apm_per_volt, amp_offset)");
+			return 1;
 		}
 		exit(0);
 	}
@@ -2013,32 +2086,27 @@ px4io_main(int argc, char *argv[])
 			errx(1, "failsafe command needs at least one channel value (ppm)");
 		}
 
-		if (g_dev != nullptr) {
+		/* set values for first 8 channels, fill unassigned channels with 1500. */
+		uint16_t failsafe[8];
 
-			/* set values for first 8 channels, fill unassigned channels with 1500. */
-			uint16_t failsafe[8];
+		for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
 
-			for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
-			{
-				/* set channel to commanline argument or to 900 for non-provided channels */
-				if (argc > i + 2) {
-					failsafe[i] = atoi(argv[i+2]);
-					if (failsafe[i] < 800 || failsafe[i] > 2200) {
-						errx(1, "value out of range of 800 < value < 2200. Aborting.");
-					}
-				} else {
-					/* a zero value will result in stopping to output any pulse */
-					failsafe[i] = 0;
+			/* set channel to commandline argument or to 900 for non-provided channels */
+			if (argc > i + 2) {
+				failsafe[i] = atoi(argv[i+2]);
+				if (failsafe[i] < 800 || failsafe[i] > 2200) {
+					errx(1, "value out of range of 800 < value < 2200. Aborting.");
 				}
+			} else {
+				/* a zero value will result in stopping to output any pulse */
+				failsafe[i] = 0;
 			}
+		}
 
-			int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+		int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
 
-			if (ret != OK)
-				errx(ret, "failed setting failsafe values");
-		} else {
-			errx(1, "not loaded");
-		}
+		if (ret != OK)
+			errx(ret, "failed setting failsafe values");
 		exit(0);
 	}
 
@@ -2149,39 +2217,27 @@ px4io_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "recovery")) {
 
-		if (g_dev != nullptr) {
-			/*
-			 * Enable in-air restart support.
-			 * We can cheat and call the driver directly, as it
-		 	 * doesn't reference filp in ioctl()
-			 */
-			g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
-		} else {
-			errx(1, "not loaded");
-		}
+		/*
+		 * Enable in-air restart support.
+		 * We can cheat and call the driver directly, as it
+	 	 * doesn't reference filp in ioctl()
+		 */
+		g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 
-		if (g_dev != nullptr) {
-			/* stop the driver */
-			delete g_dev;
-		} else {
-			errx(1, "not loaded");
-		}
+		/* stop the driver */
+		delete g_dev;
 		exit(0);
 	}
 
 
 	if (!strcmp(argv[1], "status")) {
 
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded\n");
-			g_dev->print_status();
-		} else {
-			printf("[px4io] not loaded\n");
-		}
+		printf("[px4io] loaded\n");
+		g_dev->print_status();
 
 		exit(0);
 	}
@@ -2208,58 +2264,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	/* note, stop not currently implemented */
-
-	if (!strcmp(argv[1], "update")) {
-
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded, detaching first\n");
-			/* stop the driver */
-			delete g_dev;
-		}
-
-		PX4IO_Uploader *up;
-		const char *fn[3];
-
-		/* work out what we're uploading... */
-		if (argc > 2) {
-			fn[0] = argv[2];
-			fn[1] = nullptr;
-
-		} else {
-			fn[0] = "/fs/microsd/px4io.bin";
-			fn[1] =	"/etc/px4io.bin";
-			fn[2] =	nullptr;
-		}
-
-		up = new PX4IO_Uploader;
-		int ret = up->upload(&fn[0]);
-		delete up;
-
-		switch (ret) {
-		case OK:
-			break;
-
-		case -ENOENT:
-			errx(1, "PX4IO firmware file not found");
-
-		case -EEXIST:
-		case -EIO:
-			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
-
-		case -EINVAL:
-			errx(1, "verify failed - retry the update");
-
-		case -ETIMEDOUT:
-			errx(1, "timed out waiting for bootloader - power-cycle and try again");
-
-		default:
-			errx(1, "unexpected error %d", ret);
-		}
-
-		return ret;
-	}
-
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||