aboutsummaryrefslogtreecommitdiff
path: root/src/modules/systemlib
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/systemlib')
-rw-r--r--src/modules/systemlib/mixer/mixer.cpp16
-rw-r--r--src/modules/systemlib/mixer/mixer_multirotor.cpp17
-rw-r--r--src/modules/systemlib/mixer/mixer_simple.cpp44
-rw-r--r--src/modules/systemlib/param/param.c47
-rw-r--r--src/modules/systemlib/pid/pid.c28
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(&param_sem) != 0);
}
/** unlock the parameter store */
static void
param_unlock(void)
{
- /* XXX */
+ //sem_post(&param_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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-28 15:02:54 +0000