aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2012-08-05 23:15:16 +0200
committerLorenz Meier <lm@inf.ethz.ch>2012-08-05 23:15:16 +0200
commit31850115bbe09261725d64c3eacb5896fa770cba (patch)
tree54f87c8a63a2d0b8fe1316768b53d18ef883fcdb
parentdf42d0604eb802003379d18041c6444252deb8fc (diff)
parent9804447a66391a1e216068cbd849e0011c851f7a (diff)
downloadpx4-firmware-31850115bbe09261725d64c3eacb5896fa770cba.tar.gz
px4-firmware-31850115bbe09261725d64c3eacb5896fa770cba.tar.bz2
px4-firmware-31850115bbe09261725d64c3eacb5896fa770cba.zip
Merge branch 'master' of github.com:PX4/Firmware
-rw-r--r--apps/drivers/drv_accel.h2
-rw-r--r--apps/drivers/drv_baro.h2
-rw-r--r--apps/drivers/drv_gyro.h2
-rw-r--r--apps/drivers/drv_mag.h2
-rw-r--r--apps/drivers/drv_mixer.h96
-rw-r--r--apps/drivers/drv_orb_dev.h2
-rw-r--r--apps/drivers/drv_pwm_output.h2
-rw-r--r--apps/px4/fmu/Makefile4
-rw-r--r--apps/px4/fmu/fmu.cpp154
-rw-r--r--apps/systemcmds/mixer/Makefile42
-rw-r--r--apps/systemcmds/mixer/mixer.c360
-rw-r--r--apps/systemlib/mixer.c270
-rw-r--r--apps/systemlib/mixer.h137
-rw-r--r--apps/uORB/objects_common.cpp11
-rw-r--r--apps/uORB/topics/actuator_controls.h27
-rw-r--r--nuttx/configs/px4fmu/nsh/appconfig1
16 files changed, 1017 insertions, 97 deletions
diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h
index bf13b2c32..370cc5d87 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 4cfb35454..323b25c83 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 21f6493b1..82e23f62a 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 7e90e9e46..673a3988f 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 000000000..d66fbf3a9
--- /dev/null
+++ b/apps/drivers/drv_mixer.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * 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 drv_mixer.h
+ *
+ * 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 mixer_s mixer;
+};
+
+/**
+ * Handy macro for determining the allocation size of a MixInfo structure.
+ */
+#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct scaler_s)))
+
+/*
+ * 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.
+ *
+ * If no mixer is assigned for the given index, the ioctl returns ENOENT.
+ */
+#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer)
+
+/**
+ * Copy a mixer from *(struct mixer_s *)arg to the device.
+ *
+ * If arg is zero, the mixer is deleted.
+ */
+#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 bacef1cd3..b3fc01a5f 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 551f9b1a6..73e3310ae 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 7f1f836e3..831774872 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 6c7d9f742..57eb12f4e 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Driver/configurator for the PX4 FMU multi-purpose port.
+ * @file fmu.cpp
+ *
+ * Driver/configurator for the PX4 FMU multi-purpose port.
*/
#include <nuttx/config.h>
@@ -40,6 +42,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 +57,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 +80,8 @@ public:
virtual int init();
private:
+ static const unsigned _max_actuators = 4;
+
Mode _mode;
int _task;
int _t_actuators;
@@ -85,13 +91,14 @@ private:
volatile bool _task_should_exit;
bool _armed;
- MixMixer *_mixer[4];
+ mixer_s *_mixer[_max_actuators];
static void task_main_trampoline(int argc, char *argv[]);
- void task_main();
+ void task_main();
};
-namespace {
+namespace
+{
FMUServo *g_servo;
@@ -106,17 +113,19 @@ FMUServo::FMUServo(Mode mode) :
_task_should_exit(false),
_armed(false)
{
- for (unsigned i = 0; i < 4; i++)
+ for (unsigned i = 0; i < _max_actuators; i++)
_mixer[i] = nullptr;
}
FMUServo::~FMUServo()
{
if (_task != -1) {
+
/* task should wake up every 100ms or so at least */
_task_should_exit = true;
unsigned i = 0;
+
do {
/* wait 20ms */
usleep(20000);
@@ -142,11 +151,13 @@ FMUServo::init()
/* do regular cdev init */
ret = CDev::init();
+
if (ret != OK)
return ret;
/* start the IO interface task */
_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
+
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -164,8 +175,6 @@ FMUServo::task_main_trampoline(int argc, char *argv[])
void
FMUServo::task_main()
{
- log("ready");
-
/* configure for PWM output */
switch (_mode) {
case MODE_2PWM:
@@ -173,18 +182,20 @@ FMUServo::task_main()
/* XXX magic numbers */
up_pwm_servo_init(0x3);
break;
+
case MODE_4PWM:
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
break;
+
case MODE_NONE:
/* we should never get here... */
break;
}
/* subscribe to objects that we are interested in watching */
- _t_actuators = orb_subscribe(ORB_ID(actuator_controls));
+ _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(_t_actuators, 20); /* 50Hz update rate */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
@@ -213,10 +224,11 @@ FMUServo::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
- struct actuator_controls ac;
+ struct actuator_controls_s ac;
+ float *controls[1] = { &ac.control[0] };
/* get controls */
- orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &ac);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
@@ -225,7 +237,7 @@ FMUServo::task_main()
if (_mixer[i] != nullptr) {
/* mix controls to the actuator */
- float output = mixer_mix(_mixer[i], &ac.control[0]);
+ float output = mixer_mix(_mixer[i], &controls[0]);
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * output));
@@ -235,7 +247,7 @@ FMUServo::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
- struct actuator_armed aa;
+ struct actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -249,7 +261,7 @@ FMUServo::task_main()
::close(_t_armed);
/* make sure servos are off */
- up_pwm_servo_deinit();
+ up_pwm_servo_deinit();
/* note - someone else is responsible for restoring the GPIO config */
@@ -262,6 +274,9 @@ int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = OK;
+ int channel;
+ struct MixInfo *mi;
+ struct mixer_s *mm, *tmm;
switch (cmd) {
case PWM_SERVO_ARM:
@@ -278,15 +293,18 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
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;
}
+
break;
case PWM_SERVO_GET(2):
@@ -295,37 +313,121 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
- int channel = cmd - PWM_SERVO_SET(0);
- *(servo_position_t *)arg = up_pwm_servo_get(channel);
+ 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);
+
+ /* if no mixer is assigned, we return ENOENT */
+ if (_mixer[channel] == nullptr) {
+ ret = -ENOENT;
+ break;
+ }
+
+ /* 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 - MIXERIOCSETMIXER(0);
+
+ /* get the caller-supplied mixer and check */
+ mm = (struct mixer_s *)arg;
+
+ if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) { /* only the attitude group is supported */
+ ret = -EINVAL;
+ break;
+ }
+
+ /* allocate local storage and copy from the caller*/
+ if (mm != nullptr) {
+ tmm = (struct mixer_s *)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;
}
+
return ret;
}
-namespace {
+namespace
+{
enum PortMode {
+ PORT_MODE_UNSET = 0,
PORT_FULL_GPIO,
PORT_FULL_SERIAL,
PORT_FULL_PWM,
PORT_GPIO_AND_SERIAL,
PORT_PWM_AND_SERIAL,
PORT_PWM_AND_GPIO,
- PORT_MODE_UNSET
};
PortMode g_port_mode;
int
-fmu_new_mode(PortMode new_mode)
+fmu_new_mode(PortMode new_mode)
{
int fd;
int ret = OK;
@@ -334,6 +436,7 @@ fmu_new_mode(PortMode new_mode)
/* get hold of the GPIO configuration descriptor */
fd = open(GPIO_DEVICE_PATH, 0);
+
if (fd < 0)
return -errno;
@@ -386,15 +489,19 @@ fmu_new_mode(PortMode new_mode)
/* adjust GPIO config for serial mode(s) */
if (gpio_bits != 0)
ioctl(fd, GPIO_SET_ALT_1, gpio_bits);
+
close(fd);
/* create new PWM driver if required */
if (servo_mode != FMUServo::MODE_NONE) {
g_servo = new FMUServo(servo_mode);
+
if (g_servo == nullptr) {
ret = -ENOMEM;
+
} else {
ret = g_servo->init();
+
if (ret != OK) {
delete g_servo;
g_servo = nullptr;
@@ -407,7 +514,7 @@ fmu_new_mode(PortMode new_mode)
} // namespace
-extern "C" int fmu_main(int argc, char *argv[]);
+extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
int
fmu_main(int argc, char *argv[])
@@ -421,14 +528,19 @@ fmu_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
+
} else if (!strcmp(argv[1], "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm")) {
new_mode = PORT_FULL_PWM;
+
} else if (!strcmp(argv[1], "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
}
diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile
new file mode 100644
index 000000000..b016ddc57
--- /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 = 4096
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
new file mode 100644
index 000000000..bdf54bc20
--- /dev/null
+++ b/apps/systemcmds/mixer/mixer.c
@@ -0,0 +1,360 @@
+/****************************************************************************
+ *
+ * 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.c
+ *
+ * Mixer utility.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.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(const char *reason);
+static void load(const char *devname, const char *fname);
+static void save(const char *devname, const char *fname);
+static void show(const char *devname);
+
+int
+mixer_main(int argc, char *argv[])
+{
+ if (argc < 2)
+ usage("missing command");
+
+ if (!strcmp(argv[1], "load")) {
+ if (argc < 4)
+ usage("missing device or filename");
+
+ load(argv[2], argv[3]);
+
+ } else if (!strcmp(argv[1], "save")) {
+ if (argc < 4)
+ usage("missing device or filename");
+
+ save(argv[2], argv[3]);
+
+ } else if (!strcmp(argv[1], "show")) {
+ if (argc < 3)
+ usage("missing device name");
+
+ show(argv[2]);
+
+ } else {
+ usage("unrecognised command");
+ }
+
+ return 0;
+}
+
+static void
+usage(const char *reason)
+{
+ if (reason)
+ fprintf(stderr, "%s\n", reason);
+
+ fprintf(stderr, "usage:\n");
+ fprintf(stderr, " mixer show <device>\n");
+ fprintf(stderr, " mixer {load|save} <device> [<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 mixer_s *mixer = NULL;
+
+ /* 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) {
+ fprintf(stderr, "read for mixer %d failed\n", i);
+ goto out;
+ }
+
+ /* end of file? */
+ if (ret == 0)
+ break;
+
+ if (mixer != NULL) {
+ /* sanity check the mixer */
+ ret = mixer_check(mixer, NUM_ACTUATOR_CONTROL_GROUPS, NUM_ACTUATOR_CONTROLS);
+
+ if (ret != 0) {
+ fprintf(stderr, "mixer %u fails sanity check %d\n", i, ret);
+ goto out;
+ }
+
+ /* send the mixer to the device */
+ ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer);
+
+ if (ret < 0) {
+ fprintf(stderr, "mixer %d set failed\n", i);
+ goto out;
+ }
+
+ free(mixer);
+ mixer = NULL;
+
+ } else {
+ /* delete the mixer */
+ ret = ioctl(dev, MIXERIOCSETMIXER(i), 0);
+
+ if (ret < 0) {
+ fprintf(stderr, "mixer %d clear failed\n", i);
+ goto out;
+ }
+ }
+ }
+
+ 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 = (struct MixInfo *)malloc(MIXINFO_SIZE(0));
+ mi->num_controls = 0;
+ }
+
+ /* 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)
+ 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++) {
+ struct mixer_s *mm;
+
+ ret = getmixer(dev, i, &mi);
+ mm = &mi->mixer;
+
+ if (ret < 0) {
+ if (errno != ENOENT)
+ goto out;
+
+ mm = NULL;
+ }
+
+ ret = mixer_save(defs, mm);
+
+ 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 = NULL;
+ 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) {
+ if (errno != ENOENT)
+ goto out;
+
+ continue;
+ }
+
+ printf("mixer %d:\n", i);
+ printf(" -ve scale +ve scale offset low limit high limit\n");
+ printf("output %8.4f %8.4f %8.4f %8.4f %8.4f\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("(%u,%u) %8.4f %8.4f %8.4f %8.4f %8.4f\n",
+ mi->mixer.control_scaler[j].control_group,
+ mi->mixer.control_scaler[j].control_index,
+ 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 553680fed..8b1dcc054 100644
--- a/apps/systemlib/mixer.c
+++ b/apps/systemlib/mixer.c
@@ -43,55 +43,97 @@
* 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)
+scale_check(struct scaler_s *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.1f)
+ return 1;
+
+ if (scale->offset < -1.1f)
+ return 2;
+
+ if (scale->lower_limit > scale->upper_limit)
+ return 3;
+
+ if (scale->lower_limit < -1.1f)
+ return 4;
+
+ if (scale->upper_limit > 1.1f)
+ return 5;
+
+ return 0;
}
int
-mixer_check(struct MixMixer *mixer, unsigned control_count)
+mixer_check(struct mixer_s *mixer, unsigned group_count, 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;
+ int ret;
+
+ /* sanity that presumes that a mixer includes a control no more than once */
+ if (mixer->control_count > (group_count * control_count))
+ return -2;
+
+ /* validate the output scaler */
+ ret = scale_check(&mixer->output_scaler);
+
+ if (ret != 0)
+ return ret;
+ /* validate input scalers */
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;
+
+ /* range-check input controls */
+ if (mixer->control_scaler[i].control_group >= group_count)
+ return -3;
+
+ if (mixer->control_scaler[i].control_index >= control_count)
+ return -3;
+
+ /* validate the scaler */
+ ret = scale_check(&mixer->control_scaler[i]);
+
+ if (ret != 0)
+ return (10 * i + ret);
}
+
return 0;
}
+void
+mixer_requires(struct mixer_s *mixer, uint32_t *groups)
+{
+ for (unsigned i = 0; i < mixer->control_count; i++)
+ *groups |= 1 << mixer->control_scaler[i].control_group;
+}
+
+/**
+ * Apply a scaler to a value.
+ *
+ * @param scaler The applied scaler.
+ * @param input The value to scale.
+ * @output The scaled value.
+ */
static float
-scale(struct MixScaler *scaler, float input)
+scale(struct scaler_s *scaler, float input)
{
float output;
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;
}
@@ -100,15 +142,187 @@ scale(struct MixScaler *scaler, float input)
}
float
-mixer_mix(struct MixMixer *mixer, float *controls)
+mixer_mix(struct mixer_s *mixer, float **controls)
{
- struct MixScaler *scaler;
float sum = 0.0f;
for (unsigned i = 0; i < mixer->control_count; i++) {
- scaler = &mixer->control_scaler[i];
- sum += scale(scaler, controls[scaler->control]);
+
+ struct scaler_s *scaler = &mixer->control_scaler[i];
+ float *cg = controls[scaler->control_group];
+
+ sum += scale(scaler, cg[scaler->control_index]);
}
return scale(&mixer->output_scaler, sum);
}
+
+/**
+ * Effectively fdgets()
+ */
+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 */
+ puts("line too long");
+ return -1;
+}
+
+static int
+mixer_load_scaler(const char *buf, struct scaler_s *scaler)
+{
+ 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)
+ return -1;
+
+ scaler->control_group = u[0];
+ scaler->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->lower_limit = s[3] / 10000.0f;
+ scaler->upper_limit = s[4] / 10000.0f;
+
+ return 0;
+}
+
+int
+mixer_load(int fd, struct mixer_s **mp)
+{
+ int ret, result = -1;
+ struct mixer_s *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;
+
+ /* if there are scalers, load them */
+ if (scalers > 0) {
+
+ /* allocate mixer */
+ scalers--;
+ mixer = (struct mixer_s *)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++) {
+ ret = mixer_getline(fd, buf, sizeof(buf));
+
+ if (ret < 1)
+ goto out;
+
+ if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
+ goto out;
+ }
+
+ } else {
+ /* we return NULL for the mixer, which is interpreted elsewhere as "no mixer" */
+ }
+
+ 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 scaler_s *scaler)
+{
+ int s[5];
+
+ s[0] = 10000.0f * scaler->negative_scale;
+ s[1] = 10000.0f * scaler->positive_scale;
+ s[2] = 10000.0f * scaler->offset;
+ s[3] = 10000.0f * scaler->lower_limit;
+ s[4] = 10000.0f * scaler->upper_limit;
+
+ return sprintf(buf, "S: %u %u %d %d %d %d %d\n",
+ scaler->control_group, scaler->control_index,
+ s[0], s[1], s[2], s[3], s[4]);
+}
+
+int
+mixer_save(int fd, struct mixer_s *mixer)
+{
+ char buf[100];
+ int len, ret;
+
+ /* write the mixer header */
+ len = sprintf(buf, "M: %u\n", (mixer != NULL) ? mixer->control_count : 0);
+ ret = write(fd, buf, len);
+
+ if (ret != len)
+ return -1;
+
+ if (mixer != NULL) {
+ /* 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;
+} \ No newline at end of file
diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h
index f95d03f31..3a9e31bb1 100644
--- a/apps/systemlib/mixer.h
+++ b/apps/systemlib/mixer.h
@@ -31,9 +31,12 @@
*
****************************************************************************/
+#ifndef _SYSTEMLIB_MIXER_H
+#define _SYSTEMLIB_MIXER_H
+
/**
* @file mixer.h
- *
+ *
* Generic control value mixing library.
*
* This library implements a generic mixer function that can be used
@@ -43,10 +46,13 @@
* Terminology
* ===========
*
- * control
+ * control value
* A mixer input value, typically provided by some controlling
* component of the system.
*
+ * control group
+ * A collection of controls provided by a single controlling component.
+ *
* actuator
* The mixer output value.
*
@@ -55,7 +61,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 +106,7 @@
* Mixing
* ------
*
- * Mixing is performed by summing the scaled control values.
+ * Mixing is performed by summing the scaled control values.
*
*
* Controls
@@ -109,7 +115,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,45 +127,110 @@
* 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 scaler_s {
+ 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 */
+ uint8_t control_group; /**< control group this scaler reads from */
+ uint8_t control_index; /**< control index within the group */
+};
+
+struct mixer_s {
+ unsigned control_count; /**< number of control scalers */
+ struct scaler_s output_scaler; /**< scaler applied to mixer output */
+ struct scaler_s 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 mixer_s) + ((_num_scalers) * sizeof(struct scaler_s)))
__BEGIN_DECLS
/**
* Perform a mixer calculation.
*
- * Note that the controls array is assumed to be sufficiently large for any control
- * index in the mixer.
+ * Note that the controls array, and the arrays it indexes, are assumed
+ * to be sufficiently large for any control index in the mixer.
*
* @param mixer Mixer configuration.
- * @param controls Array of input control values.
+ * @param controls Array of pointers to control group values.
* @return The mixed output.
*/
- __EXPORT float mixer_mix(struct MixMixer *mixer, float *controls);
+__EXPORT float mixer_mix(struct mixer_s *mixer, float **controls);
+
+/**
+ * Check a mixer configuration for sanity.
+ *
+ * @param mixer The mixer configuration to be checked.
+ * @param group_count The highest-numbered control group that
+ * should be considered legal.
+ * @param control_count The highest control index that should be
+ * considered legal.
+ * @return Zero if the mixer configuration is sane,
+ * nonzero otherwise.
+ */
+__EXPORT int mixer_check(struct mixer_s *mixer, unsigned group_count, unsigned control_count);
+
+/**
+ * Evaluate the control inputs to a mixer and update the bitmask of
+ * required control groups.
+ *
+ * This function allows an actuator driver to selectively fetch just
+ * the control groups required to support a particular mixer or set of
+ * mixers.
+ *
+ * @param mixer The mixer being evaluated.
+ * @param groups Pointer to a bitmask to be updated with set bits
+ * corresponding to the control groups used by the
+ * mixer.
+ */
+__EXPORT void mixer_requires(struct mixer_s *mixer, uint32_t *groups);
+
+/**
+ * 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: <scaler count>
+ * S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*>
+ * S: ...
+ *
+ * The <control ...> values for the output scaler are ignored by the mixer.
+ *
+ * Values marked * are integers representing floating point values; values are
+ * scaled by 10000 on load/save.
+ *
+ * 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.
+ *
+ * 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 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 mixer_s **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 mixer_s *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/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 8d84ff3c2..98e6c2987 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Common object definitions without a better home.
+ * @file objects_common.h
+ *
+ * Common object definitions without a better home.
*/
#include <nuttx/config.h>
@@ -99,3 +101,10 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
#include "topics/vehicle_attitude_setpoint.h"
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s);
+
+#include "topics/actuator_controls.h"
+ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+ORB_DEFINE(actuator_armed, struct actuator_armed_s);
diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h
index 03c0c7b7d..2b7d7de5d 100644
--- a/apps/uORB/topics/actuator_controls.h
+++ b/apps/uORB/topics/actuator_controls.h
@@ -32,11 +32,15 @@
****************************************************************************/
/**
- * @file Actuator control topic - mixer inputs.
+ * @file actuator_controls.h
*
- * Values published to this topic are the outputs of the vehicle control
+ * Actuator control topics - mixer inputs.
+ *
+ * Values published to these topics are the outputs of the vehicle control
* system, and are expected to be mixed and used to drive the actuators
* (servos, speed controls, etc.) that operate the vehicle.
+ *
+ * Each topic can be published by a single controller
*/
#ifndef TOPIC_ACTUATOR_CONTROLS_H
@@ -45,17 +49,24 @@
#include <stdint.h>
#include "../uORB.h"
-#define NUM_ACTUATOR_CONTROLS 16
+#define NUM_ACTUATOR_CONTROLS 8
+#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
-struct actuator_controls
-{
+struct actuator_controls_s {
float control[NUM_ACTUATOR_CONTROLS];
};
-ORB_DECLARE(actuator_controls);
+/* actuator control sets; this list can be expanded as more controllers emerge */
+ORB_DECLARE(actuator_controls_0);
+ORB_DECLARE(actuator_controls_1);
+ORB_DECLARE(actuator_controls_2);
+ORB_DECLARE(actuator_controls_3);
+
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
-struct actuator_armed
-{
+/** global 'actuator output is live' control. */
+struct actuator_armed_s {
bool armed;
};
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 85762f4c6..b9965bfe2 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