Work in progress; standard mixer API and utility

apps/drivers/drv_accel.h

@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_accel);
  * ioctl() definitions
  */
 
-#define _ACCELIOCBASE		(_SNIOCBASE + 0x20)
+#define _ACCELIOCBASE		(0x2000)
 #define _ACCELIOC(_n)		(_IOC(_ACCELIOCBASE, _n))
 
 /** set the driver polling rate to (arg) Hz, or one of the ACC_POLLRATE constants */

apps/drivers/drv_baro.h

@@ -65,7 +65,7 @@ ORB_DECLARE(sensor_baro);
  * ioctl() definitions
  */
 
-#define _BAROIOCBASE		(_SNIOCBASE + 0x10)
+#define _BAROIOCBASE		(0x2100)
 #define _BAROIOC(_n)		(_IOC(_BAROIOCBASE, _n))
 
 /** set the driver polling rate to (arg) Hz, or one of the BARO_POLLRATE constants */

apps/drivers/drv_gyro.h

@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_gyro);
  * ioctl() definitions
  */
 
-#define _GYROIOCBASE		(_SNIOCBASE + 0x10)
+#define _GYROIOCBASE		(0x2200)
 #define _GYROIOC(_n)		(_IOC(_GYROIOCBASE, _n))
 
 /** set the driver polling rate to (arg) Hz, or one of the GYRO_POLLRATE constants */

apps/drivers/drv_mag.h

@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_mag);
  * ioctl() definitions
  */
 
-#define _MAGIOCBASE		(_SNIOCBASE + 0x30)
+#define _MAGIOCBASE		(0x2300)
 #define _MAGIOC(_n)		(_IOC(_MAGIOBASE, _n))
 
 /** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */

apps/drivers/drv_mixer.h

+/****************************************************************************
+ *
+ *   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 ioctl interface.
+ *
+ * This interface can/should be exported by any device that supports
+ * control -> actuator mixing.
+ */
+
+#ifndef _DRV_MIXER_H
+#define _DRV_MIXER_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include <systemlib/mixer.h>
+
+/**
+ * Structure used for receiving mixers.
+ *
+ * Note that the mixers array is not actually an array of mixers; it
+ * simply represents the first mixer in the buffer.
+ */
+struct MixInfo
+{
+	unsigned	num_controls;
+	struct MixMixer	mixer;
+};
+
+/**
+ * Handy macro for determining the allocation size of a MixInfo structure.
+ */
+#define MIXINFO_SIZE(_num_controls)	(sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct MixScaler)))
+
+/*
+ * ioctl() definitions
+ */
+
+#define _MIXERIOCBASE		(0x2400)
+#define _MIXERIOC(_n)		(_IOC(_MIXERIOCBASE, _n))
+
+/** get the number of actuators that require mixers in *(unsigned)arg */
+#define MIXERIOCGETMIXERCOUNT	_MIXERIOC(0)
+
+/**
+ * Copy a mixer from the device into *(struct MixInfo *)arg.
+ *
+ * The num_controls field indicates the number of controls for which space
+ * is allocated following the MixInfo structure.  If the allocation
+ * is too small, no mixer data is retured.  The control_count field in
+ * the MixInfo.mixer structure is always updated.
+ */
+#define MIXERIOCGETMIXER(_mixer)	_MIXERIOC(0x20 + _mixer)
+
+/**
+ * Copy a mixer from *(struct MixMixer *)arg to the device.
+ */
+#define MIXERIOCSETMIXER(_mixer)	_MIXERIOC(0x40 + _mixer)
+
+#endif /* _DRV_ACCEL_H */

apps/drivers/drv_orb_dev.h

@@ -58,7 +58,7 @@
 /** maximum ogbject name length */
 #define ORB_MAXNAME		32
 
-#define _ORBIOCBASE		(_DIOCBASE + 0x80)
+#define _ORBIOCBASE		(0x2500)
 #define _ORBIOC(_n)		(_IOC(_ORBIOCBASE, _n))
 
 /*

apps/drivers/drv_pwm_output.h

@@ -94,7 +94,7 @@ ORB_DECLARE(output_pwm);
  * Note that ioctls and ObjDev updates should not be mixed, as the
  * behaviour of the system in this case is not defined.
  */
