diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/systemlib/mixer/mixer.cpp | 27 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer.h | 23 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_group.cpp | 15 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_load.c | 15 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_load.h | 2 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 10 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_simple.cpp | 62 | ||||
-rw-r--r-- | src/systemcmds/mixer/mixer.cpp | 2 | ||||
-rw-r--r-- | src/systemcmds/tests/test_mixer.cpp | 129 |
9 files changed, 229 insertions, 56 deletions
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b4b82c539..cce46bf5f 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -116,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler) return 0; } +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + /****************************************************************************/ NullMixer::NullMixer() : diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 6c322ff92..723bf9f3b 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -212,6 +212,24 @@ protected: */ static int scale_check(struct mixer_scaler_s &scaler); + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char * findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char * skipline(const char *buf, unsigned &buflen); + private: }; @@ -241,6 +259,11 @@ public: void reset(); /** + * Count the mixers in the group. + */ + unsigned count(); + + /** * Adds mixers to the group based on a text description in a buffer. * * Mixer definitions begin with a single capital letter and a colon. diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1dbc512cd..3ed99fba0 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space) return index; } +unsigned +MixerGroup::count() +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr)) { + mixer = mixer->_next; + index++; + } + + return index; +} + void MixerGroup::groups_required(uint32_t &groups) { @@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; + debug("SUCCESS - buflen: %d", buflen); } else { /* diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index 18c4e474a..a55ddf8a3 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -38,22 +38,22 @@ */ #include <nuttx/config.h> -#include <systemlib/err.h> #include <string.h> #include <stdio.h> #include <ctype.h> #include "mixer_load.h" -int load_mixer_file(const char *fname, char *buf) +int load_mixer_file(const char *fname, char *buf, unsigned maxlen) { FILE *fp; char line[120]; /* open the mixer definition file */ fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); + if (fp == NULL) { + return 1; + } /* read valid lines from the file into a buffer */ buf[0] = '\0'; @@ -70,7 +70,7 @@ int load_mixer_file(const char *fname, char *buf) /* compact whitespace in the buffer */ char *t, *f; - for (f = buf; *f != '\0'; f++) { + for (f = line; *f != '\0'; f++) { /* scan for space characters */ if (*f == ' ') { /* look for additional spaces */ @@ -87,8 +87,9 @@ int load_mixer_file(const char *fname, char *buf) } /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + return 1; + } /* add the line to the buffer */ strcat(buf, line); diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h index b2327a4f7..4b7091d5b 100644 --- a/src/modules/systemlib/mixer/mixer_load.h +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -44,7 +44,7 @@ __BEGIN_DECLS -__EXPORT int load_mixer_file(const char *fname, char *buf); +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); __END_DECLS diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 89afc272c..b89f341b6 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } if (used > (int)buflen) { - debug("multirotor spec used %d of %u", used, buflen); + debug("OVERFLOW: multirotor spec used %d of %u", used, buflen); return nullptr; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index c8434f991..c3985b5de 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -55,7 +55,7 @@ #include "mixer.h" #define debug(fmt, args...) do { } while(0) -// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer() free(_info); } -static const char * -findtag(const char *buf, unsigned &buflen, char tag) -{ - while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) - return buf; - buf++; - buflen--; - } - return nullptr; -} - -static void -skipline(const char *buf, unsigned &buflen) -{ - const char *p; - - /* if we can find a CR or NL in the buffer, skip up to it */ - if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) - buflen -= (p - buf); -} - int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { @@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } scaler.negative_scale = s[0] / 10000.0f; scaler.positive_scale = s[1] / 10000.0f; @@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale buf = findtag(buf, buflen, 'S'); if ((buf == nullptr) || (buflen < 16)) { - debug("contorl parser failed finding tag, ret: '%s'", buf); + debug("control parser failed finding tag, ret: '%s'", buf); return -1; } @@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale debug("control parse failed on '%s'", buf); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } control_group = u[0]; control_index = u[1]; @@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c int used; const char *end = buf + buflen; - /* enforce that the mixer ends with space or a new line */ - for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') - continue; - - /* require a space or newline at the end of the buffer, fail on printable chars */ - if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { - /* found a line ending or space, so no split symbols / numbers. good. */ - break; - } else { - debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); - goto out; - } - - } - /* 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; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index b1ebebbb4..6da39d371 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0]); + load_mixer_file(fname, &buf[0], sizeof(buf)); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acb7bd88f..4da86042d 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -46,6 +46,7 @@ #include <unistd.h> #include <fcntl.h> #include <errno.h> +#include <string.h> #include <time.h> #include <systemlib/err.h> @@ -53,6 +54,11 @@ #include "tests.h" +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -66,9 +72,128 @@ int test_mixer(int argc, char *argv[]) char buf[2048]; - load_mixer_file(filename, &buf[0]); + load_mixer_file(filename, &buf[0], sizeof(buf)); + unsigned loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + /* load the mixer in chunks, like + * in the case of a remote load, + * e.g. on PX4IO. + */ + + unsigned nused = 0; + + const unsigned chunk_size = 64; + + MixerGroup mixer_group(mixer_callback, 0); + + /* load at once test */ + unsigned xx = loaded; + mixer_group.load_from_buf(&buf[0], xx); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; + + unsigned empty_load = 2; + char empty_buf[2]; + empty_buf[0] = ' '; + empty_buf[1] = '\0'; + mixer_group.reset(); + mixer_group.load_from_buf(&empty_buf[0], empty_load); + warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); + if (empty_load != 0) + return 1; + + /* FIRST mark the mixer as invalid */ + bool mixer_ok = false; + /* THEN actually delete it */ + mixer_group.reset(); + char mixer_text[256]; /* large enough for one mixer */ + unsigned mixer_text_length = 0; + + unsigned transmitted = 0; + + warnx("transmitted: %d, loaded: %d", transmitted, loaded); + + while (transmitted < loaded) { + + unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted; + + /* check for overflow - this would be really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + bool mixer_ok = false; + return 1; + } + + /* append mixer text and nul-terminate */ + memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); + + /* if anything was parsed */ + if (resid != mixer_text_length) { + + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + mixer_ok = true; + } else { + /* not yet reached the end of the mixer, set as not ok */ + mixer_ok = false; + } + + warnx("used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } + + transmitted += text_length; + } + + warnx("chunked load: loaded %u mixers", mixer_group.count()); + + if (mixer_group.count() != 8) + return 1; + + /* load multirotor at once test */ + mixer_group.reset(); + + if (argc > 2) + filename = argv[2]; + else + filename = "/etc/mixers/FMU_quad_w.mix"; + + load_mixer_file(filename, &buf[0], sizeof(buf)); + loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + unsigned mc_loaded = loaded; + mixer_group.load_from_buf(&buf[0], mc_loaded); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; +} + +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) +{ + if (control_group != 0) + return -1; - warnx("loaded: %s", &buf[0]); + control = 0.0f; return 0; } |