Beat the px4io driver into compilable shape. Just missing RC input configuration now.

apps/drivers/px4io/px4io.cpp

@@ -58,6 +58,7 @@
 #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>
@@ -66,7 +67,6 @@
 
 #include <systemlib/mixer/mixer.h>
 #include <systemlib/perf_counter.h>
-#include <systemlib/hx_stream.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/scheduling_priorities.h>
@@ -94,27 +94,25 @@ public:
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
 
-	/**
-	 * Set the PWM via serial update rate
-	 * @warning this directly affects CPU load
-	 */
-	int 			set_pwm_rate(int hz);
-
-	bool			dump_one;
-
 private:
 	// XXX
-	uint16_t		_max_actuators;
-	uint16_t		_max_servos;
-	uint16_t		_max_rc_input;
-	uint16_t		_max_relays;
+	unsigned		_max_actuators;
+	unsigned		_max_rc_input;
+	unsigned		_max_relays;
+	unsigned		_max_transfer;
 
-	unsigned 		_update_rate;	///< serial send rate in Hz
+	unsigned 		_update_interval; ///< subscription interval limiting send rate
 
 	volatile int		_task;		///< worker task
 	volatile bool		_task_should_exit;
 	volatile bool		_connected;	///< true once we have received a valid frame
 
+	perf_counter_t		_perf_update;
+
+	/* cached IO state */
+	uint16_t		_status;
+	uint16_t		_alarms;
+
 	/* subscribed topics */
 	int			_t_actuators;	///< actuator output topic
 	int			_t_armed;	///< system armed control topic
@@ -157,15 +155,33 @@ private:
 
 	/**
 	 * Fetch status and alarms from IO
+	 *
+	 * Also publishes battery voltage/current.
 	 */
 	int			io_get_status();
 
 	/**
-	 * Fetch RC inputs from IO
+	 * Fetch RC inputs from IO.
 	 *
 	 * @param input_rc	Input structure to populate.
+	 * @return		OK if data was returned.
+	 */
+	int			io_get_raw_rc_input(rc_input_values &input_rc);
+
+	/**
+	 * Fetch and publish raw RC input data.
+	 */
+	int			io_publish_raw_rc();
+
+	/**
+	 * Fetch and publish the mixed control values.
 	 */
-	int			io_get_rc_input(rc_input_values &input_rc);
+	int			io_publish_mixed_controls();
+
+	/**
+	 * Fetch and publish the PWM servo outputs.
+	 */
+	int			io_publish_pwm_outputs();
 
 	/**
 	 * write register(s)
@@ -199,6 +215,16 @@ private:
 	 */
 	int			io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
 
+	/**
+	 * read a register
+	 *
+	 * @param page		Register page to read from.
+	 * @param offset	Register offset to start reading from.
+	 * @return		Register value that was read, or _io_reg_get_error on error.
+	 */
+	uint32_t		io_reg_get(uint8_t page, uint8_t offset);
+	static const uint32_t	_io_reg_get_error = 0x80000000;
+
 	/**
 	 * modify a register
 	 *
@@ -209,6 +235,11 @@ private:
 	 */
 	int			io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
 
+	/**
+	 * Send mixer definition text to IO
+	 */
+	int			mixer_send(const char *buf, unsigned buflen);
+
 };
 
 
@@ -220,14 +251,16 @@ PX4IO	*g_dev;
 }
 
 PX4IO::PX4IO() :