-#define _PWM_SERVO_BASE		0x7500
+#define _PWM_SERVO_BASE		0x2600
 
 /** arm all servo outputs handle by this driver */
 #define PWM_SERVO_ARM		_IOC(_PWM_SERVO_BASE, 0)

apps/px4/fmu/Makefile

@@ -35,4 +35,8 @@
 # Interface driver for the PX4FMU board
 #
 
+APPNAME		 = fmu
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
 include $(APPDIR)/mk/app.mk

apps/px4/fmu/fmu.cpp

@@ -40,6 +40,7 @@
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
+#include <stdlib.h>
 #include <semaphore.h>
 #include <string.h>
 #include <fcntl.h>
@@ -54,6 +55,7 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
+#include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/mixer.h>
@@ -76,6 +78,8 @@ public:
 	virtual int	init();
 
 private:
+	static const unsigned _max_actuators = 4;
+
 	Mode		_mode;
 	int		_task;
 	int		_t_actuators;
@@ -85,7 +89,7 @@ private:
 	volatile bool	_task_should_exit;
 	bool		_armed;
 
-	MixMixer	*_mixer[4];
+	MixMixer	*_mixer[_max_actuators];
 
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main();	
@@ -262,6 +266,9 @@ int
 FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
+	int channel;
+	struct MixInfo *mi;
+	struct MixMixer *mm, *tmm;
 
 	switch (cmd) {
 	case PWM_SERVO_ARM:
@@ -282,7 +289,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0):
 	case PWM_SERVO_SET(1):
 		if (arg < 2100) {
-			int channel = cmd - PWM_SERVO_SET(0);
+			channel = cmd - PWM_SERVO_SET(0);
 			up_pwm_servo_set(channel, arg);
 		} else {
 			ret = -EINVAL;
@@ -298,11 +305,73 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* FALLTHROUGH */
 	case PWM_SERVO_GET(0):
 	case PWM_SERVO_GET(1): {
-		int channel = cmd - PWM_SERVO_SET(0);
+		channel = cmd - PWM_SERVO_SET(0);
 		*(servo_position_t *)arg = up_pwm_servo_get(channel);
 		break;
 	}
 
+	case MIXERIOCGETMIXERCOUNT:
+		if (_mode == MODE_4PWM) {
+			*(unsigned *)arg = 4;
+		} else {
+			*(unsigned *)arg = 2;
+		}
+		break;
+
+	case MIXERIOCGETMIXER(3):
+	case MIXERIOCGETMIXER(2):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+		/* FALLTHROUGH */
+	case MIXERIOCGETMIXER(1):
+	case MIXERIOCGETMIXER(0):
+		channel = cmd - MIXERIOCGETMIXER(0);
+
+		/* caller's MixInfo */
+		mi = (struct MixInfo *)arg;
+
+		/* if MixInfo claims to be big enough, copy mixer info */
+		if (mi->num_controls >= _mixer[channel]->control_count) {
+			memcpy(&mi->mixer, _mixer[channel], MIXER_SIZE(_mixer[channel]->control_count));
+		} else {
+			/* just update MixInfo with actual size of the mixer */
+			mi->mixer.control_count = _mixer[channel]->control_count;
+		}
+		break;
+
+	case MIXERIOCSETMIXER(3):
+	case MIXERIOCSETMIXER(2):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+		/* FALLTHROUGH */
+	case MIXERIOCSETMIXER(1):
+	case MIXERIOCSETMIXER(0):
+		channel = cmd - MIXERIOCGETMIXER(0);
+
+		/* caller- supplied mixer */
+		mm = (struct MixMixer *)arg;
+
+		/* allocate local storage and copy from the caller*/
+		if (mm != nullptr) {
+			tmm = (struct MixMixer *)malloc(MIXER_SIZE(mm->control_count));
+			memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
+		} else {
+			tmm = nullptr;
+		}
+
+		/* swap in new mixer for old */
+		mm = _mixer[channel];
+		_mixer[channel] = tmm;
+
+		/* if there was an old mixer, free it */
+		if (mm != nullptr)
+			free(mm);
+		break;
+
 	default:
 		ret = -ENOTTY;
 		break;

apps/systemcmds/mixer/Makefile

+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Build the mixer tool.
+#
+
+APPNAME		 = mixer
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk

apps/systemcmds/mixer/mixer.c

+/****************************************************************************
+ *
+ *   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 utility.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <getopt.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <systemlib/mixer.h>
+#include <drivers/drv_mixer.h>
+#include <uORB/topics/actuator_controls.h>
+
+__EXPORT int mixer_main(int argc, char *argv[]);
+
+static void	usage(void);
+static void	load(const char *devname, const char *fname);
+static void	save(const char *devname, const char *fname);
+static void	show(const char *devname);
+
+enum Operation {
+	LOAD,
+	SAVE,
+	SHOW,
+	NONE
+};
+
+int
+mixer_main(int argc, char *argv[])
+{
+	const char *devname = NULL;
+	const char *fname = NULL;
+	int ch;
+	enum Operation todo = NONE;
+
+	if (argc < 2)
+		usage();
+	if (!strcmp(argv[1], "load")) {
+		todo = LOAD;
+	} else if (!strcmp(argv[1], "save")) {
+		todo = SAVE;
+	} else if (!strcmp(argv[1], "show")) {
+		todo = SHOW;
+	} else {
+		usage();
+	}
+
+	while ((ch = getopt(argc, argv, "d:f:")) != EOF) {
+		switch (ch) {
+		case 'd':
+			devname = optarg;
+			break;
+		case 'f':
+			fname = optarg;
+			break;
+		default:
+			usage();
+		}
+	}
+
+	switch (todo) {
+	case LOAD:
+		if ((devname == NULL) || (fname == NULL))
+			usage();
+		load(devname, fname);
+		break;
+
+	case SAVE:
+		if ((devname == NULL) || (fname == NULL))
+			usage();
+		save(devname, fname);
+		break;
+
+	case SHOW:
+		if (devname == NULL)
+			usage();
+		show(devname);
+		break;
+	default:
+		exit(1);
+	}
+	return 0;
+}
+
+static void
+usage(void)
+{
+	fprintf(stderr, "usage:\n");
+	fprintf(stderr, "  mixer {load|save|show} -d <device> [-f <filename>]\n");
+	exit(1);
+}
+
+static void
+load(const char *devname, const char *fname)
+{
+	int		defs = -1;
+	int		dev = -1;
+	unsigned	num_mixers = 0;
+	int		ret, result = 1;
+	struct MixMixer	*mixer;
+
+	/* open the device */
+	if ((dev = open(devname, 0)) < 0) {
+		fprintf(stderr, "can't open %s\n", devname);
+		goto out;
+	}
+
+	/* open the definition file */
+	if ((defs = open(fname, O_RDONLY)) < 0) {
+		fprintf(stderr, "can't open %s\n", fname);
+		goto out;
+	}
+
+	/* find out how many mixers the device supports */
+	ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
+	if (num_mixers < 1) {
+		fprintf(stderr, "can't get mixer count from %s\n", devname);
+		goto out;
+	}
+
+	/* send mixers to the device */
+	for (unsigned i = 0; i < num_mixers; i++) {
+		ret = mixer_load(defs, &mixer);
+		if (ret < 0)
+			goto out;
+
+		/* end of file? */
+		if (ret == 0)
+			break;
+
+		/* sanity check the mixer */
+		if (mixer_check(mixer, NUM_ACTUATOR_CONTROLS)) {
+			fprintf(stderr, "mixer %u fails sanity check\n", i);
+			goto out;
+		}
+
+		/* send the mixer to the device */
+		ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer);
+		if (ret < 0)
+			goto out;
+
+		free(mixer);
+		mixer = NULL;
+	}
+
+	printf("mixer: loaded %d mixers to %s\n", num_mixers, devname);
+	result = 0;
+
+out:
+	/* free the mixers array */
+	if (mixer != NULL)
+		free(mixer);
+	if (defs != -1)
+		close(defs);
+	if (dev != -1)
+		close(dev);
+
+	exit(result);
+}
+
+static int
+getmixer(int dev, unsigned mixer_number, struct MixInfo **mip)
+{
+	struct MixInfo	*mi = *mip;
+	int		ret;
+
+	/* first-round initialisation */
+	if (mi == NULL) {
+		mi->num_controls = 0;
+		mi = (struct MixInfo *)malloc(MIXINFO_SIZE(mi->num_controls));
+	}
+
+	/* loop trying to get the next mixer until the buffer is big enough */
+	do {
+		/* try to get the mixer into the buffer as it stands */
+		ret = ioctl(dev, MIXERIOCGETMIXER(mixer_number), (unsigned long)mi);
+		if (ret < 0) {
+			fprintf(stderr, "MIXERIOCGETMIXER error\n");
+			return -1;
+		}
+
+		/* did the mixer fit? */
+		if (mi->mixer.control_count <= mi->num_controls)
+			break;
+
+		/* re-allocate to suit */
+		mi->num_controls = mi->mixer.control_count;
+		mi = (struct MixInfo *)realloc(mi, MIXINFO_SIZE(mi->num_controls));
+
+		/* oops, blew up the heap */
+		if (mi == NULL)
+			return -1;
+
+	} while(true);
+
+	*mip = mi;
+	return 0;
+}
+
+static void
+save(const char *devname, const char *fname)
+{
+	struct MixInfo	*mi = NULL;
+	int		defs = -1;
+	int		dev = -1;
+	unsigned	num_mixers = 0;
+	int		ret, result = 1;
+
+	/* open the device */
+	if ((dev = open(devname, 0)) < 0) {
+		fprintf(stderr, "can't open %s\n", devname);
+		goto out;
+	}
+
+	/* find out how many mixers the device supports */
+	ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
+	if (num_mixers < 1) {
+		fprintf(stderr, "can't get mixer count from %s\n", devname);
+		goto out;
+	}
+
+	/* open the definition file */
+	if ((defs = open(fname, O_WRONLY | O_CREAT)) < 0) {
+		fprintf(stderr, "can't open %s\n", fname);
+		goto out;
+	}
+
+	/* get mixers from the device and save them */
+	for (unsigned i = 0; i < num_mixers; i++) {
+
+		ret = getmixer(dev, i, &mi);
+		if (ret < 0)
+			goto out;
+
+		ret = mixer_save(defs, &mi->mixer);
+		if (ret < 0)
+			goto out;
+	}
+
+	result = 0;
+
+out:
+	/* free the mixinfo */
+	if (mi != NULL)
+		free(mi);
+	if (defs != -1)
+		close(defs);
+	if (dev != -1)
+		close(dev);
+
+	exit(result);
+}
+
+static void
+show(const char *devname)
+{
+	struct MixInfo	*mi;
+	int		dev = -1;
+	unsigned	num_mixers = 0;
+	int		ret;
+
+	/* open the device */
+	if ((dev = open(devname, 0)) < 0) {
+		fprintf(stderr, "can't open %s\n", devname);
+		goto out;
+	}
+
+	/* find out how many mixers the device supports */
+	ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
+	if (num_mixers < 1) {
+		fprintf(stderr, "can't get mixer count from %s\n", devname);
+		goto out;
+	}
+
+	/* get mixers from the device and print them */
+	for (unsigned i = 0; i < num_mixers; i++) {
+
+		ret = getmixer(dev, i, &mi);
+		if (ret < 0)
+			goto out;
+
+		printf("mixer %d : %d controls\n", i, mi->mixer.control_count);
+		printf("        -ve scale  +ve scale  offset  low limit  high limit");
+		printf("output  %f   %f   %f   %f   %f\n", 
+			mi->mixer.output_scaler.negative_scale,
+			mi->mixer.output_scaler.positive_scale,
+			mi->mixer.output_scaler.offset,
+			mi->mixer.output_scaler.lower_limit,
+			mi->mixer.output_scaler.upper_limit);
+		for (unsigned j = 0; j < mi->mixer.control_count; j++) {
+			printf("%d:     %f   %f   %f   %f   %f\n", 
+				mi->mixer.control_scaler[j].negative_scale,
+				mi->mixer.control_scaler[j].positive_scale,
+				mi->mixer.control_scaler[j].offset,
+				mi->mixer.control_scaler[j].lower_limit,
+				mi->mixer.control_scaler[j].upper_limit);
+		}
+	}
+
+out:
+	/* free the mixinfo */
+	if (mi != NULL)
+		free(mi);
+	if (dev != -1)
+		close(dev);
+	exit(0);
+}

apps/systemlib/mixer.c

@@ -43,22 +43,31 @@
  * See mixer.h for more details.
  */
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
 #include "mixer.h"
 
 static int
 scale_check(struct MixScaler *scale)
 {
- 	if (scale->offset > 1.0f)
- 		return -1;
- 	if (scale->offset > 1.0f)
- 		return -1;
- 	if (scale->lower_limit > scale->upper_limit)
- 		return -1;
- 	if (scale->lower_limit < -1.0f)
- 		return -1;
- 	if (scale->upper_limit > 1.0f)
- 		return -1;
- 	return 0;
+	if (scale->offset > 1.0f)
+		return -1;
+
+	if (scale->offset > 1.0f)
+		return -1;
+
+	if (scale->lower_limit > scale->upper_limit)
+		return -1;
+
+	if (scale->lower_limit < -1.0f)
+		return -1;
+
+	if (scale->upper_limit > 1.0f)
+		return -1;
+
+	return 0;
 }
 
 int
@@ -66,17 +75,21 @@ mixer_check(struct MixMixer *mixer, unsigned control_count)
 {
 	if (mixer->control_count < 1)
 		return -1;
+
 	if (mixer->control_count > control_count)
 		return -1;
+
 	if (!scale_check(&mixer->output_scaler))
 		return -1;
 
 	for (unsigned i = 0; i < mixer->control_count; i++) {
 		if (mixer->control_scaler[i].control >= control_count)
 			return -1;
+
 		if (!scale_check(&mixer->control_scaler[i]))
 			return -1;
 	}
+
 	return 0;
 }
 
