Merged latest master

.gitignore

@@ -11,6 +11,7 @@ Make.dep
 *.o
 *.a
 *~
+*.dSYM
 Images/*.bin
 Images/*.px4
 nuttx/Make.defs
@@ -40,3 +41,4 @@ nsh_romfsimg.h
 cscope.out
 .configX-e
 nuttx-export.zip
+dot.gdbinit

apps/Makefile

@@ -112,6 +112,9 @@ endef
 
 $(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
 
+# EXTERNAL_APPS is used to add out of tree apps to the build
+INSTALLED_APPS += $(EXTERNAL_APPS)
+
 # The external/ directory may also be added to the INSTALLED_APPS.  But there
 # is no external/ directory in the repository.  Rather, this directory may be
 # provided by the user (possibly as a symbolic link) to add libraries and

apps/drivers/drv_mixer.h

@@ -100,28 +100,13 @@ struct mixer_simple_s {
  */
 #define MIXERIOCADDSIMPLE	_MIXERIOC(2)
 
-/** multirotor output definition */
+/* _MIXERIOC(3) was deprecated */
-struct mixer_rotor_output_s {
+/* _MIXERIOC(4) was deprecated */
-	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
+ * Add mixer(s) from the buffer in (const char *)arg
  */
-#define MIXERIOCADDMULTIROTOR	_MIXERIOC(3)
+#define MIXERIOCLOADBUF		_MIXERIOC(5)
-
-/**
- * Add mixers(s) from a the file in (const char *)arg
- */
-#define MIXERIOCLOADFILE	_MIXERIOC(4)
 
 /*
  * XXX Thoughts for additional operations:

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)
 

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:
+	case MIXERIOCLOADBUF: {
-		/* XXX not yet supported */
+			const char *buf = (const char *)arg;
-		ret = -ENOTTY;
+			unsigned buflen = strnlen(buf, 1024);
-		break;
 
-	case MIXERIOCLOADFILE: {
+			if (_mixers == nullptr)
-			const char *path = (const char *)arg;
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
-			if (_mixers != nullptr) {
-				delete _mixers;
-				_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_buf(buf, buflen);
-				ret = _mixers->load_from_file(path);
 
 				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;

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:
+	case MIXERIOCLOADBUF: {
-		/* XXX not yet supported */
+			const char *buf = (const char *)arg;
-		ret = -ENOTTY;
+			unsigned buflen = strnlen(buf, 1024);
-		break;
 
-	case MIXERIOCLOADFILE: {
+			if (_mixers == nullptr)
-			const char *path = (const char *)arg;
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-
-			if (_mixers != nullptr) {
-				delete _mixers;
-				_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_buf(buf, buflen);
-				ret = _mixers->load_from_file(path);
 
 				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;
 		}
 

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?
 
@@ -161,6 +165,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 */
+				/* scale controls to PWM (temporary measure) */
-				if (_mixers != nullptr) {
+				for (unsigned i = 0; i < _max_actuators; i++)
-					/* XXX is this the right count? */
+					_outputs.output[i] = 1500 + (600 * _controls.control[i]);
-					_mixers->mix(&_outputs.output[0], _max_actuators);
 
-					/* convert to PWM values */
+				/* and flag for update */
-					for (unsigned i = 0; i < _max_actuators; i++)
+				_send_needed = true;
-						_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
-
-					/* 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,10 +593,46 @@ 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)
 {
@@ -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 GPIO_RESET:
+		_relays = 0;
+		_send_needed = true;
+		break;
+
+	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 GPIO_GET:
+		*(uint32_t *)arg = _relays;
+		break;
+
 	case MIXERIOCGETOUTPUTCOUNT:
-		*(unsigned *)arg = _max_actuators;
+		*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
 		break;
 
 	case MIXERIOCRESET:
-		if (_mixers != nullptr) {
+		ret = 0;	/* load always resets */
-			delete _mixers;
-			_mixers = nullptr;
-		}
-
 		break;
 
-	case MIXERIOCADDSIMPLE: {
+	case MIXERIOCLOADBUF:
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
-			/* build the new mixer from the supplied argument */
+		/* set the buffer up for transfer */
-			SimpleMixer *mixer = new SimpleMixer(control_callback,
+		_mix_buf = (const char *)arg;
-							     (uintptr_t)&_controls, mixinfo);
+		_mix_buf_len = strnlen(_mix_buf, 1024);
 
-			/* validate the new mixer */
+		/* drop the lock and wait for the thread to clear the transmit */
-			if (mixer->check()) {
+		unlock();
-				delete mixer;
-				ret = -EINVAL;
 
-			} else {
+		while (_mix_buf != nullptr)
-				/* if we don't have a group yet, allocate one */
+			usleep(1000);
-				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
 
-				/* add the new mixer to the group */
+		lock();
-				_mixers->add_mixer(mixer);
-			}
 
-		}
+		ret = 0;
-		break;
-
-	case MIXERIOCADDMULTIROTOR:
-		/* XXX not yet supported */
-		ret = -ENOTTY;
-		break;
-
-	case MIXERIOCLOADFILE: {
-			MixerGroup *newmixers;
-			const char *path = (const char *)arg;
-
-			/* allocate a new mixer group and load it from the file */
-			newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-
-			if (newmixers->load_from_file(path) != 0) {
-				delete newmixers;
-				ret = -EINVAL;
-			}
-
-			/* swap the new mixers in for the old */
-			if (_mixers != nullptr) {
-				delete _mixers;
-			}
-
-			_mixers = newmixers;
-
-		}
 		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();
 

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);
 		}

apps/mavlink/mavlink_log.c

@@ -40,6 +40,7 @@
  */
 
 #include <string.h>
+#include <stdlib.h>
 
 #include "mavlink_log.h"
  
@@ -51,7 +52,7 @@ void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) {
 }
  
 int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) {
-    return lb->count == lb->size;
+	return lb->count == (int)lb->size;
 }
  
 int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) {
@@ -77,4 +78,4 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
     } else {
         return 1;
     }
-}
+}

apps/px4io/Makefile

@@ -41,7 +41,9 @@
 #
 CSRCS		= $(wildcard *.c) \
 		  ../systemlib/hx_stream.c \
-		  ../systemlib/perf_counter.c
+		  ../systemlib/perf_counter.c \
+		  ../systemlib/up_cxxinitialize.c
+CXXSRCS		= $(wildcard *.cpp)
 
 INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
 

apps/px4io/comms.c

@@ -69,6 +69,8 @@ static struct px4io_report	report;
 
 static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
 