-	CDev("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
-	dump_one(false),
+	I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
 	_max_actuators(0),
-	_max_servos(0),
-	_update_rate(50),
+	_max_rc_input(0),
+	_max_relays(0),
+	_max_transfer(16),	/* sensible default */
+	_update_interval(0),
 	_task(-1),
 	_task_should_exit(false),
 	_connected(false),
+	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
 	_t_actuators(-1),
 	_t_armed(-1),
 	_t_vstatus(-1),
@@ -237,11 +270,7 @@ PX4IO::PX4IO() :
 	_to_battery(0),
 	_mix_buf(nullptr),
 	_mix_buf_len(0),
-	_primary_pwm_device(false),
-	_relays(0),
-	_switch_armed(false),
-	_send_needed(false),
-	_config_needed(true)
+	_primary_pwm_device(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -280,19 +309,22 @@ PX4IO::init()
 		return ret;
 
 	/* get some parameters */
-	if ((ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT, &_max_actuators, 1)) ||
-		(_max_actuators < 1) ||
-		(ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SERVO_COUNT, &_max_servos, 1)) ||
-		(_max_servos < 1) ||
-		(ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT, &_max_relays, 1)) ||
-		(_max_relays < 1) ||
-		(ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT, &_max_rc_input, 1)) ||
-		(_max_rc_input < 1)) {
+	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_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);
+	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
+	if ((_max_actuators < 1) || (_max_actuators == _io_reg_get_error) ||
+	    (_max_relays < 1) || (_max_relays == _io_reg_get_error) ||
+	    (_max_relays < 16) || (_max_relays == _io_reg_get_error) ||
+	    (_max_rc_input < 1) || (_max_rc_input == _io_reg_get_error)) {
 
 		log("failed getting parameters from PX4IO");
 		return ret;
 	}
 
+	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
+		_max_rc_input = RC_INPUT_MAX_CHANNELS;
+
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
 
@@ -327,17 +359,6 @@ PX4IO::init()
 	return OK;
 }
 
-int
-PX4IO::set_pwm_rate(int hz)
-{
-	if (hz > 0 && hz <= 400) {
-		_update_rate = hz;
-		return OK;
-	} else {
-		return -EINVAL;
-	}
-}
-
 void
 PX4IO::task_main_trampoline(int argc, char *argv[])
 {
@@ -347,11 +368,9 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
 void
 PX4IO::task_main()
 {
-	hrt_abstime_t last_poll_time = 0;
-	unsigned poll_phase = 0;
+	hrt_abstime last_poll_time = 0;
 
 	log("starting");
-	unsigned update_rate_in_ms;
 
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -359,10 +378,7 @@ PX4IO::task_main()
 	 */
 	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
 				     ORB_ID(actuator_controls_1));
-
-	/* convert the update rate in hz to milliseconds, rounding down if necessary */
-	update_rate_in_ms = 1000 / _update_rate;
-	orb_set_interval(_t_actuators, update_rate_in_ms);
+	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
 
 	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
@@ -387,10 +403,19 @@ PX4IO::task_main()
 	/* loop talking to IO */
 	while (!_task_should_exit) {
 
-		/* sleep waiting for topic updates, but no more than 20ms */
-		/* XXX should actually be calculated to keep the poller running tidily */
+		/* adjust update interval */
+		if (_update_interval != 0) {
+			if (_update_interval < 5)
+				_update_interval = 5;
+			if (_update_interval > 100)
+				_update_interval = 100;
+			orb_set_interval(_t_actuators, _update_interval);
+			_update_interval = 0;
+		}
+
+		/* sleep waiting for topic updates, but no more than 100ms */
 		unlock();
-		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20);
+		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
 		lock();
 
 		/* this would be bad... */
@@ -407,7 +432,7 @@ PX4IO::task_main()
 		if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
 			io_set_arming_state();
 
-		hrt_abstime_t now = hrt_absolute_time();
+		hrt_abstime now = hrt_absolute_time();
 
 		/*
 		 * If this isn't time for the next tick of the polling state machine,
@@ -417,38 +442,27 @@ PX4IO::task_main()
 			continue;
 
 		/*
-		 * Pull status and alarms from IO
+		 * Pull status and alarms from IO.
 		 */
 		io_get_status();
 
-		switch (poll_phase) {
-		case 0:
-			/* XXX fetch raw RC values */
-			break;
-		case 1:
-			/* XXX fetch servo outputs */
-			break;
-		}
-
-#if 0
-	/* advertise the limited control inputs */
-	memset(&_controls_effective, 0, sizeof(_controls_effective));
-	_to_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
-				   &_controls_effective);
-
-	/* advertise the mixed control outputs */
-	memset(&_outputs, 0, sizeof(_outputs));
-	_to_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
-				   &_outputs);
-#endif
-
+		/*
+		 * Get R/C input from IO.
+		 */
+		io_publish_raw_rc();
 
+		/*
+		 * Fetch mixed servo controls and PWM outputs from IO.
+		 *
+		 * XXX We could do this at a reduced rate in many/most cases.
+		 */
+		io_publish_mixed_controls();
+		io_publish_pwm_outputs();
 
 	}
 
 	unlock();
 
