aboutsummaryrefslogtreecommitdiff
path: root/apps/systemlib/mixer
diff options
context:
space:
mode:
Diffstat (limited to 'apps/systemlib/mixer')
-rw-r--r--apps/systemlib/mixer/Makefile39
-rw-r--r--apps/systemlib/mixer/mixer.cpp211
-rw-r--r--apps/systemlib/mixer/mixer.h358
-rw-r--r--apps/systemlib/mixer/mixer_group.cpp288
-rw-r--r--apps/systemlib/mixer/mixer_multirotor.cpp81
5 files changed, 977 insertions, 0 deletions
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);
+}
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-12 22:22:54 +0000