+perf_counter_t			comms_rx_errors;
+
 static void
 comms_init(void)
 {
@@ -76,6 +78,9 @@ comms_init(void)
 	fmu_fd = open("/dev/ttyS1", O_RDWR);
 	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
 
+	comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
+	hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
+
 	/* default state in the report to FMU */
 	report.i2f_magic = I2F_MAGIC;
 
@@ -112,6 +117,7 @@ comms_main(void)
 		if (fds.revents & POLLIN) {
 			char buf[32];
 			ssize_t count = read(fmu_fd, buf, sizeof(buf));
+
 			for (int i = 0; i < count; i++)
 				hx_stream_rx(stream, buf[i]);
 		}
@@ -125,7 +131,8 @@ comms_main(void)
 		/* should we send a report to the FMU? */
 		now = hrt_absolute_time();
 		delta = now - last_report_time;
-		if ((delta > FMU_MIN_REPORT_INTERVAL) && 
+
+		if ((delta > FMU_MIN_REPORT_INTERVAL) &&
 		    (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
 
 			system_state.fmu_report_due = false;
@@ -134,6 +141,7 @@ comms_main(void)
 			/* populate the report */
 			for (unsigned i = 0; i < system_state.rc_channels; i++)
 				report.rc_channel[i] = system_state.rc_channel_data[i];
+
 			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
@@ -211,31 +219,45 @@ comms_handle_command(const void *buffer, size_t length)
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
-	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
+	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
-		system_state.fmu_channel_data[i] = cmd->servo_command[i];
+		system_state.fmu_channel_data[i] = cmd->output_control[i];
 
 	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
-	if(system_state.arm_ok && !cmd->arm_ok) {
+	if (system_state.arm_ok && !cmd->arm_ok)
 		system_state.armed = false;
-	}
 
 	system_state.arm_ok = cmd->arm_ok;
 	system_state.mixer_use_fmu = true;
 	system_state.fmu_data_received = true;
 
-
 	/* handle changes signalled by FMU */
 //	if (!system_state.arm_ok && system_state.armed)
 //		system_state.armed = false;
 
-	/* XXX do relay changes here */	
+	/* handle relay state changes here */	
-	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
+	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
+		if (system_state.relays[i] != cmd->relay_state[i]) {
+			switch (i) {
+			case 0:
+				POWER_ACC1(cmd->relay_state[i]);
+				break;
+			case 1:
+				POWER_ACC2(cmd->relay_state[i]);
+				break;
+			case 2:
+				POWER_RELAY1(cmd->relay_state[i]);
+				break;
+			case 3:
+				POWER_RELAY2(cmd->relay_state[i]);
+				break;
+			}
+		}
 		system_state.relays[i] = cmd->relay_state[i];
+	}
 
 	irqrestore(flags);
 }
 
-
 static void
 comms_handle_frame(void *arg, const void *buffer, size_t length)
 {
@@ -248,9 +270,15 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 		case F2I_MAGIC:
 			comms_handle_command(buffer, length);
 			break;
+
 		case F2I_CONFIG_MAGIC:
 			comms_handle_config(buffer, length);
 			break;
+
+		case F2I_MIXER_MAGIC:
+			mixer_handle_text(buffer, length);
+			break;
+
 		default:
 			break;
 		}

apps/px4io/controls.c

@@ -90,6 +90,7 @@ controls_main(void)
 
 		if (fds[0].revents & POLLIN)
 			locked |= dsm_input();
+
 		if (fds[1].revents & POLLIN)
 			locked |= sbus_input();
 
@@ -139,6 +140,7 @@ ppm_input(void)
 
 		/* PPM data exists, copy it */
 		system_state.rc_channels = ppm_decoded_channels;
+
 		for (unsigned i = 0; i < ppm_decoded_channels; i++)
 			system_state.rc_channel_data[i] = ppm_buffer[i];
 
@@ -150,5 +152,5 @@ ppm_input(void)
 
 		/* trigger an immediate report to the FMU */
 		system_state.fmu_report_due = true;
-	}	
+	}
 }

apps/px4io/dsm.c

@@ -47,7 +47,7 @@
 #include <termios.h>
 
 #include <systemlib/ppm_decode.h>
- 
+
 #include <drivers/drv_hrt.h>
 
 #define DEBUG
@@ -97,6 +97,7 @@ dsm_init(const char *device)
 		dsm_guess_format(true);
 
 		debug("DSM: ready");
+
 	} else {
 		debug("DSM: open failed");
 	}
@@ -118,7 +119,7 @@ dsm_input(void)
 	 * frame transmission time is ~1.4ms.
 	 *
 	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 5ms passes between calls, 
+	 * and if an interval of more than 5ms passes between calls,
 	 * the first byte we read will be the first byte of a frame.
 	 *
 	 * In the case where byte(s) are dropped from a frame, this also
@@ -126,6 +127,7 @@ dsm_input(void)
 	 * if we didn't drop bytes...
 	 */
 	now = hrt_absolute_time();