-out:
 	debug("exiting");
 
 	/* clean up the alternate device node */
@@ -468,13 +482,13 @@ PX4IO::io_set_control_state()
 
 	/* get controls */
 	orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-	     ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+	     ORB_ID(actuator_controls_1), _t_actuators, &controls);
 
 	for (unsigned i = 0; i < _max_actuators; i++)
-		regs[i] = FLOAT_TO_REG(_controls.control[i]);
+		regs[i] = FLOAT_TO_REG(controls.control[i]);
 
 	/* copy values to registers in IO */
-	io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
+	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
 }
 
 int
@@ -505,36 +519,40 @@ PX4IO::io_set_arming_state()
 		clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE;
 	}
 
-	io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
 }
 
 int
 PX4IO::io_get_status()
 {
-	struct {
-		uint16_t status;
-		uint16_t alarms;
-		uint16_t vbatt;
-	} state;
+	uint16_t	regs[4];
 	int		ret;
-	bool		rc_valid = false;
 
-	/* get STATUS_FLAGS, STATUS_ALARMS and STATUS_VBATT in that order */
-	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, state, 3);
+	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, regs, sizeof(regs) / sizeof(regs[0]));
+	if (ret != OK)
+		return ret;
+
+	_status = regs[0];
+	_alarms = regs[1];
 
 	/* XXX handle status */
 
 	/* XXX handle alarms */
 
 	/* only publish if battery has a valid minimum voltage */
-	if (state.vbatt > 3300) {
+	if (regs[2] > 3300) {
 		battery_status_s	battery_status;
 
 		battery_status.timestamp = hrt_absolute_time();
-		battery_status.voltage_v = state.vbatt / 1000.0f;
 
-		/* current and discharge are currently (ha) unknown */
-		battery_status.current_a = -1.0f;
+		/* voltage is scaled to mV */
+		battery_status.voltage_v = regs[2] / 1000.0f;
+
+		/* current scaling should be to cA in order to avoid limiting at 65A */
+		battery_status.current_a = regs[3] / 100.f;
+
+		/* this requires integration over time - not currently implemented */
 		battery_status.discharged_mah = -1.0f;
 
 		/* lazily publish the battery voltage */
@@ -542,37 +560,13 @@ PX4IO::io_get_status()
 			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
 		} else {
 			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
-	}
-
-	/*
-	 * If we have RC input, get it
-	 */
-	if (state.status & PX4IO_P_STATUS_FLAGS_RC_OK) {
-		rc_input_values		input_rc;
-
-		io_get_rc_input(input_rc);
-
-		if (state.status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
-			input_rc.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
-		} else if (state.status & RC_INPUT_SOURCE_PX4IO_DSM) {
-			input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
-		} else if (state.status & RC_INPUT_SOURCE_PX4IO_SBUS) {
-			input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
-		} else {
-			input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
-		}
-
-		if (_to_input_rc > 0) {
-			orb_publish(ORB_ID(input_rc), _to_input_rc, &input_rc);
-		} else {
-			_to_input_rc = orb_advertise(ORB_ID(input_rc), &input_rc);
 		}
 	}
-
+	return ret;
 }
 
 int
-PX4IO::io_get_rc_input(rc_input_values &input_rc)
+PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 {
 	uint16_t channel_count;
 	int	ret;
@@ -592,38 +586,165 @@ PX4IO::io_get_rc_input(rc_input_values &input_rc)
 	 *
 	 * 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...
 	 */
-	ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1);
-	if (ret)
+	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1);
+	if (ret != OK)
 		return ret;
 	input_rc.channel_count = channel_count;
 
 	if (channel_count > 0)
-		ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, channel_count);
+		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
 
 	return ret;
 }
 
