diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2012-10-30 18:30:35 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2012-10-30 18:30:35 +0100 |
commit | ee1e98babb3b46de0c64dbd0c5be3c678fc7c727 (patch) | |
tree | 64d899e3ba2fbb75c06427f22be6178dd68a34ac | |
parent | e716bd02ce378918c1f2a31a6586a94f48bcb226 (diff) | |
parent | edd2715f84532f6c4c748cc97f0fe8a2982aa885 (diff) | |
download | px4-firmware-ee1e98babb3b46de0c64dbd0c5be3c678fc7c727.tar.gz px4-firmware-ee1e98babb3b46de0c64dbd0c5be3c678fc7c727.tar.bz2 px4-firmware-ee1e98babb3b46de0c64dbd0c5be3c678fc7c727.zip |
Merge branch 'master' of https://github.com/PX4/Firmware into fw_control
33 files changed, 984 insertions, 347 deletions
diff --git a/ROMFS/scripts/rc.FMU_quad_x b/ROMFS/scripts/rc.FMU_quad_x index e9f07b4a2..d9c9a8457 100644 --- a/ROMFS/scripts/rc.FMU_quad_x +++ b/ROMFS/scripts/rc.FMU_quad_x @@ -29,7 +29,7 @@ echo "[init] commander" commander start echo "[init] attitude control" -attitude_estimator_bm & +attitude_estimator_ekf start multirotor_att_control start echo "[init] starting PWM output" diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index 13c759db2..b5a087433 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -11,6 +11,16 @@ echo "[init] doing PX4IO startup..." uorb start # +# Init the EEPROM +# +echo "[init] eeprom" +eeprom start +if [ -f /eeprom/parameters ] +then + param load +fi + +# # Start the sensors. # sh /etc/init.d/rc.sensors @@ -23,17 +33,12 @@ mavlink start -d /dev/ttyS0 -b 57600 # # Start the commander. # -# XXX this should be '<command> start'. -# -commander & +commander start # # Start the attitude estimator # -# XXX this should be '<command> start'. -# -attitude_estimator_bm & -#position_estimator & +attitude_estimator_ekf start # # Configure PX4FMU for operation with PX4IO @@ -45,9 +50,7 @@ px4fmu start # # Start the fixed-wing controller # -# XXX this should be '<command> start'. -# -fixedwing_control & +fixedwing_control start # # Fire up the PX4IO interface. @@ -57,9 +60,7 @@ px4io start # # Start looking for a GPS. # -# XXX this should not need to be backgrounded -# -gps -d /dev/ttyS3 -m all & +gps start # # Start logging to microSD if we can diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 382d8e25c..d23f03417 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -59,9 +59,9 @@ multirotor_att_control start ardrone_interface start # -# Start logging +# Start logging to microSD if we can # -#sdlog start +#sh /etc/init.d/rc.logging # # Start GPS capture diff --git a/ROMFS/scripts/rc.logging b/ROMFS/scripts/rc.logging index cbc303dab..09c2d00d1 100644 --- a/ROMFS/scripts/rc.logging +++ b/ROMFS/scripts/rc.logging @@ -5,6 +5,5 @@ if [ -d /fs/microsd ] then - # XXX this should be '<command> start'. - # sdlog & + sdlog start fi diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index b5fbfe0f5..4152494e0 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -83,7 +83,7 @@ else # # Are we attached to a PX4IOAR (AR.Drone carrier board)? # - if boardinfo -t 7 + if boardinfo test name PX4IOAR then set BOARD PX4IOAR if [ -f /etc/init.d/rc.PX4IOAR ] @@ -99,7 +99,7 @@ else # # Are we attached to a PX4IO? # - if boardinfo -t 6 + if boardinfo test name PX4IO then set BOARD PX4IO if [ -f /etc/init.d/rc.PX4IO ] diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index a8ce3ea77..f96d901fb 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -117,7 +117,7 @@ int ardrone_interface_main(int argc, char *argv[]) ardrone_interface_task = task_spawn("ardrone_interface", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, - 4096, + 2048, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 4130b1c06..05a6292a2 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -150,7 +150,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 20000, + 12000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index d93a48e31..4b49201c7 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -308,7 +308,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) uint64_t calibration_interval = 45 * 1000 * 1000; /* maximum 2000 values */ - const unsigned int calibration_maxcount = 2000; + const unsigned int calibration_maxcount = 500; unsigned int calibration_counter = 0; /* limit update rate to get equally spaced measurements over time (in ms) */ @@ -353,12 +353,18 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) uint64_t axis_deadline = hrt_absolute_time(); uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - const char axislabels[3] = { 'Z', 'X', 'Y'}; + const char axislabels[3] = { 'X', 'Z', 'Y'}; int axis_index = -1; - float *x = malloc(sizeof(float) * calibration_maxcount); - float *y = malloc(sizeof(float) * calibration_maxcount); - float *z = malloc(sizeof(float) * calibration_maxcount); + float *x = (float*)malloc(sizeof(float) * calibration_maxcount); + float *y = (float*)malloc(sizeof(float) * calibration_maxcount); + float *z = (float*)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + return; + } tune_confirm(); sleep(2); @@ -1095,7 +1101,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 50, - 8000, + 9000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/drivers/device/cdev.cpp b/apps/drivers/device/cdev.cpp index d07d26e82..422321850 100644 --- a/apps/drivers/device/cdev.cpp +++ b/apps/drivers/device/cdev.cpp @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Character device base class. + * @file cdev.cpp + * + * Character device base class. */ #include "device.h" diff --git a/apps/drivers/device/device.cpp b/apps/drivers/device/device.cpp index c14a3234d..04a5222c3 100644 --- a/apps/drivers/device/device.cpp +++ b/apps/drivers/device/device.cpp @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Fundamental driver base class for the device framework. + * @file device.cpp + * + * Fundamental driver base class for the device framework. */ #include "device.h" diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h index 01692c315..7d375aab9 100644 --- a/apps/drivers/device/device.h +++ b/apps/drivers/device/device.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Definitions for the generic base classes in the device framework. + * @file device.h + * + * Definitions for the generic base classes in the device framework. */ #ifndef _DEVICE_DEVICE_H diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 4b832b548..56112d767 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Base class for devices attached via the I2C bus. + * @file i2c.cpp + * + * Base class for devices attached via the I2C bus. * * @todo Bus frequency changes; currently we do nothing with the value * that is supplied. Should we just depend on the bus knowing? diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index eb1b6cb05..3112e8e37 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Base class for devices connected via I2C. + * @file i2c.h + * + * Base class for devices connected via I2C. */ #ifndef _DEVICE_I2C_H diff --git a/apps/drivers/device/pio.cpp b/apps/drivers/device/pio.cpp index 5179752b5..f3a805a5e 100644 --- a/apps/drivers/device/pio.cpp +++ b/apps/drivers/device/pio.cpp @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Base class for devices accessed via PIO to registers. + * @file pio.cpp + * + * Base class for devices accessed via PIO to registers. */ #include "device.h" diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp index 528333e04..63c7c12aa 100644 --- a/apps/drivers/device/spi.cpp +++ b/apps/drivers/device/spi.cpp @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Base class for devices connected via SPI. + * @file spi.cpp + * + * Base class for devices connected via SPI. * * @todo Work out if caching the mode/frequency would save any time. * diff --git a/apps/drivers/device/spi.h b/apps/drivers/device/spi.h index e8c8e2c5e..d2d01efb3 100644 --- a/apps/drivers/device/spi.h +++ b/apps/drivers/device/spi.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Base class for devices connected via SPI. + * @file spi.h + * + * Base class for devices connected via SPI. */ #ifndef _DEVICE_SPI_H diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index 5b86dd920..b8d1fa0f0 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Generic GPIO ioctl interface. + * @file drv_gpio.h + * + * Generic GPIO ioctl interface. */ #ifndef _DRV_GPIO_H @@ -78,7 +80,7 @@ * Note that there may be board-specific relationships between GPIOs; * applications using GPIOs should be aware of this. */ -#define _GPIOCBASE 0x6700 +#define _GPIOCBASE 0x2700 #define GPIOC(_x) _IOC(_GPIOCBASE, _x) /** reset all board GPIOs to their default state */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 6fefbfafc..2b4fab151 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -1045,6 +1045,7 @@ int HMC5883::check_calibration() _calibrated = false; // XXX Notify system via uORB } + return 0; } int HMC5883::set_excitement(unsigned enable) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 0cdb53554..63c296f96 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -136,6 +136,12 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); + /* store last control mode to detect mode switches */ + bool flag_control_manual_enabled = false; + bool flag_control_attitude_enabled = false; + bool flag_system_armed = false; + bool man_yaw_zero_once = false; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -197,14 +203,28 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; - //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); } if (state.flag_control_attitude_enabled) { + + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_body = att.yaw + manual.yaw * -2.0f; + + /* only move setpoint if manual input is != 0 */ + // XXX turn into param + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.25f) { + att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f; + } else if (manual.throttle <= 0.25f) { + att_sp.yaw_body = att.yaw; + } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } @@ -251,6 +271,11 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } + /* update state */ + flag_control_attitude_enabled = state.flag_control_attitude_enabled; + flag_control_manual_enabled = state.flag_control_manual_enabled; + flag_system_armed = state.flag_system_armed; + perf_end(mc_loop_perf); } @@ -318,7 +343,7 @@ int multirotor_att_control_main(int argc, char *argv[]) mc_task = task_spawn("multirotor_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, - 6000, + 2048, mc_thread_main, NULL); exit(0); diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index eb94cca8a..003180c18 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -217,7 +217,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s att->roll, att->rollspeed, deltaT); /* control yaw rate */ - rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)); + rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed); rates_sp->thrust = att_sp->thrust; diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c new file mode 100644 index 000000000..d03a8d332 --- /dev/null +++ b/apps/px4/tests/test_bson.c @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * 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 tests_bson.c + * + * Tests for the bson en/decoder + */ + +#include <stdio.h> +#include <string.h> +#include <stdlib.h> + +#include <systemlib/err.h> +#include <systemlib/bson/tinybson.h> + +#include "tests.h" + +static const bool sample_bool = true; +static const int32_t sample_small_int = 123; +static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL; +static const double sample_double = 2.5f; +static const char *sample_string = "this is a test"; +static const uint8_t sample_data[256]; +//static const char *sample_filename = "/fs/microsd/bson.test"; + +static int +encode(bson_encoder_t encoder) +{ + if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0) + warnx("FAIL: encoder: append bool failed"); + if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0) + warnx("FAIL: encoder: append int failed"); + if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0) + warnx("FAIL: encoder: append int failed"); + if (bson_encoder_append_double(encoder, "double1", sample_double) != 0) + warnx("FAIL: encoder: append double failed"); + if (bson_encoder_append_string(encoder, "string1", sample_string) != 0) + warnx("FAIL: encoder: append string failed"); + if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0) + warnx("FAIL: encoder: append data failed"); + + bson_encoder_fini(encoder); + + return 0; +} + +static int +decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + unsigned len; + + if (!strcmp(node->name, "bool1")) { + if (node->type != BSON_BOOL) { + warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL); + return 1; + } + if (node->b != sample_bool) { + warnx("FAIL: decoder: bool1 value %s, expected %s", + (node->b ? "true" : "false"), + (sample_bool ? "true" : "false")); + return 1; + } + warnx("PASS: decoder: bool1"); + return 1; + } + if (!strcmp(node->name, "int1")) { + if (node->type != BSON_INT32) { + warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32); + return 1; + } + if (node->i != sample_small_int) { + warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int); + return 1; + } + warnx("PASS: decoder: int1"); + return 1; + } + if (!strcmp(node->name, "int2")) { + if (node->type != BSON_INT64) { + warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64); + return 1; + } + if (node->i != sample_big_int) { + warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int); + return 1; + } + warnx("PASS: decoder: int2"); + return 1; + } + if (!strcmp(node->name, "double1")) { + if (node->type != BSON_DOUBLE) { + warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); + return 1; + } + if (node->d != sample_double) { + warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); + return 1; + } + warnx("PASS: decoder: double1"); + return 1; + } + if (!strcmp(node->name, "string1")) { + if (node->type != BSON_STRING) { + warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING); + return 1; + } + + len = bson_decoder_data_pending(decoder); + + if (len != strlen(sample_string) + 1) { + warnx("FAIL: decoder: string1 length %d wrong, expected %d", len, strlen(sample_string) + 1); + return 1; + } + + char sbuf[len]; + + if (bson_decoder_copy_data(decoder, sbuf)) { + warnx("FAIL: decoder: string1 copy failed"); + return 1; + } + if (bson_decoder_data_pending(decoder) != 0) { + warnx("FAIL: decoder: string1 copy did not exhaust all data"); + return 1; + } + if (sbuf[len - 1] != '\0') { + warnx("FAIL: decoder: string1 not 0-terminated"); + return 1; + } + if (strcmp(sbuf, sample_string)) { + warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string); + return 1; + } + warnx("PASS: decoder: string1"); + return 1; + } + if (!strcmp(node->name, "data1")) { + if (node->type != BSON_BINDATA) { + warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA); + return 1; + } + + len = bson_decoder_data_pending(decoder); + + if (len != sizeof(sample_data)) { + warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data)); + return 1; + } + + if (node->subtype != BSON_BIN_BINARY) { + warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY); + return 1; + } + + uint8_t dbuf[len]; + + if (bson_decoder_copy_data(decoder, dbuf)) { + warnx("FAIL: decoder: data1 copy failed"); + return 1; + } + if (bson_decoder_data_pending(decoder) != 0) { + warnx("FAIL: decoder: data1 copy did not exhaust all data"); + return 1; + } + if (memcmp(sample_data, dbuf, len)) { + warnx("FAIL: decoder: data1 compare fail"); + return 1; + } + warnx("PASS: decoder: data1"); + return 1; + } + + if (node->type != BSON_EOO) + warnx("FAIL: decoder: unexpected node name '%s'", node->name); + return 1; +} + +static void +decode(bson_decoder_t decoder) +{ + int result; + + do { + result = bson_decoder_next(decoder); + } while (result > 0); +} + +int +test_bson(int argc, char *argv[]) +{ + struct bson_encoder_s encoder; + struct bson_decoder_s decoder; + void *buf; + int len; + + /* encode data to a memory buffer */ + if (bson_encoder_init_buf(&encoder, NULL, 0)) + errx(1, "FAIL: bson_encoder_init_buf"); + encode(&encoder); + len = bson_encoder_buf_size(&encoder); + if (len <= 0) + errx(1, "FAIL: bson_encoder_buf_len"); + buf = bson_encoder_buf_data(&encoder); + if (buf == NULL) + errx(1, "FAIL: bson_encoder_buf_data"); + + /* now test-decode it */ + if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) + errx(1, "FAIL: bson_decoder_init_buf"); + decode(&decoder); + free(buf); + + exit(0); +}
\ No newline at end of file diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c index f364ea080..3730272a2 100644 --- a/apps/px4/tests/test_hrt.c +++ b/apps/px4/tests/test_hrt.c @@ -39,6 +39,7 @@ #include <nuttx/config.h> #include <sys/types.h> +#include <sys/time.h> #include <stdio.h> #include <stdlib.h> diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c index a88e617d9..f5c26e9f3 100644 --- a/apps/px4/tests/test_uart_send.c +++ b/apps/px4/tests/test_uart_send.c @@ -112,7 +112,7 @@ int test_uart_send(int argc, char *argv[]) char sample_test_uart[25];// = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'}; - int i, r, n; + int i, n; uint64_t start_time = hrt_absolute_time(); diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h index 8dc9d34e5..1023f5f51 100644 --- a/apps/px4/tests/tests.h +++ b/apps/px4/tests/tests.h @@ -96,6 +96,7 @@ extern int test_time(int argc, char *argv[]); extern int test_uart_console(int argc, char *argv[]); extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); +extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); #endif /* __APPS_PX4_TESTS_H */ diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c index 9604710c5..b9f6835b0 100644 --- a/apps/px4/tests/tests_main.c +++ b/apps/px4/tests/tests_main.c @@ -109,6 +109,7 @@ struct { {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST, 0}, {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, {"param", test_param, 0, 0}, + {"bson", test_bson, 0, 0}, {"file", test_file, 0, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0}, {NULL, NULL, 0, 0} diff --git a/apps/px4/tests/tests_param.c b/apps/px4/tests/tests_param.c index 5ac9f0343..88eff30f1 100644 --- a/apps/px4/tests/tests_param.c +++ b/apps/px4/tests/tests_param.c @@ -73,8 +73,6 @@ test_param(int argc, char *argv[]) if ((uint32_t)val != 0xa5a5a5a5) errx(1, "parameter value mismatch after write"); - param_export(-1, false); - warnx("parameter test PASS"); return 0; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 9522bd31a..e4da687c6 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -543,9 +543,9 @@ Sensors::parameters_update() param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1])); param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2])); /* mag scaling */ - param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_scale[0])); - param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_scale[1])); - param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_scale[2])); + param_get(_parameter_handles.mag_scale[0], &(_parameters.mag_scale[0])); + param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1])); + param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c index bce8c27e7..2328ebdb2 100644 --- a/apps/systemcmds/boardinfo/boardinfo.c +++ b/apps/systemcmds/boardinfo/boardinfo.c @@ -37,231 +37,374 @@ * autopilot and carrier board information app */ - #include <nuttx/config.h> #include <unistd.h> #include <stdio.h> #include <string.h> #include <stdlib.h> -#include "systemlib/systemlib.h" +#include <nuttx/i2c.h> + +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/bson/tinybson.h> __EXPORT int boardinfo_main(int argc, char *argv[]); -/** - * Reads out the board information - * - * @param argc the number of string arguments (including the executable name) - * @param argv the argument strings - * - * @return 0 on success, 1 on error - */ -int boardinfo_main(int argc, char *argv[]) +struct eeprom_info_s { - const char *commandline_usage = "\tusage: boardinfo [-c|-f] [-t id] [-w \"<info>\"]\n"; - - bool carrier_mode = false; - bool fmu_mode = false; - bool write_mode = false; - char *write_string = 0; - bool silent = false; - bool test_enabled = false; - int test_boardid = -1; - int ch; - - while ((ch = getopt(argc, argv, "cft:w:v")) != -1) { - switch (ch) { - case 'c': - carrier_mode = true; - break; - - case 'f': - fmu_mode = true; - break; - - case 't': - test_enabled = true; - test_boardid = strtol(optarg, NULL, 10); - silent = true; - break; - - case 'w': - write_mode = true; - write_string = optarg; - break; + unsigned bus; + unsigned address; + unsigned page_size; + unsigned page_count; + unsigned page_write_delay; +}; + +/* XXX currently code below only supports 8-bit addressing */ +const struct eeprom_info_s eeprom_info[] = { + {3, 0x57, 8, 16, 3300}, +}; + +struct board_parameter_s { + const char *name; + bson_type_t type; +}; + +const struct board_parameter_s board_parameters[] = { + {"name", BSON_STRING}, /* ascii board name */ + {"vers", BSON_INT32}, /* board version (major << 8) | minor */ + {"date", BSON_INT32}, /* manufacture date */ + {"build", BSON_INT32} /* build code (fab << 8) | tester */ +}; + +const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); + +static int +eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) +{ + int result = -1; - default: - printf(commandline_usage); - exit(0); - } + struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); + if (dev == NULL) { + warnx("failed to init bus %d for EEPROM", eeprom->bus); + goto out; } + I2C_SETFREQUENCY(dev, 400000); - /* Check if write is required - only one mode is allowed then */ - if (write_mode && fmu_mode && carrier_mode) { - fprintf(stderr, "[boardinfo] Please choose only one mode for writing: --carrier or --fmu\n"); - printf(commandline_usage); - return ERROR; - } + /* loop until all data has been transferred */ + for (unsigned address = 0; address < size; ) { - /* Write FMU information - if (fmu_mode && write_mode) { - struct fmu_board_info_s info; - int ret = fmu_store_board_info(&info); + uint8_t pagebuf[eeprom->page_size + 1]; + /* how many bytes available to transfer? */ + unsigned count = size - address; - if (ret == OK) { - printf("[boardinfo] Successfully wrote FMU board info\n"); - } else { - fprintf(stderr, "[boardinfo] ERROR writing board info to FMU EEPROM, aborting\n"); - return ERROR; - } - }*/ + /* constrain writes to the page size */ + if (count > eeprom->page_size) + count = eeprom->page_size; - /* write carrier board info */ - if (carrier_mode && write_mode) { + pagebuf[0] = address & 0xff; + memcpy(pagebuf + 1, buf + address, count); - struct carrier_board_info_s info; - bool parse_ok = true; - unsigned parse_idx = 0; - //int maxlen = strlen(write_string); - char *curr_char; + struct i2c_msg_s msgv[1] = { + { + .addr = eeprom->address, + .flags = 0, + .buffer = pagebuf, + .length = count + 1 + } + }; - /* Parse board info string */ - if (write_string[0] != 'P' || write_string[1] != 'X' || write_string[2] != '4') { - fprintf(stderr, "[boardinfo] header must start with 'PX4'\n"); - parse_ok = false; + result = I2C_TRANSFER(dev, msgv, 1); + if (result != OK) { + warnx("EEPROM write failed: %d", result); + goto out; } + usleep(eeprom->page_write_delay); + address += count; + } - info.header[0] = 'P'; info.header[1] = 'X'; info.header[2] = '4'; - parse_idx = 3; - /* Copy board name */ +out: + if (dev != NULL) + up_i2cuninitialize(dev); + return result; +} - int i = 0; +static int +eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) +{ + int result = -1; - while (write_string[parse_idx] != 0x20 && (parse_idx < sizeof(info.board_name) + sizeof(info.header))) { - info.board_name[i] = write_string[parse_idx]; - i++; parse_idx++; + struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); + if (dev == NULL) { + warnx("failed to init bus %d for EEPROM", eeprom->bus); + goto out; + } + I2C_SETFREQUENCY(dev, 400000); + + /* loop until all data has been transferred */ + for (unsigned address = 0; address < size; ) { + + /* how many bytes available to transfer? */ + unsigned count = size - address; + + /* constrain transfers to the page size (bus anti-hog) */ + if (count > eeprom->page_size) + count = eeprom->page_size; + + uint8_t addr = address; + struct i2c_msg_s msgv[2] = { + { + .addr = eeprom->address, + .flags = 0, + .buffer = &addr, + .length = 1 + }, + { + .addr = eeprom->address, + .flags = I2C_M_READ, + .buffer = buf + address, + .length = count + } + }; + + result = I2C_TRANSFER(dev, msgv, 2); + if (result != OK) { + warnx("EEPROM read failed: %d", result); + goto out; } + address += count; + } - /* Enforce null termination */ - info.board_name[sizeof(info.board_name) - 1] = '\0'; +out: + if (dev != NULL) + up_i2cuninitialize(dev); + return result; +} - curr_char = write_string + parse_idx; +static void * +idrom_read(const struct eeprom_info_s *eeprom) +{ + uint32_t size = 0xffffffff; + int result; + void *buf = NULL; + + result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); + if (result != 0) { + warnx("failed reading ID ROM length"); + goto fail; + } + if (size > (eeprom->page_size * eeprom->page_count)) { + warnx("ID ROM not programmed"); + goto fail; + } - /* Index is now on next field */ - info.board_id = strtol(curr_char, &curr_char, 10);//atoi(write_string + parse_index); - info.board_version = strtol(curr_char, &curr_char, 10); + buf = malloc(size); + if (buf == NULL) { + warnx("could not allocate %d bytes for ID ROM", size); + goto fail; + } + result = eeprom_read(eeprom, buf, size); + if (result != 0) { + warnx("failed reading ID ROM"); + goto fail; + } + return buf; - /* Read in multi ports */ - for (i = 0; i < MULT_COUNT; i++) { - info.multi_port_config[i] = strtol(curr_char, &curr_char, 10); - } +fail: + if (buf != NULL) + free(buf); + return NULL; +} - /* Read in production data */ - info.production_year = strtol(curr_char, &curr_char, 10); +static void +boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) +{ + struct bson_encoder_s encoder; + int result = 1; + char *state, *token; + unsigned i; - if (info.production_year < 2012 || info.production_year > 3000) { - fprintf(stderr, "[boardinfo] production year is invalid: %d\n", info.production_year); - parse_ok = false; - } + /* create the encoder and make a writable copy of the spec */ + bson_encoder_init_buf(&encoder, NULL, 0); - info.production_month = strtol(curr_char, &curr_char, 10); + for (i = 0, token = strtok_r(spec, ",", &state); + token && (i < num_parameters); + i++, token = strtok_r(NULL, ",", &state)) { - if (info.production_month < 1 || info.production_month > 12) { - fprintf(stderr, "[boardinfo] production month is invalid: %d\n", info.production_month); - parse_ok = false; + switch (board_parameters[i].type) { + case BSON_STRING: + result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); + break; + case BSON_INT32: + result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); + break; + default: + result = 1; } - - info.production_day = strtol(curr_char, &curr_char, 10); - - if (info.production_day < 1 || info.production_day > 31) { - fprintf(stderr, "[boardinfo] production day is invalid: %d\n", info.production_day); - parse_ok = false; + if (result) { + warnx("bson append failed for %s<%s>", board_parameters[i].name, token); + goto out; } + } + bson_encoder_fini(&encoder); + if (i != num_parameters) { + warnx("incorrect parameter list, expected: \"<name>,<version><date>,<buildcode>\""); + result = 1; + goto out; + } + if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { + warnx("data too large for EEPROM"); + result = 1; + goto out; + } + if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { + warnx("buffer length mismatch"); + result = 1; + goto out; + } + warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); + + result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); + if (result < 0) { + warnc(-result, "error writing EEPROM"); + result = 1; + } else { + result = 0; + } - info.production_fab = strtol(curr_char, &curr_char, 10); - info.production_tester = strtol(curr_char, &curr_char, 10); - - if (!parse_ok) { - /* Parsing failed */ - fprintf(stderr, "[boardinfo] failed parsing info string:\n\t%s\naborting\n", write_string); - return ERROR; - - } else { - int ret = carrier_store_board_info(&info); +out: + free(bson_encoder_buf_data(&encoder)); - /* Display result */ - if (ret == sizeof(info)) { - printf("[boardinfo] Successfully wrote carrier board info\n"); + exit(result); +} - } else { - fprintf(stderr, "[boardinfo] ERROR writing board info to carrier EEPROM (forgot to pull the WRITE_ENABLE line high?), aborting\n"); - return ERROR; - } - } +static int +boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) +{ + switch (node->type) { + case BSON_INT32: + printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); + break; + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + printf("%s: %s\n", node->name, buf); + break; } + case BSON_EOO: + break; + default: + warnx("unexpected node type %d", node->type); + break; + } + return 1; +} - /* Print FMU information */ - if (fmu_mode && !silent) { - struct fmu_board_info_s info; - int ret = fmu_get_board_info(&info); +static void +boardinfo_show(const struct eeprom_info_s *eeprom) +{ + struct bson_decoder_s decoder; + void *buf; + + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { + while (bson_decoder_next(&decoder) > 0) + ; + } else { + warnx("failed to init decoder"); + } + free(buf); + exit(0); +} - /* Print human readable name */ - if (ret == sizeof(info)) { - printf("[boardinfo] Autopilot:\n\t%s\n", info.header); +struct { + const char *property; + const char *value; +} test_args; - } else { - fprintf(stderr, "[boardinfo] ERROR loading board info from FMU, aborting\n"); - return ERROR; +static int +boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + /* reject nodes with non-matching names */ + if (strcmp(node->name, test_args.property)) + return 1; + + /* compare node values to check for a match */ + switch (node->type) { + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + + /* check for a match */ + if (!strcmp(test_args.value, buf)) { + return 2; } + break; } - /* print carrier information */ - if (carrier_mode && !silent) { - - struct carrier_board_info_s info; - int ret = carrier_get_board_info(&info); + case BSON_INT32: { + int32_t val = strtol(test_args.value, NULL, 0); - /* Print human readable name */ - if (ret == sizeof(info)) { - printf("[boardinfo] Carrier board:\n\t%s\n", info.header); - printf("\tboard id:\t\t%d\n", info.board_id); - printf("\tversion:\t\t%d\n", info.board_version); - - for (unsigned i = 0; i < MULT_COUNT; i++) { - printf("\tmulti port #%d:\t\t%s: function #%d\n", i, multiport_info.port_names[i], info.multi_port_config[i]); - } - - printf("\tproduction date:\t%d-%d-%d (fab #%d / tester #%d)\n", info.production_year, info.production_month, info.production_day, info.production_fab, info.production_tester); - - } else { - fprintf(stderr, "[boardinfo] ERROR loading board info from carrier EEPROM (errno #%d), aborting\n", -ret); - return ERROR; + /* check for a match */ + if (node->i == val) { + return 2; } + break; } - /* test for a specific carrier */ - if (test_enabled) { - - struct carrier_board_info_s info; - int ret = carrier_get_board_info(&info); - - if (ret != sizeof(info)) { - fprintf(stderr, "[boardinfo] no EEPROM for board %d\n", test_boardid); - exit(1); - } + default: + break; + } - if (info.board_id == test_boardid) { - printf("[boardinfo] Found carrier board with ID %d, test succeeded\n", info.board_id); - exit(0); + return 1; +} - } else { - /* exit silently with an error so we can test for multiple boards quietly */ - exit(1); - } +static void +boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) +{ + struct bson_decoder_s decoder; + void *buf; + int result = -1; + + if ((property == NULL) || (strlen(property) == 0) || + (value == NULL) || (strlen(value) == 0)) + errx(1, "missing property name or value"); + + test_args.property = property; + test_args.value = value; + + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { + do { + result = bson_decoder_next(&decoder); + } while (result == 1); + } else { + warnx("failed to init decoder"); } + free(buf); - return 0; + /* if we matched, we exit with zero success */ + exit((result == 2) ? 0 : 1); } +int +boardinfo_main(int argc, char *argv[]) +{ + if (!strcmp(argv[1], "set")) + boardinfo_set(&eeprom_info[0], argv[2]); + + if (!strcmp(argv[1], "show")) + boardinfo_show(&eeprom_info[0]); + if (!strcmp(argv[1], "test")) + boardinfo_test(&eeprom_info[0], argv[2], argv[3]); + + errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); +} diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c index 75578d2ec..321466f87 100644 --- a/apps/systemlib/bson/tinybson.c +++ b/apps/systemlib/bson/tinybson.c @@ -39,44 +39,74 @@ #include <unistd.h> #include <string.h> +#include <stdlib.h> #include <err.h> #include "tinybson.h" - #if 0 # define debug(fmt, args...) do { warnx("BSON: " fmt, ##args); } while(0) #else # define debug(fmt, args...) do { } while(0) #endif -#define CODER_CHECK(_c) do { if (_c->fd == -1) return -1; } while(0) -#define CODER_KILL(_c, _reason) do { debug("killed: %s", _reason); _c->fd = -1; return -1; } while(0) +#define CODER_CHECK(_c) do { if (_c->dead) { debug("coder dead"); return -1; }} while(0) +#define CODER_KILL(_c, _reason) do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0) + +static int +read_x(bson_decoder_t decoder, void *p, size_t s) +{ + CODER_CHECK(decoder); + + if (decoder->fd > -1) + return (read(decoder->fd, p, s) == (int)s) ? 0 : -1; + + if (decoder->buf != NULL) { + /* staged operations to avoid integer overflow for corrupt data */ + if (s >= decoder->bufsize) + CODER_KILL(decoder, "buffer too small for read"); + if ((decoder->bufsize - s) < decoder->bufpos) + CODER_KILL(decoder, "not enough data for read"); + memcpy(p, (decoder->buf + decoder->bufpos), s); + decoder->bufpos += s; + return 0; + } + debug("no source"); + return -1; +} static int read_int8(bson_decoder_t decoder, int8_t *b) { - return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1; + return read_x(decoder, b, sizeof(*b)); } static int read_int32(bson_decoder_t decoder, int32_t *i) { - return (read(decoder->fd, i, sizeof(*i)) == sizeof(*i)) ? 0 : -1; + return read_x(decoder, i, sizeof(*i)); +} + +static int +read_int64(bson_decoder_t decoder, int64_t *i) +{ + return read_x(decoder, i, sizeof(*i)); } static int read_double(bson_decoder_t decoder, double *d) { - return (read(decoder->fd, d, sizeof(*d)) == sizeof(*d)) ? 0 : -1; + return read_x(decoder, d, sizeof(*d)); } int -bson_decoder_init(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private) +bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private) { int32_t junk; decoder->fd = fd; + decoder->buf = NULL; + decoder->dead = false; decoder->callback = callback; decoder->private = private; decoder->nesting = 1; @@ -85,7 +115,42 @@ bson_decoder_init(bson_decoder_t decoder, int fd, bson_decoder_callback callback /* read and discard document size */ if (read_int32(decoder, &junk)) + CODER_KILL(decoder, "failed discarding length"); + + /* ready for decoding */ + return 0; +} + +int +bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private) +{ + int32_t len; + + /* argument sanity */ + if ((buf == NULL) || (callback == NULL)) + return -1; + + decoder->fd = -1; + decoder->buf = (uint8_t *)buf; + decoder->dead = false; + if (bufsize == 0) { + decoder->bufsize = *(uint32_t *)buf; + debug("auto-detected %u byte object", decoder->bufsize); + } else { + decoder->bufsize = bufsize; + } + decoder->bufpos = 0; + decoder->callback = callback; + decoder->private = private; + decoder->nesting = 1; + decoder->pending = 0; + decoder->node.type = BSON_UNDEFINED; + + /* read and discard document size */ + if (read_int32(decoder, &len)) CODER_KILL(decoder, "failed reading length"); + if ((len > 0) && (len > (int)decoder->bufsize)) + CODER_KILL(decoder, "document length larger than buffer"); /* ready for decoding */ return 0; @@ -95,6 +160,7 @@ int bson_decoder_next(bson_decoder_t decoder) { int8_t tbyte; + int32_t tint; unsigned nlen; CODER_CHECK(decoder); @@ -133,7 +199,9 @@ bson_decoder_next(bson_decoder_t decoder) debug("got type byte 0x%02x", decoder->node.type); /* EOO is special; it has no name/data following */ - if (decoder->node.type != BSON_EOO) { + if (decoder->node.type == BSON_EOO) { + decoder->node.name[0] = '\0'; + } else { /* get the node name */ nlen = 0; @@ -154,8 +222,20 @@ bson_decoder_next(bson_decoder_t decoder) debug("got name '%s'", decoder->node.name); switch (decoder->node.type) { - case BSON_INT: - if (read_int32(decoder, &decoder->node.i)) + case BSON_BOOL: + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on BSON_BOOL"); + decoder->node.b = (tbyte != 0); + break; + + case BSON_INT32: + if (read_int32(decoder, &tint)) + CODER_KILL(decoder, "read error on BSON_INT"); + decoder->node.i = tint; + break; + + case BSON_INT64: + if (read_int64(decoder, &decoder->node.i)) CODER_KILL(decoder, "read error on BSON_INT"); break; @@ -169,7 +249,6 @@ bson_decoder_next(bson_decoder_t decoder) case BSON_STRING: if (read_int32(decoder, &decoder->pending)) CODER_KILL(decoder, "read error on BSON_STRING length"); - break; case BSON_BINDATA: @@ -199,14 +278,10 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf) CODER_CHECK(decoder); - /* if data already copied, return zero bytes */ - if (decoder->pending == 0) - return 0; - - /* copy bytes per the node size */ - result = read(decoder->fd, buf, decoder->pending); + /* copy data */ + result = read_x(decoder, buf, decoder->pending); - if (result != decoder->pending) + if (result != 0) CODER_KILL(decoder, "read error on copy_data"); /* pending count is discharged */ @@ -221,24 +296,56 @@ bson_decoder_data_pending(bson_decoder_t decoder) } static int +write_x(bson_encoder_t encoder, const void *p, size_t s) +{ + CODER_CHECK(encoder); + + if (encoder->fd > -1) + return (write(encoder->fd, p, s) == (int)s) ? 0 : -1; + + /* do we need to extend the buffer? */ + while ((encoder->bufpos + s) > encoder->bufsize) { + if (!encoder->realloc_ok) + CODER_KILL(encoder, "fixed-size buffer overflow"); + + uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT); + if (newbuf == NULL) + CODER_KILL(encoder, "could not grow buffer"); + + encoder->buf = newbuf; + encoder->bufsize += BSON_BUF_INCREMENT; + debug("allocated %d bytes", BSON_BUF_INCREMENT); + } + + memcpy(encoder->buf + encoder->bufpos, p, s); + encoder->bufpos += s; + debug("appended %d bytes", s); + + return 0; +} + +static int write_int8(bson_encoder_t encoder, int8_t b) { - debug("write_int8 %d", b); - return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1; + return write_x(encoder, &b, sizeof(b)); } static int write_int32(bson_encoder_t encoder, int32_t i) { - debug("write_int32 %d", i); - return (write(encoder->fd, &i, sizeof(i)) == sizeof(i)) ? 0 : -1; + return write_x(encoder, &i, sizeof(i)); +} + +static int +write_int64(bson_encoder_t encoder, int64_t i) +{ + return write_x(encoder, &i, sizeof(i)); } static int write_double(bson_encoder_t encoder, double d) { - debug("write_double"); - return (write(encoder->fd, &d, sizeof(d)) == sizeof(d)) ? 0 : -1; + return write_x(encoder, &d, sizeof(d)); } static int @@ -247,16 +354,17 @@ write_name(bson_encoder_t encoder, const char *name) size_t len = strlen(name); if (len > BSON_MAXNAME) - return -1; + CODER_KILL(encoder, "node name too long"); - debug("write name '%s' len %d", name, len); - return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1; + return write_x(encoder, name, len + 1); } int -bson_encoder_init(bson_encoder_t encoder, int fd) +bson_encoder_init_file(bson_encoder_t encoder, int fd) { encoder->fd = fd; + encoder->buf = NULL; + encoder->dead = false; if (write_int32(encoder, 0)) CODER_KILL(encoder, "write error on document length"); @@ -265,6 +373,27 @@ bson_encoder_init(bson_encoder_t encoder, int fd) } int +bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize) +{ + encoder->fd = -1; + encoder->buf = (uint8_t *)buf; + encoder->bufpos = 0; + encoder->dead = false; + if (encoder->buf == NULL) { + encoder->bufsize = 0; + encoder->realloc_ok = true; + } else { + encoder->bufsize = bufsize; + encoder->realloc_ok = false; + } + + if (write_int32(encoder, 0)) + CODER_KILL(encoder, "write error on document length"); + + return 0; +} + +int bson_encoder_fini(bson_encoder_t encoder) { CODER_CHECK(encoder); @@ -272,17 +401,69 @@ bson_encoder_fini(bson_encoder_t encoder) if (write_int8(encoder, BSON_EOO)) CODER_KILL(encoder, "write error on document terminator"); + /* hack to fix up length for in-buffer documents */ + if (encoder->buf != NULL) { + int32_t len = bson_encoder_buf_size(encoder); + memcpy(encoder->buf, &len, sizeof(len)); + } + return 0; } int -bson_encoder_append_int(bson_encoder_t encoder, const char *name, int32_t value) +bson_encoder_buf_size(bson_encoder_t encoder) { CODER_CHECK(encoder); - if (write_int8(encoder, BSON_INT) || + if (encoder->fd > -1) + return -1; + + return encoder->bufpos; +} + +void * +bson_encoder_buf_data(bson_encoder_t encoder) +{ + /* note, no CODER_CHECK here as the caller has to clean up dead buffers */ + + if (encoder->fd > -1) + return NULL; + + return encoder->buf; +} + +int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_BOOL) || write_name(encoder, name) || - write_int32(encoder, value)) + write_int8(encoder, value ? 1 : 0)) + CODER_KILL(encoder, "write error on BSON_BOOL"); + + return 0; +} + +int +bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value) +{ + bool result; + + CODER_CHECK(encoder); + + /* use the smallest encoding that will hold the value */ + if (value == (int32_t)value) { + debug("encoding %lld as int32", value); + result = write_int8(encoder, BSON_INT32) || + write_name(encoder, name) || + write_int32(encoder, value); + } else { + debug("encoding %lld as int64", value); + result = write_int8(encoder, BSON_INT64) || + write_name(encoder, name) || + write_int64(encoder, value); + } + if (result) CODER_KILL(encoder, "write error on BSON_INT"); return 0; @@ -309,12 +490,12 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char CODER_CHECK(encoder); - len = strlen(string); + len = strlen(string) + 1; /* include trailing nul */ - if (write_int8(encoder, BSON_DOUBLE) || + if (write_int8(encoder, BSON_STRING) || write_name(encoder, name) || write_int32(encoder, len) || - write(encoder->fd, name, len + 1) != (int)(len + 1)) + write_x(encoder, string, len)) CODER_KILL(encoder, "write error on BSON_STRING"); return 0; @@ -329,7 +510,7 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary write_name(encoder, name) || write_int32(encoder, size) || write_int8(encoder, subtype) || - write(encoder->fd, data, size) != (int)(size)) + write_x(encoder, data, size)) CODER_KILL(encoder, "write error on BSON_BINDATA"); return 0; diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h index b6229dc50..666f8191a 100644 --- a/apps/systemlib/bson/tinybson.h +++ b/apps/systemlib/bson/tinybson.h @@ -59,9 +59,8 @@ typedef enum { BSON_BOOL = 8, BSON_DATE = 9, BSON_NULL = 10, - BSON_INT = 16, - BSON_TIMESTAMP = 17, - BSON_LONG = 18 + BSON_INT32 = 16, + BSON_INT64 = 18 } bson_type_t; typedef enum bson_binary_subtype { @@ -75,6 +74,11 @@ typedef enum bson_binary_subtype { #define BSON_MAXNAME 32 /** + * Buffer growth increment when writing to a buffer. + */ +#define BSON_BUF_INCREMENT 128 + +/** * Node structure passed to the callback. */ typedef struct bson_node_s { @@ -82,7 +86,7 @@ typedef struct bson_node_s { bson_type_t type; bson_binary_subtype_t subtype; union { - int32_t i; + int64_t i; double d; bool b; }; @@ -92,11 +96,21 @@ typedef struct bson_decoder_s *bson_decoder_t; /** * Node callback. + * + * The node callback function's return value is returned by bson_decoder_next. */ typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node); struct bson_decoder_s { + /* file reader state */ int fd; + + /* buffer reader state */ + uint8_t *buf; + size_t bufsize; + unsigned bufpos; + + bool dead; bson_decoder_callback callback; void *private; unsigned nesting; @@ -105,7 +119,7 @@ struct bson_decoder_s { }; /** - * Initialise the decoder. + * Initialise the decoder to read from a file. * * @param decoder Decoder state structure to be initialised. * @param fd File to read BSON data from. @@ -113,7 +127,21 @@ struct bson_decoder_s { * @param private Callback private data, stored in node. * @return Zero on success. */ -__EXPORT int bson_decoder_init(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private); +__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private); + +/** + * Initialise the decoder to read from a buffer in memory. + * + * @param decoder Decoder state structure to be initialised. + * @param buf Buffer to read from. + * @param bufsize Size of the buffer (BSON object may be smaller). May be + * passed as zero if the buffer size should be extracted from the + * BSON header only. + * @param callback Callback to be invoked by bson_decoder_next + * @param private Callback private data, stored in node. + * @return Zero on success. + */ +__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private); /** * Process the next node from the stream and invoke the callback. @@ -142,37 +170,104 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder); * Encoder state structure. */ typedef struct bson_encoder_s { + /* file writer state */ int fd; + /* buffer writer state */ + uint8_t *buf; + unsigned bufsize; + unsigned bufpos; + + bool realloc_ok; + bool dead; + } *bson_encoder_t; /** - * Initialze the encoder. + * Initialze the encoder for writing to a file. + * + * @param encoder Encoder state structure to be initialised. + * @param fd File to write to. + * @return Zero on success. */ -__EXPORT int bson_encoder_init(bson_encoder_t encoder, int fd); +__EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd); + +/** + * Initialze the encoder for writing to a buffer. + * + * @param encoder Encoder state structure to be initialised. + * @param buf Buffer pointer to use, or NULL if the buffer + * should be allocated by the encoder. + * @param bufsize Maximum buffer size, or zero for no limit. If + * the buffer is supplied, the size of the supplied buffer. + * @return Zero on success. + */ +__EXPORT int bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize); /** * Finalise the encoded stream. + * + * @param encoder The encoder to finalise. */ __EXPORT int bson_encoder_fini(bson_encoder_t encoder); /** + * Fetch the size of the encoded object; only valid for buffer operations. + */ +__EXPORT int bson_encoder_buf_size(bson_encoder_t encoder); + +/** + * Get a pointer to the encoded object buffer. + * + * Note that if the buffer was allocated by the encoder, it is the caller's responsibility + * to free this buffer. + */ +__EXPORT void *bson_encoder_buf_data(bson_encoder_t encoder); + +/** + * Append a boolean to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. + */ +__EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value); + +/** * Append an integer to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. */ -__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int32_t value); +__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value); /** * Append a double to the encoded stream + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. */ __EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value); /** * Append a string to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param string Nul-terminated C string. */ __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string); /** * Append a binary blob to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param subtype Binary data subtype. + * @param size Data size. + * @param data Buffer containing data to be encoded. */ __EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index eeb867f11..365bd3d19 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -452,7 +452,7 @@ param_reset(param_t param) /* if we found one, erase it */ if (s != NULL) { - int pos = utarry_eltidx(param_values, s); + int pos = utarray_eltidx(param_values, s); utarray_erase(param_values, pos, 1); } } @@ -489,7 +489,7 @@ param_export(int fd, bool only_unsaved) param_lock(); - bson_encoder_init(&encoder, fd); + bson_encoder_init_file(&encoder, fd); /* no modified parameters -> we are done */ if (param_values == NULL) { @@ -600,7 +600,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) */ switch (node->type) { - case BSON_INT: + case BSON_INT32: if (param_type(param) != PARAM_TYPE_INT32) { debug("unexpected type for '%s", node->name); goto out; @@ -680,7 +680,7 @@ param_import_internal(int fd, bool mark_saved) int result = -1; struct param_import_state state; - if (bson_decoder_init(&decoder, fd, param_import_callback, &state)) { + if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { debug("decoder init failed"); goto out; } diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 750e783f5..b596b0f0e 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -110,76 +110,3 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma return pid; } - -#define PX4_BOARD_ID_FMU (5) - -int fmu_get_board_info(struct fmu_board_info_s *info) -{ - /* Check which FMU version we're on */ - struct stat sb; - int statres; - - /* Copy version-specific fields */ - statres = stat("/dev/bma180", &sb); - - if (statres == OK) { - /* BMA180 indicates a v1.5-v1.6 board */ - strcpy(info->board_name, "FMU v1.6"); - info->board_version = 16; - - } else { - statres = stat("/dev/accel", &sb); - - if (statres == OK) { - /* MPU-6000 indicates a v1.7+ board */ - strcpy(info->board_name, "FMU v1.7"); - info->board_version = 17; - - } else { - /* If no BMA and no MPU is present, it is a v1.3 board */ - strcpy(info->board_name, "FMU v1.3"); - info->board_version = 13; - } - } - - /* Copy general FMU fields */ - memcpy(info->header, "PX4", 3); - info->board_id = PX4_BOARD_ID_FMU; - - return sizeof(struct fmu_board_info_s); -} - -int carrier_store_board_info(const struct carrier_board_info_s *info) -{ - int ret; - int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); - - if (fd < 0) fprintf(stderr, "[boardinfo carrier] ERROR opening carrier eeprom\n"); - - /* Enforce correct header */ - ret = write(fd, info, sizeof(struct carrier_board_info_s)); - //ret = write(fd, "PX4", 3); - close(fd); - - return ret; -} - -int carrier_get_board_info(struct carrier_board_info_s *info) -{ - int ret; - int fd = open("/dev/eeprom", O_RDONLY | O_NONBLOCK); - - if (fd < 0) - return -1; /* no board */ - - ret = read(fd, info, sizeof(struct carrier_board_info_s)); - - /* Enforce NUL termination of human-readable string */ - if (ret == sizeof(struct carrier_board_info_s)) { - info->board_name[sizeof(info->board_name) - 1] = '\0'; - } - - close(fd); - - return ret; -} diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h index f31c5cd1f..2c53c648b 100644 --- a/apps/systemlib/systemlib.h +++ b/apps/systemlib/systemlib.h @@ -117,11 +117,6 @@ struct __multiport_info { }; __EXPORT extern const struct __multiport_info multiport_info; -__EXPORT int carrier_store_board_info(const struct carrier_board_info_s *info); -__EXPORT int carrier_get_board_info(struct carrier_board_info_s *info); - -__EXPORT int fmu_get_board_info(struct fmu_board_info_s *info); - __END_DECLS #endif /* SYSTEMLIB_H_ */ |