Merged latest master

6 files changed, 175 insertions, 121 deletions

diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h
index 793e86b32..9f43015d9 100644
--- a/apps/drivers/drv_mixer.h
+++ b/apps/drivers/drv_mixer.h
@@ -100,28 +100,13 @@ struct mixer_simple_s {
  */
 #define MIXERIOCADDSIMPLE	_MIXERIOC(2)
 
-/** multirotor output definition */
-struct mixer_rotor_output_s {
-	float			angle;		/**< rotor angle clockwise from forward in radians */
-	float			distance;	/**< motor distance from centre in arbitrary units */
-};
-
-/** multirotor mixer */
-struct mixer_multirotor_s {
-	uint8_t			rotor_count;
-	struct mixer_control_s	controls[4];	/**< controls are roll, pitch, yaw, thrust */
-	struct mixer_rotor_output_s rotors[0];	/**< actual size of the array is set by rotor_count */
-};
-
-/**
- * Add a multirotor mixer in (struct mixer_multirotor_s *)arg
- */
-#define MIXERIOCADDMULTIROTOR	_MIXERIOC(3)
+/* _MIXERIOC(3) was deprecated */
+/* _MIXERIOC(4) was deprecated */
 
 /**
- * Add mixers(s) from a the file in (const char *)arg
+ * Add mixer(s) from the buffer in (const char *)arg
  */
-#define MIXERIOCLOADFILE	_MIXERIOC(4)
+#define MIXERIOCLOADBUF		_MIXERIOC(5)
 
 /*
  * XXX Thoughts for additional operations:
diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index b2fee65ac..c110cd5cb 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
 /** disarm all servo outputs (stop generating pulses) */
 #define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
 
+/** set update rate in Hz */
+#define PWM_SERVO_SET_UPDATE_RATE	_IOC(_PWM_SERVO_BASE, 2)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 67b16aa42..78c061819 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -503,6 +503,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		// up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		g_hil->set_pwm_rate(arg);
+		break;
+
 	case PWM_SERVO_SET(2):
 	case PWM_SERVO_SET(3):
 		if (_mode != MODE_4PWM) {
@@ -577,26 +581,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
-	case MIXERIOCADDMULTIROTOR:
-		/* XXX not yet supported */
-		ret = -ENOTTY;
-		break;
-
-	case MIXERIOCLOADFILE: {
-			const char *path = (const char *)arg;
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
 
-			if (_mixers != nullptr) {
-				delete _mixers;
-				_mixers = nullptr;
-			}
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
-			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 			if (_mixers == nullptr) {
 				ret = -ENOMEM;
+
 			} else {
 
-				debug("loading mixers from %s", path);
-				ret = _mixers->load_from_file(path);
+				ret = _mixers->load_from_buf(buf, buflen);
 
 				if (ret != 0) {
 					debug("mixer load failed with %d", ret);
@@ -605,10 +602,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
-
 			break;
 		}
 
+
 	default:
 		ret = -ENOTTY;
 		break;
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 2e3b130a9..3b835904f 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -470,6 +470,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		set_pwm_rate(arg);
+		break;
+
 	case PWM_SERVO_SET(2):
 	case PWM_SERVO_SET(3):
 		if (_mode != MODE_4PWM) {
@@ -544,28 +548,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
-	case MIXERIOCADDMULTIROTOR:
-		/* XXX not yet supported */
-		ret = -ENOTTY;
-		break;
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
 
-	case MIXERIOCLOADFILE: {
-			const char *path = (const char *)arg;
-
-			if (_mixers != nullptr) {
-				delete _mixers;
-				_mixers = nullptr;
-			}
-
-			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
 			if (_mixers == nullptr) {
 				ret = -ENOMEM;
 
 			} else {
 
-				debug("loading mixers from %s", path);
-				ret = _mixers->load_from_file(path);
+				ret = _mixers->load_from_buf(buf, buflen);
 
 				if (ret != 0) {
 					debug("mixer load failed with %d", ret);
@@ -574,7 +569,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
-
 			break;
 		}
 
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index f6b350cc9..82ff61808 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -61,6 +61,7 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
 
@@ -92,7 +93,7 @@ public:
 	bool			dump_one;
 
 private:
-	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
+	static const unsigned	_max_actuators = PX4IO_CONTROL_CHANNELS;
 
 	int			_serial_fd;	///< serial interface to PX4IO
 	hx_stream_t		_io_stream;	///< HX protocol stream
@@ -116,10 +117,13 @@ private:
 	orb_advert_t		_t_outputs;	///< mixed outputs topic
 	actuator_outputs_s	_outputs;	///< mixed outputs
 
-	MixerGroup		*_mixers;	///< loaded mixers
+	const char *volatile	_mix_buf;	///< mixer text buffer
+	volatile unsigned	_mix_buf_len;	///< size of the mixer text buffer
 
 	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
+	uint32_t		_relays;	///< state of the PX4IO relays, one bit per relay
+
 	volatile bool		_switch_armed;	///< PX4IO switch armed state
 	// XXX how should this work?
 
@@ -162,6 +166,11 @@ private:
 	void			config_send();
 
 	/**
+	 * Send a buffer containing mixer text to PX4IO
+	 */
+	int			mixer_send(const char *buf, unsigned buflen);
+
+	/**
 	 * Mixer control callback; invoked to fetch a control from a specific
 	 * group/index during mixing.
 	 */
@@ -190,8 +199,10 @@ PX4IO::PX4IO() :
 	_t_actuators(-1),
 	_t_armed(-1),
 	_t_outputs(-1),
-	_mixers(nullptr),
+	_mix_buf(nullptr),
+	_mix_buf_len(0),
 	_primary_pwm_device(false),
+	_relays(0),
 	_switch_armed(false),
 	_send_needed(false),
 	_config_needed(false)
@@ -212,6 +223,7 @@ PX4IO::~PX4IO()
 		/* give it another 100ms */
 		usleep(100000);
 	}
+
 	/* well, kill it anyway, though this will probably crash */
 	if (_task != -1)
 		task_delete(_task);
@@ -242,6 +254,7 @@ PX4IO::init()
 
 	/* start the IO interface task */
 	_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
 		return -errno;
@@ -253,13 +266,16 @@ PX4IO::init()
 			debug("PX4IO connected");
 			break;
 		}
+
 		usleep(100000);
 	}
+
 	if (!_connected) {
 		/* error here will result in everything being torn down */
 		log("PX4IO not responding");
 		return -EIO;
 	}
+
 	return OK;
 }
 
@@ -272,7 +288,7 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
 void
 PX4IO::task_main()
 {
-	log("starting");
+	debug("starting");
 
 	/* open the serial port */
 	_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@@ -294,10 +310,12 @@ PX4IO::task_main()
 
 	/* protocol stream */
 	_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
+
 	if (_io_stream == nullptr) {
 		log("failed to allocate HX protocol stream");
 		goto out;
 	}
+
 	hx_stream_set_counters(_io_stream,
 			       perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
 			       perf_alloc(PC_COUNT, "PX4IO frames received"),
@@ -338,13 +356,18 @@ PX4IO::task_main()
 	fds[2].fd = _t_armed;
 	fds[2].events = POLLIN;
 
-	log("ready");
+	debug("ready");
+
+	/* lock against the ioctl handler */
+	lock();
 
 	/* loop handling received serial bytes */
 	while (!_task_should_exit) {
 
 		/* sleep waiting for data, but no more than 100ms */
+		unlock();
 		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
+		lock();
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -361,26 +384,21 @@ PX4IO::task_main()
 			if (fds[0].revents & POLLIN)
 				io_recv();
 
-			/* if we have new data from the ORB, go handle it */
+			/* if we have new control data from the ORB, handle it */
 			if (fds[1].revents & POLLIN) {
 
 				/* get controls */
 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
 
-				/* mix */
-				if (_mixers != nullptr) {
-					/* XXX is this the right count? */
-					_mixers->mix(&_outputs.output[0], _max_actuators);
+				/* scale controls to PWM (temporary measure) */
+				for (unsigned i = 0; i < _max_actuators; i++)
+					_outputs.output[i] = 1500 + (600 * _controls.control[i]);
 
-					/* convert to PWM values */
-					for (unsigned i = 0; i < _max_actuators; i++)
-						_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
-
-					/* and flag for update */
-					_send_needed = true;
-				}
+				/* and flag for update */
+				_send_needed = true;
 			}
 
+			/* if we have an arming state update, handle it */
 			if (fds[2].revents & POLLIN) {
 
 				orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
@@ -399,14 +417,26 @@ PX4IO::task_main()
 			_config_needed = false;
 			config_send();
 		}
+
+		/* send a mixer update if needed */
+		if (_mix_buf != nullptr) {
+			mixer_send(_mix_buf, _mix_buf_len);
+
+			/* clear the buffer record so the ioctl handler knows we're done */
+			_mix_buf = nullptr;
+			_mix_buf_len = 0;
+		}
 	}
 
+	unlock();
+
 out:
 	debug("exiting");
 
 	/* kill the HX stream */
 	if (_io_stream != nullptr)
 		hx_stream_free(_io_stream);
+
 	::close(_serial_fd);
 
 	/* clean up the alternate device node */
@@ -459,25 +489,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 {
 	const px4io_report *rep = (const px4io_report *)buffer;
 
-	lock();
+//	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++)
-		{
+
+		for (int i = 0; i < rep->channel_count; i++) {
 			_input_rc.values[i] = rep->rc_channel[i];
 		}
 
@@ -511,13 +543,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 		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();
+//	unlock();
+	return;
 }
 
 void
@@ -530,17 +565,21 @@ PX4IO::io_send()
 
 	/* set outputs */
 	for (unsigned i = 0; i < _max_actuators; i++)
-		cmd.servo_command[i] = _outputs.output[i];
+		cmd.output_control[i] = _outputs.output[i];
 
 	/* publish as we send */
+	/* XXX needs to be based off post-mix values from the IO side */
 	orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
 
-	// XXX relays
+	/* update relays */
+	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
+		cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
 
 	/* armed and not locked down */
 	cmd.arm_ok = (_armed.armed && !_armed.lockdown);
 
 	ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
+
 	if (ret)
 		debug("send error %d", ret);
 }
@@ -554,11 +593,47 @@ PX4IO::config_send()
 	cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
 
 	ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
+
 	if (ret)
 		debug("config error %d", ret);
 }
 
 int
+PX4IO::mixer_send(const char *buf, unsigned buflen)
+{
+	uint8_t	frame[HX_STREAM_MAX_FRAME];
+	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+
+	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+	msg->action = F2I_MIXER_ACTION_RESET;
+
+	do {
+		unsigned count = buflen;
+
+		if (count > F2I_MIXER_MAX_TEXT)
+			count = F2I_MIXER_MAX_TEXT;
+
+		if (count > 0) {
+			memcpy(&msg->text[0], buf, count);
+			buf += count;
+			buflen -= count;
+		}
+
+		int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
+
+		if (ret) {
+			log("mixer send error %d", ret);
+			return ret;
+		}
+
+		msg->action = F2I_MIXER_ACTION_APPEND;
+
+	} while (buflen > 0);
+
+	return 0;
+}
+
+int
 PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
 	int ret = OK;
@@ -579,9 +654,14 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		_send_needed = true;
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		// not supported yet
+		ret = -EINVAL;
+		break;
+
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
 
-		/* fake an update to the selected servo channel */
+		/* fake an update to the selected 'servo' channel */
 		if ((arg >= 900) && (arg <= 2100)) {
 			_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
 			_send_needed = true;
@@ -597,68 +677,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
 		break;
 
-	case MIXERIOCGETOUTPUTCOUNT:
-		*(unsigned *)arg = _max_actuators;
+	case GPIO_RESET:
+		_relays = 0;
+		_send_needed = true;
 		break;
 
-	case MIXERIOCRESET:
-		if (_mixers != nullptr) {
-			delete _mixers;
-			_mixers = nullptr;
+	case GPIO_SET:
+	case GPIO_CLEAR:
+		/* make sure only valid bits are being set */
+		if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
+			ret = EINVAL;
+			break;
 		}
-
+		if (cmd == GPIO_SET) {
+			_relays |= arg;
+		} else {
+			_relays &= ~arg;
+		}
+		_send_needed = true;
 		break;
 
-	case MIXERIOCADDSIMPLE: {
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
-			/* build the new mixer from the supplied argument */
-			SimpleMixer *mixer = new SimpleMixer(control_callback,
-							     (uintptr_t)&_controls, mixinfo);
-
-			/* validate the new mixer */
-			if (mixer->check()) {
-				delete mixer;
-				ret = -EINVAL;
-
-			} else {
-				/* if we don't have a group yet, allocate one */
-				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
-
-				/* add the new mixer to the group */
-				_mixers->add_mixer(mixer);
-			}
+	case GPIO_GET:
+		*(uint32_t *)arg = _relays;
+		break;
 
-		}
+	case MIXERIOCGETOUTPUTCOUNT:
+		*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
 		break;
 
-	case MIXERIOCADDMULTIROTOR:
-		/* XXX not yet supported */
-		ret = -ENOTTY;
+	case MIXERIOCRESET:
+		ret = 0;	/* load always resets */
 		break;
 
-	case MIXERIOCLOADFILE: {
-			MixerGroup *newmixers;
-			const char *path = (const char *)arg;
+	case MIXERIOCLOADBUF:
 
-			/* allocate a new mixer group and load it from the file */
-			newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+		/* set the buffer up for transfer */
+		_mix_buf = (const char *)arg;
+		_mix_buf_len = strnlen(_mix_buf, 1024);
 
-			if (newmixers->load_from_file(path) != 0) {
-				delete newmixers;
-				ret = -EINVAL;
-			}
+		/* drop the lock and wait for the thread to clear the transmit */
+		unlock();
 
-			/* swap the new mixers in for the old */
-			if (_mixers != nullptr) {
-				delete _mixers;
-			}
+		while (_mix_buf != nullptr)
+			usleep(1000);
 
-			_mixers = newmixers;
+		lock();
 
-		}
+		ret = 0;
 		break;
 
 	default:
@@ -700,6 +765,13 @@ test(void)
 
 	close(fd);
 
+	actuator_armed_s aa;
+
+	aa.armed = true;
+	aa.lockdown = false;
+
+	orb_advertise(ORB_ID(actuator_armed), &aa);
+
 	exit(0);
 }
 
@@ -719,6 +791,7 @@ monitor(void)
 		if (fds[0].revents == POLLIN) {
 			int c;
 			read(0, &c, 1);
+
 			if (cancels-- == 0)
 				exit(0);
 		}
@@ -805,6 +878,7 @@ px4io_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "test"))
 		test();
+
 	if (!strcmp(argv[1], "monitor"))
 		monitor();
 
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
index 8b354ff60..6442e947c 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/apps/drivers/px4io/uploader.cpp
@@ -190,6 +190,7 @@ PX4IO_Uploader::drain()
 
 	do {
 		ret = recv(c, 250);
+
 		if (ret == OK) {
 			//log("discard 0x%02x", c);
 		}