+int
+PX4IO::io_publish_raw_rc()
+{
+	/* if no RC, just don't publish */
+	if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+		return OK;
+
+	/* fetch values from IO */
+	rc_input_values	rc_val;
+	rc_val.timestamp = hrt_absolute_time();
+
+	int ret = io_get_raw_rc_input(rc_val);
+	if (ret != OK)
+		return ret;
+
+	/* sort out the source of the values */
+	if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+	} else {
+		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
+	}
+
+	/* lazily advertise on first publication */
+	if (_to_input_rc == 0) {
+		_to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
+	} else { 
+		orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
+	}
+
+	return OK;
+}
+
+int
+PX4IO::io_publish_mixed_controls()
+{
+	/* if no FMU comms(!) just don't publish */
+	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
+		return OK;
+
+	/* if not taking raw PPM from us, must be mixing */
+	if (_status & PX4IO_P_STATUS_FLAGS_RAW_PPM)
+		return OK;
+
+	/* data we are going to fetch */
+	actuator_controls_effective_s controls_effective;
+	controls_effective.timestamp = hrt_absolute_time();
+
+	/* get actuator controls from IO */
+	uint16_t act[_max_actuators];
+	int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
+	if (ret != OK)
+		return ret;
+
+	/* convert from register format to float */
+	for (unsigned i = 0; i < _max_actuators; i++)
+		controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
+
+	/* laxily advertise on first publication */
+	if (_to_actuators_effective == 0) {
+		_to_actuators_effective = 
+			orb_advertise((_primary_pwm_device ? 
+				ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
+				ORB_ID(actuator_controls_effective_1)),
+					   &controls_effective);
+	} else {
+		orb_publish((_primary_pwm_device ? 
+			ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : 
+			ORB_ID(actuator_controls_effective_1)),
+			_to_actuators_effective, &controls_effective);
+	}
+
+	return OK;
+}
+
+int
+PX4IO::io_publish_pwm_outputs()
+{
+	/* if no FMU comms(!) just don't publish */
+	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
+		return OK;
+
+	/* data we are going to fetch */
+	actuator_outputs_s outputs;
+	outputs.timestamp = hrt_absolute_time();
+
+	/* get servo values from IO */
+	uint16_t ctl[_max_actuators];
+	int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
+	if (ret != OK)
+		return ret;
+
+	/* convert from register format to float */
+	for (unsigned i = 0; i < _max_actuators; i++)
+		outputs.output[i] = REG_TO_FLOAT(ctl[i]);
+	outputs.noutputs = _max_actuators;
+
+	/* lazily advertise on first publication */
+	if (_to_outputs == 0) {
+		_to_outputs = orb_advertise((_primary_pwm_device ?
+			ORB_ID_VEHICLE_CONTROLS :
+			ORB_ID(actuator_outputs_1)),
+			&outputs);
+	} else {
+		orb_publish((_primary_pwm_device ?
+			ORB_ID_VEHICLE_CONTROLS :
+			ORB_ID(actuator_outputs_1)),
+			_to_outputs,
+			&outputs);
+	}
+
+	return OK;
+}
+
+
 int
 PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
 	i2c_msg_s	msgv[2];
-	t8_t		hdr[2];
+	uint8_t		hdr[2];
 
 	hdr[0] = page;
 	hdr[1] = offset;
 
-	mgsv[0].flags = 0;
+	msgv[0].flags = 0;
 	msgv[0].buffer = hdr;
 	msgv[0].length = sizeof(hdr);
 
 	msgv[1].flags = 0;
-	msgv[1].buffer = const_cast<uint8_t *>(values);
+	msgv[1].buffer = (uint8_t *)(values);
 	msgv[1].length = num_values * sizeof(*values);
 
 	return transfer(msgv, 2);
 }
 
+int
+PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
+{
+	return io_reg_set(page, offset, &value, 1);
+}
+
 int
 PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
 {
@@ -633,17 +754,28 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
 	hdr[0] = page;
 	hdr[1] = offset;
 
-	mgsv[0].flags = 0;
+	msgv[0].flags = 0;
 	msgv[0].buffer = hdr;
 	msgv[0].length = sizeof(hdr);
 
 	msgv[1].flags = I2C_M_READ;
-	msgv[1].buffer = values;
+	msgv[1].buffer = (uint8_t *)values;
 	msgv[1].length = num_values * sizeof(*values);
 
 	return transfer(msgv, 2);
 }
 
