dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Rework the mixer architecture based on discussions about arbitrary geometry mixing and plugins. 

Now the mixer is a C++ library that can be fairly easily bolted into an output driver to provide mixing services.

Teach the FMU driver how to use it as an example.  More testing is still required.
This commit is contained in:
px4dev 2012-08-10 00:30:40 -07:00
parent 04d280564c
commit 67e0f8b179
16 changed files with 1170 additions and 962 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/mixers/FMU_AET.mix
View File

@ -46,7 +46,7 @@ Output 2
--------
This mixer is empty.
M: 0
Z:
Motor speed mixer
-----------------

2
ROMFS/mixers/FMU_RET.mix
View File

@ -39,7 +39,7 @@ Output 2
--------
This mixer is empty.
M: 0
Z:
Motor speed mixer
-----------------

2
ROMFS/mixers/FMU_delta.mix
View File

@ -35,7 +35,7 @@ Output 2
--------
This mixer is empty.
M: 0
Z:
Motor speed mixer
-----------------

24
ROMFS/mixers/FMU_multirotor.mix Normal file
View File

@ -0,0 +1,24 @@
Multirotor mixer for PX4FMU
===========================
This file defines passthrough mixers suitable for driving ESCs over the full
input range.
Channel group 0, channels 0-3 values 0.0 - 1.0 are scaled to the full output range.
M: 2
S: 0 0 10000 10000 0 -10000 10000
S: 0 0 0 20000 -10000 -10000 10000
M: 2
S: 0 0 10000 10000 0 -10000 10000
S: 0 1 0 20000 -10000 -10000 10000
M: 2
S: 0 0 10000 10000 0 -10000 10000
S: 0 2 0 20000 -10000 -10000 10000
M: 2
S: 0 0 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000

114
apps/drivers/drv_mixer.h
View File

@ -34,10 +34,20 @@
/**
* @file drv_mixer.h
*
* Mixer ioctl interface.
* Mixer ioctl interfaces.
*
* This interface can/should be exported by any device that supports
* control -> actuator mixing.
* Normal workflow is:
*
* - open mixer device
* - add mixer(s)
* loop:
* - mix actuators to array
*
* Each client has its own configuration.
*
* When mixing, outputs are produced by mixers in the order they are
* added. A simple mixer produces one output; a multotor mixer will
* produce several outputs, etc.
*/
#ifndef _DRV_MIXER_H
@ -46,51 +56,83 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include <systemlib/mixer.h>
/**
* Structure used for receiving mixers.
*
* Note that the mixers array is not actually an array of mixers; it
* simply represents the first mixer in the buffer.
*/
struct MixInfo {
unsigned num_controls;
struct mixer_s mixer;
};
/**
* Handy macro for determining the allocation size of a MixInfo structure.
*/
#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct scaler_s)))
#define MIXER_DEVICE_PATH "/dev/mixer"
/*
* ioctl() definitions
*/
#define _MIXERIOCBASE (0x2400)
#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n))
/** get the number of actuators that require mixers in *(unsigned)arg */
#define MIXERIOCGETMIXERCOUNT _MIXERIOC(0)
/** get the number of mixable outputs */
#define MIXERIOCGETOUTPUTCOUNT _MIXERIOC(0)
/** reset (clear) the mixer configuration */
#define MIXERIOCRESET _MIXERIOC(1)
/** simple channel scaler */
struct mixer_scaler_s
{
float negative_scale;
float positive_scale;
float offset;
float min_output;
float max_output;
};
/** mixer input */
struct mixer_input_s
{
uint8_t control_group; /**< group from which the input reads */
uint8_t control_index; /**< index within the control group */
struct mixer_scaler_s scaler; /**< scaling applied to the input before use */
};
/** simple mixer */
struct mixer_simple_s
{
uint8_t input_count; /**< number of inputs */
struct mixer_scaler_s output_scaler; /**< scaling for the output */
struct mixer_input_s inputs[0]; /**< actual size of the array is set by input_count */
};
#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_input_s))
/**
* Copy a mixer from the device into *(struct MixInfo *)arg.
*
* The num_controls field indicates the number of controls for which space
* is allocated following the MixInfo structure. If the allocation
* is too small, no mixer data is retured. The control_count field in
* the MixInfo.mixer structure is always updated.
*
* If no mixer is assigned for the given index, the ioctl returns ENOENT.
* add a simple mixer in (struct mixer_simple_s *)arg
*/
#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer)
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
/** multirotor output definition */
struct mixer_rotor_output_s
{
float angle; /**< rotor angle clockwise from forward in radians */
float distance; /**< motor distance from centre in arbitrary units */
};
/** multirotor mixer */
struct mixer_multirotor_s
{
uint8_t rotor_count;
struct mixer_input_s inputs[4]; /**< inputs are roll, pitch, yaw, thrust */
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
};
/**
* Copy a mixer from *(struct mixer_s *)arg to the device.
*
* If arg is zero, the mixer is deleted.
* Add a multirotor mixer in (struct mixer_multirotor_s *)arg
*/
#define MIXERIOCADDMULTIROTOR _MIXERIOC(3)
/**
* Add mixers(s) from a the file in (const char *)arg
*/
#define MIXERIOCLOADFILE _MIXERIOC(4)
/*
* XXX Thoughts for additional operations:
*
* - get/set output scale, for tuning center/limit values.
* - save/serialise for saving tuned mixers.
*/
#define MIXERIOCSETMIXER(_mixer) _MIXERIOC(0x40 + _mixer)
#endif /* _DRV_ACCEL_H */

