diff options
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); +} + |