+uint32_t
+PX4IO::io_reg_get(uint8_t page, uint8_t offset)
+{
+	uint16_t value;
+
+	if (io_reg_get(page, offset, &value, 1))
+		return _io_reg_get_error;
+
+	return value;
+}
+
 int
 PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits)
 {
@@ -659,104 +791,6 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 	return io_reg_set(page, offset, &value, 1);
 }
 
-
-
-
-void
-PX4IO::io_recv()
-{
-	uint8_t buf[32];
-	int count;
-
-	/*
-	 * We are here because poll says there is some data, so this
-	 * won't block even on a blocking device.  If more bytes are
-	 * available, we'll go back to poll() again...
-	 */
-	count = ::read(_serial_fd, buf, sizeof(buf));
-
-	/* pass received bytes to the packet decoder */
-	for (int i = 0; i < count; i++)
-		hx_stream_rx(_io_stream, buf[i]);
-}
-
-void
-PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received)
-{
-	g_dev->rx_callback((const uint8_t *)buffer, bytes_received);
-}
-
-void
-PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
-{
-	const px4io_report *rep = (const px4io_report *)buffer;
-
-//	lock();
-
-	/* sanity-check the received frame size */
-	if (bytes_received != sizeof(px4io_report)) {
-		debug("got %u expected %u", bytes_received, sizeof(px4io_report));
-		goto out;
-	}
-
-	if (rep->i2f_magic != I2F_MAGIC) {
-		debug("bad magic");
-		goto out;
-	}
-
-	_connected = true;
-
-	/* publish raw rc channel values from IO if valid channels are present */
-	if (rep->channel_count > 0) {
-		_input_rc.timestamp = hrt_absolute_time();
-		_input_rc.channel_count = rep->channel_count;
-
-		for (int i = 0; i < rep->channel_count; i++) {
-			_input_rc.values[i] = rep->rc_channel[i];
-		}
-
-		orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
-	}
-
-	/* remember the latched arming switch state */
-	_switch_armed = rep->armed;
-
-	/* publish battery information */
-
-	/* only publish if battery has a valid minimum voltage */
-	if (rep->battery_mv > 3300) {
-		_battery_status.timestamp = hrt_absolute_time();
-		_battery_status.voltage_v = rep->battery_mv / 1000.0f;
-		/* current and discharge are unknown */
-		_battery_status.current_a = -1.0f;
-		_battery_status.discharged_mah = -1.0f;
-		/* announce the battery voltage if needed, just publish else */
-		if (_to_battery > 0) {
-			orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
-		} else {
-			_to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
-		}
-	}
-
-	_send_needed = true;
-
-	/* if monitoring, dump the received info */
-	if (dump_one) {
-		dump_one = false;
-
-		printf("IO: %s armed ", rep->armed ? "" : "not");
-
-		for (unsigned i = 0; i < rep->channel_count; i++)
-			printf("%d: %d ", i, rep->rc_channel[i]);
-
-		printf("\n");
-	}
-
-out:
-//	unlock();
-	return;
-}
-
 #if 0
 void
 PX4IO::config_send()
@@ -812,12 +846,14 @@ PX4IO::config_send()
 	if (ret)
 		debug("config error %d", ret);
 }
+#endif
 
 int
 PX4IO::mixer_send(const char *buf, unsigned buflen)
 {
-	uint8_t	frame[HX_STREAM_MAX_FRAME];
+	uint8_t	frame[_max_transfer];
 	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+	unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
 
 	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
 	msg->action = F2I_MIXER_ACTION_RESET;
@@ -825,8 +861,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
 	do {
 		unsigned count = buflen;
 
-		if (count > F2I_MIXER_MAX_TEXT)
-			count = F2I_MIXER_MAX_TEXT;
+		if (count > max_len)
+			count = max_len;
 
 		if (count > 0) {
 			memcpy(&msg->text[0], buf, count);
@@ -834,7 +870,20 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
 			buflen -= count;
 		}
 
-		int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
+		/*
+		 * We have to send an even number of bytes.  This
+		 * will only happen on the very last transfer of a
+		 * mixer, and we are guaranteed that there will be
+		 * space left to round up as _max_transfer will be
+		 * even.
+		 */
+		unsigned total_len = sizeof(px4io_mixdata) + count;
+		if (total_len % 1) {
+			msg->text[count] = '\0';
+			total_len++;
+		}
+
+		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
 
 		if (ret) {
 			log("mixer send error %d", ret);
@@ -845,17 +894,19 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
 
 	} while (buflen > 0);
 
-	return 0;
+	/* check for the mixer-OK flag */
+	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)
+		return 0;
+
+	/* load must have failed for some reason */
+	return -EINVAL;
 }