167
apps/px4/fmu/fmu.cpp
View File

@ -57,13 +57,15 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/mixer.h>
#include <arch/board/up_pwm_servo.h>
#include <uORB/topics/actuator_controls.h>
class FMUServo : public device::CDev
{
public:
@ -91,10 +93,20 @@ private:
volatile bool _task_should_exit;
bool _armed;
mixer_s *_mixer[_max_actuators];
MixerGroup *_mixers;
actuator_controls_s _controls;
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
static int control_callback_trampoline(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
int control_callback(uint8_t control_group,
uint8_t control_index,
float &input);
};
namespace
@ -111,10 +123,9 @@ FMUServo::FMUServo(Mode mode) :
_t_actuators(-1),
_t_armed(-1),
_task_should_exit(false),
_armed(false)
_armed(false),
_mixers(nullptr)
{
for (unsigned i = 0; i < _max_actuators; i++)
_mixer[i] = nullptr;
}
FMUServo::~FMUServo()
@ -226,23 +237,19 @@ FMUServo::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
struct actuator_controls_s ac;
float *controls[1] = { &ac.control[0] };
float outputs[num_outputs];
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &ac);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* do mixing */
_mixers->mix(&outputs[0], num_outputs);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* if the actuator is configured */
if (_mixer[i] != nullptr) {
/* mix controls to the actuator */
float output = mixer_mix(_mixer[i], &controls[0]);
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * output));
}
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
}
}
@ -273,13 +280,33 @@ FMUServo::task_main()
_exit(0);
}
int
FMUServo::control_callback_trampoline(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
return ((FMUServo *)handle)->control_callback(control_group, control_index, input);
}
int
FMUServo::control_callback(uint8_t control_group,
uint8_t control_index,
float &input)
{
/* XXX currently only supporting group zero */
if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS))
return -1;
input = _controls.control[control_index];
return 0;
}
int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
struct MixInfo *mi;
struct mixer_s *mm, *tmm;
switch (cmd) {
case PWM_SERVO_ARM:
@ -325,89 +352,57 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCGETMIXERCOUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}
break;
case MIXERIOCGETMIXER(3):
case MIXERIOCGETMIXER(2):
if (_mode != MODE_4PWM) {
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case MIXERIOCGETMIXER(1):
case MIXERIOCGETMIXER(0):
channel = cmd - MIXERIOCGETMIXER(0);
/* if no mixer is assigned, we return ENOENT */
if (_mixer[channel] == nullptr) {
ret = -ENOENT;
break;
}
/* caller's MixInfo */
mi = (struct MixInfo *)arg;
/* if MixInfo claims to be big enough, copy mixer info */
if (mi->num_controls >= _mixer[channel]->control_count) {
memcpy(&mi->mixer, _mixer[channel], MIXER_SIZE(_mixer[channel]->control_count));
} else {
/* just update MixInfo with actual size of the mixer */
mi->mixer.control_count = _mixer[channel]->control_count;
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
_mixers->add_mixer(mixer);
}
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCSETMIXER(3):
case MIXERIOCSETMIXER(2):
if (_mode != MODE_4PWM) {
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
if (_mixers->load_from_file(path) != 0) {
delete _mixers;
_mixers = nullptr;
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case MIXERIOCSETMIXER(1):
case MIXERIOCSETMIXER(0):
channel = cmd - MIXERIOCSETMIXER(0);
/* get the caller-supplied mixer and check */
mm = (struct mixer_s *)arg;
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) {
/* only the attitude group is supported */
ret = -EINVAL;
break;
}
/* allocate a new mixer struct */
tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count));
memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
} else {
tmm = nullptr;
}
/* swap in new mixer for old */
mm = _mixer[channel];
_mixer[channel] = tmm;
/* if there was an old mixer, free it */
if (mm != nullptr)
free(mm);
break;
}
default:
ret = -ENOTTY;

