aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-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;
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-22 22:30:32 +0000