diff options
| -rw-r--r-- | ROMFS/mixers/FMU_AET.mix | 2 | ||||
| -rw-r--r-- | ROMFS/mixers/FMU_RET.mix | 2 | ||||
| -rw-r--r-- | ROMFS/mixers/FMU_delta.mix | 2 | ||||
| -rw-r--r-- | ROMFS/mixers/FMU_multirotor.mix | 24 | ||||
| -rw-r--r-- | apps/drivers/drv_mixer.h | 110 | ||||
| -rw-r--r-- | apps/px4/fmu/fmu.cpp | 161 | ||||
| -rw-r--r-- | apps/systemcmds/mixer/mixer.c | 261 | ||||
| -rw-r--r-- | apps/systemlib/Makefile | 1 | ||||
| -rw-r--r-- | apps/systemlib/mixer.c | 345 | ||||
| -rw-r--r-- | apps/systemlib/mixer.h | 236 | ||||
| -rw-r--r-- | apps/systemlib/mixer/Makefile | 39 | ||||
| -rw-r--r-- | apps/systemlib/mixer/mixer.cpp | 211 | ||||
| -rw-r--r-- | apps/systemlib/mixer/mixer.h | 358 | ||||
| -rw-r--r-- | apps/systemlib/mixer/mixer_group.cpp | 288 | ||||
| -rw-r--r-- | apps/systemlib/mixer/mixer_multirotor.cpp | 81 | ||||
| -rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 |
16 files changed, 1165 insertions, 957 deletions
diff --git a/ROMFS/mixers/FMU_AET.mix b/ROMFS/mixers/FMU_AET.mix index 9ae23f264..1f1931861 100644 --- a/ROMFS/mixers/FMU_AET.mix +++ b/ROMFS/mixers/FMU_AET.mix @@ -46,7 +46,7 @@ Output 2 -------- This mixer is empty. -M: 0 +Z: Motor speed mixer ----------------- diff --git a/ROMFS/mixers/FMU_RET.mix b/ROMFS/mixers/FMU_RET.mix index 94815b48e..b2fff7224 100644 --- a/ROMFS/mixers/FMU_RET.mix +++ b/ROMFS/mixers/FMU_RET.mix @@ -39,7 +39,7 @@ Output 2 -------- This mixer is empty. -M: 0 +Z: Motor speed mixer ----------------- diff --git a/ROMFS/mixers/FMU_delta.mix b/ROMFS/mixers/FMU_delta.mix index 7b878e40b..b01284ef7 100644 --- a/ROMFS/mixers/FMU_delta.mix +++ b/ROMFS/mixers/FMU_delta.mix @@ -35,7 +35,7 @@ Output 2 -------- This mixer is empty. -M: 0 +Z: Motor speed mixer ----------------- diff --git a/ROMFS/mixers/FMU_multirotor.mix b/ROMFS/mixers/FMU_multirotor.mix new file mode 100644 index 000000000..6175b18e1 --- /dev/null +++ b/ROMFS/mixers/FMU_multirotor.mix @@ -0,0 +1,24 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines passthrough mixers suitable for driving ESCs over the full +input range. + +Channel group 0, channels 0-3 values 0.0 - 1.0 are scaled to the full output range. + +M: 2 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 0 0 20000 -10000 -10000 10000 + +M: 2 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 1 0 20000 -10000 -10000 10000 + +M: 2 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 2 0 20000 -10000 -10000 10000 + +M: 2 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index d66fbf3a9..daa79c265 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -34,10 +34,20 @@ /** * @file drv_mixer.h * - * Mixer ioctl interface. + * Mixer ioctl interfaces. * - * This interface can/should be exported by any device that supports - * control -> actuator mixing. + * Normal workflow is: + * + * - open mixer device + * - add mixer(s) + * loop: + * - mix actuators to array + * + * Each client has its own configuration. + * + * When mixing, outputs are produced by mixers in the order they are + * added. A simple mixer produces one output; a multotor mixer will + * produce several outputs, etc. */ #ifndef _DRV_MIXER_H @@ -46,51 +56,83 @@ #include <stdint.h> #include <sys/ioctl.h> -#include <systemlib/mixer.h> +#define MIXER_DEVICE_PATH "/dev/mixer" -/** - * 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. +/* + * ioctl() definitions */ -struct MixInfo { - unsigned num_controls; - struct mixer_s mixer; +#define _MIXERIOCBASE (0x2400) +#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n)) + +/** get the number of mixable outputs */ +#define MIXERIOCGETOUTPUTCOUNT _MIXERIOC(0) + +/** reset (clear) the mixer configuration */ +#define MIXERIOCRESET _MIXERIOC(1) + +/** simple channel scaler */ +struct mixer_scaler_s +{ + float negative_scale; + float positive_scale; + float offset; + float min_output; + float max_output; }; -/** - * Handy macro for determining the allocation size of a MixInfo structure. - */ -#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct scaler_s))) +/** mixer input */ +struct mixer_input_s +{ + uint8_t control_group; /**< group from which the input reads */ + uint8_t control_index; /**< index within the control group */ + struct mixer_scaler_s scaler; /**< scaling applied to the input before use */ +}; -/* - * ioctl() definitions +/** simple mixer */ +struct mixer_simple_s +{ + uint8_t input_count; /**< number of inputs */ + struct mixer_scaler_s output_scaler; /**< scaling for the output */ + struct mixer_input_s inputs[0]; /**< actual size of the array is set by input_count */ +}; + +#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_input_s)) + +/** + * add a simple mixer in (struct mixer_simple_s *)arg */ +#define MIXERIOCADDSIMPLE _MIXERIOC(2) -#define _MIXERIOCBASE (0x2400) -#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n)) +/** multirotor output definition */ +struct mixer_rotor_output_s +{ + float angle; /**< rotor angle clockwise from forward in radians */ + float distance; /**< motor distance from centre in arbitrary units */ +}; -/** get the number of actuators that require mixers in *(unsigned)arg */ -#define MIXERIOCGETMIXERCOUNT _MIXERIOC(0) +/** multirotor mixer */ +struct mixer_multirotor_s +{ + uint8_t rotor_count; + struct mixer_input_s inputs[4]; /**< inputs are roll, pitch, yaw, thrust */ + struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */ +}; /** - * Copy a mixer from the device into *(struct MixInfo *)arg. - * - * The num_controls field indicates the number of controls for which space - * is allocated following the MixInfo structure. If the allocation - * is too small, no mixer data is retured. The control_count field in - * the MixInfo.mixer structure is always updated. - * - * If no mixer is assigned for the given index, the ioctl returns ENOENT. + * Add a multirotor mixer in (struct mixer_multirotor_s *)arg */ -#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer) +#define MIXERIOCADDMULTIROTOR _MIXERIOC(3) /** - * Copy a mixer from *(struct mixer_s *)arg to the device. + * Add mixers(s) from a the file in (const char *)arg + */ +#define MIXERIOCLOADFILE _MIXERIOC(4) + +/* + * XXX Thoughts for additional operations: * - * If arg is zero, the mixer is deleted. + * - get/set output scale, for tuning center/limit values. + * - save/serialise for saving tuned mixers. */ -#define MIXERIOCSETMIXER(_mixer) _MIXERIOC(0x40 + _mixer) #endif /* _DRV_ACCEL_H */ diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 3021321da..779df4ba1 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -57,12 +57,14 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> + +#include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> +#include <arch/board/up_pwm_servo.h> + #include <uORB/topics/actuator_controls.h> -#include <systemlib/mixer.h> -#include <arch/board/up_pwm_servo.h> class FMUServo : public device::CDev { @@ -91,10 +93,20 @@ private: volatile bool _task_should_exit; bool _armed; - mixer_s *_mixer[_max_actuators]; + MixerGroup *_mixers; + + actuator_controls_s _controls; static void task_main_trampoline(int argc, char *argv[]); void task_main(); + + static int control_callback_trampoline(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + int control_callback(uint8_t control_group, + uint8_t control_index, + float &input); }; namespace @@ -111,10 +123,9 @@ FMUServo::FMUServo(Mode mode) : _t_actuators(-1), _t_armed(-1), _task_should_exit(false), - _armed(false) + _armed(false), + _mixers(nullptr) { - for (unsigned i = 0; i < _max_actuators; i++) - _mixer[i] = nullptr; } FMUServo::~FMUServo() @@ -226,23 +237,19 @@ 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] }; + float outputs[num_outputs]; /* get controls */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &ac); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* do mixing */ + _mixers->mix(&outputs[0], num_outputs); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* if the actuator is configured */ - if (_mixer[i] != nullptr) { - /* mix controls to the actuator */ - float output = mixer_mix(_mixer[i], &controls[0]); - - /* scale for PWM output 900 - 2100us */ - up_pwm_servo_set(i, 1500 + (600 * output)); - } + /* scale for PWM output 900 - 2100us */ + up_pwm_servo_set(i, 1500 + (600 * outputs[i])); } } @@ -274,12 +281,32 @@ FMUServo::task_main() } int +FMUServo::control_callback_trampoline(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + return ((FMUServo *)handle)->control_callback(control_group, control_index, input); +} + +int +FMUServo::control_callback(uint8_t control_group, + uint8_t control_index, + float &input) +{ + /* XXX currently only supporting group zero */ + if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS)) + return -1; + + input = _controls.control[control_index]; + return 0; +} + +int FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = OK; int channel; - struct MixInfo *mi; - struct mixer_s *mm, *tmm; switch (cmd) { case PWM_SERVO_ARM: @@ -325,89 +352,57 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCGETMIXERCOUNT: + case MIXERIOCGETOUTPUTCOUNT: if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; } else { *(unsigned *)arg = 2; } - break; - case MIXERIOCGETMIXER(3): - case MIXERIOCGETMIXER(2): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case MIXERIOCGETMIXER(1): - case MIXERIOCGETMIXER(0): - channel = cmd - MIXERIOCGETMIXER(0); - - /* if no mixer is assigned, we return ENOENT */ - if (_mixer[channel] == nullptr) { - ret = -ENOENT; - break; + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; } + break; - /* caller's MixInfo */ - mi = (struct MixInfo *)arg; - - /* if MixInfo claims to be big enough, copy mixer info */ - if (mi->num_controls >= _mixer[channel]->control_count) { - memcpy(&mi->mixer, _mixer[channel], MIXER_SIZE(_mixer[channel]->control_count)); + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo); + if (mixer->check()) { + delete mixer; + ret = -EINVAL; } else { - /* just update MixInfo with actual size of the mixer */ - mi->mixer.control_count = _mixer[channel]->control_count; + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers->add_mixer(mixer); } - break; + } - case MIXERIOCSETMIXER(3): - case MIXERIOCSETMIXER(2): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case MIXERIOCSETMIXER(1): - case MIXERIOCSETMIXER(0): - channel = cmd - MIXERIOCSETMIXER(0); - - /* get the caller-supplied mixer and check */ - mm = (struct mixer_s *)arg; - - /* allocate local storage and copy from the caller*/ - if (mm != nullptr) { - - if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) { - /* only the attitude group is supported */ - ret = -EINVAL; - break; - } + case MIXERIOCADDMULTIROTOR: + /* XXX not yet supported */ + ret = -ENOTTY; + break; - /* allocate a new mixer struct */ - tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count)); - memcpy(tmm, mm, MIXER_SIZE(mm->control_count)); + case MIXERIOCLOADFILE: { + const char *path = (const char *)arg; - } else { - tmm = nullptr; + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + if (_mixers->load_from_file(path) != 0) { + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; } - - /* 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; diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index bdf54bc20..9d52557e7 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -44,7 +44,6 @@ #include <fcntl.h> #include <errno.h> -#include <systemlib/mixer.h> #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> @@ -52,8 +51,6 @@ __EXPORT int mixer_main(int argc, char *argv[]); static void usage(const char *reason); static void load(const char *devname, const char *fname); -static void save(const char *devname, const char *fname); -static void show(const char *devname); int mixer_main(int argc, char *argv[]) @@ -67,18 +64,6 @@ mixer_main(int argc, char *argv[]) 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"); } @@ -93,146 +78,15 @@ usage(const char *reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, "usage:\n"); - fprintf(stderr, " mixer show <device>\n"); - fprintf(stderr, " mixer {load|save} <device> [<filename>]\n"); + fprintf(stderr, " mixer load <device> <filename>\n"); + /* XXX automatic setups for quad, etc. */ exit(1); } static void load(const char *devname, const char *fname) { - int defs = -1; - int dev = -1; - unsigned num_mixers = 0; - int ret, result = 1; - struct mixer_s *mixer = NULL; - - /* open the device */ - if ((dev = open(devname, 0)) < 0) { - fprintf(stderr, "can't open %s\n", devname); - goto out; - } - - /* open the definition file */ - if ((defs = open(fname, O_RDONLY)) < 0) { - fprintf(stderr, "can't open %s\n", fname); - goto out; - } - - /* find out how many mixers the device supports */ - ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers); - - if (num_mixers < 1) { - fprintf(stderr, "can't get mixer count from %s\n", devname); - goto out; - } - - /* send mixers to the device */ - for (unsigned i = 0; i < num_mixers; i++) { - ret = mixer_load(defs, &mixer); - - if (ret < 0) { - fprintf(stderr, "read for mixer %d failed\n", i); - goto out; - } - - /* end of file? */ - if (ret == 0) - break; - - if (mixer != NULL) { - /* sanity check the mixer */ - ret = mixer_check(mixer, NUM_ACTUATOR_CONTROL_GROUPS, NUM_ACTUATOR_CONTROLS); - - if (ret != 0) { - fprintf(stderr, "mixer %u fails sanity check %d\n", i, ret); - goto out; - } - - /* send the mixer to the device */ - ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer); - - if (ret < 0) { - fprintf(stderr, "mixer %d set failed\n", i); - goto out; - } - - free(mixer); - mixer = NULL; - - } else { - /* delete the mixer */ - ret = ioctl(dev, MIXERIOCSETMIXER(i), 0); - - if (ret < 0) { - fprintf(stderr, "mixer %d clear failed\n", i); - goto out; - } - } - } - - result = 0; - -out: - - /* free the mixers array */ - if (mixer != NULL) - free(mixer); - - if (defs != -1) - close(defs); - - if (dev != -1) - close(dev); - - exit(result); -} - -static int -getmixer(int dev, unsigned mixer_number, struct MixInfo **mip) -{ - struct MixInfo *mi = *mip; - int ret; - - /* first-round initialisation */ - if (mi == NULL) { - mi = (struct MixInfo *)malloc(MIXINFO_SIZE(0)); - mi->num_controls = 0; - } - - /* loop trying to get the next mixer until the buffer is big enough */ - do { - /* try to get the mixer into the buffer as it stands */ - ret = ioctl(dev, MIXERIOCGETMIXER(mixer_number), (unsigned long)mi); - - if (ret < 0) - return -1; - - /* did the mixer fit? */ - if (mi->mixer.control_count <= mi->num_controls) - break; - - /* re-allocate to suit */ - mi->num_controls = mi->mixer.control_count; - mi = (struct MixInfo *)realloc(mi, MIXINFO_SIZE(mi->num_controls)); - - /* oops, blew up the heap */ - if (mi == NULL) - return -1; - - } while (true); - - *mip = mi; - return 0; -} - -static void -save(const char *devname, const char *fname) -{ - struct MixInfo *mi = NULL; - int defs = -1; int dev = -1; - unsigned num_mixers = 0; int ret, result = 1; /* open the device */ @@ -241,120 +95,17 @@ save(const char *devname, const char *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; - } - - /* open the definition file */ - if ((defs = open(fname, O_WRONLY | O_CREAT)) < 0) { - fprintf(stderr, "can't open %s\n", fname); - goto out; - } - - /* get mixers from the device and save them */ - for (unsigned i = 0; i < num_mixers; i++) { - struct mixer_s *mm; - - ret = getmixer(dev, i, &mi); - mm = &mi->mixer; - - if (ret < 0) { - if (errno != ENOENT) - goto out; - - mm = NULL; - } - - ret = mixer_save(defs, mm); - - if (ret < 0) - goto out; + /* tell it to load the file */ + ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname); + if (ret != 0) { + fprintf(stderr, "failed loading %s\n", fname); } result = 0; - out: - /* free the mixinfo */ - if (mi != NULL) - free(mi); - - if (defs != -1) - close(defs); - if (dev != -1) close(dev); exit(result); } - -static void -show(const char *devname) -{ - struct MixInfo *mi = NULL; - int dev = -1; - unsigned num_mixers = 0; - int ret; - - /* open the device */ - if ((dev = open(devname, 0)) < 0) { - fprintf(stderr, "can't open %s\n", devname); - goto out; - } - - /* find out how many mixers the device supports */ - ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers); - - if (num_mixers < 1) { - fprintf(stderr, "can't get mixer count from %s\n", devname); - goto out; - } - - /* get mixers from the device and print them */ - for (unsigned i = 0; i < num_mixers; i++) { - - ret = getmixer(dev, i, &mi); - - if (ret < 0) { - if (errno != ENOENT) - goto out; - - continue; - } - - printf("mixer %d:\n", i); - printf(" -ve scale +ve scale offset low limit high limit\n"); - printf("output %8.4f %8.4f %8.4f %8.4f %8.4f\n", - mi->mixer.output_scaler.negative_scale, - mi->mixer.output_scaler.positive_scale, - mi->mixer.output_scaler.offset, - mi->mixer.output_scaler.lower_limit, - mi->mixer.output_scaler.upper_limit); - - for (unsigned j = 0; j < mi->mixer.control_count; j++) { - printf("(%u,%u) %8.4f %8.4f %8.4f %8.4f %8.4f\n", - mi->mixer.control_scaler[j].control_group, - mi->mixer.control_scaler[j].control_index, - mi->mixer.control_scaler[j].negative_scale, - mi->mixer.control_scaler[j].positive_scale, - mi->mixer.control_scaler[j].offset, - mi->mixer.control_scaler[j].lower_limit, - mi->mixer.control_scaler[j].upper_limit); - } - } - -out: - - /* free the mixinfo */ - if (mi != NULL) - free(mi); - - if (dev != -1) - close(dev); - - exit(0); -} diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index cb126825d..13cf8271e 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -36,7 +36,6 @@ # CSRCS = hx_stream.c \ - mixer.c \ perf_counter.c # diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c deleted file mode 100644 index b068d3958..000000000 --- a/apps/systemlib/mixer.c +++ /dev/null @@ -1,345 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mixer.c - * - * Generic control value mixing library. - * - * This library implements a generic mixer function that can be used - * by any driver or subsytem that wants to combine several control signals - * into a single output. - * - * See mixer.h for more details. - */ - -#include <stdio.h> -#include <stdlib.h> -#include <unistd.h> - -#include "mixer.h" - -static int -scale_check(struct scaler_s *scale) -{ - if (scale->offset > 1.1f) - return 1; - - if (scale->offset < -1.1f) - return 2; - - if (scale->lower_limit > scale->upper_limit) - return 3; - - if (scale->lower_limit < -1.1f) - return 4; - - if (scale->upper_limit > 1.1f) - return 5; - - return 0; -} - -int -mixer_check(struct mixer_s *mixer, unsigned group_count, unsigned control_count) -{ - int ret; - - /* sanity that presumes that a mixer includes a control no more than once */ - if (mixer->control_count > (group_count * control_count)) - return -2; - - /* validate the output scaler */ - ret = scale_check(&mixer->output_scaler); - - if (ret != 0) - return ret; - - /* validate input scalers */ - for (unsigned i = 0; i < mixer->control_count; i++) { - - /* range-check input controls */ - if (mixer->control_scaler[i].control_group >= group_count) - return -3; - - if (mixer->control_scaler[i].control_index >= control_count) - return -3; - - /* validate the scaler */ - ret = scale_check(&mixer->control_scaler[i]); - - if (ret != 0) - return (10 * i + ret); - } - - return 0; -} - -void -mixer_requires(struct mixer_s *mixer, uint32_t *groups) -{ - for (unsigned i = 0; i < mixer->control_count; i++) - *groups |= 1 << mixer->control_scaler[i].control_group; -} - -/** - * Apply a scaler to a value. - * - * @param scaler The applied scaler. - * @param input The value to scale. - * @output The scaled value. - */ -static float -scale(struct scaler_s *scaler, float input) -{ - float output; - - if (input < 0.0f) { - output = (input * scaler->negative_scale) + scaler->offset; - - } else { - output = (input * scaler->positive_scale) + scaler->offset; - } - - if (output > scaler->upper_limit) { - output = scaler->upper_limit; - - } else if (output < scaler->lower_limit) { - output = scaler->lower_limit; - } - - return output; -} - -float -mixer_mix(struct mixer_s *mixer, float **controls) -{ - float sum = 0.0f; - - for (unsigned i = 0; i < mixer->control_count; i++) { - - 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) -{ - /* reduce line budget by 1 to account for terminal NUL */ - maxlen--; - - /* loop looking for a non-comment line */ - for (;;) { - int ret; - char c; - char *p = line; - - /* loop reading characters for this line */ - for (;;) { - ret = read(fd, &c, 1); - - /* on error or EOF, return same */ - if (ret <= 0) - return ret; - - /* ignore carriage returns */ - if (c == '\r') - continue; - - /* line termination */ - if (c == '\n') { - /* ignore malformed lines */ - if ((p - line) < 4) - break; - - if (line[1] != ':') - break; - - /* terminate line as string and return */ - *p = '\0'; - return 1; - } - - /* if we have space, accumulate the byte and go on */ - if ((p - line) < maxlen) - *p++ = c; - } - } -} - -static int -mixer_load_scaler(const char *buf, struct scaler_s *scaler) -{ - unsigned u[2]; - int s[5]; - - if (sscanf(buf, "S: %u %u %d %d %d %d %d", - &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) - return -1; - - scaler->control_group = u[0]; - scaler->control_index = u[1]; - scaler->negative_scale = s[0] / 10000.0f; - scaler->positive_scale = s[1] / 10000.0f; - scaler->offset = s[2] / 10000.0f; - scaler->lower_limit = s[3] / 10000.0f; - scaler->upper_limit = s[4] / 10000.0f; - - return 0; -} - -int -mixer_load(int fd, struct mixer_s **mp) -{ - int ret, result = -1; - struct mixer_s *mixer = NULL; - char buf[60]; - unsigned scalers; - - ret = mixer_getline(fd, buf, sizeof(buf)); - - /* end of file? */ - if (ret == 0) - result = 0; - - /* can't proceed */ - if (ret < 1) - goto out; - - /* get header */ - if (sscanf(buf, "M: %u", &scalers) != 1) - goto out; - - /* if there are scalers, load them */ - if (scalers > 0) { - - /* allocate mixer */ - scalers--; - mixer = (struct mixer_s *)malloc(MIXER_SIZE(scalers)); - - if (mixer == NULL) - goto out; - - mixer->control_count = scalers; - - ret = mixer_getline(fd, buf, sizeof(buf)); - - if (ret < 1) - goto out; - - if (mixer_load_scaler(buf, &mixer->output_scaler)) - goto out; - - for (unsigned i = 0; i < scalers; i++) { - ret = mixer_getline(fd, buf, sizeof(buf)); - - if (ret < 1) - goto out; - - if (mixer_load_scaler(buf, &mixer->control_scaler[i])) - goto out; - } - - } else { - /* we return NULL for the mixer, which is interpreted elsewhere as "no mixer" */ - } - - result = 1; - -out: - - /* on error, discard allocated mixer */ - if ((result <= 0) && (mixer != NULL)) - free(mixer); - - *mp = mixer; - return result; -} - -static int -mixer_save_scaler(char *buf, struct scaler_s *scaler) -{ - int s[5]; - - s[0] = 10000.0f * scaler->negative_scale; - s[1] = 10000.0f * scaler->positive_scale; - s[2] = 10000.0f * scaler->offset; - s[3] = 10000.0f * scaler->lower_limit; - s[4] = 10000.0f * scaler->upper_limit; - - return sprintf(buf, "S: %u %u %d %d %d %d %d\n", - scaler->control_group, scaler->control_index, - s[0], s[1], s[2], s[3], s[4]); -} - -int -mixer_save(int fd, struct mixer_s *mixer) -{ - char buf[100]; - int len, ret; - - /* write the mixer header */ - len = sprintf(buf, "M: %u\n", (mixer != NULL) ? mixer->control_count : 0); - ret = write(fd, buf, len); - - if (ret != len) - return -1; - - if (mixer != NULL) { - /* write the output scaler */ - len = mixer_save_scaler(buf, &mixer->output_scaler); - write(fd, buf, len); - - if (ret != len) - return -1; - - /* write the control scalers */ - for (unsigned j = 0; j < mixer->control_count; j++) { - len = mixer_save_scaler(buf, &mixer->control_scaler[j]); - write(fd, buf, len); - - if (ret != len) - return -1; - } - } - - return 0; -}
\ No newline at end of file diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h deleted file mode 100644 index 3a9e31bb1..000000000 --- a/apps/systemlib/mixer.h +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#ifndef _SYSTEMLIB_MIXER_H -#define _SYSTEMLIB_MIXER_H - -/** - * @file mixer.h - * - * Generic control value mixing library. - * - * This library implements a generic mixer function that can be used - * by any driver or subsytem that wants to combine several control signals - * into a single output. - * - * Terminology - * =========== - * - * 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. - * - * Mixing basics - * ============= - * - * 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 - * actuator value, which may then be further scaled to suit the specific - * output type. - * - * Internally, all scaling is performed using floating point values. - * Inputs and outputs are clamped to the range -1.0 to 1.0. - * - * control control control - * | | | - * v v v - * scale scale scale - * | | | - * | v | - * +-------> mix <------+ - * | - * scale - * | - * v - * out - * - * Scaling - * ------- - * - * Each scaler allows the input value to be scaled independently for - * inputs greater/less than zero. An offset can be applied to the output, - * as well as lower and upper boundary constraints. - * Negative scaling factors cause the output to be inverted (negative input - * produces positive output). - * - * Scaler pseudocode: - * - * if (input < 0) - * output = (input * NEGATIVE_SCALE) + OFFSET - * else - * output = (input * POSITIVE_SCALE) + OFFSET - * - * if (output < LOWER_LIMIT) - * output = LOWER_LIMIT - * if (output > UPPER_LIMIT) - * output = UPPER_LIMIT - * - * - * Mixing - * ------ - * - * Mixing is performed by summing the scaled control values. - * - * - * Controls - * -------- - * - * 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 - * application, but the following assignments should be used - * when appropriate. - * - * control | standard meaning - * --------+----------------------- - * 0 | roll - * 1 | pitch - * 2 | yaw - * 3 | primary thrust - */ - -struct scaler_s { - float negative_scale; /**< scale for inputs < 0 */ - float positive_scale; /**< scale for inputs > 0 */ - float offset; /**< bias applied to output */ - float lower_limit; /**< minimum output value */ - float upper_limit; /**< maximum output value */ - uint8_t control_group; /**< control group this scaler reads from */ - uint8_t control_index; /**< control index within the group */ -}; - -struct mixer_s { - unsigned control_count; /**< number of control scalers */ - struct scaler_s output_scaler; /**< scaler applied to mixer output */ - struct scaler_s control_scaler[0]; /**< array of control scalers */ -}; - -/** - * Handy macro for determining the allocation size of a mixer. - */ -#define MIXER_SIZE(_num_scalers) (sizeof(struct mixer_s) + ((_num_scalers) * sizeof(struct scaler_s))) - -__BEGIN_DECLS - -/** - * Perform a mixer calculation. - * - * Note that the controls array, 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 pointers to control group values. - * @return The mixed output. - */ -__EXPORT float mixer_mix(struct mixer_s *mixer, float **controls); - -/** - * Check a mixer configuration for sanity. - * - * @param mixer The mixer configuration to be checked. - * @param group_count The highest-numbered control group that - * should be considered legal. - * @param control_count The highest control index that should be - * considered legal. - * @return Zero if the mixer configuration is sane, - * nonzero otherwise. - */ -__EXPORT int mixer_check(struct mixer_s *mixer, unsigned group_count, unsigned control_count); - -/** - * Evaluate the control inputs to a mixer and update the bitmask of - * required control groups. - * - * This function allows an actuator driver to selectively fetch just - * the control groups required to support a particular mixer or set of - * mixers. - * - * @param mixer The mixer being evaluated. - * @param groups Pointer to a bitmask to be updated with set bits - * corresponding to the control groups used by the - * mixer. - */ -__EXPORT void mixer_requires(struct mixer_s *mixer, uint32_t *groups); - -/** - * Read a mixer definition from a file. - * - * A mixer definition is a text representation of the configuration of a - * mixer. The definition consists of a single-line header indicating the - * number of scalers and then one line defining each scaler. The first - * scaler in the file is always the output scaler, followed by the input - * scalers. - * - * M: <scaler count> - * S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*> - * S: ... - * - * The <control ...> values for the output scaler are ignored by the mixer. - * - * Values marked * are integers representing floating point values; values are - * scaled by 10000 on load/save. - * - * Multiple mixer definitions may be stored in a single file; it is assumed that - * the reader will know how many to expect and read accordingly. - * - * A mixer entry with a scaler count of zero indicates a disabled mixer. This - * will return NULL for the mixer when processed by this function, and will be - * generated by passing NULL as the mixer to mixer_save. - * - * @param fd The file to read the definitions from. - * @param mixer Mixer is returned here. - * @return 1 if a mixer was read, zero on EOF or negative on error. - */ -__EXPORT int mixer_load(int fd, struct mixer_s **mixer); - -/** - * Save a mixer definition to a file. - * - * @param fd The file to write the definitions to. - * @param mixer The mixer definition to save. - * @return Zero on success, negative on error. - */ -__EXPORT int mixer_save(int fd, struct mixer_s *mixers); - - -__END_DECLS - -#endif /* _SYSTEMLIB_MIXER_H */ diff --git a/apps/systemlib/mixer/Makefile b/apps/systemlib/mixer/Makefile new file mode 100644 index 000000000..f8b02f194 --- /dev/null +++ b/apps/systemlib/mixer/Makefile @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# mixer library +# +LIBNAME = mixerlib + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp new file mode 100644 index 000000000..5184c2f13 --- /dev/null +++ b/apps/systemlib/mixer/mixer.cpp @@ -0,0 +1,211 @@ +/**************************************************************************** + * + * 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.cpp + * + * Programmable multi-channel mixer library. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include "mixer.h" + +Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _control_cb(control_cb), + _cb_handle(cb_handle) +{ +} + +float +Mixer::scale(const mixer_scaler_s &scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler.negative_scale) + scaler.offset; + + } else { + output = (input * scaler.positive_scale) + scaler.offset; + } + + if (output > scaler.max_output) { + output = scaler.max_output; + + } else if (output < scaler.min_output) { + output = scaler.min_output; + } + + return output; +} + +int +Mixer::scale_check(struct mixer_scaler_s &scaler) +{ + if (scaler.offset > 1.001f) + return 1; + + if (scaler.offset < -1.001f) + return 2; + + if (scaler.min_output > scaler.max_output) + return 3; + + if (scaler.min_output < -1.001f) + return 4; + + if (scaler.max_output > 1.001f) + return 5; + + return 0; +} + +/****************************************************************************/ + +NullMixer::NullMixer() : + Mixer(nullptr, 0) +{ +} + +unsigned +NullMixer::mix(float *outputs, unsigned space) +{ + if (space > 0) { + *outputs = 0.0f; + return 1; + } + return 0; +} + +void +NullMixer::groups_required(uint32_t &groups) +{ + +} + +/****************************************************************************/ + +SimpleMixer::SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _info(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_info != nullptr) + free(_info); +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_info == nullptr) + return 0; + if (space < 1) + return 0; + + for (unsigned i = 0; i < _info->input_count; i++) { + float input; + + _control_cb(_cb_handle, + _info->inputs[i].control_group, + _info->inputs[i].control_index, + input); + + sum += scale(_info->inputs[i].scaler, input); + } + *outputs = scale(_info->output_scaler, sum); + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _info->input_count; i++) + groups |= 1 << _info->inputs[i].control_group; +} + +int +SimpleMixer::check() +{ + int ret; + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_info->input_count > 32) + return -2; + + /* validate the output scaler */ + ret = scale_check(_info->output_scaler); + + if (ret != 0) + return ret; + + /* validate input scalers */ + for (unsigned i = 0; i < _info->input_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _info->inputs[i].control_group, + _info->inputs[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_info->inputs[i].scaler); + + if (ret != 0) + return (10 * i + ret); + } + + return 0; +} diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h new file mode 100644 index 000000000..5a3857a00 --- /dev/null +++ b/apps/systemlib/mixer/mixer.h @@ -0,0 +1,358 @@ +/**************************************************************************** + * + * 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.h + * + * Generic, programmable, procedural control signal mixers. + * + * This library implements a generic mixer interface that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * 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. + * + * + * Mixing basics + * ============= + * + * 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 + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing behaviour varies based on the specific mixer class; each + * mixer class describes its behaviour in more detail. + * + * + * Controls + * -------- + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. Some mixer classes have specific assumptions + * about the assignment of controls. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + + +#ifndef _SYSTEMLIB_MIXER_MIXER_H +#define _SYSTEMLIB_MIXER_MIXER_H value + +#include "drivers/drv_mixer.h" + +/** + * Abstract class defining a mixer mixing zero or more inputs to + * one or more outputs. + */ +class __EXPORT Mixer +{ +public: + /** next mixer in a list */ + Mixer *_next; + + /** + * Fetch a control value. + * + * @param handle Token passed when the callback is registered. + * @param control_group The group to fetch the control from. + * @param control_index The group-relative index to fetch the control from. + * @param control The returned control + * @return Zero if the value was fetched, nonzero otherwise. + */ + typedef int (* ControlCallback)(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + + /** + * Constructor. + * + * @param control_cb Callback invoked when reading controls. + */ + Mixer(ControlCallback control_cb, uintptr_t cb_handle); + ~Mixer() {}; + + /** + * Perform the mixing function. + * + * @param outputs Array into which mixed output(s) should be placed. + * @param space The number of available entries in the output array; + * @return The number of entries in the output array that were populated. + */ + virtual unsigned mix(float *outputs, unsigned space) = 0; + + /** + * Analyses the mix configuration and updates a bitmask of groups + * that are required. + * + * @param groups A bitmask of groups (0-31) that the mixer requires. + */ + virtual void groups_required(uint32_t &groups) = 0; + +protected: + /** client-supplied callback used when fetching control values */ + ControlCallback _control_cb; + uintptr_t _cb_handle; + + /** + * Perform simpler linear scaling. + * + * @param scaler The scaler configuration. + * @param input The value to be scaled. + * @return The scaled value. + */ + static float scale(const mixer_scaler_s &scaler, float input); + + /** + * Validate a scaler + * + * @param scaler The scaler to be validated. + * @return Zero if good, nonzero otherwise. + */ + static int scale_check(struct mixer_scaler_s &scaler); + +private: +}; + +/** + * Group of mixers, built up from single mixers and processed + * in order when mixing. + */ +class __EXPORT MixerGroup : public Mixer +{ +public: + MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); + ~MixerGroup(); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Add a mixer to the group. + * + * @param mixer The mixer to be added. + */ + void add_mixer(Mixer *mixer); + + /** + * Reads a mixer definition from a file and configures a corresponding + * group. + * + * The mixer group must be empty when this function is called. + * + * A mixer definition is a text representation of the configuration of a + * mixer. Definition lines begin with a capital letter followed by a colon. + * + * Null Mixer: + * + * Z: + * + * This mixer generates a constant zero output, and is normally used to + * skip over outputs that are not in use. + * + * Simple Mixer: + * + * M: <scaler count> + * S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*> + * S: ... + * + * 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. + * + * The <control ...> values for the output scaler are ignored by the mixer. + * + * + * + * Values marked * are integers representing floating point values; values are + * scaled by 10000 on load/save. + * + * Multiple mixer definitions may be stored in a single file; it is assumed that + * the reader will know how many to expect and read accordingly. + * + * A mixer entry with a scaler count of zero indicates a disabled mixer. This + * will return NULL for the mixer when processed by this function, and will be + * generated by passing NULL as the mixer to mixer_save. + * + * @param path The mixer configuration file to read. + * @return Zero on successful load, nonzero otherwise. + */ + int load_from_file(const char *path); + +private: + Mixer *_first; /**< linked list of mixers */ +}; + +/** + * Null mixer; returns zero. + * + * Used as a placeholder for output channels that are unassigned in groups. + */ +class __EXPORT NullMixer : public Mixer +{ +public: + NullMixer(); + ~NullMixer() {}; + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); +}; + +/** + * Simple summing mixer. + * + * Collects zero or more inputs and mixes them to a single output. + */ +class __EXPORT SimpleMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param mixinfo Mixer configuration. The pointer passed + * becomes the property of the mixer and + * will be freed when the mixer is deleted. + */ + SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo); + ~SimpleMixer(); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Check that the mixer configuration as loaded is sensible. + * + * Note that this function will call control_cb, but only cares about + * error returns, not the input value. + * + * @return Zero if the mixer makes sense, nonzero otherwise. + */ + int check(); +protected: + +private: + mixer_simple_s *_info; +}; + +/** + * Multi-rotor mixer. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ +class __EXPORT MultirotorMixer : public Mixer +{ +public: + enum Geometry + { + MULTIROTOR_QUAD_PLUS, + MULTIROTOR_QUAD_X + /* XXX add more here */ + }; + + MultirotorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + Geometry geom); + ~MultirotorMixer(); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + +private: + Geometry _geometry; +}; + +#endif diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp new file mode 100644 index 000000000..72a2ff3e5 --- /dev/null +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -0,0 +1,288 @@ +/**************************************************************************** + * + * 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_group.cpp + * + * Mixer collection. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include "mixer.h" + +namespace +{ + +/** + * Effectively fdgets() with some extra smarts. + */ +static int +mixer_getline(int fd, char *line, unsigned maxlen) +{ + /* reduce line budget by 1 to account for terminal NUL */ + maxlen--; + + /* loop looking for a non-comment line */ + for (;;) { + int ret; + char c; + char *p = line; + + /* loop reading characters for this line */ + for (;;) { + ret = read(fd, &c, 1); + + /* on error or EOF, return same */ + if (ret <= 0) + return ret; + + /* ignore carriage returns */ + if (c == '\r') + continue; + + /* line termination */ + if (c == '\n') { + /* ignore malformed lines */ + if ((p - line) < 4) + break; + + if (line[1] != ':') + break; + + /* terminate line as string and return */ + *p = '\0'; + return 1; + } + + /* if we have space, accumulate the byte and go on */ + if ((p - line) < maxlen) + *p++ = c; + } + } +} + +/** + * Parse a scaler from the buffer. + */ +static int +mixer_parse_scaler(const char *buf, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +{ + 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; + + control_group = u[0]; + 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.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, unsigned inputs) +{ + mixer_simple_s *mixinfo = nullptr; + char buf[60]; + uint8_t control_group, control_index; + int ret; + + /* let's assume we're going to read a simple mixer */ + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + + /* first, get the output scaler */ + ret = mixer_getline(fd, buf, sizeof(buf)); + if (ret < 1) + goto fail; + if (mixer_parse_scaler(buf, mixinfo->output_scaler, control_group, control_index)) + goto fail; + + /* now get any inputs */ + for (unsigned i = 0; i < inputs; i++) { + ret = mixer_getline(fd, buf, sizeof(buf)); + if (ret < 1) + goto fail; + if (mixer_parse_scaler(buf, + mixinfo->inputs[i].scaler, + mixinfo->inputs[i].control_group, + mixinfo->inputs[i].control_index)) { + goto fail; + } + } + + /* XXX should be a factory that validates the mixinfo ... */ + return new SimpleMixer(control_cb, cb_handle, mixinfo); + +fail: + free(mixinfo); + return nullptr; +} + +int +mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer) +{ + int ret; + char buf[60]; + unsigned scalers; + + ret = mixer_getline(fd, buf, sizeof(buf)); + + /* end of file or error ?*/ + if (ret < 1) + return ret; + + /* slot is empty - allocate a null mixer */ + if (buf[0] == 'Z') { + mixer = new NullMixer(); + return 0; + } + + /* is it a simple mixer? */ + if (sscanf(buf, "M: %u", &scalers) == 1) { + mixer = mixer_load_simple(control_cb, cb_handle, fd, scalers); + return (mixer == nullptr) ? -1 : 0; + } + + /* we don't recognise the mixer type */ + return -1; +} + + +} // namespace + +MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : + Mixer(control_cb, cb_handle), + _first(nullptr) +{ +} + +MixerGroup::~MixerGroup() +{ + Mixer *mixer; + + /* discard sub-mixers */ + while (_first != nullptr) { + mixer = _first; + _first = mixer->_next; + delete mixer; + } +} + +void +MixerGroup::add_mixer(Mixer *mixer) +{ + Mixer **mpp; + + mpp = &_first; + while (*mpp != nullptr) + mpp = &((*mpp)->_next); + *mpp = mixer; + mixer->_next = nullptr; +} + +unsigned +MixerGroup::mix(float *outputs, unsigned space) +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr) && (index < space)) { + index += mixer->mix(outputs + index, space - index); + mixer = mixer->_next; + } + return index; +} + +void +MixerGroup::groups_required(uint32_t &groups) +{ + Mixer *mixer = _first; + + while (mixer != nullptr) { + mixer->groups_required(groups); + mixer = mixer->_next; + } +} + +int +MixerGroup::load_from_file(const char *path) +{ + if (_first != nullptr) + return -1; + + int fd = open(path, O_RDONLY); + if (fd < 0) + return -1; + + for (;;) { + int result; + Mixer *mixer; + + result = mixer_load(_control_cb, + _cb_handle, + fd, + mixer); + + /* error? */ + if (result < 0) + return -1; + + /* EOF or error */ + if (result < 1) + break; + + add_mixer(mixer); + } + + close(fd); + return 0; +} diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp new file mode 100644 index 000000000..713b1e25d --- /dev/null +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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_multirotor.cpp + * + * Multi-rotor mixers. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include "mixer.h" + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + MultirotorMixer::Geometry geom) : + Mixer(control_cb, cb_handle), + _geometry(geom) +{ +} + +MultirotorMixer::~MultirotorMixer() +{ +} + +unsigned +MultirotorMixer::mix(float *outputs, unsigned space) +{ + /* XXX implement this */ + return 0; +} + +void +MultirotorMixer::groups_required(uint32_t &groups) +{ + /* XXX for now, hardcoded to indexes 0-3 in control group zero */ + groups |= (1 << 0); +} + diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 585348b81..84bfc653a 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -43,6 +43,7 @@ CONFIGURED_APPS += system/readline # System library - utility functions CONFIGURED_APPS += systemlib +CONFIGURED_APPS += systemlib/mixer CONFIGURED_APPS += systemcmds/reboot CONFIGURED_APPS += systemcmds/perf |