+
 	if ((now - last_rx_time) > 5000) {
 		if (partial_frame_count > 0) {
 			dsm_frame_drops++;
@@ -142,6 +144,7 @@ dsm_input(void)
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
 		goto out;
+
 	last_rx_time = now;
 
 	/*
@@ -153,7 +156,7 @@ dsm_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < DSM_FRAME_SIZE)
-	 	goto out;
+		goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -164,7 +167,7 @@ dsm_input(void)
 
 out:
 	/*
-	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
 	 */
 	return (now - last_frame_time) < 200000;
 }
@@ -212,6 +215,7 @@ dsm_guess_format(bool reset)
 		/* if the channel decodes, remember the assigned number */
 		if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
 			cs10 |= (1 << channel);
+
 		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
 			cs11 |= (1 << channel);
 
@@ -222,7 +226,7 @@ dsm_guess_format(bool reset)
 	if (samples++ < 5)
 		return;
 
-	/* 
+	/*
 	 * Iterate the set of sensible sniffed channel sets and see whether
 	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
 	 *
@@ -233,7 +237,7 @@ dsm_guess_format(bool reset)
 	 *     See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
 	 *     of this issue.
 	 */
-	static uint32_t masks[] = { 
+	static uint32_t masks[] = {
 		0x3f,	/* 6 channels (DX6) */
 		0x7f,	/* 7 channels (DX7) */
 		0xff,	/* 8 channels (DX8) */
@@ -247,14 +251,17 @@ dsm_guess_format(bool reset)
 
 		if (cs10 == masks[i])
 			votes10++;
+
 		if (cs11 == masks[i])
 			votes11++;
 	}
+
 	if ((votes11 == 1) && (votes10 == 0)) {
 		channel_shift = 11;
 		debug("DSM: detected 11-bit format");
 		return;
 	}
+
 	if ((votes10 == 1) && (votes11 == 0)) {
 		channel_shift = 10;
 		debug("DSM: detected 10-bit format");
@@ -270,13 +277,13 @@ static void
 dsm_decode(hrt_abstime frame_time)
 {
 
-/*
-	debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
-	      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
-	      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
-*/
 	/*
-	 * If we have lost signal for at least a second, reset the 
+		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
+		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	*/
+	/*
+	 * If we have lost signal for at least a second, reset the
 	 * format guessing heuristic.
 	 */
 	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
@@ -292,7 +299,7 @@ dsm_decode(hrt_abstime frame_time)
 	}
 
 	/*
-	 * The encoding of the first two bytes is uncertain, so we're 
+	 * The encoding of the first two bytes is uncertain, so we're
 	 * going to ignore them for now.
 	 *
 	 * Each channel is a 16-bit unsigned value containing either a 10-
@@ -322,9 +329,10 @@ dsm_decode(hrt_abstime frame_time)
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		if (channel_shift == 11)
 			value /= 2;
+
 		value += 998;
 
-		/* 
+		/*
 		 * Store the decoded channel into the R/C input buffer, taking into
 		 * account the different ideas about channel assignement that we have.
 		 *
@@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time)
 		case 0:
 			channel = 2;
 			break;
+
 		case 1:
 			channel = 0;
 			break;
+
 		case 2:
 			channel = 1;
+
 		default:
 			break;
 		}
+
 		system_state.rc_channel_data[channel] = value;
 	}
 

apps/px4io/mixer.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mixer.c
+ * @file mixer.cpp
  *
  * Control channel input/output mixer and failsafe.
  */
@@ -49,8 +49,12 @@
 #include <fcntl.h>
 
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/mixer/mixer.h>
 
+extern "C" {
+//#define DEBUG
 #include "px4io.h"
+}
 
 /*
  * Count of periodic calls in which we have no FMU input.
@@ -58,28 +62,23 @@
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
 
-/*
- * Update a mixer based on the current control signals.
- */
-static void	mixer_update(int mixer, uint16_t *inputs, int input_count);
-
 /* current servo arm/disarm state */
 bool mixer_servos_armed = false;
 
-/*
+/* selected control values and count for mixing */
- * Each mixer consumes a set of inputs and produces a single output.
+static uint16_t *control_values;
- */
+static int control_count;
-struct mixer {
+
-	uint16_t current_value;
+static int	mixer_callback(uintptr_t handle,
-	/* XXX more config here */
+			       uint8_t control_group,
-} mixers[IO_SERVO_COUNT];
+			       uint8_t control_index,
+			       float &control);
+
+static MixerGroup mixer_group(mixer_callback, 0);
 
 void
 mixer_tick(void)
 {
-	uint16_t *control_values;
-	int control_count;
-	int i;
 	bool should_arm;
 
 	/*
@@ -87,7 +86,7 @@ mixer_tick(void)
 	 */
 	if (system_state.mixer_use_fmu) {
 		/* we have recent control data from the FMU */
-		control_count = PX4IO_OUTPUT_CHANNELS;
+		control_count = PX4IO_CONTROL_CHANNELS;
 		control_values = &system_state.fmu_channel_data[0];
 
 		/* check that we are receiving fresh data from the FMU */
@@ -98,6 +97,7 @@ mixer_tick(void)
 			if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
 				system_state.mixer_use_fmu = false;
 			}
+
 		} else {
 			fmu_input_drops = 0;
 			system_state.fmu_data_received = false;
@@ -115,17 +115,31 @@ mixer_tick(void)
 	}
 
 	/*
-	 * Tickle each mixer, if we have control data.
+	 * Run the mixers if we have any control data at all.
 	 */
 	if (control_count > 0) {
-		for (i = 0; i < IO_SERVO_COUNT; i++) {
+		float	outputs[IO_SERVO_COUNT];
-			mixer_update(i, control_values, control_count);
+		unsigned mixed;
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+		/* scale to PWM and update the servo outputs as required */
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
+			if (i < mixed) {
+				/* scale to servo output */
+				system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
+
+			} else {
+				/* set to zero to inhibit PWM pulse output */
+				system_state.servos[i] = 0;
+			}
 
 			/*
 			 * If we are armed, update the servo output.
 			 */
 			if (system_state.armed && system_state.arm_ok)
-				up_pwm_servo_set(i, mixers[i].current_value);
+				up_pwm_servo_set(i, system_state.servos[i]);
 		}
 	}
 
@@ -133,6 +147,7 @@ mixer_tick(void)
 	 * Decide whether the servos should be armed right now.
 	 */
 	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
+
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
@@ -145,13 +160,73 @@ mixer_tick(void)
 	}
 }
 
-static void
+static int
-mixer_update(int mixer, uint16_t *inputs, int input_count)
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
 {
-	/* simple passthrough for now */
+	/* if the control index refers to an input that's not valid, we can't return it */
-	if (mixer < input_count) {
+	if (control_index >= control_count)
-		mixers[mixer].current_value = inputs[mixer];
+		return -1;
-	} else {
+
-		mixers[mixer].current_value = 0;
+	/* scale from current PWM units (1000-2000) to mixer input values */
-	}
+	control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
+
+	return 0;
 }
+
+static char mixer_text[256];
+static unsigned mixer_text_length = 0;
+
+void
+mixer_handle_text(const void *buffer, size_t length)
+{
+
+	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
+
+	debug("mixer text %u", length);
+
+	if (length < sizeof(px4io_mixdata))
+		return;
+
+	unsigned	text_length = length - sizeof(px4io_mixdata);
+
+	switch (msg->action) {
+	case F2I_MIXER_ACTION_RESET:
+		debug("reset");
+		mixer_group.reset();
+		mixer_text_length = 0;
+
+		/* FALLTHROUGH */
+	case F2I_MIXER_ACTION_APPEND:
+		debug("append %d", length);
+
+		/* check for overflow - this is really fatal */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
+			return;
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		debug("buflen %u", mixer_text_length);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+			debug("used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		break;
+	}
+}

apps/px4io/protocol.h

@@ -41,31 +41,27 @@
 
 #pragma once
 
-#define PX4IO_OUTPUT_CHANNELS	8
+#define PX4IO_CONTROL_CHANNELS	8
 #define PX4IO_INPUT_CHANNELS	12
 #define PX4IO_RELAY_CHANNELS	4
 
 #pragma pack(push, 1)
 
-/* command from FMU to IO */
+/**
+ * Periodic command from FMU to IO.
+ */
 struct px4io_command {
 	uint16_t	f2i_magic;
-#define F2I_MAGIC	0x636d
+#define F2I_MAGIC		0x636d
 
-	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];
+	uint16_t	output_control[PX4IO_CONTROL_CHANNELS];
 	bool		relay_state[PX4IO_RELAY_CHANNELS];
 	bool		arm_ok;
 };
 
-/* config message from FMU to IO */
+/**
-struct px4io_config {
+ * Periodic report from IO to FMU
-	uint16_t f2i_config_magic;
+ */
-#define F2I_CONFIG_MAGIC 0x6366
-
-	/* XXX currently nothing here */
-};
-
-/* report from IO to FMU */
 struct px4io_report {
 	uint16_t	i2f_magic;
 #define I2F_MAGIC		0x7570
@@ -79,4 +75,34 @@ struct px4io_report {
 	uint8_t		overcurrent;
 };
 
+/**
+ * As-needed config message from FMU to IO
+ */
+struct px4io_config {
+	uint16_t	f2i_config_magic;
+#define F2I_CONFIG_MAGIC	 0x6366
+
+	/* XXX currently nothing here */
+};
+
+/**
+ * As-needed mixer data upload.
+ *
+ * This message adds text to the mixer text buffer; the text
+ * buffer is drained as the definitions are consumed.
+ */
+struct px4io_mixdata {
+	uint16_t	f2i_mixer_magic;
+#define F2I_MIXER_MAGIC		0x6d74
+
+	uint8_t		action;
+#define F2I_MIXER_ACTION_RESET		0
+#define F2I_MIXER_ACTION_APPEND		1
+
+	char		text[0];	/* actual text size may vary */
+};
+
+/* maximum size is limited by the HX frame size */
+#define F2I_MIXER_MAX_TEXT	(HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
+
 #pragma pack(pop)

apps/px4io/px4io.c

@@ -55,10 +55,15 @@
 
 __EXPORT int user_start(int argc, char *argv[]);
 
+extern void up_cxxinitialize(void);
+
 struct sys_state_s 	system_state;
 
 int user_start(int argc, char *argv[])
 {
+	/* run C++ ctors before we go any further */
+	up_cxxinitialize();
+
 	/* reset all to zero */
 	memset(&system_state, 0, sizeof(system_state));
 
@@ -83,7 +88,7 @@ int user_start(int argc, char *argv[])
 	up_pwm_servo_init(0xff);
 
 	/* start the flight control signal handler */
-	task_create("FCon", 
+	task_create("FCon",
 		    SCHED_PRIORITY_DEFAULT,
 		    1024,
 		    (main_t)controls_main,

apps/px4io/px4io.h

@@ -31,11 +31,11 @@
  *
  ****************************************************************************/
 
- /**
+/**
-  * @file px4io.h
+ * @file px4io.h
-  *
+ *
-  * General defines and structures for the PX4IO module firmware.
+ * General defines and structures for the PX4IO module firmware.
-  */
+ */
 
 #include <nuttx/config.h>
 
@@ -66,8 +66,7 @@
 /*
  * System state structure.
  */
-struct sys_state_s 
+struct sys_state_s {
-{
 
 	bool		armed;			/* IO armed */
 	bool		arm_ok;			/* FMU says OK to arm */
@@ -82,7 +81,12 @@ struct sys_state_s
 	/*
 	 * Control signals from FMU.
 	 */
-	uint16_t	fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
+	uint16_t	fmu_channel_data[PX4IO_CONTROL_CHANNELS];
+
+	/*
+	 * Mixed servo outputs
+	 */
+	uint16_t	servos[IO_SERVO_COUNT];
 
 	/*
 	 * Relay controls
@@ -145,8 +149,8 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
 #define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
 
 #define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s))
+#define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s))
+#define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
 #define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
 #define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
 
@@ -161,6 +165,7 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
  * Mixer
  */
 extern void	mixer_tick(void);
+extern void	mixer_handle_text(const void *buffer, size_t length);
 
 /*
  * Safety switch/LED.

apps/px4io/safety.c

@@ -31,9 +31,9 @@
  *
  ****************************************************************************/
 
- /**
+/**
-  * @file Safety button logic.
+ * @file Safety button logic.
-  */
+ */
 
 #include <nuttx/config.h>
 #include <stdio.h>
@@ -95,13 +95,13 @@ safety_init(void)
 static void
 safety_check_button(void *arg)
 {
-	/* 
+	/*
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
 	 */
 	safety_button_pressed = BUTTON_SAFETY;
 
-	if(safety_button_pressed) {
+	if (safety_button_pressed) {
 		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
 	}
 
@@ -109,34 +109,42 @@ safety_check_button(void *arg)
 	if (safety_button_pressed && !system_state.armed) {
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
+
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to armed state and notify the FMU */
 			system_state.armed = true;
 			counter++;
 			system_state.fmu_report_due = true;
 		}
-	/* Disarm quickly */
+
+		/* Disarm quickly */
+
 	} else if (safety_button_pressed && system_state.armed) {
 		if (counter < DISARM_COUNTER_THRESHOLD) {
 			counter++;
+
 		} else if (counter == DISARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
 			system_state.armed = false;
 			counter++;
 			system_state.fmu_report_due = true;
 		}
+
 	} else {
 		counter = 0;
 	}
 
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_SAFE;
+
 	if (system_state.armed) {
 		if (system_state.arm_ok) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
+
 		} else {
 			pattern = LED_PATTERN_IO_ARMED;
 		}
+
 	} else if (system_state.arm_ok) {
 		pattern = LED_PATTERN_FMU_ARMED;
 	}
@@ -167,8 +175,10 @@ failsafe_blink(void *arg)
 	/* blink the failsafe LED if we don't have FMU input */
 	if (!system_state.mixer_use_fmu) {
 		failsafe = !failsafe;
+
 	} else {
 		failsafe = false;
 	}
+
 	LED_AMBER(failsafe);
 }

apps/px4io/sbus.c

@@ -53,7 +53,7 @@
 #include "debug.h"
 
 #define SBUS_FRAME_SIZE		25
-#define SBUS_INPUT_CHANNELS	16
+#define SBUS_INPUT_CHANNELS	18
 
 static int sbus_fd = -1;
 
@@ -87,9 +87,9 @@ sbus_init(const char *device)
 		partial_frame_count = 0;
 		last_rx_time = hrt_absolute_time();
 
-		debug("Sbus: ready");
+		debug("S.Bus: ready");
 	} else {
-		debug("Sbus: open failed");
+		debug("S.Bus: open failed");
 	}
 
 	return sbus_fd;
@@ -109,7 +109,7 @@ sbus_input(void)
 	 * frame transmission time is ~2ms.
 	 *
 	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 3ms passes between calls, 
+	 * and if an interval of more than 3ms passes between calls,
 	 * the first byte we read will be the first byte of a frame.
 	 *
 	 * In the case where byte(s) are dropped from a frame, this also
@@ -117,6 +117,7 @@ sbus_input(void)
 	 * if we didn't drop bytes...
 	 */
 	now = hrt_absolute_time();
+
 	if ((now - last_rx_time) > 3000) {
 		if (partial_frame_count > 0) {
 			sbus_frame_drops++;
@@ -133,6 +134,7 @@ sbus_input(void)
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
 		goto out;
+
 	last_rx_time = now;
 
 	/*
@@ -144,7 +146,7 @@ sbus_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < SBUS_FRAME_SIZE)
-	 	goto out;
+		goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -155,7 +157,7 @@ sbus_input(void)
 
 out:
 	/*
-	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
 	 */
 	return (now - last_frame_time) < 200000;
 }
@@ -177,23 +179,23 @@ struct sbus_bit_pick {
 	uint8_t mask;
 	uint8_t lshift;
 };
-static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {	
+static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
-/*  0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00,  0} },
+	/*  0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
-/*  1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00,  0} },
+	/*  1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
-/*  2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
+	/*  2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
-/*  3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00,  0} },
+	/*  3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
-/*  4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00,  0} },
+	/*  4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
-/*  5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03,  9} },
+	/*  5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03,  9} },
-/*  6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00,  0} },
+	/*  6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
-/*  7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00,  0} },
+	/*  7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00,  0} },
-/*  8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00,  0} },
+	/*  8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
-/*  9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00,  0} },
+	/*  9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
-/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
+	/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
-/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00,  0} },
+	/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
-/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00,  0} },
+	/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
-/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03,  9} },
+	/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03,  9} },
-/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00,  0} },
+	/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
-/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00,  0} }
+	/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00,  0} }
 };
 
 static void
@@ -205,16 +207,20 @@ sbus_decode(hrt_abstime frame_time)
 		return;
 	}
 
-	/* if the failsafe bit is set, we consider the frame invalid */
+	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
-	if (frame[23] & (1 << 4)) {
+	if ((frame[23] & (1 << 2)) && /* signal lost */
-		return;
+		(frame[23] & (1 << 3))) { /* failsafe */
+
+		/* actively announce signal loss */
+		system_state.rc_channels = 0;
+		return 1;
 	}
 
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
-	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? 
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
-		SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
+			     SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {
@@ -232,20 +238,24 @@ sbus_decode(hrt_abstime frame_time)
 				value |= piece;
 			}
 		}
+
 		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		system_state.rc_channel_data[channel] = (value / 2) + 998;
 	}
 
-	if (PX4IO_INPUT_CHANNELS >= 18) {
+	/* decode switch channels if data fields are wide enough */
-		chancount = 18;
+	if (chancount > 17) {
-		/* XXX decode the two switch channels */
+		/* channel 17 (index 16) */
+		system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		/* channel 18 (index 17) */
+		system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
 	}
 
 	/* note the number of channels decoded */
 	system_state.rc_channels = chancount;
 
 	/* and note that we have received data from the R/C controller */
-	system_state.rc_channels_timestamp = frame_time;	
+	system_state.rc_channels_timestamp = frame_time;
 
 	/* trigger an immediate report to the FMU */
 	system_state.fmu_report_due = true;

apps/systemcmds/mixer/mixer.c

@@ -43,14 +43,17 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
+#include <ctype.h>
+#include <nuttx/compiler.h>
 
+#include <systemlib/err.h>
 #include <drivers/drv_mixer.h>
 #include <uORB/topics/actuator_controls.h>
 
 __EXPORT int mixer_main(int argc, char *argv[]);
 
-static void	usage(const char *reason);
+static void	usage(const char *reason) noreturn_function;
-static void	load(const char *devname, const char *fname);
+static void	load(const char *devname, const char *fname) noreturn_function;
 
 int
 mixer_main(int argc, char *argv[])
@@ -63,12 +66,9 @@ mixer_main(int argc, char *argv[])
 			usage("missing device or filename");
 
 		load(argv[2], argv[3]);
-
-	} else {
-		usage("unrecognised command");
 	}
 
-	return 0;
+	usage("unrecognised command");
 }
 
 static void
@@ -79,34 +79,58 @@ usage(const char *reason)
 
 	fprintf(stderr, "usage:\n");
 	fprintf(stderr, "  mixer load <device> <filename>\n");
-	/* XXX automatic setups for quad, etc. */
+	/* XXX other useful commands? */
 	exit(1);
 }
 
 static void
 load(const char *devname, const char *fname)
 {
-	int		dev = -1;
+	int		dev;
-	int		ret, result = 1;
+	FILE		*fp;
+	char		line[80];
+	char		buf[512];
 
 	/* open the device */
-	if ((dev = open(devname, 0)) < 0) {
+	if ((dev = open(devname, 0)) < 0)
-		fprintf(stderr, "can't open %s\n", devname);
+		err(1, "can't open %s\n", devname);
-		goto out;
+
+	/* reset mixers on the device */
+	if (ioctl(dev, MIXERIOCRESET, 0))
+		err(1, "can't reset mixers on %s", devname);
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+	if (fp == NULL)
+		err(1, "can't open %s", fname);
+
+	/* read valid lines from the file into a buffer */
+	buf[0] = '\0';
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* if the line doesn't look like a mixer definition line, skip it */
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+			continue;
+
+		/* XXX an optimisation here would be to strip extra whitespace */
+
+		/* if the line is too long to fit in the buffer, bail */
+		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
+			break;
+
+		/* add the line to the buffer */
+		strcat(buf, line);
 	}
 
-	/* tell it to load the file */
+	/* XXX pass the buffer to the device */
-	ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname);
+	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
 
-	if (ret != 0) {
+	if (ret < 0)
-		fprintf(stderr, "failed loading %s\n", fname);
+		err(1, "error loading mixers from %s", fname);
-	}
+	exit(0);
-
-	result = 0;
-out:
-
-	if (dev != -1)
-		close(dev);
-
-	exit(result);
 }

apps/systemlib/mixer/mixer.cpp

@@ -137,89 +137,15 @@ NullMixer::groups_required(uint32_t &groups)
 
 }
 
-/****************************************************************************/
+NullMixer *
-
+NullMixer::from_text(const char *buf, unsigned &buflen)
-SimpleMixer::SimpleMixer(ControlCallback control_cb,
-			 uintptr_t cb_handle,
-			 mixer_simple_s *mixinfo) :
-	Mixer(control_cb, cb_handle),
-	_info(mixinfo)
 {
-}
+	NullMixer *nm = nullptr;
 
-SimpleMixer::~SimpleMixer()
+	if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
-{
+		nm = new NullMixer;
-	if (_info != nullptr)
+		buflen -= 2;
-		free(_info);
-}
-
-unsigned
-SimpleMixer::mix(float *outputs, unsigned space)
-{
-	float		sum = 0.0f;
-
-	if (_info == nullptr)
-		return 0;
-
-	if (space < 1)
-		return 0;
-
-	for (unsigned i = 0; i < _info->control_count; i++) {
-		float input;
-
-		_control_cb(_cb_handle,
-			    _info->controls[i].control_group,
-			    _info->controls[i].control_index,
-			    input);
-
-		sum += scale(_info->controls[i].scaler, input);
 	}
 
-	*outputs = scale(_info->output_scaler, sum);
+	return nm;
-	return 1;
-}
-
-void
-SimpleMixer::groups_required(uint32_t &groups)
-{
-	for (unsigned i = 0; i < _info->control_count; i++)
-		groups |= 1 << _info->controls[i].control_group;
-}
-
-int
-SimpleMixer::check()
-{
-	int ret;
-	float junk;
-
-	/* sanity that presumes that a mixer includes a control no more than once */
-	/* max of 32 groups due to groups_required API */
-	if (_info->control_count > 32)
-		return -2;
-
-	/* validate the output scaler */
-	ret = scale_check(_info->output_scaler);
-
-	if (ret != 0)
-		return ret;
-
-	/* validate input scalers */
-	for (unsigned i = 0; i < _info->control_count; i++) {
-
-		/* verify that we can fetch the control */
-		if (_control_cb(_cb_handle,
-				_info->controls[i].control_group,
-				_info->controls[i].control_index,
-				junk) != 0) {
-			return -3;
-		}
-
-		/* validate the scaler */
-		ret = scale_check(_info->controls[i].scaler);
-
-		if (ret != 0)
-			return (10 * i + ret);
-	}
-
-	return 0;
 }

apps/systemlib/mixer/mixer.h

@@ -234,50 +234,51 @@ public:
 	void				add_mixer(Mixer *mixer);
 
 	/**
-	 * Reads a mixer definition from a file and configures a corresponding
+	 * Remove all the mixers from the group.
-	 * group.
+	 */
+	void				reset();
+
+	/**
+	 * Adds mixers to the group based on a text description in a buffer.
 	 *
-	 * The mixer group must be empty when this function is called.
+	 * Mixer definitions begin with a single capital letter and a colon.
+	 * The actual format of the mixer definition varies with the individual
+	 * mixers; they are summarised here, but see ROMFS/mixers/README for
+	 * more details.
 	 *
-	 * A mixer definition is a text representation of the configuration of a
+	 * Null Mixer
-	 * mixer. Definition lines begin with a capital letter followed by a colon.
+	 * ..........
 	 *
-	 * Null Mixer:
+	 * The null mixer definition has the form:
 	 *
-	 * Z:
+	 *   Z:
 	 *
-	 * This mixer generates a constant zero output, and is normally used to
+	 * Simple Mixer
-	 * skip over outputs that are not in use.
+	 * ............
 	 *
-	 * Simple Mixer:
+	 * A simple mixer definition begins with:
 	 *
-	 * M: <scaler count>
+	 *   M: <control count>
-	 * S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*>
+	 *   O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
-	 * S: ...
 	 *
-	 * The definition consists of a single-line header indicating the
+	 * The definition continues with <control count> entries describing the control
-	 * number of scalers and then one line defining each scaler.  The first
+	 * inputs and their scaling, in the form:
-	 * scaler in the file is always the output scaler, followed by the input
-	 * scalers.
 	 *
-	 * The <control ...> values for the output scaler are ignored by the mixer.
+	 *   S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
 	 *
+	 * Multirotor Mixer
+	 * ................
 	 *
+	 * The multirotor mixer definition is a single line of the form:
 	 *
-	 * Values marked * are integers representing floating point values; values are
+	 * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
-	 * scaled by 10000 on load/save.
 	 *
-	 * Multiple mixer definitions may be stored in a single file; it is assumed that
+	 * @param buf			The mixer configuration buffer.
-	 * the reader will know how many to expect and read accordingly.
+	 * @param buflen		The length of the buffer, updated to reflect
-	 *
+	 *				bytes as they are consumed.
-	 * A mixer entry with a scaler count of zero indicates a disabled mixer. This
-	 * will return NULL for the mixer when processed by this function, and will be
-	 * generated by passing NULL as the mixer to mixer_save.
-	 *
-	 * @param path			The mixer configuration file to read.
 	 * @return			Zero on successful load, nonzero otherwise.
 	 */
-	int				load_from_file(const char *path);
+	int				load_from_buf(const char *buf, unsigned &buflen);
 
 private:
 	Mixer				*_first;	/**< linked list of mixers */
@@ -294,6 +295,21 @@ public:
 	NullMixer();
 	~NullMixer() {};
 
+	/**
+	 * Factory method.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new NullMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static NullMixer		*from_text(const char *buf, unsigned &buflen);
+
 	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual void			groups_required(uint32_t &groups);
 };
@@ -318,6 +334,47 @@ public:
 		    mixer_simple_s *mixinfo);
 	~SimpleMixer();
 
+	/**
+	 * Factory method with full external configuration.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new SimpleMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static SimpleMixer		*from_text(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
+	/**
+	 * Factory method for PWM/PPM input to internal float representation.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param input			The control index used when fetching the input.
+	 * @param min			The PWM/PPM value considered to be "minimum" (gives -1.0 out)
+	 * @param mid			The PWM/PPM value considered to be the midpoint (gives 0.0 out)
+	 * @param max			The PWM/PPM value considered to be "maximum" (gives 1.0 out)
+	 * @return			A new SimpleMixer instance, or nullptr if one could not be
+	 *				allocated.
+	 */
+	static SimpleMixer		*pwm_input(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			unsigned input,
+			uint16_t min,
+			uint16_t mid,
+			uint16_t max);
+
 	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual void			groups_required(uint32_t &groups);
 
@@ -330,10 +387,18 @@ public:
 	 * @return			Zero if the mixer makes sense, nonzero otherwise.
 	 */
 	int				check();
+
 protected:
 
 private:
-	mixer_simple_s		*_info;
+	mixer_simple_s			*_info;
+
+	static int			parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
+	static int			parse_control_scaler(const char *buf,
+			unsigned &buflen,
+			mixer_scaler_s &scaler,
+			uint8_t &control_group,
+			uint8_t &control_index);
 };
 
 /**
@@ -395,6 +460,27 @@ public:
 			float deadband);
 	~MultirotorMixer();
 
+	/**
+	 * Factory method.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new MultirotorMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static MultirotorMixer		*from_text(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
 	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual void			groups_required(uint32_t &groups);
 

apps/systemlib/mixer/mixer_group.cpp

@@ -56,250 +56,6 @@
 #define debug(fmt, args...)	do { } while(0)
 //#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
-namespace
-{
-
-/**
- * Effectively fdgets() with some extra smarts.
- */
-static int
-mixer_getline(int fd, char *line, unsigned maxlen)
-{
-	/* reduce line budget by 1 to account for terminal NUL */
-	maxlen--;
-
-	/* loop looking for a non-comment line */
-	for (;;) {
-		int	ret;
-		char	c;
-		char	*p = line;
-
-		/* loop reading characters for this line */
-		for (;;) {
-			ret = read(fd, &c, 1);
-
-			/* on error or EOF, return same */
-			if (ret <= 0) {
-				debug("read: EOF");
-				return ret;
-			}
-
-			/* ignore carriage returns */
-			if (c == '\r')
-				continue;
-
-			/* line termination */
-			if (c == '\n') {
-				/* ignore malformed lines */
-				if ((p - line) < 4)
-					break;
-
-				if (line[1] != ':')
-					break;
-
-				/* terminate line as string and return */
-				*p = '\0';
-				debug("read: '%s'", line);
-				return 1;
-			}
-
-			/* if we have space, accumulate the byte and go on */
-			if ((p - line) < maxlen)
-				*p++ = c;
-		}
-	}
-}
-
-/**
- * Parse an output scaler from the buffer.
- */
-static int
-mixer_parse_output_scaler(const char *buf, mixer_scaler_s &scaler)
-{
-	int s[5];
-
-	if (sscanf(buf, "O: %d %d %d %d %d",
-		   &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
-		debug("scaler parse failed on '%s'", buf);
-		return -1;
-	}
-
-	scaler.negative_scale	= s[0] / 10000.0f;
-	scaler.positive_scale	= s[1] / 10000.0f;
-	scaler.offset		= s[2] / 10000.0f;
-	scaler.min_output	= s[3] / 10000.0f;
-	scaler.max_output	= s[4] / 10000.0f;
-
-	return 0;
-}
-
-/**
- * Parse a control scaler from the buffer.
- */
-static int
-mixer_parse_control_scaler(const char *buf, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
-{
-	unsigned u[2];
-	int s[5];
-
-	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
-		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
-		debug("scaler parse failed on '%s'", buf);
-		return -1;
-	}
-
-	control_group	= u[0];
-	control_index	= u[1];
-	scaler.negative_scale	= s[0] / 10000.0f;
-	scaler.positive_scale	= s[1] / 10000.0f;
-	scaler.offset		= s[2] / 10000.0f;
-	scaler.min_output	= s[3] / 10000.0f;
-	scaler.max_output	= s[4] / 10000.0f;
-
-	return 0;
-}
-
-SimpleMixer *
-mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, unsigned inputs)
-{
-	mixer_simple_s	*mixinfo = nullptr;
-	char		buf[60];
-	int		ret;
-
-	/* let's assume we're going to read a simple mixer */
-	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
-	mixinfo->control_count = inputs;
-
-	/* first, get the output scaler */
-	ret = mixer_getline(fd, buf, sizeof(buf));
-
-	if (ret < 1) {
-		debug("failed reading for output scaler");
-		goto fail;
-	}
-
-	if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) {
-		debug("failed parsing output scaler");
-		goto fail;
-	}
-
-	/* now get any inputs */
-	for (unsigned i = 0; i < inputs; i++) {
-		ret = mixer_getline(fd, buf, sizeof(buf));
-
-		if (ret < 1) {
-			debug("failed reading for control scaler");
-			goto fail;
-		}
-
-		if (mixer_parse_control_scaler(buf,
-					       mixinfo->controls[i].scaler,
-					       mixinfo->controls[i].control_group,
-					       mixinfo->controls[i].control_index)) {
-			debug("failed parsing control scaler");
-			goto fail;
-		}
-
-		debug("got control %d", i);
-	}
-
-	/* XXX should be a factory that validates the mixinfo ... */
-	return new SimpleMixer(control_cb, cb_handle, mixinfo);
-
-fail:
-	free(mixinfo);
-	return nullptr;
-}
-
-MultirotorMixer *
-mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf)
-{
-	MultirotorMixer::Geometry geometry;
-	char geomname[8];
-	int s[4];
-
-	if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) {
-		debug("multirotor parse failed on '%s'", buf);
-		return nullptr;
-	}
-
-	if (!strcmp(geomname, "4+")) {
-		geometry = MultirotorMixer::QUAD_PLUS;
-
-	} else if (!strcmp(geomname, "4x")) {
-		geometry = MultirotorMixer::QUAD_X;
-
-	} else if (!strcmp(geomname, "6+")) {
-		geometry = MultirotorMixer::HEX_PLUS;
-
-	} else if (!strcmp(geomname, "6x")) {
-		geometry = MultirotorMixer::HEX_X;
-
-	} else if (!strcmp(geomname, "8+")) {
-		geometry = MultirotorMixer::OCTA_PLUS;
-
-	} else if (!strcmp(geomname, "8x")) {
-		geometry = MultirotorMixer::OCTA_X;
-
-	} else {
-		debug("unrecognised geometry '%s'", geomname);
-		return nullptr;
-	}
-
-	return new MultirotorMixer(
-		       control_cb,
-		       cb_handle,
-		       geometry,
-		       s[0] / 10000.0f,
-		       s[1] / 10000.0f,
-		       s[2] / 10000.0f,
-		       s[3] / 10000.0f);
-}
-
-int
-mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer)
-{
-	int		ret;
-	char		buf[60];
-	unsigned	inputs;
-
-	ret = mixer_getline(fd, buf, sizeof(buf));
-
-	/* end of file or error ?*/
-	if (ret < 1) {
-		debug("getline %d", ret);
-		return ret;
-	}
-
-	/* slot is empty - allocate a null mixer */
-	if (buf[0] == 'Z') {
-		debug("got null mixer");
-		mixer = new NullMixer();
-		return 1;
-	}
-
-	/* is it a simple mixer? */
-	if (sscanf(buf, "M: %u", &inputs) == 1) {
-		debug("got simple mixer with %d inputs", inputs);
-		mixer = mixer_load_simple(control_cb, cb_handle, fd, inputs);
-		return (mixer == nullptr) ? -1 : 1;
-	}
-
-	/* is it a multirotor mixer? */
-	if (buf[0] == 'R') {
-		debug("got a multirotor mixer");
-		mixer = mixer_load_multirotor(control_cb, cb_handle, buf);
-		return (mixer == nullptr) ? -1 : 1;
-	}
-
-	/* we don't recognise the mixer type */
-	debug("unrecognized mixer type '%c'", buf[0]);
-	return -1;
-}
-
-
-} // namespace
-
 MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
 	Mixer(control_cb, cb_handle),
 	_first(nullptr)
