From 145a6c4c49b1aac9a8b8065ac5e48ba50754ba7f Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 4 Aug 2012 20:05:47 -0700 Subject: [PATCH 1/3] Work in progress; standard mixer API and utility --- apps/drivers/drv_accel.h | 2 +- apps/drivers/drv_baro.h | 2 +- apps/drivers/drv_gyro.h | 2 +- apps/drivers/drv_mag.h | 2 +- apps/drivers/drv_mixer.h | 91 ++++++++ apps/drivers/drv_orb_dev.h | 2 +- apps/drivers/drv_pwm_output.h | 2 +- apps/px4/fmu/Makefile | 4 + apps/px4/fmu/fmu.cpp | 75 ++++++- apps/systemcmds/mixer/Makefile | 42 ++++ apps/systemcmds/mixer/mixer.c | 342 +++++++++++++++++++++++++++++ apps/systemlib/mixer.c | 167 +++++++++++++- apps/systemlib/mixer.h | 101 ++++++--- nuttx/configs/px4fmu/nsh/appconfig | 1 + 14 files changed, 787 insertions(+), 48 deletions(-) create mode 100644 apps/drivers/drv_mixer.h create mode 100644 apps/systemcmds/mixer/Makefile create mode 100644 apps/systemcmds/mixer/mixer.c diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index bf13b2c32b..370cc5d877 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -75,7 +75,7 @@ ORB_DECLARE(sensor_accel); * ioctl() definitions */ -#define _ACCELIOCBASE (_SNIOCBASE + 0x20) +#define _ACCELIOCBASE (0x2000) #define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) /** set the driver polling rate to (arg) Hz, or one of the ACC_POLLRATE constants */ diff --git a/apps/drivers/drv_baro.h b/apps/drivers/drv_baro.h index 4cfb354547..323b25c835 100644 --- a/apps/drivers/drv_baro.h +++ b/apps/drivers/drv_baro.h @@ -65,7 +65,7 @@ ORB_DECLARE(sensor_baro); * ioctl() definitions */ -#define _BAROIOCBASE (_SNIOCBASE + 0x10) +#define _BAROIOCBASE (0x2100) #define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n)) /** set the driver polling rate to (arg) Hz, or one of the BARO_POLLRATE constants */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 21f6493b13..82e23f62af 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -75,7 +75,7 @@ ORB_DECLARE(sensor_gyro); * ioctl() definitions */ -#define _GYROIOCBASE (_SNIOCBASE + 0x10) +#define _GYROIOCBASE (0x2200) #define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n)) /** set the driver polling rate to (arg) Hz, or one of the GYRO_POLLRATE constants */ diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 7e90e9e463..673a3988fd 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -75,7 +75,7 @@ ORB_DECLARE(sensor_mag); * ioctl() definitions */ -#define _MAGIOCBASE (_SNIOCBASE + 0x30) +#define _MAGIOCBASE (0x2300) #define _MAGIOC(_n) (_IOC(_MAGIOBASE, _n)) /** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */ diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h new file mode 100644 index 0000000000..10cda792a9 --- /dev/null +++ b/apps/drivers/drv_mixer.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Mixer ioctl interface. + * + * This interface can/should be exported by any device that supports + * control -> actuator mixing. + */ + +#ifndef _DRV_MIXER_H +#define _DRV_MIXER_H + +#include +#include + +#include + +/** + * Structure used for receiving mixers. + * + * Note that the mixers array is not actually an array of mixers; it + * simply represents the first mixer in the buffer. + */ +struct MixInfo +{ + unsigned num_controls; + struct MixMixer mixer; +}; + +/** + * Handy macro for determining the allocation size of a MixInfo structure. + */ +#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct MixScaler))) + +/* + * ioctl() definitions + */ + +#define _MIXERIOCBASE (0x2400) +#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n)) + +/** get the number of actuators that require mixers in *(unsigned)arg */ +#define MIXERIOCGETMIXERCOUNT _MIXERIOC(0) + +/** + * Copy a mixer from the device into *(struct MixInfo *)arg. + * + * The num_controls field indicates the number of controls for which space + * is allocated following the MixInfo structure. If the allocation + * is too small, no mixer data is retured. The control_count field in + * the MixInfo.mixer structure is always updated. + */ +#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer) + +/** + * Copy a mixer from *(struct MixMixer *)arg to the device. + */ +#define MIXERIOCSETMIXER(_mixer) _MIXERIOC(0x40 + _mixer) + +#endif /* _DRV_ACCEL_H */ diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h index bacef1cd3a..b3fc01a5fa 100644 --- a/apps/drivers/drv_orb_dev.h +++ b/apps/drivers/drv_orb_dev.h @@ -58,7 +58,7 @@ /** maximum ogbject name length */ #define ORB_MAXNAME 32 -#define _ORBIOCBASE (_DIOCBASE + 0x80) +#define _ORBIOCBASE (0x2500) #define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n)) /* diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 551f9b1a65..73e3310aed 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -94,7 +94,7 @@ ORB_DECLARE(output_pwm); * Note that ioctls and ObjDev updates should not be mixed, as the * behaviour of the system in this case is not defined. */ -#define _PWM_SERVO_BASE 0x7500 +#define _PWM_SERVO_BASE 0x2600 /** arm all servo outputs handle by this driver */ #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) diff --git a/apps/px4/fmu/Makefile b/apps/px4/fmu/Makefile index 7f1f836e30..8317748729 100644 --- a/apps/px4/fmu/Makefile +++ b/apps/px4/fmu/Makefile @@ -35,4 +35,8 @@ # Interface driver for the PX4FMU board # +APPNAME = fmu +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + include $(APPDIR)/mk/app.mk diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 6c7d9f7426..ff47261e7d 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -54,6 +55,7 @@ #include #include #include +#include #include #include @@ -76,6 +78,8 @@ public: virtual int init(); private: + static const unsigned _max_actuators = 4; + Mode _mode; int _task; int _t_actuators; @@ -85,7 +89,7 @@ private: volatile bool _task_should_exit; bool _armed; - MixMixer *_mixer[4]; + MixMixer *_mixer[_max_actuators]; static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -262,6 +266,9 @@ int FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = OK; + int channel; + struct MixInfo *mi; + struct MixMixer *mm, *tmm; switch (cmd) { case PWM_SERVO_ARM: @@ -282,7 +289,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): if (arg < 2100) { - int channel = cmd - PWM_SERVO_SET(0); + channel = cmd - PWM_SERVO_SET(0); up_pwm_servo_set(channel, arg); } else { ret = -EINVAL; @@ -298,11 +305,73 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): { - int channel = cmd - PWM_SERVO_SET(0); + channel = cmd - PWM_SERVO_SET(0); *(servo_position_t *)arg = up_pwm_servo_get(channel); break; } + case MIXERIOCGETMIXERCOUNT: + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + } else { + *(unsigned *)arg = 2; + } + break; + + case MIXERIOCGETMIXER(3): + case MIXERIOCGETMIXER(2): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + /* FALLTHROUGH */ + case MIXERIOCGETMIXER(1): + case MIXERIOCGETMIXER(0): + channel = cmd - MIXERIOCGETMIXER(0); + + /* caller's MixInfo */ + mi = (struct MixInfo *)arg; + + /* if MixInfo claims to be big enough, copy mixer info */ + if (mi->num_controls >= _mixer[channel]->control_count) { + memcpy(&mi->mixer, _mixer[channel], MIXER_SIZE(_mixer[channel]->control_count)); + } else { + /* just update MixInfo with actual size of the mixer */ + mi->mixer.control_count = _mixer[channel]->control_count; + } + break; + + case MIXERIOCSETMIXER(3): + case MIXERIOCSETMIXER(2): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + /* FALLTHROUGH */ + case MIXERIOCSETMIXER(1): + case MIXERIOCSETMIXER(0): + channel = cmd - MIXERIOCGETMIXER(0); + + /* caller- supplied mixer */ + mm = (struct MixMixer *)arg; + + /* allocate local storage and copy from the caller*/ + if (mm != nullptr) { + tmm = (struct MixMixer *)malloc(MIXER_SIZE(mm->control_count)); + memcpy(tmm, mm, MIXER_SIZE(mm->control_count)); + } else { + tmm = nullptr; + } + + /* swap in new mixer for old */ + mm = _mixer[channel]; + _mixer[channel] = tmm; + + /* if there was an old mixer, free it */ + if (mm != nullptr) + free(mm); + break; + default: ret = -ENOTTY; break; diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile new file mode 100644 index 0000000000..a83085bfc5 --- /dev/null +++ b/apps/systemcmds/mixer/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the mixer tool. +# + +APPNAME = mixer +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c new file mode 100644 index 0000000000..c5adf8dbfc --- /dev/null +++ b/apps/systemcmds/mixer/mixer.c @@ -0,0 +1,342 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Mixer utility. + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +__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 [-f ]\n"); + exit(1); +} + +static void +load(const char *devname, const char *fname) +{ + int defs = -1; + int dev = -1; + unsigned num_mixers = 0; + int ret, result = 1; + struct MixMixer *mixer; + + /* open the device */ + if ((dev = open(devname, 0)) < 0) { + fprintf(stderr, "can't open %s\n", devname); + goto out; + } + + /* open the definition file */ + if ((defs = open(fname, O_RDONLY)) < 0) { + fprintf(stderr, "can't open %s\n", fname); + goto out; + } + + /* find out how many mixers the device supports */ + ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers); + if (num_mixers < 1) { + fprintf(stderr, "can't get mixer count from %s\n", devname); + goto out; + } + + /* send mixers to the device */ + for (unsigned i = 0; i < num_mixers; i++) { + ret = mixer_load(defs, &mixer); + if (ret < 0) + goto out; + + /* end of file? */ + if (ret == 0) + break; + + /* sanity check the mixer */ + if (mixer_check(mixer, NUM_ACTUATOR_CONTROLS)) { + fprintf(stderr, "mixer %u fails sanity check\n", i); + goto out; + } + + /* send the mixer to the device */ + ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer); + if (ret < 0) + goto out; + + free(mixer); + mixer = NULL; + } + + printf("mixer: loaded %d mixers to %s\n", num_mixers, devname); + result = 0; + +out: + /* free the mixers array */ + if (mixer != NULL) + free(mixer); + if (defs != -1) + close(defs); + if (dev != -1) + close(dev); + + exit(result); +} + +static int +getmixer(int dev, unsigned mixer_number, struct MixInfo **mip) +{ + struct MixInfo *mi = *mip; + int ret; + + /* first-round initialisation */ + if (mi == NULL) { + mi->num_controls = 0; + mi = (struct MixInfo *)malloc(MIXINFO_SIZE(mi->num_controls)); + } + + /* loop trying to get the next mixer until the buffer is big enough */ + do { + /* try to get the mixer into the buffer as it stands */ + ret = ioctl(dev, MIXERIOCGETMIXER(mixer_number), (unsigned long)mi); + if (ret < 0) { + fprintf(stderr, "MIXERIOCGETMIXER error\n"); + return -1; + } + + /* did the mixer fit? */ + if (mi->mixer.control_count <= mi->num_controls) + break; + + /* re-allocate to suit */ + mi->num_controls = mi->mixer.control_count; + mi = (struct MixInfo *)realloc(mi, MIXINFO_SIZE(mi->num_controls)); + + /* oops, blew up the heap */ + if (mi == NULL) + return -1; + + } while(true); + + *mip = mi; + return 0; +} + +static void +save(const char *devname, const char *fname) +{ + struct MixInfo *mi = NULL; + int defs = -1; + int dev = -1; + unsigned num_mixers = 0; + int ret, result = 1; + + /* open the device */ + if ((dev = open(devname, 0)) < 0) { + fprintf(stderr, "can't open %s\n", devname); + goto out; + } + + /* find out how many mixers the device supports */ + ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers); + if (num_mixers < 1) { + fprintf(stderr, "can't get mixer count from %s\n", devname); + goto out; + } + + /* open the definition file */ + if ((defs = open(fname, O_WRONLY | O_CREAT)) < 0) { + fprintf(stderr, "can't open %s\n", fname); + goto out; + } + + /* get mixers from the device and save them */ + for (unsigned i = 0; i < num_mixers; i++) { + + ret = getmixer(dev, i, &mi); + if (ret < 0) + goto out; + + ret = mixer_save(defs, &mi->mixer); + if (ret < 0) + goto out; + } + + result = 0; + +out: + /* free the mixinfo */ + if (mi != NULL) + free(mi); + if (defs != -1) + close(defs); + if (dev != -1) + close(dev); + + exit(result); +} + +static void +show(const char *devname) +{ + struct MixInfo *mi; + int dev = -1; + unsigned num_mixers = 0; + int ret; + + /* open the device */ + if ((dev = open(devname, 0)) < 0) { + fprintf(stderr, "can't open %s\n", devname); + goto out; + } + + /* find out how many mixers the device supports */ + ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers); + if (num_mixers < 1) { + fprintf(stderr, "can't get mixer count from %s\n", devname); + goto out; + } + + /* get mixers from the device and print them */ + for (unsigned i = 0; i < num_mixers; i++) { + + ret = getmixer(dev, i, &mi); + if (ret < 0) + goto out; + + printf("mixer %d : %d controls\n", i, mi->mixer.control_count); + printf(" -ve scale +ve scale offset low limit high limit"); + printf("output %f %f %f %f %f\n", + mi->mixer.output_scaler.negative_scale, + mi->mixer.output_scaler.positive_scale, + mi->mixer.output_scaler.offset, + mi->mixer.output_scaler.lower_limit, + mi->mixer.output_scaler.upper_limit); + for (unsigned j = 0; j < mi->mixer.control_count; j++) { + printf("%d: %f %f %f %f %f\n", + mi->mixer.control_scaler[j].negative_scale, + mi->mixer.control_scaler[j].positive_scale, + mi->mixer.control_scaler[j].offset, + mi->mixer.control_scaler[j].lower_limit, + mi->mixer.control_scaler[j].upper_limit); + } + } + +out: + /* free the mixinfo */ + if (mi != NULL) + free(mi); + if (dev != -1) + close(dev); + exit(0); +} diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c index 553680fed9..cc7baebc2d 100644 --- a/apps/systemlib/mixer.c +++ b/apps/systemlib/mixer.c @@ -43,22 +43,31 @@ * See mixer.h for more details. */ +#include +#include +#include + #include "mixer.h" static int scale_check(struct MixScaler *scale) { - if (scale->offset > 1.0f) - return -1; - if (scale->offset > 1.0f) - return -1; - if (scale->lower_limit > scale->upper_limit) - return -1; - if (scale->lower_limit < -1.0f) - return -1; - if (scale->upper_limit > 1.0f) - return -1; - return 0; + if (scale->offset > 1.0f) + return -1; + + if (scale->offset > 1.0f) + return -1; + + if (scale->lower_limit > scale->upper_limit) + return -1; + + if (scale->lower_limit < -1.0f) + return -1; + + if (scale->upper_limit > 1.0f) + return -1; + + return 0; } int @@ -66,17 +75,21 @@ mixer_check(struct MixMixer *mixer, unsigned control_count) { if (mixer->control_count < 1) return -1; + if (mixer->control_count > control_count) return -1; + if (!scale_check(&mixer->output_scaler)) return -1; for (unsigned i = 0; i < mixer->control_count; i++) { if (mixer->control_scaler[i].control >= control_count) return -1; + if (!scale_check(&mixer->control_scaler[i])) return -1; } + return 0; } @@ -87,11 +100,14 @@ scale(struct MixScaler *scaler, float input) if (input < 0.0f) { output = (input * scaler->negative_scale) + scaler->offset; + } else { output = (input * scaler->positive_scale) + scaler->offset; } + if (output > scaler->upper_limit) { output = scaler->upper_limit; + } else if (output < scaler->lower_limit) { output = scaler->lower_limit; } @@ -112,3 +128,132 @@ mixer_mix(struct MixMixer *mixer, float *controls) return scale(&mixer->output_scaler, sum); } + +static int +mixer_getline(int fd, char *line, unsigned maxlen) +{ + int ret; + char c; + + while (--maxlen) { + ret = read(fd, &c, 1); + if (ret <= 0) + return ret; + if (c == '\r') + continue; + if (c == '\n') { + *line = '\0'; + return 1; + } + *line++ = c; + } + /* line too long */ + return -1; +} + +static int +mixer_load_scaler(const char *buf, struct MixScaler *scaler) +{ + if (sscanf(buf, "S: %u %f %f %f %f %f", + &scaler->control, &scaler->negative_scale, &scaler->positive_scale, + &scaler->offset, &scaler->lower_limit, &scaler->upper_limit) != 6) + return -1; + + return 0; +} + +int +mixer_load(int fd, struct MixMixer **mp) +{ + int ret, result = -1; + struct MixMixer *mixer = NULL; + char buf[100]; + unsigned scalers; + + ret = mixer_getline(fd, buf, sizeof(buf)); + + /* end of file? */ + if (ret == 0) + result = 0; + + /* can't proceed */ + if (ret < 1) + goto out; + + /* get header */ + if (sscanf(buf, "M: %u", &scalers) != 1) + goto out; + + /* must have at least one scaler */ + if (scalers < 1) + goto out; + + /* allocate mixer */ + scalers--; + mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers)); + + if (mixer == NULL) + goto out; + + mixer->control_count = scalers; + + ret = mixer_getline(fd, buf, sizeof(buf)); + + if (ret < 1) + goto out; + + if (mixer_load_scaler(buf, &mixer->output_scaler)) + goto out; + + for (unsigned i = 0; i < scalers; i++) { + if (mixer_getline(fd, buf, sizeof(buf))) + goto out; + if (mixer_load_scaler(buf, &mixer->control_scaler[i])) + goto out; + } + + result = 1; + +out: + /* on error, discard allocated mixer */ + if ((result <= 0) && (mixer != NULL)) + free(mixer); + *mp = mixer; + return result; +} + +static int +mixer_save_scaler(char *buf, struct MixScaler *scaler) +{ + return sprintf(buf, "S: %u %f %f %f %f %f\n", + scaler->control, scaler->negative_scale, scaler->positive_scale, + scaler->offset, scaler->lower_limit, scaler->upper_limit); +} + +int +mixer_save(int fd, struct MixMixer *mixer) +{ + char buf[100]; + int len, ret; + + /* write the mixer header */ + len = sprintf(buf, "M: %u\n", mixer->control_count); + ret = write(fd, buf, len); + if (ret != len) + return -1; + + /* write the output scaler */ + len = mixer_save_scaler(buf, &mixer->output_scaler); + write(fd, buf, len); + if (ret != len) + return -1; + + /* write the control scalers */ + for (unsigned j = 0; j < mixer->control_count; j++) { + len = mixer_save_scaler(buf, &mixer->control_scaler[j]); + write(fd, buf, len); + if (ret != len) + return -1; + } + return 0; +} diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h index f95d03f315..cd110f01e5 100644 --- a/apps/systemlib/mixer.h +++ b/apps/systemlib/mixer.h @@ -31,6 +31,9 @@ * ****************************************************************************/ +#ifndef _SYSTEMLIB_MIXER_H +#define _SYSTEMLIB_MIXER_H + /** * @file mixer.h * @@ -55,7 +58,7 @@ * * An actuator derives its value from the combination of one or more * control values. Each of the control values is scaled according to - * the actuator's configuration and then combined to produce the + * the actuator's configuration and then combined to produce the * actuator value, which may then be further scaled to suit the specific * output type. * @@ -100,7 +103,7 @@ * Mixing * ------ * - * Mixing is performed by summing the scaled control values. + * Mixing is performed by summing the scaled control values. * * * Controls @@ -109,7 +112,7 @@ * Each mixer is presented with an array of controls from which it * selects the set that will be mixed for each actuator. * - * The precise assignment of controls may vary depending on the + * The precise assignment of controls may vary depending on the * application, but the following assignments should be used * when appropriate. * @@ -121,22 +124,25 @@ * 3 | primary thrust */ - struct MixScaler - { - unsigned control; /**< control consumed by this scaler */ - float negative_scale; /**< scale for inputs < 0 */ - float positive_scale; /**< scale for inputs > 0 */ - float offset; /**< bias applied to output */ - float lower_limit; /**< minimum output value */ - float upper_limit; /**< maximum output value */ - }; +struct MixScaler { + unsigned control; /**< control consumed by this scaler */ + float negative_scale; /**< scale for inputs < 0 */ + float positive_scale; /**< scale for inputs > 0 */ + float offset; /**< bias applied to output */ + float lower_limit; /**< minimum output value */ + float upper_limit; /**< maximum output value */ +}; - struct MixMixer - { - unsigned control_count; /**< number of control scalers */ - struct MixScaler output_scaler; /**< scaler applied to mixer output */ - struct MixScaler control_scaler[0]; /**< array of control scalers */ - }; +struct MixMixer { + unsigned control_count; /**< number of control scalers */ + struct MixScaler output_scaler; /**< scaler applied to mixer output */ + struct MixScaler control_scaler[0]; /**< array of control scalers */ +}; + +/** + * Handy macro for determining the allocation size of a mixer. + */ +#define MIXER_SIZE(_num_scalers) (sizeof(struct MixMixer) + ((_num_scalers) * sizeof(struct MixScaler))) __BEGIN_DECLS @@ -150,16 +156,55 @@ __BEGIN_DECLS * @param controls Array of input control values. * @return The mixed output. */ - __EXPORT float mixer_mix(struct MixMixer *mixer, float *controls); +__EXPORT float mixer_mix(struct MixMixer *mixer, float *controls); + +/** + * Check a mixer configuration for sanity. + * + * @param mixer The mixer configuration to be checked. + * @param control_count The number of controls in the system. + * @return Zero if the mixer configuration is sane, + * nonzero otherwise. + */ +__EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count); + +/** + * Read a mixer definition from a file. + * + * A mixer definition is a text representation of the configuration of a + * mixer. The definition consists of a single-line header indicating the + * number of scalers and then one line defining each scaler. The first + * scaler in the file is always the output scaler, followed by the input + * scalers. + * + * M: + * S: + * S: ... + * + * The value for the output scaler is ignored. + * + * Multiple mixer definitions may be stored in a single file; it is assumed that + * the reader will know how many to expect and read accordingly. Mixers may be + * 'skipped' in a file by setting indicating that the mixer has only one scaler + * (the output scaler). This results in a mixer with zero controls, which will + * always generate output corresponding to the output scaler offset. + * + * @param fd The file to read the definitions from. + * @param mixer Mixer is returned here. + * @return 1 if a mixer was read, zero on EOF or negative on error. + */ +__EXPORT int mixer_load(int fd, struct MixMixer **mixer); + +/** + * Save a mixer definition to a file. + * + * @param fd The file to write the definitions to. + * @param mixer The mixer definition to save. + * @return Zero on success, negative on error. + */ +__EXPORT int mixer_save(int fd, struct MixMixer *mixers); - /** - * Check a mixer configuration for sanity. - * - * @param mixer The mixer configuration to be checked. - * @param control_count The number of controls in the system. - * @return Zero if the mixer configuration is sane, - * nonzero otherwise. - */ - __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count); __END_DECLS + +#endif /* _SYSTEMLIB_MIXER_H */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 85762f4c6b..b9965bfe21 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -48,6 +48,7 @@ CONFIGURED_APPS += systemcmds/reboot CONFIGURED_APPS += systemcmds/perf CONFIGURED_APPS += systemcmds/top CONFIGURED_APPS += systemcmds/boardinfo +CONFIGURED_APPS += systemcmds/mixer #CONFIGURED_APPS += systemcmds/calibration CONFIGURED_APPS += uORB From 9804776a0c8bd67d4a533e3302f1a598c35b868b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 5 Aug 2012 02:09:11 -0700 Subject: [PATCH 2/3] Checkpoint: more work in progress on mixer load/save --- apps/drivers/drv_mixer.h | 4 + apps/px4/fmu/fmu.cpp | 27 +++-- apps/systemcmds/mixer/Makefile | 2 +- apps/systemcmds/mixer/mixer.c | 156 ++++++++++++--------------- apps/systemlib/mixer.c | 98 +++++++++-------- apps/systemlib/mixer.h | 17 +-- apps/uORB/objects_common.cpp | 4 + apps/uORB/topics/actuator_controls.h | 4 +- 8 files changed, 161 insertions(+), 151 deletions(-) diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index 10cda792a9..f3a39e60c3 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -80,11 +80,15 @@ struct MixInfo * 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 MixMixer *)arg to the device. + * + * If arg is zero, the mixer is deleted. */ #define MIXERIOCSETMIXER(_mixer) _MIXERIOC(0x40 + _mixer) diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index ff47261e7d..8e468c0124 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -110,13 +110,14 @@ 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; @@ -168,8 +169,6 @@ FMUServo::task_main_trampoline(int argc, char *argv[]) void FMUServo::task_main() { - log("ready"); - /* configure for PWM output */ switch (_mode) { case MODE_2PWM: @@ -217,7 +216,7 @@ FMUServo::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { - struct actuator_controls ac; + struct actuator_controls_s ac; /* get controls */ orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac); @@ -239,7 +238,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); @@ -329,6 +328,12 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) 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; @@ -350,10 +355,14 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case MIXERIOCSETMIXER(1): case MIXERIOCSETMIXER(0): - channel = cmd - MIXERIOCGETMIXER(0); + channel = cmd - MIXERIOCSETMIXER(0); - /* caller- supplied mixer */ + /* get the caller-supplied mixer and check */ mm = (struct MixMixer *)arg; + if (mixer_check(mm, NUM_ACTUATOR_CONTROLS)) { + ret = -EINVAL; + break; + } /* allocate local storage and copy from the caller*/ if (mm != nullptr) { @@ -382,13 +391,13 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) 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; @@ -476,7 +485,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[]) diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile index a83085bfc5..b016ddc578 100644 --- a/apps/systemcmds/mixer/Makefile +++ b/apps/systemcmds/mixer/Makefile @@ -37,6 +37,6 @@ APPNAME = mixer PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index c5adf8dbfc..bbbe67ff79 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -38,9 +38,9 @@ #include #include #include -#include #include #include +#include #include #include @@ -48,80 +48,43 @@ __EXPORT int mixer_main(int argc, char *argv[]); -static void usage(void); +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); -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(); + usage("missing command"); if (!strcmp(argv[1], "load")) { - todo = LOAD; + if (argc < 4) + usage("missing device or filename"); + load(argv[2], argv[3]); } else if (!strcmp(argv[1], "save")) { - todo = SAVE; + if (argc < 4) + usage("missing device or filename"); + save(argv[2], argv[3]); } else if (!strcmp(argv[1], "show")) { - todo = SHOW; + if (argc < 3) + usage("missing device name"); + show(argv[2]); + } 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); + usage("unrecognised command"); } return 0; } static void -usage(void) +usage(const char *reason) { + if (reason) + fprintf(stderr, "%s\n", reason); fprintf(stderr, "usage:\n"); - fprintf(stderr, " mixer {load|save|show} -d [-f ]\n"); + fprintf(stderr, " mixer show \n"); + fprintf(stderr, " mixer {load|save} []\n"); exit(1); } @@ -132,7 +95,7 @@ load(const char *devname, const char *fname) int dev = -1; unsigned num_mixers = 0; int ret, result = 1; - struct MixMixer *mixer; + struct MixMixer *mixer = NULL; /* open the device */ if ((dev = open(devname, 0)) < 0) { @@ -156,29 +119,42 @@ load(const char *devname, const char *fname) /* send mixers to the device */ for (unsigned i = 0; i < num_mixers; i++) { ret = mixer_load(defs, &mixer); - if (ret < 0) + if (ret < 0) { + fprintf(stderr, "read for mixer %d failed\n", i); 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; + if (mixer != NULL) { + /* sanity check the mixer */ + ret = mixer_check(mixer, 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; + } } - - /* 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: @@ -201,18 +177,16 @@ getmixer(int dev, unsigned mixer_number, struct MixInfo **mip) /* first-round initialisation */ if (mi == NULL) { + mi = (struct MixInfo *)malloc(MIXINFO_SIZE(0)); 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"); + if (ret < 0) return -1; - } /* did the mixer fit? */ if (mi->mixer.control_count <= mi->num_controls) @@ -262,12 +236,16 @@ save(const char *devname, const char *fname) /* get mixers from the device and save them */ for (unsigned i = 0; i < num_mixers; i++) { + struct MixMixer *mm; ret = getmixer(dev, i, &mi); - if (ret < 0) - goto out; - - ret = mixer_save(defs, &mi->mixer); + mm = &mi->mixer; + if (ret < 0) { + if (errno != ENOENT) + goto out; + mm = NULL; + } + ret = mixer_save(defs, mm); if (ret < 0) goto out; } @@ -289,7 +267,7 @@ out: static void show(const char *devname) { - struct MixInfo *mi; + struct MixInfo *mi = NULL; int dev = -1; unsigned num_mixers = 0; int ret; @@ -311,19 +289,23 @@ show(const char *devname) for (unsigned i = 0; i < num_mixers; i++) { ret = getmixer(dev, i, &mi); - if (ret < 0) - goto out; + if (ret < 0) { + if (errno != ENOENT) + goto out; + continue; + } - 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", + 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("%d: %f %f %f %f %f\n", + printf("%d: %8.4f %8.4f %8.4f %8.4f %8.4f\n", + j, mi->mixer.control_scaler[j].negative_scale, mi->mixer.control_scaler[j].positive_scale, mi->mixer.control_scaler[j].offset, @@ -333,6 +315,8 @@ show(const char *devname) } out: + printf("done\n"); + usleep(100000); /* free the mixinfo */ if (mi != NULL) free(mi); diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c index cc7baebc2d..94fabe87a6 100644 --- a/apps/systemlib/mixer.c +++ b/apps/systemlib/mixer.c @@ -53,19 +53,19 @@ static int scale_check(struct MixScaler *scale) { if (scale->offset > 1.0f) - return -1; + return 1; - if (scale->offset > 1.0f) - return -1; + if (scale->offset < -1.0f) + return 2; if (scale->lower_limit > scale->upper_limit) - return -1; + return 3; if (scale->lower_limit < -1.0f) - return -1; + return 4; if (scale->upper_limit > 1.0f) - return -1; + return 5; return 0; } @@ -73,21 +73,25 @@ scale_check(struct MixScaler *scale) int mixer_check(struct MixMixer *mixer, unsigned control_count) { + int ret; + if (mixer->control_count < 1) return -1; if (mixer->control_count > control_count) - return -1; + return -2; - if (!scale_check(&mixer->output_scaler)) - return -1; + ret = scale_check(&mixer->output_scaler); + if (ret != 0) + return ret; for (unsigned i = 0; i < mixer->control_count; i++) { if (mixer->control_scaler[i].control >= control_count) - return -1; + return -3; - if (!scale_check(&mixer->control_scaler[i])) - return -1; + ret = scale_check(&mixer->control_scaler[i]); + if (ret != 0) + return (10 * i + ret); } return 0; @@ -148,6 +152,7 @@ mixer_getline(int fd, char *line, unsigned maxlen) *line++ = c; } /* line too long */ + puts("line too long"); return -1; } @@ -184,32 +189,33 @@ mixer_load(int fd, struct MixMixer **mp) if (sscanf(buf, "M: %u", &scalers) != 1) goto out; - /* must have at least one scaler */ - if (scalers < 1) - goto out; + /* if there are scalers, load them */ + if (scalers > 0) { - /* allocate mixer */ - scalers--; - mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers)); + /* 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))) + if (mixer == NULL) goto out; - if (mixer_load_scaler(buf, &mixer->control_scaler[i])) + + 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; + } } result = 1; @@ -237,23 +243,25 @@ mixer_save(int fd, struct MixMixer *mixer) int len, ret; /* write the mixer header */ - len = sprintf(buf, "M: %u\n", mixer->control_count); + len = sprintf(buf, "M: %u\n", (mixer != NULL) ? mixer->control_count : 0); 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]); + 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 cd110f01e5..d65041ea75 100644 --- a/apps/systemlib/mixer.h +++ b/apps/systemlib/mixer.h @@ -161,8 +161,8 @@ __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. + * @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. */ @@ -177,17 +177,18 @@ __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count); * scaler in the file is always the output scaler, followed by the input * scalers. * - * M: + * M: * S: * S: ... * - * The value for the output scaler is ignored. + * The value for the output scaler is ignored by the mixer. * * 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. + * 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. diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 8d84ff3c26..78708cdba7 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -99,3 +99,7 @@ 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, 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 03c0c7b7da..5cdaf0a4e5 100644 --- a/apps/uORB/topics/actuator_controls.h +++ b/apps/uORB/topics/actuator_controls.h @@ -47,14 +47,14 @@ #define NUM_ACTUATOR_CONTROLS 16 -struct actuator_controls +struct actuator_controls_s { float control[NUM_ACTUATOR_CONTROLS]; }; ORB_DECLARE(actuator_controls); -struct actuator_armed +struct actuator_armed_s { bool armed; }; From 9804447a66391a1e216068cbd849e0011c851f7a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 5 Aug 2012 13:43:16 -0700 Subject: [PATCH 3/3] More work on the mixer architecture. Solve the multiple publishers issue with 'control groups', one group per controller. Mixer inputs now specify both group and control offset within the group. Avoid using %f when loading/saving mixers; use scaled integers instead. --- apps/drivers/drv_mixer.h | 13 +-- apps/px4/fmu/fmu.cpp | 70 ++++++++++++---- apps/systemcmds/mixer/mixer.c | 74 ++++++++++++----- apps/systemlib/mixer.c | 119 ++++++++++++++++++++------- apps/systemlib/mixer.h | 65 ++++++++++----- apps/uORB/objects_common.cpp | 9 +- apps/uORB/topics/actuator_controls.h | 27 ++++-- 7 files changed, 274 insertions(+), 103 deletions(-) diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index f3a39e60c3..d66fbf3a97 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Mixer ioctl interface. + * @file drv_mixer.h + * + * Mixer ioctl interface. * * This interface can/should be exported by any device that supports * control -> actuator mixing. @@ -52,16 +54,15 @@ * Note that the mixers array is not actually an array of mixers; it * simply represents the first mixer in the buffer. */ -struct MixInfo -{ +struct MixInfo { unsigned num_controls; - struct MixMixer mixer; + 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 MixScaler))) +#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct scaler_s))) /* * ioctl() definitions @@ -86,7 +87,7 @@ struct MixInfo #define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer) /** - * Copy a mixer from *(struct MixMixer *)arg to the device. + * Copy a mixer from *(struct mixer_s *)arg to the device. * * If arg is zero, the mixer is deleted. */ diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 8e468c0124..57eb12f4eb 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 @@ -89,13 +91,14 @@ private: volatile bool _task_should_exit; bool _armed; - MixMixer *_mixer[_max_actuators]; + 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; @@ -122,6 +125,7 @@ FMUServo::~FMUServo() _task_should_exit = true; unsigned i = 0; + do { /* wait 20ms */ usleep(20000); @@ -147,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; @@ -176,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)); @@ -217,9 +225,10 @@ FMUServo::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { 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++) { @@ -228,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)); @@ -252,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 */ @@ -267,7 +276,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; int channel; struct MixInfo *mi; - struct MixMixer *mm, *tmm; + struct mixer_s *mm, *tmm; switch (cmd) { case PWM_SERVO_ARM: @@ -284,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) { channel = cmd - PWM_SERVO_SET(0); up_pwm_servo_set(channel, arg); + } else { ret = -EINVAL; } + break; case PWM_SERVO_GET(2): @@ -301,20 +313,23 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) ret = -EINVAL; break; } + /* FALLTHROUGH */ case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): { - channel = cmd - PWM_SERVO_SET(0); - *(servo_position_t *)arg = up_pwm_servo_get(channel); - break; - } + 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): @@ -323,6 +338,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) ret = -EINVAL; break; } + /* FALLTHROUGH */ case MIXERIOCGETMIXER(1): case MIXERIOCGETMIXER(0): @@ -340,10 +356,12 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long 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): @@ -352,22 +370,25 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) ret = -EINVAL; break; } + /* FALLTHROUGH */ case MIXERIOCSETMIXER(1): case MIXERIOCSETMIXER(0): channel = cmd - MIXERIOCSETMIXER(0); /* get the caller-supplied mixer and check */ - mm = (struct MixMixer *)arg; - if (mixer_check(mm, NUM_ACTUATOR_CONTROLS)) { + 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 MixMixer *)malloc(MIXER_SIZE(mm->control_count)); + tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count)); memcpy(tmm, mm, MIXER_SIZE(mm->control_count)); + } else { tmm = nullptr; } @@ -379,16 +400,19 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) /* 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, @@ -403,7 +427,7 @@ enum PortMode { PortMode g_port_mode; int -fmu_new_mode(PortMode new_mode) +fmu_new_mode(PortMode new_mode) { int fd; int ret = OK; @@ -412,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; @@ -464,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; @@ -499,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/mixer.c b/apps/systemcmds/mixer/mixer.c index bbbe67ff79..bdf54bc20d 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Mixer utility. + * @file mixer.c + * + * Mixer utility. */ #include @@ -58,22 +60,29 @@ 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; } @@ -82,6 +91,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage:\n"); fprintf(stderr, " mixer show \n"); fprintf(stderr, " mixer {load|save} []\n"); @@ -95,7 +105,7 @@ load(const char *devname, const char *fname) int dev = -1; unsigned num_mixers = 0; int ret, result = 1; - struct MixMixer *mixer = NULL; + struct mixer_s *mixer = NULL; /* open the device */ if ((dev = open(devname, 0)) < 0) { @@ -111,6 +121,7 @@ load(const char *devname, const char *fname) /* 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; @@ -119,6 +130,7 @@ load(const char *devname, const char *fname) /* 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; @@ -130,7 +142,8 @@ load(const char *devname, const char *fname) if (mixer != NULL) { /* sanity check the mixer */ - ret = mixer_check(mixer, NUM_ACTUATOR_CONTROLS); + 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; @@ -138,6 +151,7 @@ load(const char *devname, const char *fname) /* 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; @@ -145,9 +159,11 @@ load(const char *devname, const char *fname) 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; @@ -158,11 +174,14 @@ load(const char *devname, const char *fname) result = 0; out: + /* free the mixers array */ if (mixer != NULL) free(mixer); + if (defs != -1) close(defs); + if (dev != -1) close(dev); @@ -185,6 +204,7 @@ getmixer(int dev, unsigned mixer_number, struct MixInfo **mip) 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; @@ -200,7 +220,7 @@ getmixer(int dev, unsigned mixer_number, struct MixInfo **mip) if (mi == NULL) return -1; - } while(true); + } while (true); *mip = mi; return 0; @@ -223,6 +243,7 @@ save(const char *devname, const char *fname) /* 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; @@ -236,16 +257,20 @@ save(const char *devname, const char *fname) /* get mixers from the device and save them */ for (unsigned i = 0; i < num_mixers; i++) { - struct MixMixer *mm; + 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; } @@ -253,11 +278,14 @@ save(const char *devname, const char *fname) result = 0; out: + /* free the mixinfo */ if (mi != NULL) free(mi); + if (defs != -1) close(defs); + if (dev != -1) close(dev); @@ -280,6 +308,7 @@ show(const char *devname) /* 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; @@ -289,38 +318,43 @@ show(const char *devname) 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); + 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("%d: %8.4f %8.4f %8.4f %8.4f %8.4f\n", - j, - 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); + 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: - printf("done\n"); - usleep(100000); + /* 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 94fabe87a6..8b1dcc054e 100644 --- a/apps/systemlib/mixer.c +++ b/apps/systemlib/mixer.c @@ -50,46 +50,54 @@ #include "mixer.h" static int -scale_check(struct MixScaler *scale) +scale_check(struct scaler_s *scale) { - if (scale->offset > 1.0f) + if (scale->offset > 1.1f) return 1; - if (scale->offset < -1.0f) + if (scale->offset < -1.1f) return 2; if (scale->lower_limit > scale->upper_limit) return 3; - if (scale->lower_limit < -1.0f) + if (scale->lower_limit < -1.1f) return 4; - if (scale->upper_limit > 1.0f) + 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) { int ret; - if (mixer->control_count < 1) - return -1; - - if (mixer->control_count > control_count) + /* 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) + + /* 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); } @@ -97,8 +105,22 @@ mixer_check(struct MixMixer *mixer, unsigned control_count) 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; @@ -120,19 +142,24 @@ 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) { @@ -141,37 +168,52 @@ mixer_getline(int fd, char *line, unsigned maxlen) 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 MixScaler *scaler) +mixer_load_scaler(const char *buf, struct scaler_s *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) + 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 MixMixer **mp) +mixer_load(int fd, struct mixer_s **mp) { int ret, result = -1; - struct MixMixer *mixer = NULL; + struct mixer_s *mixer = NULL; char buf[100]; unsigned scalers; @@ -194,7 +236,7 @@ mixer_load(int fd, struct MixMixer **mp) /* allocate mixer */ scalers--; - mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers)); + mixer = (struct mixer_s *)malloc(MIXER_SIZE(scalers)); if (mixer == NULL) goto out; @@ -211,40 +253,56 @@ mixer_load(int fd, struct MixMixer **mp) 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 MixScaler *scaler) +mixer_save_scaler(char *buf, struct scaler_s *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 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 MixMixer *mixer) +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; @@ -252,6 +310,7 @@ mixer_save(int fd, struct MixMixer *mixer) /* write the output scaler */ len = mixer_save_scaler(buf, &mixer->output_scaler); write(fd, buf, len); + if (ret != len) return -1; @@ -259,9 +318,11 @@ mixer_save(int fd, struct MixMixer *mixer) 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 d65041ea75..3a9e31bb1e 100644 --- a/apps/systemlib/mixer.h +++ b/apps/systemlib/mixer.h @@ -36,7 +36,7 @@ /** * @file mixer.h - * + * * Generic control value mixing library. * * This library implements a generic mixer function that can be used @@ -46,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. * @@ -124,49 +127,68 @@ * 3 | primary thrust */ -struct MixScaler { - unsigned control; /**< control consumed by this scaler */ +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 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 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 */ }; /** * Handy macro for determining the allocation size of a mixer. */ -#define MIXER_SIZE(_num_scalers) (sizeof(struct MixMixer) + ((_num_scalers) * sizeof(struct MixScaler))) +#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 control_count The number of controls in the system. + * @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 MixMixer *mixer, unsigned control_count); +__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. @@ -178,13 +200,16 @@ __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count); * scalers. * * M: - * S: + * S: * S: ... * - * The value for the output scaler is ignored by the mixer. + * The 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. + * 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 @@ -194,7 +219,7 @@ __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count); * @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); +__EXPORT int mixer_load(int fd, struct mixer_s **mixer); /** * Save a mixer definition to a file. @@ -203,7 +228,7 @@ __EXPORT int mixer_load(int fd, struct MixMixer **mixer); * @param mixer The mixer definition to save. * @return Zero on success, negative on error. */ -__EXPORT int mixer_save(int fd, struct MixMixer *mixers); +__EXPORT int mixer_save(int fd, struct mixer_s *mixers); __END_DECLS diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 78708cdba7..98e6c29873 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 @@ -101,5 +103,8 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s); #include "topics/actuator_controls.h" -ORB_DEFINE(actuator_controls, struct actuator_controls_s); +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 5cdaf0a4e5..2b7d7de5d9 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 #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_s -{ +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); -struct actuator_armed_s -{ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + +/** global 'actuator output is live' control. */ +struct actuator_armed_s { bool armed; };