diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c
index a12485423d036166d9e28f2a83e5d8407d556d63..fa6f6924d055f340c2d2550e8f80e2a3c4a42539 100644
--- a/src/modules/systemlib/bson/tinybson.c
+++ b/src/modules/systemlib/bson/tinybson.c
@@ -338,12 +338,35 @@ write_x(bson_encoder_t encoder, const void *p, size_t s)
{
CODER_CHECK(encoder);
- if (encoder->fd > -1) {
+ /* bson file encoder (non-buffered) */
+ if (encoder->fd > -1 && encoder->buf == NULL) {
return (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1;
}
/* do we need to extend the buffer? */
while ((encoder->bufpos + s) > encoder->bufsize) {
+
+ /* bson buffered file encoder */
+ if (encoder->fd > -1) {
+ // write to disk
+ debug("writing buffer (%d) to disk", encoder->bufpos);
+ int ret = BSON_WRITE(encoder->fd, encoder->buf, encoder->bufpos);
+
+ if (ret == encoder->bufpos) {
+ // reset buffer to beginning and continue
+ encoder->bufpos = 0;
+
+ if ((encoder->bufpos + s) > encoder->bufsize) {
+ CODER_KILL(encoder, "fixed-size buffer overflow");
+ }
+
+ break;
+
+ } else {
+ CODER_KILL(encoder, "file write error");
+ }
+ }
+
if (!encoder->realloc_ok) {
CODER_KILL(encoder, "fixed-size buffer overflow");
}
@@ -416,6 +439,23 @@ bson_encoder_init_file(bson_encoder_t encoder, int fd)
return 0;
}
+int
+bson_encoder_init_buf_file(bson_encoder_t encoder, int fd, void *buf, unsigned bufsize)
+{
+ encoder->fd = fd;
+ encoder->buf = (uint8_t *)buf;
+ encoder->bufpos = 0;
+ encoder->bufsize = bufsize;
+ encoder->dead = false;
+ encoder->realloc_ok = false;
+
+ if (write_int32(encoder, 0)) {
+ CODER_KILL(encoder, "write error on document length");
+ }
+
+ return 0;
+}
+
int
bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize)
{
@@ -449,6 +489,20 @@ bson_encoder_fini(bson_encoder_t encoder)
CODER_KILL(encoder, "write error on document terminator");
}
+ if (encoder->fd > -1 && encoder->buf != NULL) {
+ /* write final buffer to disk */
+ int ret = BSON_WRITE(encoder->fd, encoder->buf, encoder->bufpos);
+
+ if (ret != encoder->bufpos) {
+ CODER_KILL(encoder, "write error");
+ }
+
+ } else if (encoder->buf != NULL) {
+ /* update buffer length */
+ int32_t len = bson_encoder_buf_size(encoder);
+ memcpy(encoder->buf, &len, sizeof(len));
+ }
+
/* sync file */
if (encoder->fd > -1) {
BSON_FSYNC(encoder->fd);
diff --git a/src/modules/systemlib/bson/tinybson.h b/src/modules/systemlib/bson/tinybson.h
index 10c244e503c2aa72d93a1ca6d53df92969a5ea20..79b7e5a2ca82371170ad67a46bdcfe6c02227fae 100644
--- a/src/modules/systemlib/bson/tinybson.h
+++ b/src/modules/systemlib/bson/tinybson.h
@@ -193,6 +193,17 @@ typedef struct bson_encoder_s {
*/
__EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd);
+/**
+ * Initialze the encoder for writing to a file.
+ *
+ * @param encoder Encoder state structure to be initialised.
+ * @param fd File to write to.
+ * @param buf Buffer pointer to use, can't be NULL
+ * @param bufsize Supplied buffer size
+ * @return Zero on success.
+ */
+__EXPORT int bson_encoder_init_buf_file(bson_encoder_t encoder, int fd, void *buf, unsigned bufsize);
+
/**
* Initialze the encoder for writing to a buffer.
*
diff --git a/src/modules/systemlib/param/CMakeLists.txt b/src/modules/systemlib/param/CMakeLists.txt
index 3abfd318bcdf9117c0e8c8212deefd864ee324da..ee24a8927b9e9003c8521668f8ab27956b94312c 100644
--- a/src/modules/systemlib/param/CMakeLists.txt
+++ b/src/modules/systemlib/param/CMakeLists.txt
@@ -78,7 +78,13 @@ add_custom_command(OUTPUT ${parameters_xml}
--inject-xml ${CMAKE_CURRENT_SOURCE_DIR}/parameters_injected.xml
--overrides ${PARAM_DEFAULT_OVERRIDES}
#--verbose
- DEPENDS ${param_src_files} px_process_params.py px4params/srcparser.py px4params/srcscanner.py px4params/xmlout.py parameters_injected.xml
+ DEPENDS
+ ${param_src_files}
+ parameters_injected.xml
+ px4params/srcparser.py
+ px4params/srcscanner.py
+ px4params/xmlout.py
+ px_process_params.py
COMMENT "Generating parameters.xml"
)
add_custom_target(parameters_xml DEPENDS ${parameters_xml})
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 322b1fc5c3279b7cdbc8cbc004b63ff136da5a9b..ba3ed5803b4a1f5119183fced463a5be884debbc 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -76,7 +76,7 @@ static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters";
static char *param_user_file = NULL;
#if 0
-# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
+# define debug(fmt, args...) do { PX4_INFO(fmt, ##args); } while(0)
#else
# define debug(fmt, args...) do { } while(0)
#endif
@@ -950,28 +950,27 @@ param_get_default_file(void)
int
param_save_default(void)
{
- int res;
+ int res = PX4_ERROR;
#if !defined(FLASH_BASED_PARAMS)
- int fd;
const char *filename = param_get_default_file();
/* write parameters to temp file */
- fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
+ int fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
if (fd < 0) {
PX4_ERR("failed to open param file: %s", filename);
return ERROR;
}
- res = 1;
int attempts = 5;
while (res != OK && attempts > 0) {
res = param_export(fd, false);
attempts--;
- if (res != OK) {
+ if (res != PX4_OK) {
+ PX4_ERR("param_export failed, retrying %d", attempts);
lseek(fd, 0, SEEK_SET); // jump back to the beginning of the file
}
}
@@ -1047,7 +1046,7 @@ param_export(int fd, bool only_unsaved)
param_lock_reader();
uint8_t bson_buffer[256];
- bson_encoder_init_buf(&encoder, &bson_buffer, sizeof(bson_buffer));
+ bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer));
/* no modified parameters -> we are done */
if (param_values == NULL) {
@@ -1069,31 +1068,14 @@ param_export(int fd, bool only_unsaved)
const char *name = param_name(s->param);
const size_t size = param_size(s->param);
- // check remaining buffer size and commit to disk
- // total size = strlen(name) + 1 (null char) + param size + 1 (bson header) + 1 (bson end)
- // size is doubled (floats saved as doubles)
- const size_t total_size = strlen(name) + 2 * size + 3;
-
- if (encoder.bufpos > encoder.bufsize - total_size) {
- // write buffer to disk and continue
- int ret = write(fd, encoder.buf, encoder.bufpos);
-
- if (ret == encoder.bufpos) {
- // reset buffer to beginning and continue
- encoder.bufpos = 0;
-
- } else {
- PX4_ERR("param write error %d %d", ret, encoder.bufpos);
- goto out;
- }
- }
-
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32: {
const int32_t i = s->val.i;
+ debug("exporting: %s (%d) size: %d val: %d", name, s->param, size, i);
+
if (bson_encoder_append_int(&encoder, name, i)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
@@ -1104,6 +1086,8 @@ param_export(int fd, bool only_unsaved)
case PARAM_TYPE_FLOAT: {
const float f = s->val.f;
+ debug("exporting: %s (%d) size: %d val: %.3f", name, s->param, size, (double)f);
+
if (bson_encoder_append_double(&encoder, name, f)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
@@ -1138,14 +1122,8 @@ param_export(int fd, bool only_unsaved)
out:
if (result == 0) {
- result = bson_encoder_fini(&encoder);
-
- // write and finish
- if ((result != 0) || write(fd, encoder.buf, encoder.bufpos) != encoder.bufpos) {
- PX4_ERR("param write error");
-
- } else {
- fsync(fd);
+ if (bson_encoder_fini(&encoder) != PX4_OK) {
+ PX4_ERR("bson encoder finish failed");
}
}
@@ -1209,6 +1187,8 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
i = node->i;
v = &i;
+
+ debug("importing: %s (%d) = %d", node->name, param, i);
break;
case BSON_DOUBLE:
@@ -1220,6 +1200,8 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
f = node->d;
v = &f;
+
+ debug("importing: %s (%d) = %.3f", node->name, param, (double)f);
break;
case BSON_BINDATA: