From 145a6c4c49b1aac9a8b8065ac5e48ba50754ba7f Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sat, 4 Aug 2012 20:05:47 -0700
Subject: [PATCH] Work in progress; standard mixer API and utility
---
apps/drivers/drv_accel.h | 2 +-
apps/drivers/drv_baro.h | 2 +-
apps/drivers/drv_gyro.h | 2 +-
apps/drivers/drv_mag.h | 2 +-
apps/drivers/drv_mixer.h | 91 ++++++++
apps/drivers/drv_orb_dev.h | 2 +-
apps/drivers/drv_pwm_output.h | 2 +-
apps/px4/fmu/Makefile | 4 +
apps/px4/fmu/fmu.cpp | 75 ++++++-
apps/systemcmds/mixer/Makefile | 42 ++++
apps/systemcmds/mixer/mixer.c | 342 +++++++++++++++++++++++++++++
apps/systemlib/mixer.c | 167 +++++++++++++-
apps/systemlib/mixer.h | 101 ++++++---
nuttx/configs/px4fmu/nsh/appconfig | 1 +
14 files changed, 787 insertions(+), 48 deletions(-)
create mode 100644 apps/drivers/drv_mixer.h
create mode 100644 apps/systemcmds/mixer/Makefile
create mode 100644 apps/systemcmds/mixer/mixer.c
diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h
index bf13b2c32b..370cc5d877 100644
--- a/apps/drivers/drv_accel.h
+++ b/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 */
diff --git a/apps/drivers/drv_baro.h b/apps/drivers/drv_baro.h
index 4cfb354547..323b25c835 100644
--- a/apps/drivers/drv_baro.h
+++ b/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 */
diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h
index 21f6493b13..82e23f62af 100644
--- a/apps/drivers/drv_gyro.h
+++ b/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 */
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index 7e90e9e463..673a3988fd 100644
--- a/apps/drivers/drv_mag.h
+++ b/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 */
diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h
new file mode 100644
index 0000000000..10cda792a9
--- /dev/null
+++ b/apps/drivers/drv_mixer.h
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ * 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 */
diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h
index bacef1cd3a..b3fc01a5fa 100644
--- a/apps/drivers/drv_orb_dev.h
+++ b/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))
/*
diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index 551f9b1a65..73e3310aed 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/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)
diff --git a/apps/px4/fmu/Makefile b/apps/px4/fmu/Makefile
index 7f1f836e30..8317748729 100644
--- a/apps/px4/fmu/Makefile
+++ b/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
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 6c7d9f7426..ff47261e7d 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/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;
diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile
new file mode 100644
index 0000000000..a83085bfc5
--- /dev/null
+++ b/apps/systemcmds/mixer/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+# 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
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
new file mode 100644
index 0000000000..c5adf8dbfc
--- /dev/null
+++ b/apps/systemcmds/mixer/mixer.c
@@ -0,0 +1,342 @@
+/****************************************************************************
+ *
+ * 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);
+}
diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c
index 553680fed9..cc7baebc2d 100644
--- a/apps/systemlib/mixer.c
+++ b/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;
+}
diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h
index f95d03f315..cd110f01e5 100644
--- a/apps/systemlib/mixer.h
+++ b/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 */
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 85762f4c6b..b9965bfe21 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/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
--
GitLab