fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged with master

Browse Source
This commit is contained in:
Lorenz Meier
2013-01-07 17:50:49 +01:00
parent e4bfd78847 659543f852
commit 41f657e3e5
54 changed files with 2150 additions and 905 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/systemlib/mixer/mixer.cpp
+7 -81
View File
@@ -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)
NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
{
}
NullMixer *nm = nullptr;
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->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);
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;
}
apps/systemlib/mixer/mixer.h
+116 -30
View File
@@ -234,50 +234,51 @@ public:
void add_mixer(Mixer *mixer);
/**
* Reads a mixer definition from a file and configures a corresponding
* group.
* Remove all the mixers from the group.
*/
void reset();
/**
* 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
* ..........
*
* Null Mixer:
* The null mixer definition has the form:
*
* Z:
* Z:
*
* This mixer generates a constant zero output, and is normally used to
* skip over outputs that are not in use.
* Simple Mixer
* ............
*
* Simple Mixer:
* A simple mixer definition begins with:
*
* M: <scaler count>
* S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*>
* S: ...
* M: <control count>
* O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* 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 definition continues with <control count> entries describing the control
* inputs and their scaling, in the form:
*
* The <control ...> values for the output scaler are ignored by the mixer.
* 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:
*
* Values marked * are integers representing floating point values; values are
* scaled by 10000 on load/save.
* R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
*
* 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, updated to reflect
* bytes as they are consumed.
* @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 +295,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 +334,47 @@ public:
mixer_simple_s *mixinfo);
~SimpleMixer();
/**
* Factory method with full external configuration.
*
* 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);
/**
* Factory method for PWM/PPM input to internal float representation.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param input The control index used when fetching the input.
* @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out)
* @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out)
* @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out)
* @return A new SimpleMixer instance, or nullptr if one could not be
* allocated.
*/
static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
unsigned input,
uint16_t min,
uint16_t mid,
uint16_t max);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
@@ -330,10 +387,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 +460,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);
apps/systemlib/mixer/mixer_group.cpp
+61 -280
View File
@@ -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)
@@ -308,14 +64,7 @@ MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
MixerGroup::~MixerGroup()
{
Mixer *mixer;
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
reset();
}
void
@@ -332,6 +81,19 @@ MixerGroup::add_mixer(Mixer *mixer)
mixer->_next = nullptr;
}
void
MixerGroup::reset()
{
Mixer *mixer;
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
}
unsigned
MixerGroup::mix(float *outputs, unsigned space)
{
@@ -358,44 +120,63 @@ 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 ret = -1;
const char *end = buf + buflen;
int fd = open(path, O_RDONLY);
/*
* Loop until either we have emptied the buffer, or we have failed to
* allocate something when we expected to.
*/
while (buflen > 0) {
Mixer *m = nullptr;
const char *p = end - buflen;
unsigned resid = buflen;
if (fd < 0) {
debug("failed to open %s", path);
return -1;
}
/*
* Use the next character as a hint to decide which mixer class to construct.
*/
switch (*p) {
case 'Z':
m = NullMixer::from_text(p, resid);
break;
for (unsigned count = 0;; count++) {
int result;
Mixer *mixer;
case 'M':
m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
result = mixer_load(_control_cb,
_cb_handle,
fd,
mixer);
case 'R':
m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
/* error? */
if (result < 0) {
debug("error");
return -1;
default:
/* it's probably junk or whitespace, skip a byte and retry */
buflen--;
continue;
}
/* EOF or error */
if (result < 1) {
printf("[mixer] loaded %u mixers\n", count);
debug("EOF");
/*
* If we constructed something, add it to the group.
*/
if (m != nullptr) {
add_mixer(m);
/* we constructed something */
ret = 0;
/* only adjust buflen if parsing was successful */
buflen = resid;
} else {
/*
* There is data in the buffer that we expected to parse, but it didn't,
* so give up for now.
*/
break;
}
debug("loaded mixer %p", mixer);
add_mixer(mixer);
}
close(fd);
return 0;
/* nothing more in the buffer for us now */
return ret;
}
apps/systemlib/mixer/mixer_multirotor.cpp
+65 -3
View File
@@ -54,6 +54,11 @@
#include "mixer.h"
/*
* Clockwise: 1
* Counter-clockwise: -1
*/
namespace
{
@@ -148,6 +153,61 @@ 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)
{
@@ -167,10 +227,12 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
/* and then stay at full control */
output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */
} else {
output_factor = max_thrust;
}
apps/systemlib/mixer/mixer_simple.cpp
+334
View File
@@ -0,0 +1,334 @@
/****************************************************************************
*
* 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 *
findtag(const char *buf, unsigned &buflen, char tag)
{
while (buflen >= 2) {
if ((buf[0] == tag) && (buf[1] == ':'))
return buf;
buf++;
buflen--;
}
return nullptr;
}
static void
skipline(const char *buf, unsigned &buflen)
{
const char *p;
/* if we can find a CR or NL in the buffer, skip up to it */
if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
buflen -= (p - buf);
}
int
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
{
int ret;
int s[5];
buf = findtag(buf, buflen, 'O');
if ((buf == nullptr) || (buflen < 12))
return -1;
if ((ret = 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' (got %d)", buf, ret);
return -1;
}
skipline(buf, buflen);
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];
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16))
return -1;
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("control parse failed on '%s'", buf);
return -1;
}
skipline(buf, buflen);
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;
}
SimpleMixer *
SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
{
SimpleMixer *sm = nullptr;
mixer_simple_s *mixinfo = nullptr;
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1));
if (mixinfo == nullptr) {
debug("could not allocate memory for mixer info");
goto out;
}
mixinfo->control_count = 1;
/*
* Always pull from group 0, with the input value giving the channel.
*/
mixinfo->controls[0].control_group = 0;
mixinfo->controls[0].control_index = input;
/*
* Conversion uses both the input and output side of the mixer.
*
* The input side is used to slide the control value such that the min argument
* results in a value of zero.
*
* The output side is used to apply the scaling for the min/max values so that
* the resulting output is a -1.0 ... 1.0 value for the min...max range.
*/
mixinfo->controls[0].scaler.negative_scale = 1.0f;
mixinfo->controls[0].scaler.positive_scale = 1.0f;
mixinfo->controls[0].scaler.offset = -mid;
mixinfo->controls[0].scaler.min_output = -(mid - min);
mixinfo->controls[0].scaler.max_output = (max - mid);
mixinfo->output_scaler.negative_scale = 500.0f / (mid - min);
mixinfo->output_scaler.positive_scale = 500.0f / (max - mid);
mixinfo->output_scaler.offset = 0.0f;
mixinfo->output_scaler.min_output = -1.0f;
mixinfo->output_scaler.max_output = 1.0f;
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
debug("PWM input mixer for %d", input);
} else {
debug("could not allocate memory for PWM input 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;
}