@@ -87,11 +100,14 @@ scale(struct MixScaler *scaler, float input)
 
 	if (input < 0.0f) {
 		output = (input * scaler->negative_scale) + scaler->offset;
+
 	} else {
 		output = (input * scaler->positive_scale) + scaler->offset;
 	}
+
 	if (output > scaler->upper_limit) {
 		output = scaler->upper_limit;
+
 	} else if (output < scaler->lower_limit) {
 		output = scaler->lower_limit;
 	}
@@ -112,3 +128,132 @@ mixer_mix(struct MixMixer *mixer, float *controls)
 
 	return scale(&mixer->output_scaler, sum);
 }
+
+static int
+mixer_getline(int fd, char *line, unsigned maxlen)
+{
+	int	ret;
+	char	c;
+
+	while (--maxlen) {
+		ret = read(fd, &c, 1);
+		if (ret <= 0)
+			return ret;
+		if (c == '\r')
+			continue;
+		if (c == '\n') {
+			*line = '\0';
+			return 1;
+		}
+		*line++ = c;
+	}
+	/* line too long */
+	return -1;
+}
+
+static int
+mixer_load_scaler(const char *buf, struct MixScaler *scaler)
+{
+	if (sscanf(buf, "S: %u %f %f %f %f %f",
+		   &scaler->control, &scaler->negative_scale, &scaler->positive_scale,
+		   &scaler->offset, &scaler->lower_limit, &scaler->upper_limit) != 6)
+		return -1;
+
+	return 0;
+}
+
+int
+mixer_load(int fd, struct MixMixer **mp)
+{
+	int		ret, result = -1;
+	struct MixMixer *mixer = NULL;
+	char		buf[100];
+	unsigned	scalers;
+
+	ret = mixer_getline(fd, buf, sizeof(buf));
+
+	/* end of file? */
+	if (ret == 0)
+		result = 0;
+
+	/* can't proceed */
+	if (ret < 1)
+		goto out;
+
+	/* get header */
+	if (sscanf(buf, "M: %u", &scalers) != 1)
+		goto out;
+
+	/* must have at least one scaler */
+	if (scalers < 1)
+		goto out;
+
+	/* allocate mixer */
+	scalers--;
+	mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
+
+	if (mixer == NULL)
+		goto out;
+
+	mixer->control_count = scalers;
+
+	ret = mixer_getline(fd, buf, sizeof(buf));
+
+	if (ret < 1)
+		goto out;
+
+	if (mixer_load_scaler(buf, &mixer->output_scaler))
+		goto out;
+
+	for (unsigned i = 0; i < scalers; i++) {
+		if (mixer_getline(fd, buf, sizeof(buf)))
+			goto out;
+		if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
+			goto out;
+	}
+
+	result = 1;
+
+out:
+	/* on error, discard allocated mixer */
+	if ((result <= 0) && (mixer != NULL))
+		free(mixer);
+	*mp = mixer;
+	return result;
+}
+
+static int
+mixer_save_scaler(char *buf, struct MixScaler *scaler)
+{
+	return sprintf(buf, "S: %u %f %f %f %f %f\n",
+		       scaler->control, scaler->negative_scale, scaler->positive_scale,
+		       scaler->offset, scaler->lower_limit, scaler->upper_limit);
+}
+
+int
+mixer_save(int fd, struct MixMixer *mixer)
+{
+	char		buf[100];
+	int		len, ret;
+	
+	/* write the mixer header */
+	len = sprintf(buf, "M: %u\n", mixer->control_count);
+	ret = write(fd, buf, len);
+	if (ret != len)
+		return -1;
+
+	/* write the output scaler */
+	len = mixer_save_scaler(buf, &mixer->output_scaler);
+	write(fd, buf, len);
+	if (ret != len)
+		return -1;
+
+	/* write the control scalers */
+	for (unsigned j = 0; j < mixer->control_count; j++) {
+		len = mixer_save_scaler(buf, &mixer->control_scaler[j]);
+		write(fd, buf, len);
+		if (ret != len)
+			return -1;
+	}
+	return 0;
+}

