diff options
author | px4dev <px4@purgatory.org> | 2012-08-10 00:30:40 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-10 00:30:40 -0700 |
commit | 67e0f8b1791dfffe780a5add528bbcd1358c0421 (patch) | |
tree | a0db28b6cfe874aabbad67e8b0a270316984d963 /apps/systemlib | |
parent | 04d280564cf915e73aa4bddd23cbfdd5b1c19796 (diff) | |
download | px4-firmware-67e0f8b1791dfffe780a5add528bbcd1358c0421.tar.gz px4-firmware-67e0f8b1791dfffe780a5add528bbcd1358c0421.tar.bz2 px4-firmware-67e0f8b1791dfffe780a5add528bbcd1358c0421.zip |
Rework the mixer architecture based on discussions about arbitrary geometry mixing and plugins.
Now the mixer is a C++ library that can be fairly easily bolted into an output driver to provide mixing services.
Teach the FMU driver how to use it as an example. More testing is still required.
Diffstat (limited to 'apps/systemlib')
-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 |
8 files changed, 977 insertions, 582 deletions
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); +} + |