diff options
author | px4dev <px4@purgatory.org> | 2012-08-04 20:05:47 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-05 14:13:33 -0700 |
commit | 145a6c4c49b1aac9a8b8065ac5e48ba50754ba7f (patch) | |
tree | e335a9579f8a86500d245daf38a29604ebdd379b | |
parent | 62e18b580cc56ecabf0a586ab64efbcd1c4139ba (diff) | |
download | px4-firmware-145a6c4c49b1aac9a8b8065ac5e48ba50754ba7f.tar.gz px4-firmware-145a6c4c49b1aac9a8b8065ac5e48ba50754ba7f.tar.bz2 px4-firmware-145a6c4c49b1aac9a8b8065ac5e48ba50754ba7f.zip |
Work in progress; standard mixer API and utility
-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 | 91 | ||||
-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 | 75 | ||||
-rw-r--r-- | apps/systemcmds/mixer/Makefile | 42 | ||||
-rw-r--r-- | apps/systemcmds/mixer/mixer.c | 342 | ||||
-rw-r--r-- | apps/systemlib/mixer.c | 167 | ||||
-rw-r--r-- | apps/systemlib/mixer.h | 101 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 |
14 files changed, 787 insertions, 48 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..10cda792a --- /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 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..ff47261e7 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 000000000..a83085bfc --- /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 000000000..c5adf8dbf --- /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 553680fed..cc7baebc2 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 f95d03f31..cd110f01e 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 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 |