diff options
-rw-r--r-- | apps/drivers/drv_mixer.h | 13 | ||||
-rw-r--r-- | apps/px4/fmu/fmu.cpp | 70 | ||||
-rw-r--r-- | apps/systemcmds/mixer/mixer.c | 74 | ||||
-rw-r--r-- | apps/systemlib/mixer.c | 119 | ||||
-rw-r--r-- | apps/systemlib/mixer.h | 65 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 9 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_controls.h | 27 |
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; }; |