From 9804776a0c8bd67d4a533e3302f1a598c35b868b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 5 Aug 2012 02:09:11 -0700 Subject: Checkpoint: more work in progress on mixer load/save --- apps/drivers/drv_mixer.h | 4 + apps/px4/fmu/fmu.cpp | 27 ++++-- apps/systemcmds/mixer/Makefile | 2 +- apps/systemcmds/mixer/mixer.c | 156 ++++++++++++++++------------------- apps/systemlib/mixer.c | 92 +++++++++++---------- apps/systemlib/mixer.h | 17 ++-- apps/uORB/objects_common.cpp | 4 + apps/uORB/topics/actuator_controls.h | 4 +- 8 files changed, 158 insertions(+), 148 deletions(-) diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index 10cda792a..f3a39e60c 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -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) diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index ff47261e7..8e468c012 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -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[]) diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile index a83085bfc..b016ddc57 100644 --- a/apps/systemcmds/mixer/Makefile +++ b/apps/systemcmds/mixer/Makefile @@ -37,6 +37,6 @@ APPNAME = mixer PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index c5adf8dbf..bbbe67ff7 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -38,9 +38,9 @@ #include #include #include -#include #include #include +#include #include #include @@ -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; - } else { - usage(); - } - - while ((ch = getopt(argc, argv, "d:f:")) != EOF) { - switch (ch) { - case 'd': - devname = optarg; - break; - case 'f': - fname = optarg; - break; - default: - usage(); - } - } + if (argc < 3) + usage("missing device name"); + show(argv[2]); - 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); + } else { + 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 [-f ]\n"); + fprintf(stderr, " mixer show \n"); + fprintf(stderr, " mixer {load|save} []\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); diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c index cc7baebc2..94fabe87a 100644 --- a/apps/systemlib/mixer.c +++ b/apps/systemlib/mixer.c @@ -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; - - /* allocate mixer */ - scalers--; - mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers)); - - if (mixer == NULL) - goto out; + /* if there are scalers, load them */ + if (scalers > 0) { - mixer->control_count = scalers; + /* allocate mixer */ + scalers--; + mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers)); - ret = mixer_getline(fd, buf, sizeof(buf)); + if (mixer == NULL) + goto out; - if (ret < 1) - goto out; + mixer->control_count = scalers; - if (mixer_load_scaler(buf, &mixer->output_scaler)) - goto out; + ret = mixer_getline(fd, buf, sizeof(buf)); - for (unsigned i = 0; i < scalers; i++) { - if (mixer_getline(fd, buf, sizeof(buf))) + if (ret < 1) goto out; - if (mixer_load_scaler(buf, &mixer->control_scaler[i])) + + 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; -} +} \ No newline at end of file diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h index cd110f01e..d65041ea7 100644 --- a/apps/systemlib/mixer.h +++ b/apps/systemlib/mixer.h @@ -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: + * M: * S: * S: ... * - * The value for the output scaler is ignored. + * The 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. diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 8d84ff3c2..78708cdba 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -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); diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h index 03c0c7b7d..5cdaf0a4e 100644 --- a/apps/uORB/topics/actuator_controls.h +++ b/apps/uORB/topics/actuator_controls.h @@ -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; }; -- cgit v1.2.3