@@ -308,14 +64,7 @@ MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
 
 MixerGroup::~MixerGroup()
 {
-	Mixer *mixer;
+	reset();
-
-	/* discard sub-mixers */
-	while (_first != nullptr) {
-		mixer = _first;
-		_first = mixer->_next;
-		delete mixer;
-	}
 }
 
 void
@@ -332,6 +81,19 @@ MixerGroup::add_mixer(Mixer *mixer)
 	mixer->_next = nullptr;
 }
 
+void
+MixerGroup::reset()
+{
+	Mixer *mixer;
+
+	/* discard sub-mixers */
+	while (_first != nullptr) {
+		mixer = _first;
+		_first = mixer->_next;
+		delete mixer;
+	}
+}
+
 unsigned
 MixerGroup::mix(float *outputs, unsigned space)
 {
@@ -358,44 +120,63 @@ MixerGroup::groups_required(uint32_t &groups)
 }
 
 int
-MixerGroup::load_from_file(const char *path)
+MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 {
-	if (_first != nullptr)
+	int ret = -1;
-		return -1;
+	const char *end = buf + buflen;
 
-	int fd = open(path, O_RDONLY);
+	/*
+	 * Loop until either we have emptied the buffer, or we have failed to
+	 * allocate something when we expected to.
+	 */
+	while (buflen > 0) {
+		Mixer *m = nullptr;
+		const char *p = end - buflen;
+		unsigned resid = buflen;
 
-	if (fd < 0) {
+		/*
-		debug("failed to open %s", path);
+		 * Use the next character as a hint to decide which mixer class to construct.
-		return -1;
+		 */
-	}
+		switch (*p) {
+		case 'Z':
+			m = NullMixer::from_text(p, resid);
+			break;
 
-	for (unsigned count = 0;; count++) {
+		case 'M':
-		int	result;
+			m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
-		Mixer	*mixer;
+			break;
 
-		result = mixer_load(_control_cb,
+		case 'R':
-				    _cb_handle,
+			m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
-				    fd,
+			break;
-				    mixer);
 
-		/* error? */
+		default:
-		if (result < 0) {
+			/* it's probably junk or whitespace, skip a byte and retry */
-			debug("error");
+			buflen--;
-			return -1;
+			continue;
 		}
 
-		/* EOF or error */
+		/*
-		if (result < 1) {
+		 * If we constructed something, add it to the group.
-			printf("[mixer] loaded %u mixers\n", count);
+		 */
-			debug("EOF");
+		if (m != nullptr) {
+			add_mixer(m);
+
+			/* we constructed something */
+			ret = 0;
+
+			/* only adjust buflen if parsing was successful */
+			buflen = resid;
+		} else {
+
+			/*
+			 * There is data in the buffer that we expected to parse, but it didn't,
+			 * so give up for now.
+			 */
 			break;
 		}
-
-		debug("loaded mixer %p", mixer);
-		add_mixer(mixer);
 	}
 
-	close(fd);
+	/* nothing more in the buffer for us now */
-	return 0;
+	return ret;
 }

apps/systemlib/mixer/mixer_multirotor.cpp

@@ -54,6 +54,9 @@
 
 #include "mixer.h"
 
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
 #define		CW		(-1.0f)
 #define		CCW		(1.0f)
 
@@ -151,6 +154,61 @@ MultirotorMixer::~MultirotorMixer()
 {
 }
 
+MultirotorMixer *
+MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	MultirotorMixer::Geometry geometry;
+	char geomname[8];
+	int s[4];
+	int used;
+
+	if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
+		debug("multirotor parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	if (used > (int)buflen) {
+		debug("multirotor spec used %d of %u", used, buflen);
+		return nullptr;
+	}
+
+	buflen -= used;
+
+	if (!strcmp(geomname, "4+")) {
+		geometry = MultirotorMixer::QUAD_PLUS;
+
+	} else if (!strcmp(geomname, "4x")) {
+		geometry = MultirotorMixer::QUAD_X;
+
+	} else if (!strcmp(geomname, "6+")) {
+		geometry = MultirotorMixer::HEX_PLUS;
+
+	} else if (!strcmp(geomname, "6x")) {
+		geometry = MultirotorMixer::HEX_X;
+
+	} else if (!strcmp(geomname, "8+")) {
+		geometry = MultirotorMixer::OCTA_PLUS;
+
+	} else if (!strcmp(geomname, "8x")) {
+		geometry = MultirotorMixer::OCTA_X;
+
+	} else {
+		debug("unrecognised geometry '%s'", geomname);
+		return nullptr;
+	}
+
+	debug("adding multirotor mixer '%s'", geomname);
+
+	return new MultirotorMixer(
+		       control_cb,
+		       cb_handle,
+		       geometry,
+		       s[0] / 10000.0f,
+		       s[1] / 10000.0f,
+		       s[2] / 10000.0f,
+		       s[3] / 10000.0f);
+}
+
 unsigned
 MultirotorMixer::mix(float *outputs, unsigned space)
 {
@@ -170,10 +228,12 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	/* keep roll, pitch and yaw control to 0 below min thrust */
 	if (thrust <= min_thrust) {
 		output_factor = 0.0f;
-	/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+		/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+
 	} else if (thrust < startpoint_full_control && thrust > min_thrust) {
-		output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
+		output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
-	/* and then stay at full control */
+		/* and then stay at full control */
+
 	} else {
 		output_factor = max_thrust;
 	}

apps/systemlib/mixer/mixer_simple.cpp

@@ -0,0 +1,334 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_simple.cpp
+ *
+ * Simple summing mixer.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <ctype.h>
+
+#include "mixer.h"
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
+SimpleMixer::SimpleMixer(ControlCallback control_cb,
+			 uintptr_t cb_handle,
+			 mixer_simple_s *mixinfo) :
+	Mixer(control_cb, cb_handle),
+	_info(mixinfo)
+{
+}
+
+SimpleMixer::~SimpleMixer()
+{
+	if (_info != nullptr)
+		free(_info);
+}
+
+static const char *
+findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
+	}
+	return nullptr;
+}
+
+static void
+skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
+		buflen -= (p - buf);
+}
+
+int
+SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
+{
+	int ret;
+	int s[5];
+	
+	buf = findtag(buf, buflen, 'O');
+	if ((buf == nullptr) || (buflen < 12))
+		return -1;
+
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d",
+			  &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
+		debug("scaler parse failed on '%s' (got %d)", buf, ret);
+		return -1;
+	}
+	skipline(buf, buflen);
+
+	scaler.negative_scale	= s[0] / 10000.0f;
+	scaler.positive_scale	= s[1] / 10000.0f;
+	scaler.offset		= s[2] / 10000.0f;
+	scaler.min_output	= s[3] / 10000.0f;
+	scaler.max_output	= s[4] / 10000.0f;
+
+	return 0;
+}
+
+int
+SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
+{
+	unsigned u[2];
+	int s[5];
+
+	buf = findtag(buf, buflen, 'S');
+	if ((buf == nullptr) || (buflen < 16))
+		return -1;
+
+	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
+		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
+		debug("control parse failed on '%s'", buf);
+		return -1;
+	}
+	skipline(buf, buflen);
+
+	control_group		= u[0];
+	control_index		= u[1];
+	scaler.negative_scale	= s[0] / 10000.0f;
+	scaler.positive_scale	= s[1] / 10000.0f;
+	scaler.offset		= s[2] / 10000.0f;
+	scaler.min_output	= s[3] / 10000.0f;
+	scaler.max_output	= s[4] / 10000.0f;
+
+	return 0;
+}
+
+SimpleMixer *
+SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	SimpleMixer *sm = nullptr;
+	mixer_simple_s *mixinfo = nullptr;
+	unsigned inputs;
+	int used;
+	const char *end = buf + buflen;
+
+	/* get the base info for the mixer */
+	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
+		debug("simple parse failed on '%s'", buf);
+		goto out;
+	}
+
+	buflen -= used;
+
+	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
+
+	if (mixinfo == nullptr) {
+		debug("could not allocate memory for mixer info");
+		goto out;
+	}
+
+	mixinfo->control_count = inputs;
+
+	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+		goto out;
+
+	for (unsigned i = 0; i < inputs; i++) {
+		if (parse_control_scaler(end - buflen, buflen,
+					 mixinfo->controls[i].scaler,
+					 mixinfo->controls[i].control_group,
+					 mixinfo->controls[i].control_index))
+			goto out;
+	}
+
+	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+
+	if (sm != nullptr) {
+		mixinfo = nullptr;
+		debug("loaded mixer with %d inputs", inputs);
+
+	} else {
+		debug("could not allocate memory for mixer");
+	}
+
+out:
+
+	if (mixinfo != nullptr)
+		free(mixinfo);
+
+	return sm;
+}
+
+SimpleMixer *
+SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
+{
+	SimpleMixer *sm = nullptr;
+	mixer_simple_s *mixinfo = nullptr;
+
+	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1));
+
+	if (mixinfo == nullptr) {
+		debug("could not allocate memory for mixer info");
+		goto out;
+	}
+
+	mixinfo->control_count = 1;
+
+	/*
+	 * Always pull from group 0, with the input value giving the channel.
+	 */
+	mixinfo->controls[0].control_group = 0;
+	mixinfo->controls[0].control_index = input;
+
+	/*
+	 * Conversion uses both the input and output side of the mixer.
+	 *
+	 * The input side is used to slide the control value such that the min argument
+	 * results in a value of zero.
+	 *
+	 * The output side is used to apply the scaling for the min/max values so that
+	 * the resulting output is a -1.0 ... 1.0 value for the min...max range.
+	 */
+	mixinfo->controls[0].scaler.negative_scale = 1.0f;
+	mixinfo->controls[0].scaler.positive_scale = 1.0f;
+	mixinfo->controls[0].scaler.offset = -mid;
+	mixinfo->controls[0].scaler.min_output = -(mid - min);
+	mixinfo->controls[0].scaler.max_output = (max - mid);
+
+	mixinfo->output_scaler.negative_scale = 500.0f / (mid - min);
+	mixinfo->output_scaler.positive_scale = 500.0f / (max - mid);
+	mixinfo->output_scaler.offset = 0.0f;
+	mixinfo->output_scaler.min_output = -1.0f;
+	mixinfo->output_scaler.max_output = 1.0f;
+
+	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+
+	if (sm != nullptr) {
+		mixinfo = nullptr;
+		debug("PWM input mixer for %d", input);
+
+	} else {
+		debug("could not allocate memory for PWM input mixer");
+	}
+
+out:
+
+	if (mixinfo != nullptr)
+		free(mixinfo);
+
+	return sm;
+}
+
+unsigned
+SimpleMixer::mix(float *outputs, unsigned space)
+{
+	float		sum = 0.0f;
+
+	if (_info == nullptr)
+		return 0;
+
+	if (space < 1)
+		return 0;
+
+	for (unsigned i = 0; i < _info->control_count; i++) {
+		float input;
+
+		_control_cb(_cb_handle,
+			    _info->controls[i].control_group,
+			    _info->controls[i].control_index,
+			    input);
+
+		sum += scale(_info->controls[i].scaler, input);
+	}
+
+	*outputs = scale(_info->output_scaler, sum);
+	return 1;
+}
+
+void
+SimpleMixer::groups_required(uint32_t &groups)
+{
+	for (unsigned i = 0; i < _info->control_count; i++)
+		groups |= 1 << _info->controls[i].control_group;
+}
+
+int
+SimpleMixer::check()
+{
+	int ret;
+	float junk;
+
+	/* sanity that presumes that a mixer includes a control no more than once */
+	/* max of 32 groups due to groups_required API */
+	if (_info->control_count > 32)
+		return -2;
+
+	/* validate the output scaler */
+	ret = scale_check(_info->output_scaler);
+
+	if (ret != 0)
+		return ret;
+
+	/* validate input scalers */
+	for (unsigned i = 0; i < _info->control_count; i++) {
+
+		/* verify that we can fetch the control */
+		if (_control_cb(_cb_handle,
+				_info->controls[i].control_group,
+				_info->controls[i].control_index,
+				junk) != 0) {
+			return -3;
+		}
+
+		/* validate the scaler */
+		ret = scale_check(_info->controls[i].scaler);
+
+		if (ret != 0)
+			return (10 * i + ret);
+	}
+
+	return 0;
+}

