Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Checkpoint: more work in progress on mixer load/save

Browse Source
This commit is contained in:
px4dev
2012-08-05 02:09:11 -07:00
parent 145a6c4c49
commit 9804776a0c
8 changed files with 161 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/drv_mixer.h
+4
View File
@@ -80,11 +80,15 @@ struct MixInfo
* 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.
*/
#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer)
/**
* Copy a mixer from *(struct MixMixer *)arg to the device.
*
* If arg is zero, the mixer is deleted.
*/
#define MIXERIOCSETMIXER(_mixer) _MIXERIOC(0x40 + _mixer)
apps/px4/fmu/fmu.cpp
+18 -9
View File
@@ -110,13 +110,14 @@ FMUServo::FMUServo(Mode mode) :
_task_should_exit(false),
_armed(false)
{
for (unsigned i = 0; i < 4; i++)
for (unsigned i = 0; i < _max_actuators; i++)
_mixer[i] = nullptr;
}
FMUServo::~FMUServo()
{
if (_task != -1) {
/* task should wake up every 100ms or so at least */
_task_should_exit = true;
@@ -168,8 +169,6 @@ FMUServo::task_main_trampoline(int argc, char *argv[])
void
FMUServo::task_main()
{
log("ready");
/* configure for PWM output */
switch (_mode) {
case MODE_2PWM:
@@ -217,7 +216,7 @@ FMUServo::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
struct actuator_controls ac;
struct actuator_controls_s ac;
/* get controls */
orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
@@ -239,7 +238,7 @@ FMUServo::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
struct actuator_armed aa;
struct actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -329,6 +328,12 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
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;
@@ -350,10 +355,14 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case MIXERIOCSETMIXER(1):
case MIXERIOCSETMIXER(0):
channel = cmd - MIXERIOCGETMIXER(0);
channel = cmd - MIXERIOCSETMIXER(0);
/* caller- supplied mixer */
/* get the caller-supplied mixer and check */
mm = (struct MixMixer *)arg;
if (mixer_check(mm, NUM_ACTUATOR_CONTROLS)) {
ret = -EINVAL;
break;
}
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
@@ -382,13 +391,13 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
namespace {
enum PortMode {
PORT_MODE_UNSET = 0,
PORT_FULL_GPIO,
PORT_FULL_SERIAL,
PORT_FULL_PWM,
PORT_GPIO_AND_SERIAL,
PORT_PWM_AND_SERIAL,
PORT_PWM_AND_GPIO,
PORT_MODE_UNSET
};
PortMode g_port_mode;
@@ -476,7 +485,7 @@ fmu_new_mode(PortMode new_mode)
} // namespace
extern "C" int fmu_main(int argc, char *argv[]);
extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
int
fmu_main(int argc, char *argv[])
apps/systemcmds/mixer/Makefile
+1 -1
View File
@@ -37,6 +37,6 @@
APPNAME = mixer
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/systemcmds/mixer/mixer.c
+70 -86
View File
@@ -38,9 +38,9 @@
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <getopt.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <systemlib/mixer.h>
#include <drivers/drv_mixer.h>
@@ -48,80 +48,43 @@
__EXPORT int mixer_main(int argc, char *argv[]);
static void usage(void);
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);
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();
usage("missing command");
if (!strcmp(argv[1], "load")) {
todo = LOAD;
if (argc < 4)
usage("missing device or filename");
load(argv[2], argv[3]);
} else if (!strcmp(argv[1], "save")) {
todo = SAVE;
if (argc < 4)
usage("missing device or filename");
save(argv[2], argv[3]);
} else if (!strcmp(argv[1], "show")) {
todo = SHOW;
if (argc < 3)
usage("missing device name");
show(argv[2]);
} 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);
usage("unrecognised command");
}
return 0;
}
static void
usage(void)
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer {load|save|show} -d <device> [-f <filename>]\n");
fprintf(stderr, " mixer show <device>\n");
fprintf(stderr, " mixer {load|save} <device> [<filename>]\n");
exit(1);
}
@@ -132,7 +95,7 @@ load(const char *devname, const char *fname)
int dev = -1;
unsigned num_mixers = 0;
int ret, result = 1;
struct MixMixer *mixer;
struct MixMixer *mixer = NULL;
/* open the device */
if ((dev = open(devname, 0)) < 0) {
@@ -156,29 +119,42 @@ 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)
if (ret < 0) {
fprintf(stderr, "read for mixer %d failed\n", i);
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;
if (mixer != NULL) {
/* sanity check the mixer */
ret = mixer_check(mixer, 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;
}
}
/* 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:
@@ -201,18 +177,16 @@ getmixer(int dev, unsigned mixer_number, struct MixInfo **mip)
/* first-round initialisation */
if (mi == NULL) {
mi = (struct MixInfo *)malloc(MIXINFO_SIZE(0));
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");
if (ret < 0)
return -1;
}
/* did the mixer fit? */
if (mi->mixer.control_count <= mi->num_controls)
@@ -262,12 +236,16 @@ 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;
ret = getmixer(dev, i, &mi);
if (ret < 0)
goto out;
ret = mixer_save(defs, &mi->mixer);
mm = &mi->mixer;
if (ret < 0) {
if (errno != ENOENT)
goto out;
mm = NULL;
}
ret = mixer_save(defs, mm);
if (ret < 0)
goto out;
}
@@ -289,7 +267,7 @@ out:
static void
show(const char *devname)
{
struct MixInfo *mi;
struct MixInfo *mi = NULL;
int dev = -1;
unsigned num_mixers = 0;
int ret;
@@ -311,19 +289,23 @@ show(const char *devname)
for (unsigned i = 0; i < num_mixers; i++) {
ret = getmixer(dev, i, &mi);
if (ret < 0)
goto out;
if (ret < 0) {
if (errno != ENOENT)
goto out;
continue;
}
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",
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("%d: %f %f %f %f %f\n",
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,
@@ -333,6 +315,8 @@ show(const char *devname)
}
out:
printf("done\n");
usleep(100000);
/* free the mixinfo */
if (mi != NULL)
free(mi);
apps/systemlib/mixer.c
+53 -45
View File
@@ -53,19 +53,19 @@ static int
scale_check(struct MixScaler *scale)
{
if (scale->offset > 1.0f)
return -1;
return 1;
if (scale->offset > 1.0f)
return -1;
if (scale->offset < -1.0f)
return 2;
if (scale->lower_limit > scale->upper_limit)
return -1;
return 3;
if (scale->lower_limit < -1.0f)
return -1;
return 4;
if (scale->upper_limit > 1.0f)
return -1;
return 5;
return 0;
}
@@ -73,21 +73,25 @@ scale_check(struct MixScaler *scale)
int
mixer_check(struct MixMixer *mixer, unsigned control_count)
{
int ret;
if (mixer->control_count < 1)
return -1;
if (mixer->control_count > control_count)
return -1;
return -2;
if (!scale_check(&mixer->output_scaler))
return -1;
ret = scale_check(&mixer->output_scaler);
if (ret != 0)
return ret;
for (unsigned i = 0; i < mixer->control_count; i++) {
if (mixer->control_scaler[i].control >= control_count)
return -1;
return -3;
if (!scale_check(&mixer->control_scaler[i]))
return -1;
ret = scale_check(&mixer->control_scaler[i]);
if (ret != 0)
return (10 * i + ret);
}
return 0;
@@ -148,6 +152,7 @@ mixer_getline(int fd, char *line, unsigned maxlen)
*line++ = c;
}
/* line too long */
puts("line too long");
return -1;
}
@@ -184,32 +189,33 @@ mixer_load(int fd, struct MixMixer **mp)
if (sscanf(buf, "M: %u", &scalers) != 1)
goto out;
/* must have at least one scaler */
if (scalers < 1)
goto out;
/* if there are scalers, load them */
if (scalers > 0) {
/* allocate mixer */
scalers--;
mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
/* 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)))
if (mixer == NULL)
goto out;
if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
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;
}
}
result = 1;
@@ -237,23 +243,25 @@ mixer_save(int fd, struct MixMixer *mixer)
int len, ret;
/* write the mixer header */
len = sprintf(buf, "M: %u\n", mixer->control_count);
len = sprintf(buf, "M: %u\n", (mixer != NULL) ? mixer->control_count : 0);
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]);
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;
}
}
apps/systemlib/mixer.h
+9 -8
View File
@@ -161,8 +161,8 @@ __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.
* @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.
*/
@@ -177,17 +177,18 @@ __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count);
* scaler in the file is always the output scaler, followed by the input
* scalers.
*
* M: <control count + 1>
* M: <scaler count>
* S: <control> <negative_scale> <positive_scale> <offset> <lower_limit> <upper_limit>
* S: ...
*
* The <control> value for the output scaler is ignored.
* The <control> value for the output scaler is ignored by the mixer.
*
* 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.
* 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.
apps/uORB/objects_common.cpp
+4
View File
@@ -99,3 +99,7 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
#include "topics/vehicle_attitude_setpoint.h"
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_armed, struct actuator_armed_s);
apps/uORB/topics/actuator_controls.h
+2 -2
View File
@@ -47,14 +47,14 @@
#define NUM_ACTUATOR_CONTROLS 16
struct actuator_controls
struct actuator_controls_s
{
float control[NUM_ACTUATOR_CONTROLS];
};
ORB_DECLARE(actuator_controls);
struct actuator_armed
struct actuator_armed_s
{
bool armed;
};