261
apps/systemcmds/mixer/mixer.c
View File

@ -44,7 +44,6 @@
#include <fcntl.h>
#include <errno.h>
#include <systemlib/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
@ -52,8 +51,6 @@ __EXPORT int mixer_main(int argc, char *argv[]);
static void usage(const char *reason);
static void load(const char *devname, const char *fname);
static void save(const char *devname, const char *fname);
static void show(const char *devname);
int
mixer_main(int argc, char *argv[])
@ -67,18 +64,6 @@ mixer_main(int argc, char *argv[])
load(argv[2], argv[3]);
} else if (!strcmp(argv[1], "save")) {
if (argc < 4)
usage("missing device or filename");
save(argv[2], argv[3]);
} else if (!strcmp(argv[1], "show")) {
if (argc < 3)
usage("missing device name");
show(argv[2]);
} else {
usage("unrecognised command");
}
@ -93,146 +78,15 @@ usage(const char *reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer show <device>\n");
fprintf(stderr, " mixer {load|save} <device> [<filename>]\n");
fprintf(stderr, " mixer load <device> <filename>\n");
/* XXX automatic setups for quad, etc. */
exit(1);
}
static void
load(const char *devname, const char *fname)
{
int defs = -1;
int dev = -1;
unsigned num_mixers = 0;
int ret, result = 1;
struct mixer_s *mixer = NULL;
/* open the device */
if ((dev = open(devname, 0)) < 0) {
fprintf(stderr, "can't open %s\n", devname);
goto out;
}
/* open the definition file */
if ((defs = open(fname, O_RDONLY)) < 0) {
fprintf(stderr, "can't open %s\n", fname);
goto out;
}
/* find out how many mixers the device supports */
ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
if (num_mixers < 1) {
fprintf(stderr, "can't get mixer count from %s\n", devname);
goto out;
}
/* send mixers to the device */
for (unsigned i = 0; i < num_mixers; i++) {
ret = mixer_load(defs, &mixer);
if (ret < 0) {
fprintf(stderr, "read for mixer %d failed\n", i);
goto out;
}
/* end of file? */
if (ret == 0)
break;
if (mixer != NULL) {
/* sanity check the mixer */
ret = mixer_check(mixer, NUM_ACTUATOR_CONTROL_GROUPS, NUM_ACTUATOR_CONTROLS);
if (ret != 0) {
fprintf(stderr, "mixer %u fails sanity check %d\n", i, ret);
goto out;
}
/* send the mixer to the device */
ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer);
if (ret < 0) {
fprintf(stderr, "mixer %d set failed\n", i);
goto out;
}
free(mixer);
mixer = NULL;
} else {
/* delete the mixer */
ret = ioctl(dev, MIXERIOCSETMIXER(i), 0);
if (ret < 0) {
fprintf(stderr, "mixer %d clear failed\n", i);
goto out;
}
}
}
result = 0;
out:
/* free the mixers array */
if (mixer != NULL)
free(mixer);
if (defs != -1)
close(defs);
if (dev != -1)
close(dev);
exit(result);
}
static int
getmixer(int dev, unsigned mixer_number, struct MixInfo **mip)
{
struct MixInfo *mi = *mip;
int ret;
/* first-round initialisation */
if (mi == NULL) {
mi = (struct MixInfo *)malloc(MIXINFO_SIZE(0));
mi->num_controls = 0;
}
/* loop trying to get the next mixer until the buffer is big enough */
do {
/* try to get the mixer into the buffer as it stands */
ret = ioctl(dev, MIXERIOCGETMIXER(mixer_number), (unsigned long)mi);
if (ret < 0)
return -1;
/* did the mixer fit? */
if (mi->mixer.control_count <= mi->num_controls)
break;
/* re-allocate to suit */
mi->num_controls = mi->mixer.control_count;
mi = (struct MixInfo *)realloc(mi, MIXINFO_SIZE(mi->num_controls));
/* oops, blew up the heap */
if (mi == NULL)
return -1;
} while (true);
*mip = mi;
return 0;
}
static void
save(const char *devname, const char *fname)
{
struct MixInfo *mi = NULL;
int defs = -1;
int dev = -1;
unsigned num_mixers = 0;
int ret, result = 1;
/* open the device */
@ -241,120 +95,17 @@ save(const char *devname, const char *fname)
goto out;
}
/* find out how many mixers the device supports */
ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
if (num_mixers < 1) {
fprintf(stderr, "can't get mixer count from %s\n", devname);
goto out;
}
/* open the definition file */
if ((defs = open(fname, O_WRONLY | O_CREAT)) < 0) {
fprintf(stderr, "can't open %s\n", fname);
goto out;
}
/* get mixers from the device and save them */
for (unsigned i = 0; i < num_mixers; i++) {
struct mixer_s *mm;
ret = getmixer(dev, i, &mi);
mm = &mi->mixer;
if (ret < 0) {
if (errno != ENOENT)
goto out;
mm = NULL;
}
ret = mixer_save(defs, mm);
if (ret < 0)
goto out;
/* tell it to load the file */
ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname);
if (ret != 0) {
fprintf(stderr, "failed loading %s\n", fname);
}
result = 0;
out:
/* free the mixinfo */
if (mi != NULL)
free(mi);
if (defs != -1)
close(defs);
if (dev != -1)
close(dev);
exit(result);
}
static void
show(const char *devname)
{
struct MixInfo *mi = NULL;
int dev = -1;
unsigned num_mixers = 0;
int ret;
/* open the device */
if ((dev = open(devname, 0)) < 0) {
fprintf(stderr, "can't open %s\n", devname);
goto out;
}
/* find out how many mixers the device supports */
ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
if (num_mixers < 1) {
fprintf(stderr, "can't get mixer count from %s\n", devname);
goto out;
}
/* get mixers from the device and print them */
for (unsigned i = 0; i < num_mixers; i++) {
ret = getmixer(dev, i, &mi);
if (ret < 0) {
if (errno != ENOENT)
goto out;
continue;
}
printf("mixer %d:\n", i);
printf(" -ve scale +ve scale offset low limit high limit\n");
printf("output %8.4f %8.4f %8.4f %8.4f %8.4f\n",
mi->mixer.output_scaler.negative_scale,
mi->mixer.output_scaler.positive_scale,
mi->mixer.output_scaler.offset,
mi->mixer.output_scaler.lower_limit,
mi->mixer.output_scaler.upper_limit);
for (unsigned j = 0; j < mi->mixer.control_count; j++) {
printf("(%u,%u) %8.4f %8.4f %8.4f %8.4f %8.4f\n",
mi->mixer.control_scaler[j].control_group,
mi->mixer.control_scaler[j].control_index,
mi->mixer.control_scaler[j].negative_scale,
mi->mixer.control_scaler[j].positive_scale,
mi->mixer.control_scaler[j].offset,
mi->mixer.control_scaler[j].lower_limit,
mi->mixer.control_scaler[j].upper_limit);
}
}
out:
/* free the mixinfo */
if (mi != NULL)
free(mi);
if (dev != -1)
close(dev);
exit(0);
}

1
apps/systemlib/Makefile
View File

@ -36,7 +36,6 @@
#
CSRCS = hx_stream.c \
mixer.c \
perf_counter.c
#

345
apps/systemlib/mixer.c
View File

@ -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;
}

236
apps/systemlib/mixer.h
View File

@ -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 */

39
apps/systemlib/mixer/Makefile Normal file
View File

@ -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

211
apps/systemlib/mixer/mixer.cpp Normal file
View File

@ -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;
}

358
apps/systemlib/mixer/mixer.h Normal file
View File

@ -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

288
apps/systemlib/mixer/mixer_group.cpp Normal file
View File

@ -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;
}

81
apps/systemlib/mixer/mixer_multirotor.cpp Normal file
View File

@ -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);
}

1
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -43,6 +43,7 @@ CONFIGURED_APPS += system/readline
# System library - utility functions
CONFIGURED_APPS += systemlib
CONFIGURED_APPS += systemlib/mixer
CONFIGURED_APPS += systemcmds/reboot
CONFIGURED_APPS += systemcmds/perf