diff options
-rw-r--r-- | apps/drivers/drv_accel.h | 2 | ||||
-rw-r--r-- | apps/drivers/drv_baro.h | 2 | ||||
-rw-r--r-- | apps/drivers/drv_gyro.h | 2 | ||||
-rw-r--r-- | apps/drivers/drv_mag.h | 2 | ||||
-rw-r--r-- | apps/drivers/drv_mixer.h | 96 | ||||
-rw-r--r-- | apps/drivers/drv_orb_dev.h | 2 | ||||
-rw-r--r-- | apps/drivers/drv_pwm_output.h | 2 | ||||
-rw-r--r-- | apps/px4/fmu/Makefile | 4 | ||||
-rw-r--r-- | apps/px4/fmu/fmu.cpp | 154 | ||||
-rw-r--r-- | apps/systemcmds/mixer/Makefile | 42 | ||||
-rw-r--r-- | apps/systemcmds/mixer/mixer.c | 360 | ||||
-rw-r--r-- | apps/systemlib/mixer.c | 270 | ||||
-rw-r--r-- | apps/systemlib/mixer.h | 137 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 11 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_controls.h | 27 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 |
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 |