aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-08-05 13:43:16 -0700
committerpx4dev <px4@purgatory.org>2012-08-05 14:13:34 -0700
commit9804447a66391a1e216068cbd849e0011c851f7a (patch)
tree290375f61f5fcab6a837f39e45e65cb5fbce3308
parent9804776a0c8bd67d4a533e3302f1a598c35b868b (diff)
downloadpx4-firmware-9804447a66391a1e216068cbd849e0011c851f7a.tar.gz
px4-firmware-9804447a66391a1e216068cbd849e0011c851f7a.tar.bz2
px4-firmware-9804447a66391a1e216068cbd849e0011c851f7a.zip
More work on the mixer architecture.
Solve the multiple publishers issue with 'control groups', one group per controller. Mixer inputs now specify both group and control offset within the group. Avoid using %f when loading/saving mixers; use scaled integers instead.
-rw-r--r--apps/drivers/drv_mixer.h13
-rw-r--r--apps/px4/fmu/fmu.cpp70
-rw-r--r--apps/systemcmds/mixer/mixer.c74
-rw-r--r--apps/systemlib/mixer.c119
-rw-r--r--apps/systemlib/mixer.h65
-rw-r--r--apps/uORB/objects_common.cpp9
-rw-r--r--apps/uORB/topics/actuator_controls.h27
7 files changed, 274 insertions, 103 deletions
diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h
index f3a39e60c..d66fbf3a9 100644
--- a/apps/drivers/drv_mixer.h
+++ b/apps/drivers/drv_mixer.h
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Mixer ioctl interface.
+ * @file drv_mixer.h
+ *
+ * Mixer ioctl interface.
*
* This interface can/should be exported by any device that supports
* control -> actuator mixing.
@@ -52,16 +54,15 @@
* Note that the mixers array is not actually an array of mixers; it
* simply represents the first mixer in the buffer.
*/
-struct MixInfo
-{
+struct MixInfo {
unsigned num_controls;
- struct MixMixer mixer;
+ 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 MixScaler)))
+#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct scaler_s)))
/*
* ioctl() definitions
@@ -86,7 +87,7 @@ struct MixInfo
#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer)
/**
- * Copy a mixer from *(struct MixMixer *)arg to the device.
+ * Copy a mixer from *(struct mixer_s *)arg to the device.
*
* If arg is zero, the mixer is deleted.
*/
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 8e468c012..57eb12f4e 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Driver/configurator for the PX4 FMU multi-purpose port.
+ * @file fmu.cpp
+ *
+ * Driver/configurator for the PX4 FMU multi-purpose port.
*/
#include <nuttx/config.h>
@@ -89,13 +91,14 @@ private:
volatile bool _task_should_exit;
bool _armed;
- MixMixer *_mixer[_max_actuators];
+ mixer_s *_mixer[_max_actuators];
static void task_main_trampoline(int argc, char *argv[]);
- void task_main();
+ void task_main();
};
-namespace {
+namespace
+{
FMUServo *g_servo;
@@ -122,6 +125,7 @@ FMUServo::~FMUServo()
_task_should_exit = true;
unsigned i = 0;
+
do {
/* wait 20ms */
usleep(20000);
@@ -147,11 +151,13 @@ FMUServo::init()
/* do regular cdev init */
ret = CDev::init();
+
if (ret != OK)
return ret;
/* start the IO interface task */
_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
+
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -176,18 +182,20 @@ FMUServo::task_main()
/* XXX magic numbers */
up_pwm_servo_init(0x3);
break;
+
case MODE_4PWM:
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
break;
+
case MODE_NONE:
/* we should never get here... */
break;
}
/* subscribe to objects that we are interested in watching */
- _t_actuators = orb_subscribe(ORB_ID(actuator_controls));
+ _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(_t_actuators, 20); /* 50Hz update rate */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
@@ -217,9 +225,10 @@ 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] };
/* get controls */
- orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &ac);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
@@ -228,7 +237,7 @@ FMUServo::task_main()
if (_mixer[i] != nullptr) {
/* mix controls to the actuator */
- float output = mixer_mix(_mixer[i], &ac.control[0]);
+ float output = mixer_mix(_mixer[i], &controls[0]);
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * output));
@@ -252,7 +261,7 @@ FMUServo::task_main()
::close(_t_armed);
/* make sure servos are off */
- up_pwm_servo_deinit();
+ up_pwm_servo_deinit();
/* note - someone else is responsible for restoring the GPIO config */
@@ -267,7 +276,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
int ret = OK;
int channel;
struct MixInfo *mi;
- struct MixMixer *mm, *tmm;
+ struct mixer_s *mm, *tmm;
switch (cmd) {
case PWM_SERVO_ARM:
@@ -284,15 +293,18 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
if (arg < 2100) {
channel = cmd - PWM_SERVO_SET(0);
up_pwm_servo_set(channel, arg);
+
} else {
ret = -EINVAL;
}
+
break;
case PWM_SERVO_GET(2):
@@ -301,20 +313,23 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
- channel = cmd - PWM_SERVO_SET(0);
- *(servo_position_t *)arg = up_pwm_servo_get(channel);
- break;
- }
+ 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):
@@ -323,6 +338,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case MIXERIOCGETMIXER(1):
case MIXERIOCGETMIXER(0):
@@ -340,10 +356,12 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long 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):
@@ -352,22 +370,25 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case MIXERIOCSETMIXER(1):
case MIXERIOCSETMIXER(0):
channel = cmd - MIXERIOCSETMIXER(0);
/* get the caller-supplied mixer and check */
- mm = (struct MixMixer *)arg;
- if (mixer_check(mm, NUM_ACTUATOR_CONTROLS)) {
+ mm = (struct mixer_s *)arg;
+
+ if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) { /* only the attitude group is supported */
ret = -EINVAL;
break;
}
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
- tmm = (struct MixMixer *)malloc(MIXER_SIZE(mm->control_count));
+ tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count));
memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
+
} else {
tmm = nullptr;
}
@@ -379,16 +400,19 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if there was an old mixer, free it */
if (mm != nullptr)
free(mm);
+
break;
default:
ret = -ENOTTY;
break;
}
+
return ret;
}
-namespace {
+namespace
+{
enum PortMode {
PORT_MODE_UNSET = 0,
@@ -403,7 +427,7 @@ enum PortMode {
PortMode g_port_mode;
int
-fmu_new_mode(PortMode new_mode)
+fmu_new_mode(PortMode new_mode)
{
int fd;
int ret = OK;
@@ -412,6 +436,7 @@ fmu_new_mode(PortMode new_mode)
/* get hold of the GPIO configuration descriptor */
fd = open(GPIO_DEVICE_PATH, 0);
+
if (fd < 0)
return -errno;
@@ -464,15 +489,19 @@ fmu_new_mode(PortMode new_mode)
/* adjust GPIO config for serial mode(s) */
if (gpio_bits != 0)
ioctl(fd, GPIO_SET_ALT_1, gpio_bits);
+
close(fd);
/* create new PWM driver if required */
if (servo_mode != FMUServo::MODE_NONE) {
g_servo = new FMUServo(servo_mode);
+
if (g_servo == nullptr) {
ret = -ENOMEM;
+
} else {
ret = g_servo->init();
+
if (ret != OK) {
delete g_servo;
g_servo = nullptr;
@@ -499,14 +528,19 @@ fmu_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
+
} else if (!strcmp(argv[1], "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm")) {
new_mode = PORT_FULL_PWM;
+
} else if (!strcmp(argv[1], "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
}
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
index bbbe67ff7..bdf54bc20 100644
--- a/apps/systemcmds/mixer/mixer.c
+++ b/apps/systemcmds/mixer/mixer.c
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Mixer utility.
+ * @file mixer.c
+ *
+ * Mixer utility.
*/
#include <string.h>
@@ -58,22 +60,29 @@ mixer_main(int argc, char *argv[])
{
if (argc < 2)
usage("missing command");
+
if (!strcmp(argv[1], "load")) {
if (argc < 4)
usage("missing device or filename");
+
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");
}
+
return 0;
}
@@ -82,6 +91,7 @@ usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
+
fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer show <device>\n");
fprintf(stderr, " mixer {load|save} <device> [<filename>]\n");
@@ -95,7 +105,7 @@ load(const char *devname, const char *fname)
int dev = -1;
unsigned num_mixers = 0;
int ret, result = 1;
- struct MixMixer *mixer = NULL;
+ struct mixer_s *mixer = NULL;
/* open the device */
if ((dev = open(devname, 0)) < 0) {
@@ -111,6 +121,7 @@ load(const char *devname, const char *fname)
/* 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;
@@ -119,6 +130,7 @@ load(const char *devname, const char *fname)
/* 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;
@@ -130,7 +142,8 @@ load(const char *devname, const char *fname)
if (mixer != NULL) {
/* sanity check the mixer */
- ret = mixer_check(mixer, NUM_ACTUATOR_CONTROLS);
+ 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;
@@ -138,6 +151,7 @@ load(const char *devname, const char *fname)
/* 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;
@@ -145,9 +159,11 @@ load(const char *devname, const char *fname)
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;
@@ -158,11 +174,14 @@ load(const char *devname, const char *fname)
result = 0;
out:
+
/* free the mixers array */
if (mixer != NULL)
free(mixer);
+
if (defs != -1)
close(defs);
+
if (dev != -1)
close(dev);
@@ -185,6 +204,7 @@ getmixer(int dev, unsigned mixer_number, struct MixInfo **mip)
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;
@@ -200,7 +220,7 @@ getmixer(int dev, unsigned mixer_number, struct MixInfo **mip)
if (mi == NULL)
return -1;
- } while(true);
+ } while (true);
*mip = mi;
return 0;
@@ -223,6 +243,7 @@ save(const char *devname, const char *fname)
/* 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;
@@ -236,16 +257,20 @@ save(const char *devname, const char *fname)
/* get mixers from the device and save them */
for (unsigned i = 0; i < num_mixers; i++) {
- struct MixMixer *mm;
+ 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;
}
@@ -253,11 +278,14 @@ save(const char *devname, const char *fname)
result = 0;
out:
+
/* free the mixinfo */
if (mi != NULL)
free(mi);
+
if (defs != -1)
close(defs);
+
if (dev != -1)
close(dev);
@@ -280,6 +308,7 @@ show(const char *devname)
/* 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;
@@ -289,38 +318,43 @@ show(const char *devname)
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);
+ 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("%d: %8.4f %8.4f %8.4f %8.4f %8.4f\n",
- j,
- 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);
+ 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:
- printf("done\n");
- usleep(100000);
+
/* free the mixinfo */
if (mi != NULL)
free(mi);
+
if (dev != -1)
close(dev);
+
exit(0);
}
diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c
index 94fabe87a..8b1dcc054 100644
--- a/apps/systemlib/mixer.c
+++ b/apps/systemlib/mixer.c
@@ -50,46 +50,54 @@
#include "mixer.h"
static int
-scale_check(struct MixScaler *scale)
+scale_check(struct scaler_s *scale)
{
- if (scale->offset > 1.0f)
+ if (scale->offset > 1.1f)
return 1;
- if (scale->offset < -1.0f)
+ if (scale->offset < -1.1f)
return 2;
if (scale->lower_limit > scale->upper_limit)
return 3;
- if (scale->lower_limit < -1.0f)
+ if (scale->lower_limit < -1.1f)
return 4;
- if (scale->upper_limit > 1.0f)
+ if (scale->upper_limit > 1.1f)
return 5;
return 0;
}
int
-mixer_check(struct MixMixer *mixer, unsigned control_count)
+mixer_check(struct mixer_s *mixer, unsigned group_count, unsigned control_count)
{
int ret;
- if (mixer->control_count < 1)
- return -1;
-
- if (mixer->control_count > control_count)
+ /* 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++) {
- if (mixer->control_scaler[i].control >= control_count)
+
+ /* 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);
}
@@ -97,8 +105,22 @@ mixer_check(struct MixMixer *mixer, unsigned control_count)
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 MixScaler *scaler, float input)
+scale(struct scaler_s *scaler, float input)
{
float output;
@@ -120,19 +142,24 @@ scale(struct MixScaler *scaler, float input)
}
float
-mixer_mix(struct MixMixer *mixer, float *controls)
+mixer_mix(struct mixer_s *mixer, float **controls)
{
- struct MixScaler *scaler;
float sum = 0.0f;
for (unsigned i = 0; i < mixer->control_count; i++) {
- scaler = &mixer->control_scaler[i];
- sum += scale(scaler, controls[scaler->control]);
+
+ 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)
{
@@ -141,37 +168,52 @@ mixer_getline(int fd, char *line, unsigned maxlen)
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 */
puts("line too long");
return -1;
}
static int
-mixer_load_scaler(const char *buf, struct MixScaler *scaler)
+mixer_load_scaler(const char *buf, struct scaler_s *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)
+ 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 MixMixer **mp)
+mixer_load(int fd, struct mixer_s **mp)
{
int ret, result = -1;
- struct MixMixer *mixer = NULL;
+ struct mixer_s *mixer = NULL;
char buf[100];
unsigned scalers;
@@ -194,7 +236,7 @@ mixer_load(int fd, struct MixMixer **mp)
/* allocate mixer */
scalers--;
- mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
+ mixer = (struct mixer_s *)malloc(MIXER_SIZE(scalers));
if (mixer == NULL)
goto out;
@@ -211,40 +253,56 @@ mixer_load(int fd, struct MixMixer **mp)
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 MixScaler *scaler)
+mixer_save_scaler(char *buf, struct scaler_s *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 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 MixMixer *mixer)
+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;
@@ -252,6 +310,7 @@ mixer_save(int fd, struct MixMixer *mixer)
/* write the output scaler */
len = mixer_save_scaler(buf, &mixer->output_scaler);
write(fd, buf, len);
+
if (ret != len)
return -1;
@@ -259,9 +318,11 @@ mixer_save(int fd, struct MixMixer *mixer)
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;
} \ No newline at end of file
diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h
index d65041ea7..3a9e31bb1 100644
--- a/apps/systemlib/mixer.h
+++ b/apps/systemlib/mixer.h
@@ -36,7 +36,7 @@
/**
* @file mixer.h
- *
+ *
* Generic control value mixing library.
*
* This library implements a generic mixer function that can be used
@@ -46,10 +46,13 @@
* Terminology
* ===========
*
- * control
+ * 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.
*
@@ -124,49 +127,68 @@
* 3 | primary thrust
*/
-struct MixScaler {
- unsigned control; /**< control consumed by this scaler */
+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 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 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 MixMixer) + ((_num_scalers) * sizeof(struct MixScaler)))
+#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 is assumed to be sufficiently large for any control
- * index in the mixer.
+ * 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 input control values.
+ * @param controls Array of pointers to control group values.
* @return The mixed output.
*/
-__EXPORT float mixer_mix(struct MixMixer *mixer, float *controls);
+__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 control_count The number of controls in the system.
+ * @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 MixMixer *mixer, unsigned control_count);
+__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.
@@ -178,13 +200,16 @@ __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count);
* scalers.
*
* M: <scaler count>
- * S: <control> <negative_scale> <positive_scale> <offset> <lower_limit> <upper_limit>
+ * S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*>
* S: ...
*
- * The <control> value for the output scaler is ignored by the mixer.
+ * 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.
+ * 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
@@ -194,7 +219,7 @@ __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count);
* @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);
+__EXPORT int mixer_load(int fd, struct mixer_s **mixer);
/**
* Save a mixer definition to a file.
@@ -203,7 +228,7 @@ __EXPORT int mixer_load(int fd, struct MixMixer **mixer);
* @param mixer The mixer definition to save.
* @return Zero on success, negative on error.
*/
-__EXPORT int mixer_save(int fd, struct MixMixer *mixers);
+__EXPORT int mixer_save(int fd, struct mixer_s *mixers);
__END_DECLS
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 78708cdba..98e6c2987 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Common object definitions without a better home.
+ * @file objects_common.h
+ *
+ * Common object definitions without a better home.
*/
#include <nuttx/config.h>
@@ -101,5 +103,8 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s);
#include "topics/actuator_controls.h"
-ORB_DEFINE(actuator_controls, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h
index 5cdaf0a4e..2b7d7de5d 100644
--- a/apps/uORB/topics/actuator_controls.h
+++ b/apps/uORB/topics/actuator_controls.h
@@ -32,11 +32,15 @@
****************************************************************************/
/**
- * @file Actuator control topic - mixer inputs.
+ * @file actuator_controls.h
*
- * Values published to this topic are the outputs of the vehicle control
+ * Actuator control topics - mixer inputs.
+ *
+ * Values published to these topics are the outputs of the vehicle control
* system, and are expected to be mixed and used to drive the actuators
* (servos, speed controls, etc.) that operate the vehicle.
+ *
+ * Each topic can be published by a single controller
*/
#ifndef TOPIC_ACTUATOR_CONTROLS_H
@@ -45,17 +49,24 @@
#include <stdint.h>
#include "../uORB.h"
-#define NUM_ACTUATOR_CONTROLS 16
+#define NUM_ACTUATOR_CONTROLS 8
+#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
-struct actuator_controls_s
-{
+struct actuator_controls_s {
float control[NUM_ACTUATOR_CONTROLS];
};
-ORB_DECLARE(actuator_controls);
+/* actuator control sets; this list can be expanded as more controllers emerge */
+ORB_DECLARE(actuator_controls_0);
+ORB_DECLARE(actuator_controls_1);
+ORB_DECLARE(actuator_controls_2);
+ORB_DECLARE(actuator_controls_3);
+
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
-struct actuator_armed_s
-{
+/** global 'actuator output is live' control. */
+struct actuator_armed_s {
bool armed;
};