diff options
Diffstat (limited to 'src/modules/systemlib')
-rw-r--r-- | src/modules/systemlib/mixer/mixer.cpp | 16 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 17 | ||||
-rw-r--r-- | src/modules/systemlib/mixer/mixer_simple.cpp | 44 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.c | 47 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 28 |
5 files changed, 125 insertions, 27 deletions
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index df0dfe838..b1bb1a66d 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -142,6 +142,22 @@ NullMixer * NullMixer::from_text(const char *buf, unsigned &buflen) { NullMixer *nm = nullptr; + 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 { + return nm; + } + + } if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { nm = new NullMixer; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 8ded0b05c..2446cc3fb 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -181,6 +181,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; 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]); + return nullptr; + } + + } 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); diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 07dc5f37f..c8434f991 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, @@ -98,14 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler { int ret; int s[5]; + int n = -1; buf = findtag(buf, buflen, 'O'); - if ((buf == nullptr) || (buflen < 12)) + if ((buf == nullptr) || (buflen < 12)) { + debug("output parser failed finding tag, ret: '%s'", buf); return -1; + } - if ((ret = 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' (got %d)", buf, ret); + if ((ret = sscanf(buf, "O: %d %d %d %d %d %n", + &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) { + debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); return -1; } skipline(buf, buflen); @@ -126,8 +129,10 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale int s[5]; buf = findtag(buf, buflen, 'S'); - if ((buf == nullptr) || (buflen < 16)) + if ((buf == nullptr) || (buflen < 16)) { + debug("contorl parser failed finding tag, ret: '%s'", buf); return -1; + } 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) { @@ -156,6 +161,22 @@ 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); @@ -173,22 +194,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c mixinfo->control_count = inputs; - if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) { + debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); 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)) + mixinfo->controls[i].control_index)) { + debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); goto out; + } + } sm = new SimpleMixer(control_cb, cb_handle, mixinfo); if (sm != nullptr) { mixinfo = nullptr; - debug("loaded mixer with %d inputs", inputs); + debug("loaded mixer with %d input(s)", inputs); } else { debug("could not allocate memory for mixer"); diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index c69de52b7..ccdb2ea38 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -48,6 +48,7 @@ #include <unistd.h> #include <systemlib/err.h> #include <errno.h> +#include <semaphore.h> #include <sys/stat.h> @@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static sem_t param_sem = { .semcount = 1 }; + /** lock the parameter store */ static void param_lock(void) { - /* XXX */ + //do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - /* XXX */ + //sem_post(¶m_sem); } /** assert that the parameter store is locked */ @@ -506,29 +509,59 @@ int param_save_default(void) { int result; + unsigned retries = 0; /* delete the file in case it exists */ struct stat buffer; if (stat(param_get_default_file(), &buffer) == 0) { - result = unlink(param_get_default_file()); + + do { + result = unlink(param_get_default_file()); + if (result != 0) { + retries++; + usleep(1000 * retries); + } + } while (result != OK && retries < 10); + if (result != OK) warnx("unlinking file %s failed.", param_get_default_file()); } /* create the file */ - int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + int fd; + + do { + /* do another attempt in case the unlink call is not synced yet */ + fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + if (fd < 0) { + retries++; + usleep(1000 * retries); + } + + } while (fd < 0 && retries < 10); if (fd < 0) { + warn("opening '%s' for writing failed", param_get_default_file()); return fd; } - result = param_export(fd, false); + do { + result = param_export(fd, false); + + if (result != OK) { + retries++; + usleep(1000 * retries); + } + + } while (result != 0 && retries < 10); + + close(fd); - if (result != 0) { + if (result != OK) { warn("error exporting parameters to '%s'", param_get_default_file()); - unlink(param_get_default_file()); + (void)unlink(param_get_default_file()); return result; } diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 739503ed4..77c952f52 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -167,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); - - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; + if (pid->ki > 0.0f) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + + } else { + if (!isfinite(i)) { + i = 0.0f; + } - } else { - if (!isfinite(i)) { - i = 0.0f; + pid->integral = i; + pid->saturated = 0; } - pid->integral = i; + } else { + i = 0.0f; pid->saturated = 0; } |