diff options
author | px4dev <px4@purgatory.org> | 2012-12-29 00:01:04 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-12-29 00:01:04 -0800 |
commit | 35c82ff2fc63ab823770f9776e6b6a0f81cd4452 (patch) | |
tree | 86956f9f00f1270eafc921748ac024a7daa92f80 | |
parent | f0da789626c32695e670b55dab29283eed4a05c6 (diff) | |
download | px4-firmware-35c82ff2fc63ab823770f9776e6b6a0f81cd4452.tar.gz px4-firmware-35c82ff2fc63ab823770f9776e6b6a0f81cd4452.tar.bz2 px4-firmware-35c82ff2fc63ab823770f9776e6b6a0f81cd4452.zip |
Make mixer ioctls load from a memory buffer rather than a file. This is prep for uploading the memory buffer to IO to be processed there.
-rw-r--r-- | apps/drivers/drv_mixer.h | 7 | ||||
-rw-r--r-- | apps/drivers/hil/hil.cpp | 18 | ||||
-rw-r--r-- | apps/drivers/px4fmu/fmu.cpp | 17 | ||||
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 32 | ||||
-rw-r--r-- | apps/systemcmds/mixer/mixer.c | 68 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer.cpp | 88 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer.h | 140 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_group.cpp | 310 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_multirotor.cpp | 56 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_simple.cpp | 260 |
10 files changed, 534 insertions, 462 deletions
diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index 498ca9836..9f43015d9 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -100,10 +100,13 @@ struct mixer_simple_s { */ #define MIXERIOCADDSIMPLE _MIXERIOC(2) +/* _MIXERIOC(3) was deprecated */ +/* _MIXERIOC(4) was deprecated */ + /** - * Add mixers(s) from a the file in (const char *)arg + * Add mixer(s) from the buffer in (const char *)arg */ -#define MIXERIOCLOADFILE _MIXERIOC(4) +#define MIXERIOCLOADBUF _MIXERIOC(5) /* * XXX Thoughts for additional operations: diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 0780f3bb2..f227db1f7 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -577,21 +577,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCLOADFILE: { - const char *path = (const char *)arg; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); if (_mixers == nullptr) { ret = -ENOMEM; + } else { - debug("loading mixers from %s", path); - ret = _mixers->load_from_file(path); + ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { debug("mixer load failed with %d", ret); @@ -600,10 +598,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } - break; } + default: ret = -ENOTTY; break; diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index e21250727..a8d160117 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -544,23 +544,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCLOADFILE: { - const char *path = (const char *)arg; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); if (_mixers == nullptr) { ret = -ENOMEM; } else { - debug("loading mixers from %s", path); - ret = _mixers->load_from_file(path); + ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { debug("mixer load failed with %d", ret); @@ -569,7 +565,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } - break; } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index d185a1ac4..cb69856c8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -609,27 +609,29 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } break; - case MIXERIOCLOADFILE: { - MixerGroup *newmixers; - const char *path = (const char *)arg; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - /* allocate a new mixer group and load it from the file */ - newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - if (newmixers->load_from_file(path) != 0) { - delete newmixers; - ret = -EINVAL; - } + if (_mixers == nullptr) { + ret = -ENOMEM; - /* swap the new mixers in for the old */ - if (_mixers != nullptr) { - delete _mixers; - } + } else { - _mixers = newmixers; + ret = _mixers->load_from_buf(buf, buflen); + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; } - break; default: /* not a recognised value */ diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index 3f52bdbf1..8d73bfcc4 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -43,14 +43,17 @@ #include <unistd.h> #include <fcntl.h> #include <errno.h> +#include <ctype.h> +#include <nuttx/compiler.h> +#include <systemlib/err.h> #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> __EXPORT int mixer_main(int argc, char *argv[]); -static void usage(const char *reason); -static void load(const char *devname, const char *fname); +static void usage(const char *reason) noreturn_function; +static void load(const char *devname, const char *fname) noreturn_function; int mixer_main(int argc, char *argv[]) @@ -63,12 +66,9 @@ mixer_main(int argc, char *argv[]) usage("missing device or filename"); load(argv[2], argv[3]); - - } else { - usage("unrecognised command"); } - return 0; + usage("unrecognised command"); } static void @@ -79,34 +79,56 @@ usage(const char *reason) fprintf(stderr, "usage:\n"); fprintf(stderr, " mixer load <device> <filename>\n"); - /* XXX automatic setups for quad, etc. */ + /* XXX other useful commands? */ exit(1); } static void load(const char *devname, const char *fname) { - int dev = -1; - int ret, result = 1; + int dev; + FILE *fp; + char line[80]; + char buf[512]; /* open the device */ - if ((dev = open(devname, 0)) < 0) { - fprintf(stderr, "can't open %s\n", devname); - goto out; - } + if ((dev = open(devname, 0)) < 0) + err(1, "can't open %s\n", devname); - /* tell it to load the file */ - ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname); + /* reset mixers on the device */ + if (ioctl(dev, MIXERIOCRESET, 0)) + err(1, "can't reset mixers on %s", devname); - if (ret != 0) { - fprintf(stderr, "failed loading %s\n", fname); - } + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) + err(1, "can't open %s", fname); + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { - result = 0; -out: + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) + break; + + /* add the line to the buffer */ + strcat(buf, line); + } - if (dev != -1) - close(dev); + /* XXX pass the buffer to the device */ + int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); - exit(result); + if (ret < 0) + err(1, "error loading mixers from %s", fname); + exit(0); } diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 6c1bbe469..72f765d90 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -137,89 +137,15 @@ NullMixer::groups_required(uint32_t &groups) } -/****************************************************************************/ - -SimpleMixer::SimpleMixer(ControlCallback control_cb, - uintptr_t cb_handle, - mixer_simple_s *mixinfo) : - Mixer(control_cb, cb_handle), - _info(mixinfo) -{ -} - -SimpleMixer::~SimpleMixer() -{ - if (_info != nullptr) - free(_info); -} - -unsigned -SimpleMixer::mix(float *outputs, unsigned space) +NullMixer * +NullMixer::from_text(const char *buf, unsigned &buflen) { - float sum = 0.0f; - - if (_info == nullptr) - return 0; - - if (space < 1) - return 0; - - for (unsigned i = 0; i < _info->control_count; i++) { - float input; + NullMixer *nm = nullptr; - _control_cb(_cb_handle, - _info->controls[i].control_group, - _info->controls[i].control_index, - input); - - sum += scale(_info->controls[i].scaler, input); + if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { + nm = new NullMixer; + buflen -= 2; } - *outputs = scale(_info->output_scaler, sum); - return 1; -} - -void -SimpleMixer::groups_required(uint32_t &groups) -{ - for (unsigned i = 0; i < _info->control_count; i++) - groups |= 1 << _info->controls[i].control_group; -} - -int -SimpleMixer::check() -{ - int ret; - float junk; - - /* sanity that presumes that a mixer includes a control no more than once */ - /* max of 32 groups due to groups_required API */ - if (_info->control_count > 32) - return -2; - - /* validate the output scaler */ - ret = scale_check(_info->output_scaler); - - if (ret != 0) - return ret; - - /* validate input scalers */ - for (unsigned i = 0; i < _info->control_count; i++) { - - /* verify that we can fetch the control */ - if (_control_cb(_cb_handle, - _info->controls[i].control_group, - _info->controls[i].control_index, - junk) != 0) { - return -3; - } - - /* validate the scaler */ - ret = scale_check(_info->controls[i].scaler); - - if (ret != 0) - return (10 * i + ret); - } - - return 0; + return nm; } diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 94c179eba..0023dc380 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -234,50 +234,45 @@ public: void add_mixer(Mixer *mixer); /** - * Reads a mixer definition from a file and configures a corresponding - * group. + * Adds mixers to the group based on a text description in a buffer. * - * The mixer group must be empty when this function is called. + * Mixer definitions begin with a single capital letter and a colon. + * The actual format of the mixer definition varies with the individual + * mixers; they are summarised here, but see ROMFS/mixers/README for + * more details. * - * A mixer definition is a text representation of the configuration of a - * mixer. Definition lines begin with a capital letter followed by a colon. + * Null Mixer + * .......... + * + * The null mixer definition has the form: + * + * Z: + * + * Simple Mixer + * ............ + * + * A simple mixer definition begins with: + * + * M: <control count> + * O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> + * + * The definition continues with <control count> entries describing the control + * inputs and their scaling, in the form: + * + * S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> + * + * Multirotor Mixer + * ................ + * + * The multirotor mixer definition is a single line of the form: + * + * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband> * - * Null Mixer: - * - * Z: - * - * This mixer generates a constant zero output, and is normally used to - * skip over outputs that are not in use. - * - * Simple Mixer: - * - * M: <scaler count> - * S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*> - * S: ... - * - * The definition consists of a single-line header indicating the - * number of scalers and then one line defining each scaler. The first - * scaler in the file is always the output scaler, followed by the input - * scalers. - * - * 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. - * - * 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 path The mixer configuration file to read. + * @param buf The mixer configuration buffer. + * @param buflen The length of the buffer. * @return Zero on successful load, nonzero otherwise. */ - int load_from_file(const char *path); + int load_from_buf(const char *buf, unsigned buflen); private: Mixer *_first; /**< linked list of mixers */ @@ -294,6 +289,21 @@ public: NullMixer(); ~NullMixer() {}; + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new NullMixer instance, or nullptr + * if the text format is bad. + */ + static NullMixer *from_text(const char *buf, unsigned &buflen); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); }; @@ -318,6 +328,27 @@ public: mixer_simple_s *mixinfo); ~SimpleMixer(); + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new SimpleMixer instance, or nullptr + * if the text format is bad. + */ + static SimpleMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); @@ -330,10 +361,18 @@ public: * @return Zero if the mixer makes sense, nonzero otherwise. */ int check(); + protected: private: - mixer_simple_s *_info; + mixer_simple_s *_info; + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); + static int parse_control_scaler(const char *buf, + unsigned &buflen, + mixer_scaler_s &scaler, + uint8_t &control_group, + uint8_t &control_index); }; /** @@ -395,6 +434,27 @@ public: float deadband); ~MultirotorMixer(); + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new MultirotorMixer instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 19ce25553..af5569001 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -56,250 +56,6 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) -namespace -{ - -/** - * Effectively fdgets() with some extra smarts. - */ -static int -mixer_getline(int fd, char *line, unsigned maxlen) -{ - /* reduce line budget by 1 to account for terminal NUL */ - maxlen--; - - /* loop looking for a non-comment line */ - for (;;) { - int ret; - char c; - char *p = line; - - /* loop reading characters for this line */ - for (;;) { - ret = read(fd, &c, 1); - - /* on error or EOF, return same */ - if (ret <= 0) { - debug("read: EOF"); - return ret; - } - - /* ignore carriage returns */ - if (c == '\r') - continue; - - /* line termination */ - if (c == '\n') { - /* ignore malformed lines */ - if ((p - line) < 4) - break; - - if (line[1] != ':') - break; - - /* terminate line as string and return */ - *p = '\0'; - debug("read: '%s'", line); - return 1; - } - - /* if we have space, accumulate the byte and go on */ - if ((p - line) < maxlen) - *p++ = c; - } - } -} - -/** - * Parse an output scaler from the buffer. - */ -static int -mixer_parse_output_scaler(const char *buf, mixer_scaler_s &scaler) -{ - int s[5]; - - if (sscanf(buf, "O: %d %d %d %d %d", - &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) { - debug("scaler parse failed on '%s'", buf); - return -1; - } - - scaler.negative_scale = s[0] / 10000.0f; - scaler.positive_scale = s[1] / 10000.0f; - scaler.offset = s[2] / 10000.0f; - scaler.min_output = s[3] / 10000.0f; - scaler.max_output = s[4] / 10000.0f; - - return 0; -} - -/** - * Parse a control scaler from the buffer. - */ -static int -mixer_parse_control_scaler(const char *buf, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) -{ - 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) { - debug("scaler parse failed on '%s'", buf); - return -1; - } - - control_group = u[0]; - 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.min_output = s[3] / 10000.0f; - scaler.max_output = s[4] / 10000.0f; - - return 0; -} - -SimpleMixer * -mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, unsigned inputs) -{ - mixer_simple_s *mixinfo = nullptr; - char buf[60]; - int ret; - - /* let's assume we're going to read a simple mixer */ - mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); - mixinfo->control_count = inputs; - - /* first, get the output scaler */ - ret = mixer_getline(fd, buf, sizeof(buf)); - - if (ret < 1) { - debug("failed reading for output scaler"); - goto fail; - } - - if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) { - debug("failed parsing output scaler"); - goto fail; - } - - /* now get any inputs */ - for (unsigned i = 0; i < inputs; i++) { - ret = mixer_getline(fd, buf, sizeof(buf)); - - if (ret < 1) { - debug("failed reading for control scaler"); - goto fail; - } - - if (mixer_parse_control_scaler(buf, - mixinfo->controls[i].scaler, - mixinfo->controls[i].control_group, - mixinfo->controls[i].control_index)) { - debug("failed parsing control scaler"); - goto fail; - } - - debug("got control %d", i); - } - - /* XXX should be a factory that validates the mixinfo ... */ - return new SimpleMixer(control_cb, cb_handle, mixinfo); - -fail: - free(mixinfo); - return nullptr; -} - -MultirotorMixer * -mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf) -{ - MultirotorMixer::Geometry geometry; - char geomname[8]; - int s[4]; - - if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) { - debug("multirotor parse failed on '%s'", buf); - return nullptr; - } - - if (!strcmp(geomname, "4+")) { - geometry = MultirotorMixer::QUAD_PLUS; - - } else if (!strcmp(geomname, "4x")) { - geometry = MultirotorMixer::QUAD_X; - - } else if (!strcmp(geomname, "6+")) { - geometry = MultirotorMixer::HEX_PLUS; - - } else if (!strcmp(geomname, "6x")) { - geometry = MultirotorMixer::HEX_X; - - } else if (!strcmp(geomname, "8+")) { - geometry = MultirotorMixer::OCTA_PLUS; - - } else if (!strcmp(geomname, "8x")) { - geometry = MultirotorMixer::OCTA_X; - - } else { - debug("unrecognised geometry '%s'", geomname); - return nullptr; - } - - return new MultirotorMixer( - control_cb, - cb_handle, - geometry, - s[0] / 10000.0f, - s[1] / 10000.0f, - s[2] / 10000.0f, - s[3] / 10000.0f); -} - -int -mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer) -{ - int ret; - char buf[60]; - unsigned inputs; - - ret = mixer_getline(fd, buf, sizeof(buf)); - - /* end of file or error ?*/ - if (ret < 1) { - debug("getline %d", ret); - return ret; - } - - /* slot is empty - allocate a null mixer */ - if (buf[0] == 'Z') { - debug("got null mixer"); - mixer = new NullMixer(); - return 1; - } - - /* is it a simple mixer? */ - if (sscanf(buf, "M: %u", &inputs) == 1) { - debug("got simple mixer with %d inputs", inputs); - mixer = mixer_load_simple(control_cb, cb_handle, fd, inputs); - return (mixer == nullptr) ? -1 : 1; - } - - /* is it a multirotor mixer? */ - if (buf[0] == 'R') { - debug("got a multirotor mixer"); - mixer = mixer_load_multirotor(control_cb, cb_handle, buf); - return (mixer == nullptr) ? -1 : 1; - } - - /* we don't recognise the mixer type */ - debug("unrecognized mixer type '%c'", buf[0]); - return -1; -} - - -} // namespace - MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : Mixer(control_cb, cb_handle), _first(nullptr) @@ -358,44 +114,38 @@ MixerGroup::groups_required(uint32_t &groups) } int -MixerGroup::load_from_file(const char *path) +MixerGroup::load_from_buf(const char *buf, unsigned buflen) { - if (_first != nullptr) - return -1; - - int fd = open(path, O_RDONLY); - - if (fd < 0) { - debug("failed to open %s", path); - return -1; - } - - for (unsigned count = 0;; count++) { - int result; - Mixer *mixer; - - result = mixer_load(_control_cb, - _cb_handle, - fd, - mixer); - - /* error? */ - if (result < 0) { - debug("error"); - return -1; + int ret = -1; + const char *end = buf + buflen; + + /* loop until we have consumed the buffer */ + while (buflen > 0) { + Mixer *m = nullptr; + const char *p = end - buflen; + + /* use the next character as a hint to decide which mixer class to construct */ + switch (*p) { + case 'Z': + m = NullMixer::from_text(p, buflen); + break; + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + default: + /* it's probably junk or whitespace */ + break; } - - /* EOF or error */ - if (result < 1) { - printf("[mixer] loaded %u mixers\n", count); - debug("EOF"); - break; + if (m != nullptr) { + add_mixer(m); + ret = 0; + } else { + /* skip whitespace or junk in the buffer */ + buflen--; } - - debug("loaded mixer %p", mixer); - add_mixer(mixer); } - - close(fd); - return 0; + return ret; } diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index b5b25e532..233025b21 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -54,6 +54,9 @@ #include "mixer.h" +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + #define CW (-1.0f) #define CCW (1.0f) @@ -151,6 +154,59 @@ MultirotorMixer::~MultirotorMixer() { } +MultirotorMixer * +MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + MultirotorMixer::Geometry geometry; + char geomname[8]; + int s[4]; + int used; + + if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + if (used > (int)buflen) { + debug("multirotor spec used %d of %u", used, buflen); + return nullptr; + } + buflen -= used; + + if (!strcmp(geomname, "4+")) { + geometry = MultirotorMixer::QUAD_PLUS; + + } else if (!strcmp(geomname, "4x")) { + geometry = MultirotorMixer::QUAD_X; + + } else if (!strcmp(geomname, "6+")) { + geometry = MultirotorMixer::HEX_PLUS; + + } else if (!strcmp(geomname, "6x")) { + geometry = MultirotorMixer::HEX_X; + + } else if (!strcmp(geomname, "8+")) { + geometry = MultirotorMixer::OCTA_PLUS; + + } else if (!strcmp(geomname, "8x")) { + geometry = MultirotorMixer::OCTA_X; + + } else { + debug("unrecognised geometry '%s'", geomname); + return nullptr; + } + + debug("adding multirotor mixer '%s'", geomname); + + return new MultirotorMixer( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); +} + unsigned MultirotorMixer::mix(float *outputs, unsigned space) { diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp new file mode 100644 index 000000000..c0066ac8d --- /dev/null +++ b/apps/systemlib/mixer/mixer_simple.cpp @@ -0,0 +1,260 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_simple.cpp + * + * Simple summing mixer. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <ctype.h> + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +SimpleMixer::SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _info(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_info != nullptr) + free(_info); +} + +static const char * +skipspace(const char *p, unsigned &len) +{ + while (isspace(*p)) { + if (len == 0) + return nullptr; + len--; + p++; + } + return p; +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) +{ + int ret; + int s[5]; + int used; + + buf = skipspace(buf, buflen); + if (buflen < 16) + return -1; + + if ((ret = sscanf(buf, "O: %d %d %d %d %d%n", + &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) { + debug("scaler parse failed on '%s' (got %d)", buf, ret); + return -1; + } + buflen -= used; + + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +int +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +{ + unsigned u[2]; + int s[5]; + int used; + + buf = skipspace(buf, buflen); + if (buflen < 16) + return -1; + + if (sscanf(buf, "S: %u %u %d %d %d %d %d%n", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4], &used) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + buflen -= used; + + control_group = u[0]; + 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.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + unsigned inputs; + int used; + const char *end = buf + buflen; + + /* get the base info for the mixer */ + if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { + debug("simple parse failed on '%s'", buf); + goto out; + } + buflen -= used; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + mixinfo->control_count = inputs; + + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + goto out; + + for (unsigned i = 0; i < inputs; i++) { + if (parse_control_scaler(end - buflen, buflen, + mixinfo->controls[i].scaler, + mixinfo->controls[i].control_group, + mixinfo->controls[i].control_index)) + goto out; + } + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + if (sm != nullptr) { + mixinfo = nullptr; + debug("loaded mixer with %d inputs", inputs); + } else { + debug("could not allocate memory for mixer"); + } + +out: + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_info == nullptr) + return 0; + + if (space < 1) + return 0; + + for (unsigned i = 0; i < _info->control_count; i++) { + float input; + + _control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + input); + + sum += scale(_info->controls[i].scaler, input); + } + + *outputs = scale(_info->output_scaler, sum); + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _info->control_count; i++) + groups |= 1 << _info->controls[i].control_group; +} + +int +SimpleMixer::check() +{ + int ret; + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_info->control_count > 32) + return -2; + + /* validate the output scaler */ + ret = scale_check(_info->output_scaler); + + if (ret != 0) + return ret; + + /* validate input scalers */ + for (unsigned i = 0; i < _info->control_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_info->controls[i].scaler); + + if (ret != 0) + return (10 * i + ret); + } + + return 0; +} |