fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Work in progress; standard mixer API and utility

Browse Source
This commit is contained in:
px4dev
2012-08-05 14:13:33 -07:00
parent 62e18b580c
commit 145a6c4c49
14 changed files with 787 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/drv_accel.h
+1 -1
View File
@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_accel);
* ioctl() definitions
*/
#define _ACCELIOCBASE (_SNIOCBASE + 0x20)
#define _ACCELIOCBASE (0x2000)
#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the ACC_POLLRATE constants */
apps/drivers/drv_baro.h
+1 -1
View File
@@ -65,7 +65,7 @@ ORB_DECLARE(sensor_baro);
* ioctl() definitions
*/
#define _BAROIOCBASE (_SNIOCBASE + 0x10)
#define _BAROIOCBASE (0x2100)
#define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the BARO_POLLRATE constants */
apps/drivers/drv_gyro.h
+1 -1
View File
@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_gyro);
* ioctl() definitions
*/
#define _GYROIOCBASE (_SNIOCBASE + 0x10)
#define _GYROIOCBASE (0x2200)
#define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the GYRO_POLLRATE constants */
apps/drivers/drv_mag.h
+1 -1
View File
@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_mag);
* ioctl() definitions
*/
#define _MAGIOCBASE (_SNIOCBASE + 0x30)
#define _MAGIOCBASE (0x2300)
#define _MAGIOC(_n) (_IOC(_MAGIOBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */
apps/drivers/drv_mixer.h
+91
View File
@@ -0,0 +1,91 @@
/****************************************************************************
*
* 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 ioctl interface.
*
* This interface can/should be exported by any device that supports
* control -> actuator mixing.
*/
#ifndef _DRV_MIXER_H
#define _DRV_MIXER_H
#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 MixMixer mixer;
};
/**
* Handy macro for determining the allocation size of a MixInfo structure.
*/
#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct MixScaler)))
/*
* 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)
/**
* 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.
*/
#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer)
/**
* Copy a mixer from *(struct MixMixer *)arg to the device.
*/
#define MIXERIOCSETMIXER(_mixer) _MIXERIOC(0x40 + _mixer)
#endif /* _DRV_ACCEL_H */
apps/drivers/drv_orb_dev.h
+1 -1
View File
@@ -58,7 +58,7 @@
/** maximum ogbject name length */
#define ORB_MAXNAME 32
#define _ORBIOCBASE (_DIOCBASE + 0x80)
#define _ORBIOCBASE (0x2500)
#define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n))
/*
apps/drivers/drv_pwm_output.h
+1 -1
View File
@@ -94,7 +94,7 @@ ORB_DECLARE(output_pwm);
* Note that ioctls and ObjDev updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _PWM_SERVO_BASE 0x7500
#define _PWM_SERVO_BASE 0x2600
/** arm all servo outputs handle by this driver */
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)
apps/px4/fmu/Makefile
+4
View File
@@ -35,4 +35,8 @@
# Interface driver for the PX4FMU board
#
APPNAME = fmu
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/px4/fmu/fmu.cpp
+72 -3
View File
@@ -40,6 +40,7 @@
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
@@ -54,6 +55,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/mixer.h>
@@ -76,6 +78,8 @@ public:
virtual int init();
private:
static const unsigned _max_actuators = 4;
Mode _mode;
int _task;
int _t_actuators;
@@ -85,7 +89,7 @@ private:
volatile bool _task_should_exit;
bool _armed;
MixMixer *_mixer[4];
MixMixer *_mixer[_max_actuators];
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
@@ -262,6 +266,9 @@ int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
struct MixInfo *mi;
struct MixMixer *mm, *tmm;
switch (cmd) {
case PWM_SERVO_ARM:
@@ -282,7 +289,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
if (arg < 2100) {
int channel = cmd - PWM_SERVO_SET(0);
channel = cmd - PWM_SERVO_SET(0);
up_pwm_servo_set(channel, arg);
} else {
ret = -EINVAL;
@@ -298,11 +305,73 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
int channel = cmd - PWM_SERVO_SET(0);
channel = cmd - PWM_SERVO_SET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
case MIXERIOCGETMIXERCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}
break;
case MIXERIOCGETMIXER(3):
case MIXERIOCGETMIXER(2):
if (_mode != MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case MIXERIOCGETMIXER(1):
case MIXERIOCGETMIXER(0):
channel = cmd - MIXERIOCGETMIXER(0);
/* 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;
}
break;
case MIXERIOCSETMIXER(3):
case MIXERIOCSETMIXER(2):
if (_mode != MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case MIXERIOCSETMIXER(1):
case MIXERIOCSETMIXER(0):
channel = cmd - MIXERIOCGETMIXER(0);
/* caller- supplied mixer */
mm = (struct MixMixer *)arg;
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
tmm = (struct MixMixer *)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;
break;
apps/systemcmds/mixer/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Build the mixer tool.
#
APPNAME = mixer
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/systemcmds/mixer/mixer.c
+342
View File
@@ -0,0 +1,342 @@
/****************************************************************************
*
* 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 utility.
*/
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <getopt.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
__EXPORT int mixer_main(int argc, char *argv[]);
static void usage(void);
static void load(const char *devname, const char *fname);
static void save(const char *devname, const char *fname);
static void show(const char *devname);
enum Operation {
LOAD,
SAVE,
SHOW,
NONE
};
int
mixer_main(int argc, char *argv[])
{
const char *devname = NULL;
const char *fname = NULL;
int ch;
enum Operation todo = NONE;
if (argc < 2)
usage();
if (!strcmp(argv[1], "load")) {
todo = LOAD;
} else if (!strcmp(argv[1], "save")) {
todo = SAVE;
} else if (!strcmp(argv[1], "show")) {
todo = SHOW;
} else {
usage();
}
while ((ch = getopt(argc, argv, "d:f:")) != EOF) {
switch (ch) {
case 'd':
devname = optarg;
break;
case 'f':
fname = optarg;
break;
default:
usage();
}
}
switch (todo) {
case LOAD:
if ((devname == NULL) || (fname == NULL))
usage();
load(devname, fname);
break;
case SAVE:
if ((devname == NULL) || (fname == NULL))
usage();
save(devname, fname);
break;
case SHOW:
if (devname == NULL)
usage();
show(devname);
break;
default:
exit(1);
}
return 0;
}
static void
usage(void)
{
fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer {load|save|show} -d <device> [-f <filename>]\n");
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 MixMixer *mixer;
/* 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)
goto out;
/* end of file? */
if (ret == 0)
break;
/* sanity check the mixer */
if (mixer_check(mixer, NUM_ACTUATOR_CONTROLS)) {
fprintf(stderr, "mixer %u fails sanity check\n", i);
goto out;
}
/* send the mixer to the device */
ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer);
if (ret < 0)
goto out;
free(mixer);
mixer = NULL;
}
printf("mixer: loaded %d mixers to %s\n", num_mixers, devname);
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->num_controls = 0;
mi = (struct MixInfo *)malloc(MIXINFO_SIZE(mi->num_controls));
}
/* 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) {
fprintf(stderr, "MIXERIOCGETMIXER error\n");
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 */
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;
}
/* 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++) {
ret = getmixer(dev, i, &mi);
if (ret < 0)
goto out;
ret = mixer_save(defs, &mi->mixer);
if (ret < 0)
goto out;
}
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;
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)
goto out;
printf("mixer %d : %d controls\n", i, mi->mixer.control_count);
printf(" -ve scale +ve scale offset low limit high limit");
printf("output %f %f %f %f %f\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("%d: %f %f %f %f %f\n",
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);
}
apps/systemlib/mixer.c
+156 -11
View File
@@ -43,22 +43,31 @@
* See mixer.h for more details.
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "mixer.h"
static int
scale_check(struct MixScaler *scale)
{
if (scale->offset > 1.0f)
return -1;
if (scale->offset > 1.0f)
return -1;
if (scale->lower_limit > scale->upper_limit)
return -1;
if (scale->lower_limit < -1.0f)
return -1;
if (scale->upper_limit > 1.0f)
return -1;
return 0;
if (scale->offset > 1.0f)
return -1;
if (scale->offset > 1.0f)
return -1;
if (scale->lower_limit > scale->upper_limit)
return -1;
if (scale->lower_limit < -1.0f)
return -1;
if (scale->upper_limit > 1.0f)
return -1;
return 0;
}
int
@@ -66,17 +75,21 @@ mixer_check(struct MixMixer *mixer, unsigned control_count)
{
if (mixer->control_count < 1)
return -1;
if (mixer->control_count > control_count)
return -1;
if (!scale_check(&mixer->output_scaler))
return -1;
for (unsigned i = 0; i < mixer->control_count; i++) {
if (mixer->control_scaler[i].control >= control_count)
return -1;
if (!scale_check(&mixer->control_scaler[i]))
return -1;
}
return 0;
}
@@ -87,11 +100,14 @@ scale(struct MixScaler *scaler, float input)
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;
}
@@ -112,3 +128,132 @@ mixer_mix(struct MixMixer *mixer, float *controls)
return scale(&mixer->output_scaler, sum);
}
static int
mixer_getline(int fd, char *line, unsigned maxlen)
{
int ret;
char c;
while (--maxlen) {
ret = read(fd, &c, 1);
if (ret <= 0)
return ret;
if (c == '\r')
continue;
if (c == '\n') {
*line = '\0';
return 1;
}
*line++ = c;
}
/* line too long */
return -1;
}
static int
mixer_load_scaler(const char *buf, struct MixScaler *scaler)
{
if (sscanf(buf, "S: %u %f %f %f %f %f",
&scaler->control, &scaler->negative_scale, &scaler->positive_scale,
&scaler->offset, &scaler->lower_limit, &scaler->upper_limit) != 6)
return -1;
return 0;
}
int
mixer_load(int fd, struct MixMixer **mp)
{
int ret, result = -1;
struct MixMixer *mixer = NULL;
char buf[100];
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;
/* must have at least one scaler */
if (scalers < 1)
goto out;
/* allocate mixer */
scalers--;
mixer = (struct MixMixer *)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++) {
if (mixer_getline(fd, buf, sizeof(buf)))
goto out;
if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
goto out;
}
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 MixScaler *scaler)
{
return sprintf(buf, "S: %u %f %f %f %f %f\n",
scaler->control, scaler->negative_scale, scaler->positive_scale,
scaler->offset, scaler->lower_limit, scaler->upper_limit);
}
int
mixer_save(int fd, struct MixMixer *mixer)
{
char buf[100];
int len, ret;
/* write the mixer header */
len = sprintf(buf, "M: %u\n", mixer->control_count);
ret = write(fd, buf, len);
if (ret != len)
return -1;
/* 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;
}
apps/systemlib/mixer.h
+73 -28
View File
@@ -31,6 +31,9 @@
*
****************************************************************************/
#ifndef _SYSTEMLIB_MIXER_H
#define _SYSTEMLIB_MIXER_H
/**
* @file mixer.h
*
@@ -55,7 +58,7 @@
*
* 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
* the actuator's configuration and then combined to produce the
* actuator value, which may then be further scaled to suit the specific
* output type.
*
@@ -100,7 +103,7 @@
* Mixing
* ------
*
* Mixing is performed by summing the scaled control values.
* Mixing is performed by summing the scaled control values.
*
*
* Controls
@@ -109,7 +112,7 @@
* 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
* The precise assignment of controls may vary depending on the
* application, but the following assignments should be used
* when appropriate.
*
@@ -121,22 +124,25 @@
* 3 | primary thrust
*/
struct MixScaler
{
unsigned control; /**< control consumed by this scaler */
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 */
};
struct MixScaler {
unsigned control; /**< control consumed by this scaler */
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 */
};
struct MixMixer
{
unsigned control_count; /**< number of control scalers */
struct MixScaler output_scaler; /**< scaler applied to mixer output */
struct MixScaler control_scaler[0]; /**< array of control scalers */
};
struct MixMixer {
unsigned control_count; /**< number of control scalers */
struct MixScaler output_scaler; /**< scaler applied to mixer output */
struct MixScaler control_scaler[0]; /**< array of control scalers */
};
/**
* Handy macro for determining the allocation size of a mixer.
*/
#define MIXER_SIZE(_num_scalers) (sizeof(struct MixMixer) + ((_num_scalers) * sizeof(struct MixScaler)))
__BEGIN_DECLS
@@ -150,16 +156,55 @@ __BEGIN_DECLS
* @param controls Array of input control values.
* @return The mixed output.
*/
__EXPORT float mixer_mix(struct MixMixer *mixer, float *controls);
__EXPORT float mixer_mix(struct MixMixer *mixer, float *controls);
/**
* Check a mixer configuration for sanity.
*
* @param mixer The mixer configuration to be checked.
* @param control_count The number of controls in the system.
* @return Zero if the mixer configuration is sane,
* nonzero otherwise.
*/
__EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count);
/**
* 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: <control count + 1>
* S: <control> <negative_scale> <positive_scale> <offset> <lower_limit> <upper_limit>
* S: ...
*
* The <control> value for the output scaler is ignored.
*
* 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. Mixers may be
* 'skipped' in a file by setting indicating that the mixer has only one scaler
* (the output scaler). This results in a mixer with zero controls, which will
* always generate output corresponding to the output scaler offset.
*
* @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 MixMixer **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 MixMixer *mixers);
/**
* Check a mixer configuration for sanity.
*
* @param mixer The mixer configuration to be checked.
* @param control_count The number of controls in the system.
* @return Zero if the mixer configuration is sane,
* nonzero otherwise.
*/
__EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count);
__END_DECLS
#endif /* _SYSTEMLIB_MIXER_H */
nuttx/configs/px4fmu/nsh/appconfig
+1
View File
@@ -48,6 +48,7 @@ CONFIGURED_APPS += systemcmds/reboot
CONFIGURED_APPS += systemcmds/perf
CONFIGURED_APPS += systemcmds/top
CONFIGURED_APPS += systemcmds/boardinfo
CONFIGURED_APPS += systemcmds/mixer
#CONFIGURED_APPS += systemcmds/calibration
CONFIGURED_APPS += uORB