aboutsummaryrefslogtreecommitdiff
path: root/apps/systemlib
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-12-29 00:01:04 -0800
committerpx4dev <px4@purgatory.org>2012-12-29 00:01:04 -0800
commit35c82ff2fc63ab823770f9776e6b6a0f81cd4452 (patch)
tree86956f9f00f1270eafc921748ac024a7daa92f80 /apps/systemlib
parentf0da789626c32695e670b55dab29283eed4a05c6 (diff)
downloadpx4-firmware-35c82ff2fc63ab823770f9776e6b6a0f81cd4452.tar.gz
px4-firmware-35c82ff2fc63ab823770f9776e6b6a0f81cd4452.tar.bz2
px4-firmware-35c82ff2fc63ab823770f9776e6b6a0f81cd4452.zip
Make mixer ioctls load from a memory buffer rather than a file. This is prep for uploading the memory buffer to IO to be processed there.
Diffstat (limited to 'apps/systemlib')
-rw-r--r--apps/systemlib/mixer/mixer.cpp88
-rw-r--r--apps/systemlib/mixer/mixer.h140
-rw-r--r--apps/systemlib/mixer/mixer_group.cpp310
-rw-r--r--apps/systemlib/mixer/mixer_multirotor.cpp56
-rw-r--r--apps/systemlib/mixer/mixer_simple.cpp260
5 files changed, 453 insertions, 401 deletions
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index 6c1bbe469..72f765d90 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -137,89 +137,15 @@ 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)
+NullMixer *
+NullMixer::from_text(const char *buf, unsigned &buflen)
{
- float sum = 0.0f;
-
- if (_info == nullptr)
- return 0;
-
- if (space < 1)
- return 0;
-
- for (unsigned i = 0; i < _info->control_count; i++) {
- float input;
+ NullMixer *nm = nullptr;
- _control_cb(_cb_handle,
- _info->controls[i].control_group,
- _info->controls[i].control_index,
- input);
-
- sum += scale(_info->controls[i].scaler, input);
+ if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
+ nm = new NullMixer;
+ buflen -= 2;
}
- *outputs = scale(_info->output_scaler, sum);
- return 1;
-}
-
-void
-SimpleMixer::groups_required(uint32_t &groups)
-{
- for (unsigned i = 0; i < _info->control_count; i++)
- groups |= 1 << _info->controls[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->control_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->control_count; i++) {
-
- /* verify that we can fetch the control */
- if (_control_cb(_cb_handle,
- _info->controls[i].control_group,
- _info->controls[i].control_index,
- junk) != 0) {
- return -3;
- }
-
- /* validate the scaler */
- ret = scale_check(_info->controls[i].scaler);
-
- if (ret != 0)
- return (10 * i + ret);
- }
-
- return 0;
+ return nm;
}
diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h
index 94c179eba..0023dc380 100644
--- a/apps/systemlib/mixer/mixer.h
+++ b/apps/systemlib/mixer/mixer.h
@@ -234,50 +234,45 @@ public:
void add_mixer(Mixer *mixer);
/**
- * Reads a mixer definition from a file and configures a corresponding
- * group.
+ * Adds mixers to the group based on a text description in a buffer.
*
- * The mixer group must be empty when this function is called.
+ * Mixer definitions begin with a single capital letter and a colon.
+ * The actual format of the mixer definition varies with the individual
+ * mixers; they are summarised here, but see ROMFS/mixers/README for
+ * more details.
*
- * 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
+ * ..........
+ *
+ * The null mixer definition has the form:
+ *
+ * Z:
+ *
+ * Simple Mixer
+ * ............
+ *
+ * A simple mixer definition begins with:
+ *
+ * M: <control count>
+ * O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+ *
+ * The definition continues with <control count> entries describing the control
+ * inputs and their scaling, in the form:
+ *
+ * S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+ *
+ * Multirotor Mixer
+ * ................
+ *
+ * The multirotor mixer definition is a single line of the form:
+ *
+ * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
*
- * 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.
+ * @param buf The mixer configuration buffer.
+ * @param buflen The length of the buffer.
* @return Zero on successful load, nonzero otherwise.
*/
- int load_from_file(const char *path);
+ int load_from_buf(const char *buf, unsigned buflen);
private:
Mixer *_first; /**< linked list of mixers */
@@ -294,6 +289,21 @@ public:
NullMixer();
~NullMixer() {};
+ /**
+ * Factory method.
+ *
+ * Given a pointer to a buffer containing a text description of the mixer,
+ * returns a pointer to a new instance of the mixer.
+ *
+ * @param buf Buffer containing a text description of
+ * the mixer.
+ * @param buflen Length of the buffer in bytes, adjusted
+ * to reflect the bytes consumed.
+ * @return A new NullMixer instance, or nullptr
+ * if the text format is bad.
+ */
+ static NullMixer *from_text(const char *buf, unsigned &buflen);
+
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
};
@@ -318,6 +328,27 @@ public:
mixer_simple_s *mixinfo);
~SimpleMixer();
+ /**
+ * Factory method.
+ *
+ * Given a pointer to a buffer containing a text description of the mixer,
+ * returns a pointer to a new instance of the mixer.
+ *
+ * @param control_cb The callback to invoke when fetching a
+ * control value.
+ * @param cb_handle Handle passed to the control callback.
+ * @param buf Buffer containing a text description of
+ * the mixer.
+ * @param buflen Length of the buffer in bytes, adjusted
+ * to reflect the bytes consumed.
+ * @return A new SimpleMixer instance, or nullptr
+ * if the text format is bad.
+ */
+ static SimpleMixer *from_text(Mixer::ControlCallback control_cb,
+ uintptr_t cb_handle,
+ const char *buf,
+ unsigned &buflen);
+
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
@@ -330,10 +361,18 @@ public:
* @return Zero if the mixer makes sense, nonzero otherwise.
*/
int check();
+
protected:
private:
- mixer_simple_s *_info;
+ mixer_simple_s *_info;
+
+ static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
+ static int parse_control_scaler(const char *buf,
+ unsigned &buflen,
+ mixer_scaler_s &scaler,
+ uint8_t &control_group,
+ uint8_t &control_index);
};
/**
@@ -395,6 +434,27 @@ public:
float deadband);
~MultirotorMixer();
+ /**
+ * Factory method.
+ *
+ * Given a pointer to a buffer containing a text description of the mixer,
+ * returns a pointer to a new instance of the mixer.
+ *
+ * @param control_cb The callback to invoke when fetching a
+ * control value.
+ * @param cb_handle Handle passed to the control callback.
+ * @param buf Buffer containing a text description of
+ * the mixer.
+ * @param buflen Length of the buffer in bytes, adjusted
+ * to reflect the bytes consumed.
+ * @return A new MultirotorMixer instance, or nullptr
+ * if the text format is bad.
+ */
+ static MultirotorMixer *from_text(Mixer::ControlCallback control_cb,
+ uintptr_t cb_handle,
+ const char *buf,
+ unsigned &buflen);
+
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 19ce25553..af5569001 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -56,250 +56,6 @@
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
-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) {
- debug("read: EOF");
- 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';
- debug("read: '%s'", line);
- return 1;
- }
-
- /* if we have space, accumulate the byte and go on */
- if ((p - line) < maxlen)
- *p++ = c;
- }
- }
-}
-
-/**
- * Parse an output scaler from the buffer.
- */
-static int
-mixer_parse_output_scaler(const char *buf, mixer_scaler_s &scaler)
-{
- int s[5];
-
- if (sscanf(buf, "O: %d %d %d %d %d",
- &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
- debug("scaler parse failed on '%s'", buf);
- return -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;
-}
-
-/**
- * Parse a control scaler from the buffer.
- */
-static int
-mixer_parse_control_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) {
- debug("scaler parse failed on '%s'", buf);
- 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];
- int ret;
-
- /* let's assume we're going to read a simple mixer */
- mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
- mixinfo->control_count = inputs;
-
- /* first, get the output scaler */
- ret = mixer_getline(fd, buf, sizeof(buf));
-
- if (ret < 1) {
- debug("failed reading for output scaler");
- goto fail;
- }
-
- if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) {
- debug("failed parsing output scaler");
- goto fail;
- }
-
- /* now get any inputs */
- for (unsigned i = 0; i < inputs; i++) {
- ret = mixer_getline(fd, buf, sizeof(buf));
-
- if (ret < 1) {
- debug("failed reading for control scaler");
- goto fail;
- }
-
- if (mixer_parse_control_scaler(buf,
- mixinfo->controls[i].scaler,
- mixinfo->controls[i].control_group,
- mixinfo->controls[i].control_index)) {
- debug("failed parsing control scaler");
- goto fail;
- }
-
- debug("got control %d", i);
- }
-
- /* XXX should be a factory that validates the mixinfo ... */
- return new SimpleMixer(control_cb, cb_handle, mixinfo);
-
-fail:
- free(mixinfo);
- return nullptr;
-}
-
-MultirotorMixer *
-mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf)
-{
- MultirotorMixer::Geometry geometry;
- char geomname[8];
- int s[4];
-
- if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) {
- debug("multirotor parse failed on '%s'", buf);
- return nullptr;
- }
-
- if (!strcmp(geomname, "4+")) {
- geometry = MultirotorMixer::QUAD_PLUS;
-
- } else if (!strcmp(geomname, "4x")) {
- geometry = MultirotorMixer::QUAD_X;
-
- } else if (!strcmp(geomname, "6+")) {
- geometry = MultirotorMixer::HEX_PLUS;
-
- } else if (!strcmp(geomname, "6x")) {
- geometry = MultirotorMixer::HEX_X;
-
- } else if (!strcmp(geomname, "8+")) {
- geometry = MultirotorMixer::OCTA_PLUS;
-
- } else if (!strcmp(geomname, "8x")) {
- geometry = MultirotorMixer::OCTA_X;
-
- } else {
- debug("unrecognised geometry '%s'", geomname);
- return nullptr;
- }
-
- return new MultirotorMixer(
- control_cb,
- cb_handle,
- geometry,
- s[0] / 10000.0f,
- s[1] / 10000.0f,
- s[2] / 10000.0f,
- s[3] / 10000.0f);
-}
-
-int
-mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer)
-{
- int ret;
- char buf[60];
- unsigned inputs;
-
- ret = mixer_getline(fd, buf, sizeof(buf));
-
- /* end of file or error ?*/
- if (ret < 1) {
- debug("getline %d", ret);
- return ret;
- }
-
- /* slot is empty - allocate a null mixer */
- if (buf[0] == 'Z') {
- debug("got null mixer");
- mixer = new NullMixer();
- return 1;
- }
-
- /* is it a simple mixer? */
- if (sscanf(buf, "M: %u", &inputs) == 1) {
- debug("got simple mixer with %d inputs", inputs);
- mixer = mixer_load_simple(control_cb, cb_handle, fd, inputs);
- return (mixer == nullptr) ? -1 : 1;
- }
-
- /* is it a multirotor mixer? */
- if (buf[0] == 'R') {
- debug("got a multirotor mixer");
- mixer = mixer_load_multirotor(control_cb, cb_handle, buf);
- return (mixer == nullptr) ? -1 : 1;
- }
-
- /* we don't recognise the mixer type */
- debug("unrecognized mixer type '%c'", buf[0]);
- return -1;
-}
-
-
-} // namespace
-
MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
Mixer(control_cb, cb_handle),
_first(nullptr)
@@ -358,44 +114,38 @@ MixerGroup::groups_required(uint32_t &groups)
}
int
-MixerGroup::load_from_file(const char *path)
+MixerGroup::load_from_buf(const char *buf, unsigned buflen)
{
- if (_first != nullptr)
- return -1;
-
- int fd = open(path, O_RDONLY);
-
- if (fd < 0) {
- debug("failed to open %s", path);
- return -1;
- }
-
- for (unsigned count = 0;; count++) {
- int result;
- Mixer *mixer;
-
- result = mixer_load(_control_cb,
- _cb_handle,
- fd,
- mixer);
-
- /* error? */
- if (result < 0) {
- debug("error");
- return -1;
+ int ret = -1;
+ const char *end = buf + buflen;
+
+ /* loop until we have consumed the buffer */
+ while (buflen > 0) {
+ Mixer *m = nullptr;
+ const char *p = end - buflen;
+
+ /* use the next character as a hint to decide which mixer class to construct */
+ switch (*p) {
+ case 'Z':
+ m = NullMixer::from_text(p, buflen);
+ break;
+ case 'M':
+ m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen);
+ break;
+ case 'R':
+ m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen);
+ break;
+ default:
+ /* it's probably junk or whitespace */
+ break;
}
-
- /* EOF or error */
- if (result < 1) {
- printf("[mixer] loaded %u mixers\n", count);
- debug("EOF");
- break;
+ if (m != nullptr) {
+ add_mixer(m);
+ ret = 0;
+ } else {
+ /* skip whitespace or junk in the buffer */
+ buflen--;
}
-
- debug("loaded mixer %p", mixer);
- add_mixer(mixer);
}
-
- close(fd);
- return 0;
+ return ret;
}
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index b5b25e532..233025b21 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -54,6 +54,9 @@
#include "mixer.h"
+#define debug(fmt, args...) do { } while(0)
+//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
#define CW (-1.0f)
#define CCW (1.0f)
@@ -151,6 +154,59 @@ MultirotorMixer::~MultirotorMixer()
{
}
+MultirotorMixer *
+MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+ MultirotorMixer::Geometry geometry;
+ char geomname[8];
+ int s[4];
+ int used;
+
+ if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
+ debug("multirotor parse failed on '%s'", buf);
+ return nullptr;
+ }
+ if (used > (int)buflen) {
+ debug("multirotor spec used %d of %u", used, buflen);
+ return nullptr;
+ }
+ buflen -= used;
+
+ if (!strcmp(geomname, "4+")) {
+ geometry = MultirotorMixer::QUAD_PLUS;
+
+ } else if (!strcmp(geomname, "4x")) {
+ geometry = MultirotorMixer::QUAD_X;
+
+ } else if (!strcmp(geomname, "6+")) {
+ geometry = MultirotorMixer::HEX_PLUS;
+
+ } else if (!strcmp(geomname, "6x")) {
+ geometry = MultirotorMixer::HEX_X;
+
+ } else if (!strcmp(geomname, "8+")) {
+ geometry = MultirotorMixer::OCTA_PLUS;
+
+ } else if (!strcmp(geomname, "8x")) {
+ geometry = MultirotorMixer::OCTA_X;
+
+ } else {
+ debug("unrecognised geometry '%s'", geomname);
+ return nullptr;
+ }
+
+ debug("adding multirotor mixer '%s'", geomname);
+
+ return new MultirotorMixer(
+ control_cb,
+ cb_handle,
+ geometry,
+ s[0] / 10000.0f,
+ s[1] / 10000.0f,
+ s[2] / 10000.0f,
+ s[3] / 10000.0f);
+}
+
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp
new file mode 100644
index 000000000..c0066ac8d
--- /dev/null
+++ b/apps/systemlib/mixer/mixer_simple.cpp
@@ -0,0 +1,260 @@
+/****************************************************************************
+ *
+ * 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_simple.cpp
+ *
+ * Simple summing mixer.
+ */
+
+#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 <ctype.h>
+
+#include "mixer.h"
+
+#define debug(fmt, args...) do { } while(0)
+//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
+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);
+}
+
+static const char *
+skipspace(const char *p, unsigned &len)
+{
+ while (isspace(*p)) {
+ if (len == 0)
+ return nullptr;
+ len--;
+ p++;
+ }
+ return p;
+}
+
+int
+SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
+{
+ int ret;
+ int s[5];
+ int used;
+
+ buf = skipspace(buf, buflen);
+ if (buflen < 16)
+ return -1;
+
+ if ((ret = sscanf(buf, "O: %d %d %d %d %d%n",
+ &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) {
+ debug("scaler parse failed on '%s' (got %d)", buf, ret);
+ return -1;
+ }
+ buflen -= used;
+
+ 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;
+}
+
+int
+SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
+{
+ unsigned u[2];
+ int s[5];
+ int used;
+
+ buf = skipspace(buf, buflen);
+ if (buflen < 16)
+ return -1;
+
+ if (sscanf(buf, "S: %u %u %d %d %d %d %d%n",
+ &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4], &used) != 7) {
+ debug("control parse failed on '%s'", buf);
+ return -1;
+ }
+ buflen -= used;
+
+ 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 *
+SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+ SimpleMixer *sm = nullptr;
+ mixer_simple_s *mixinfo = nullptr;
+ unsigned inputs;
+ int used;
+ const char *end = buf + buflen;
+
+ /* get the base info for the mixer */
+ if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
+ debug("simple parse failed on '%s'", buf);
+ goto out;
+ }
+ buflen -= used;
+
+ mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
+ if (mixinfo == nullptr) {
+ debug("could not allocate memory for mixer info");
+ goto out;
+ }
+ mixinfo->control_count = inputs;
+
+ if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+ goto out;
+
+ for (unsigned i = 0; i < inputs; i++) {
+ if (parse_control_scaler(end - buflen, buflen,
+ mixinfo->controls[i].scaler,
+ mixinfo->controls[i].control_group,
+ mixinfo->controls[i].control_index))
+ goto out;
+ }
+
+ sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+ if (sm != nullptr) {
+ mixinfo = nullptr;
+ debug("loaded mixer with %d inputs", inputs);
+ } else {
+ debug("could not allocate memory for mixer");
+ }
+
+out:
+ if (mixinfo != nullptr)
+ free(mixinfo);
+
+ return sm;
+}
+
+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->control_count; i++) {
+ float input;
+
+ _control_cb(_cb_handle,
+ _info->controls[i].control_group,
+ _info->controls[i].control_index,
+ input);
+
+ sum += scale(_info->controls[i].scaler, input);
+ }
+
+ *outputs = scale(_info->output_scaler, sum);
+ return 1;
+}
+
+void
+SimpleMixer::groups_required(uint32_t &groups)
+{
+ for (unsigned i = 0; i < _info->control_count; i++)
+ groups |= 1 << _info->controls[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->control_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->control_count; i++) {
+
+ /* verify that we can fetch the control */
+ if (_control_cb(_cb_handle,
+ _info->controls[i].control_group,
+ _info->controls[i].control_index,
+ junk) != 0) {
+ return -3;
+ }
+
+ /* validate the scaler */
+ ret = scale_check(_info->controls[i].scaler);
+
+ if (ret != 0)
+ return (10 * i + ret);
+ }
+
+ return 0;
+}