apps/systemlib/mixer.h

@@ -31,6 +31,9 @@
  *
  ****************************************************************************/
 
+#ifndef _SYSTEMLIB_MIXER_H
+#define _SYSTEMLIB_MIXER_H
+
 /**
  * @file mixer.h
  * 
@@ -55,7 +58,7 @@
  *
  * An actuator derives its value from the combination of one or more
  * control values. Each of the control values is scaled according to
- * the actuator's configuration and then combined to produce the 
+ * the actuator's configuration and then combined to produce the
  * actuator value, which may then be further scaled to suit the specific
  * output type.
  *
@@ -100,7 +103,7 @@
  * Mixing
  * ------
  *
- * Mixing is performed by summing the scaled control values.  
+ * Mixing is performed by summing the scaled control values.
  *
  *
  * Controls
@@ -109,7 +112,7 @@
  * Each mixer is presented with an array of controls from which it
  * selects the set that will be mixed for each actuator.
  *
- * The precise assignment of controls may vary depending on the 
+ * The precise assignment of controls may vary depending on the
  * application, but the following assignments should be used
  * when appropriate.
  *
@@ -121,22 +124,25 @@
  *     3   | primary thrust
  */
 
- struct MixScaler
- {
- 	unsigned	control;	/**< control consumed by this scaler */
- 	float		negative_scale;	/**< scale for inputs < 0 */
- 	float		positive_scale;	/**< scale for inputs > 0 */
- 	float		offset;		/**< bias applied to output */
- 	float		lower_limit;	/**< minimum output value */
- 	float		upper_limit;	/**< maximum output value */
- };
+struct MixScaler {
+	unsigned	control;	/**< control consumed by this scaler */
+	float		negative_scale;	/**< scale for inputs < 0 */
+	float		positive_scale;	/**< scale for inputs > 0 */
+	float		offset;		/**< bias applied to output */
+	float		lower_limit;	/**< minimum output value */
+	float		upper_limit;	/**< maximum output value */
+};
+
+struct MixMixer {
+	unsigned		control_count;	/**< number of control scalers */
+	struct MixScaler	output_scaler;	/**< scaler applied to mixer output */
+	struct MixScaler	control_scaler[0]; /**< array of control scalers */
+};
 