-#endif
 
 int
 PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
-	lock();
-
 	/* regular ioctl? */
 	switch (cmd) {
 	case PWM_SERVO_ARM:
@@ -877,50 +928,59 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		}
 		break;
 
-	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_servos - 1): {
+	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): {
 
 		unsigned channel = cmd - PWM_SERVO_SET(0);
-
-		/* send a direct PWM value */
-		if ((arg >= 900) && (arg <= 2100)) {
-			ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
-		} else {
+		if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
 			ret = -EINVAL;
+		} else {
+			/* send a direct PWM value */
+			ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
 		}
 
 		break;
 	}
 
-	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_servos - 1): {
+	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): {
 
 		unsigned channel = cmd - PWM_SERVO_GET(0);
 
-		/* fetch a current PWM value */
-		ret = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel, (uint16_t *)arg, 1);
+		if (channel >= _max_actuators) {
+			ret = -EINVAL;
+		} else {
+			/* fetch a current PWM value */
+			uint32_t value = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel);
+			if (value == _io_reg_get_error) {
+				ret = -EIO;
+			} else {
+				*(servo_position_t *)arg = value;
+			}
+		}
 		break;
 	}
 
-	case GPIO_RESET: {
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg);
+	case GPIO_RESET:
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
 		break;
-	}
 
 	case GPIO_SET:
 		arg &= ((1 << _max_relays) - 1);
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, value);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
 		break;
 
 	case GPIO_CLEAR:
 		arg &= ((1 << _max_relays) - 1);
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, value, 0);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
 		break;
 
 	case GPIO_GET:
-		ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, (uint16_t *)arg, 1);
+		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
+		if (*(uint32_t *)arg == _io_reg_get_error)
+			ret = -EIO;
 		break;
 
 	case MIXERIOCGETOUTPUTCOUNT:
-		*(unsigned *)arg = _max_servos;
+		*(unsigned *)arg = _max_actuators;
 		break;
 
 	case MIXERIOCRESET:
@@ -928,23 +988,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCLOADBUF:
-
-
-XXX
-
-		/* set the buffer up for transfer */
-		_mix_buf = (const char *)arg;
-		_mix_buf_len = strnlen(_mix_buf, 1024);
-
-		/* drop the lock and wait for the thread to clear the transmit */
-		unlock();
-
-		while (_mix_buf != nullptr)
-			usleep(1000);
-
-		lock();
-
-		ret = 0;
+		ret = mixer_send((const char *)arg, strnlen(_mix_buf, 1024));
 		break;
 
 	default:
@@ -952,21 +996,19 @@ XXX
 		ret = -ENOTTY;
 	}
 
-	unlock();
-
 	return ret;
 }
 
 ssize_t
-write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
 	int ret;
 
 	if (count > 0) {
-		if (count > _max_servos)
-			count = _max_servos;
-		ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, buffer, count);
+		if (count > _max_actuators)
+			count = _max_actuators;
+		ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
 	} else {
 		ret = -EINVAL;
 	}
@@ -1034,8 +1076,10 @@ monitor(void)
 				exit(0);
 		}
 
-		if (g_dev != nullptr)
-			g_dev->dump_one = true;
+#warning implement this
+
+//		if (g_dev != nullptr)
+//			g_dev->dump_one = true;
 	}
 }
 
@@ -1063,7 +1107,7 @@ px4io_main(int argc, char *argv[])
 		/* look for the optional pwm update rate for the supported modes */
 		if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
 			if (argc > 2 + 1) {
-				g_dev->set_pwm_rate(atoi(argv[2 + 1]));
+#warning implement this 
 			} else {
 				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
 				return 1;

apps/px4io/mixer.cpp

@@ -274,6 +274,12 @@ mixer_callback(uintptr_t handle,
 	return 0;
 }
 
+/*
+ * XXX error handling here should be more aggressive; currently it is
+ * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
+ * not loaded faithfully.
+ */
+
 static char mixer_text[256];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;