apps/uORB/topics/vehicle_gps_position.h

@@ -65,8 +65,8 @@ struct vehicle_gps_position_s
 	uint16_t counter_pos_valid;		/**< is only increased when new lat/lon/alt information was added   */
 	uint16_t eph;				/**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME   */
 	uint16_t epv;				/**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535   */
-	float s_variance;			// XXX testing
+	float s_variance;			/**< speed accuracy estimate cm/s */
-	float p_variance;			// XXX testing
+	float p_variance;			/**< position accuracy estimate cm */
 	uint16_t vel;				/**< GPS ground speed (m/s * 100). If unknown, set to: 65535   */
 	float vel_n;				/**< GPS ground speed in m/s */
 	float vel_e;				/**< GPS ground speed in m/s */
@@ -84,7 +84,6 @@ struct vehicle_gps_position_s
 
 	/* flags */
 	float vel_ned_valid;			/**< Flag to indicate if NED speed is valid */
-
 };
 
 /**

nuttx/configs/px4fmu/nsh/defconfig

@@ -246,18 +246,18 @@ CONFIG_USART6_SERIAL_CONSOLE=n
 
 #Mavlink messages can be bigger than 128
 CONFIG_USART1_TXBUFSIZE=512
-CONFIG_USART2_TXBUFSIZE=128
+CONFIG_USART2_TXBUFSIZE=256
-CONFIG_USART3_TXBUFSIZE=128
+CONFIG_USART3_TXBUFSIZE=256
-CONFIG_UART4_TXBUFSIZE=128
+CONFIG_UART4_TXBUFSIZE=256
-CONFIG_UART5_TXBUFSIZE=64
+CONFIG_UART5_TXBUFSIZE=256
 CONFIG_USART6_TXBUFSIZE=128
 
 CONFIG_USART1_RXBUFSIZE=512
-CONFIG_USART2_RXBUFSIZE=128
+CONFIG_USART2_RXBUFSIZE=256
-CONFIG_USART3_RXBUFSIZE=128
+CONFIG_USART3_RXBUFSIZE=256
-CONFIG_UART4_RXBUFSIZE=128
+CONFIG_UART4_RXBUFSIZE=256
-CONFIG_UART5_RXBUFSIZE=128
+CONFIG_UART5_RXBUFSIZE=256
-CONFIG_USART6_RXBUFSIZE=128
+CONFIG_USART6_RXBUFSIZE=256
 
 CONFIG_USART1_BAUD=57600
 CONFIG_USART2_BAUD=115200

nuttx/configs/px4io/common/ld.script

@@ -74,6 +74,15 @@ SECTIONS
 		_etext = ABSOLUTE(.);
 	} > flash
 
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
 	.ARM.extab : {
 		*(.ARM.extab*)
 	} > flash

nuttx/configs/px4io/io/appconfig

@@ -35,3 +35,6 @@ CONFIGURED_APPS += drivers/boards/px4io
 CONFIGURED_APPS += drivers/stm32
 
 CONFIGURED_APPS += px4io
+
+# Mixer library from systemlib
+CONFIGURED_APPS += systemlib/mixer

nuttx/configs/px4io/io/defconfig

@@ -87,6 +87,8 @@ CONFIG_ARCH_LEDS=n
 CONFIG_ARCH_BUTTONS=n
 CONFIG_ARCH_CALIBRATION=n
 CONFIG_ARCH_DMA=n
+CONFIG_ARCH_MATH_H=y
+
 CONFIG_ARMV7M_CMNVECTOR=y
 
 #
@@ -343,8 +345,8 @@ CONFIG_DEBUG_I2C=n
 CONFIG_DEBUG_INPUT=n
 
 CONFIG_MSEC_PER_TICK=1
-CONFIG_HAVE_CXX=n
+CONFIG_HAVE_CXX=y
-CONFIG_HAVE_CXXINITIALIZE=n
+CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_MM_REGIONS=1
 CONFIG_MM_SMALL=y
 CONFIG_ARCH_LOWPUTC=y

nuttx/drivers/serial/serial.c

@@ -660,8 +660,10 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
 
   int ret = dev->ops->ioctl(filep, cmd, arg);
 
-  /* If the low-level handler didn't handle the call, see if we can handle it here */
+  /*
-
+   * The device ioctl() handler returns -ENOTTY when it doesn't know
+   * how to handle the command. Check if we can handle it here.
+   */
   if (ret == -ENOTTY)
     {
       switch (cmd)

nuttx/include/stdio.h

@@ -133,6 +133,7 @@ EXTERN void   perror(FAR const char *s);
 EXTERN int    ungetc(int c, FAR FILE *stream);
 EXTERN int    vprintf(FAR const char *format, va_list ap);
 EXTERN int    vfprintf(FAR FILE *stream, const char *format, va_list ap);
+EXTERN int    vdprintf(FAR int fd, const char *format, va_list ap);
 EXTERN int    vsprintf(FAR char *buf, const char *format, va_list ap);
 EXTERN int    avsprintf(FAR char **ptr, const char *fmt, va_list ap);
 EXTERN int    vsnprintf(FAR char *buf, size_t size, const char *format, va_list ap);

nuttx/lib/lib_internal.h

@@ -140,6 +140,7 @@ extern int lib_vsprintf(FAR struct lib_outstream_s *obj,
 /* Defined lib_rawprintf.c */
 
 extern int lib_rawvprintf(const char *src, va_list ap);
+extern int lib_rawvdprintf(int fd, const char *fmt, va_list ap);
 
 /* Defined lib_lowprintf.c */
 

nuttx/lib/stdio/Make.defs

@@ -50,7 +50,7 @@ CSRCS += lib_fopen.c lib_fclose.c lib_fread.c lib_libfread.c lib_fseek.c \
 		   lib_gets.c lib_fwrite.c lib_libfwrite.c lib_fflush.c \
 		   lib_libflushall.c lib_libfflush.c lib_rdflush.c lib_wrflush.c \
 		   lib_fputc.c lib_puts.c lib_fputs.c lib_ungetc.c lib_vprintf.c \
-		   lib_fprintf.c lib_vfprintf.c lib_stdinstream.c lib_stdoutstream.c \
+		   lib_fprintf.c lib_vfprintf.c lib_vdprintf.c lib_stdinstream.c lib_stdoutstream.c \
 		   lib_perror.c
 endif
 endif

nuttx/lib/stdio/lib_rawprintf.c

@@ -149,3 +149,18 @@ int lib_rawprintf(const char *fmt, ...)
 
   return ret;
 }
+
+
+/****************************************************************************
+ * Name: lib_rawvdprintf
+ ****************************************************************************/
+
+int lib_rawvdprintf(int fd, const char *fmt, va_list ap)
+{
+  /* Wrap the stdout in a stream object and let lib_vsprintf
+   * do the work.
+   */
+  struct lib_rawoutstream_s rawoutstream;
+  lib_rawoutstream(&rawoutstream, fd);
+  return lib_vsprintf(&rawoutstream.public, fmt, ap);
+}

nuttx/lib/stdio/lib_vdprintf.c

@@ -0,0 +1,81 @@
+/****************************************************************************
+ * lib/stdio/lib_vdprintf.c
+ *
+ *   Copyright (C) 2007-2009, 2011 Andrew Tridgell. All rights reserved.
+ *   Author: Andrew Tridgell <andrew@tridgell.net>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+
+#include "lib_internal.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Global Constant Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Constant Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+int vdprintf(int fd, FAR const char *fmt, va_list ap)
+{
+	return lib_rawvdprintf(fd, fmt, ap);
+}

nuttx/tools/mkdeps.sh

@@ -86,8 +86,12 @@ dodep ()
       fi
     done
     if [ -z "$fullpath" ]; then
-      echo "# ERROR: No readable file for $1 found at any location"
+	if [ -r $1 ]; then
-      show_usage
+	    fullpath=$1
+	else
+	    echo "# ERROR: No readable file for $1 found at any location"
+	    show_usage
+	fi
     fi
   fi