- struct MixMixer
- {
- 	unsigned		control_count;	/**< number of control scalers */
- 	struct MixScaler	output_scaler;	/**< scaler applied to mixer output */
- 	struct MixScaler	control_scaler[0]; /**< array of control scalers */
- };
+/**
+ * Handy macro for determining the allocation size of a mixer.
+ */
+#define MIXER_SIZE(_num_scalers)	(sizeof(struct MixMixer) + ((_num_scalers) * sizeof(struct MixScaler)))
 
 __BEGIN_DECLS
 
@@ -150,16 +156,55 @@ __BEGIN_DECLS
  * @param controls		Array of input control values.
  * @return			The mixed output.
  */
- __EXPORT float	mixer_mix(struct MixMixer *mixer, float *controls);
+__EXPORT float	mixer_mix(struct MixMixer *mixer, float *controls);
+
+/**
+ * Check a mixer configuration for sanity.
+ *
+ * @param mixer		The mixer configuration to be checked.
+ * @param control_count	The number of controls in the system.
+ * @return			Zero if the mixer configuration is sane,
+ *				nonzero otherwise.
+ */
+__EXPORT int	mixer_check(struct MixMixer *mixer, unsigned control_count);
+
+/**
+ * Read a mixer definition from a file.
+ *
+ * A mixer definition is a text representation of the configuration of a
+ * mixer.  The definition consists of a single-line header indicating the
+ * number of scalers and then one line defining each scaler.  The first
+ * scaler in the file is always the output scaler, followed by the input
+ * scalers.
+ *
+ * M: <control count + 1>
+ * S: <control> <negative_scale> <positive_scale> <offset> <lower_limit> <upper_limit>
+ * S: ...
+ *
+ * The <control> value for the output scaler is ignored.
+ *
+ * Multiple mixer definitions may be stored in a single file; it is assumed that
+ * the reader will know how many to expect and read accordingly. Mixers may be
+ * 'skipped' in a file by  setting indicating that the mixer has only one scaler 
+ * (the output scaler). This results in a mixer with zero controls, which will
+ * always generate output corresponding to the output scaler offset.
+ *
+ * @param fd			The file to read the definitions from.
+ * @param mixer			Mixer is returned here.
+ * @return			1 if a mixer was read, zero on EOF or negative on error.
+ */
+__EXPORT int	mixer_load(int fd, struct MixMixer **mixer);
+
+/**
+ * Save a mixer definition to a file.
+ *
+ * @param fd			The file to write the definitions to.
+ * @param mixer			The mixer definition to save.
+ * @return			Zero on success, negative on error.
+ */
+__EXPORT int	mixer_save(int fd, struct MixMixer *mixers);
 
- /**
-  * Check a mixer configuration for sanity.
-  *
-  * @param mixer		The mixer configuration to be checked.
-  * @param control_count	The number of controls in the system.
-  * @return			Zero if the mixer configuration is sane,
-  *				nonzero otherwise.
-  */
- __EXPORT int	mixer_check(struct MixMixer *mixer, unsigned control_count);
 
 __END_DECLS
+
+#endif /* _SYSTEMLIB_MIXER_H */

nuttx/configs/px4fmu/nsh/appconfig

@@ -48,6 +48,7 @@ CONFIGURED_APPS += systemcmds/reboot
 CONFIGURED_APPS += systemcmds/perf
 CONFIGURED_APPS += systemcmds/top
 CONFIGURED_APPS += systemcmds/boardinfo
+CONFIGURED_APPS += systemcmds/mixer
 #CONFIGURED_APPS += systemcmds/calibration
 
 CONFIGURED_APPS += uORB