Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

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"

ROMFS/scripts/rc.PX4IO

@@ -10,6 +10,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.
 #
@@ -23,17 +33,12 @@ mavlink start -d /dev/ttyS0 -b 57600
 #
 # Start the commander.
 #
-# XXX this should be '<command> start'.
+commander start
-#
-commander &
 
 #
 # Start the attitude estimator
 #
-# XXX this should be '<command> start'.
+attitude_estimator_ekf start
-#
-attitude_estimator_bm &
-#position_estimator &
 
 #
 # 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 start
-#
-fixedwing_control &
 
 #
 # 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 start
-#
-gps -d /dev/ttyS3 -m all &
 
 #
 # Start logging to microSD if we can

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

ROMFS/scripts/rc.logging

@@ -5,6 +5,5 @@
 
 if [ -d /fs/microsd ]
 then
-	# XXX this should be '<command> start'.
+	sdlog start
-	# sdlog &
 fi

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 ]

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);

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);

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 *x = (float*)malloc(sizeof(float) * calibration_maxcount);
-	float *y = malloc(sizeof(float) * calibration_maxcount);
+	float *y = (float*)malloc(sizeof(float) * calibration_maxcount);
-	float *z = 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;

apps/drivers/device/cdev.cpp

@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Character device base class.
+ * @file cdev.cpp
+ *
+ * Character device base class.
  */
 
 #include "device.h"

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"

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

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?

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

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"

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.
  *

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

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 */

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)

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);

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;
 

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);
+}

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>

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();
 

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 */

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}

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;

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_scale[0], &(_parameters.mag_scale[0]));
-	param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_scale[1]));
+	param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1]));
-	param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_scale[2]));
+	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) {

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[]);
 
-/**
+struct eeprom_info_s
- * 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[])
 {
-	const char *commandline_usage = "\tusage: boardinfo [-c|-f] [-t id] [-w \"<info>\"]\n";
+	unsigned	bus;
+	unsigned	address;
+	unsigned	page_size;
+	unsigned	page_count;
+	unsigned	page_write_delay;
+};
 
-	bool carrier_mode = false;
+/* XXX currently code below only supports 8-bit addressing */
-	bool fmu_mode = false;
+const struct eeprom_info_s eeprom_info[] = {
-	bool write_mode = false;
+	{3, 0x57, 8, 16, 3300},
-	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) {
+struct board_parameter_s {
-		switch (ch) {
+	const char	*name;
-		case 'c':
+	bson_type_t	type;
-			carrier_mode = true;
+};
-			break;
 
-		case 'f':
+const struct board_parameter_s board_parameters[] = {
-			fmu_mode = true;
+	{"name",	BSON_STRING},	/* ascii board name */
-			break;
+	{"vers",	BSON_INT32},	/* board version (major << 8) | minor */
+	{"date",	BSON_INT32},	/* manufacture date */
+	{"build",	BSON_INT32}	/* build code (fab << 8) | tester */
+};
 
-		case 't':
+const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]);
-			test_enabled = true;
-			test_boardid = strtol(optarg, NULL, 10);
-			silent = true;
-			break;
 
-		case 'w':
+static int
-			write_mode = true;
+eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
-			write_string = optarg;
+{
-			break;
+	int result = -1;
 
-		default:
+	struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus);
-			printf(commandline_usage);
+	if (dev == NULL) {
-			exit(0);
+		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 */
+	/* loop until all data has been transferred */
-	if (write_mode && fmu_mode && carrier_mode) {
+	for (unsigned address = 0; address < size; ) {
-		fprintf(stderr, "[boardinfo] Please choose only one mode for writing: --carrier or --fmu\n");
-		printf(commandline_usage);
-		return ERROR;
-	}
 
-	/* Write FMU information
+		uint8_t pagebuf[eeprom->page_size + 1];
-	if (fmu_mode && write_mode) {
-		struct fmu_board_info_s info;
-		int ret = fmu_store_board_info(&info);
 
+		/* how many bytes available to transfer? */
+		unsigned count = size - address;
 
-		if (ret == OK) {
+		/* constrain writes to the page size */
-			printf("[boardinfo] Successfully wrote FMU board info\n");
+		if (count > eeprom->page_size)
-		} else {
+			count = eeprom->page_size;
-			fprintf(stderr, "[boardinfo] ERROR writing board info to FMU EEPROM, aborting\n");
-			return ERROR;
-		}
-	}*/
 
-	/* write carrier board info */
+		pagebuf[0] = address & 0xff;
-	if (carrier_mode && write_mode) {
+		memcpy(pagebuf + 1, buf + address, count);
 
-		struct carrier_board_info_s info;
+		struct i2c_msg_s msgv[1] = {
-		bool parse_ok = true;
+			{
-		unsigned parse_idx = 0;
+				.addr = eeprom->address,
-		//int maxlen = strlen(write_string);
+				.flags = 0,
-		char *curr_char;
+				.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;
-		}
-
-		info.header[0] = 'P'; info.header[1] = 'X'; info.header[2] = '4';
-		parse_idx = 3;
-		/* Copy board name */
-
-		int i = 0;
-
-		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++;
-		}
-
-		/* Enforce null termination */
-		info.board_name[sizeof(info.board_name) - 1] = '\0';
-
-		curr_char = write_string + parse_idx;
-
-		/* 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);
-
-		/* Read in multi ports */
-		for (i = 0; i < MULT_COUNT; i++) {
-			info.multi_port_config[i] = strtol(curr_char, &curr_char, 10);
-		}
-
-		/* Read in production data */
-		info.production_year = strtol(curr_char, &curr_char, 10);
-
-		if (info.production_year < 2012 || info.production_year > 3000) {
-			fprintf(stderr, "[boardinfo] production year is invalid: %d\n", info.production_year);
-			parse_ok = false;
-		}
-
-		info.production_month = strtol(curr_char, &curr_char, 10);
-
-		if (info.production_month < 1 || info.production_month > 12) {
-			fprintf(stderr, "[boardinfo] production month is invalid: %d\n", info.production_month);
-			parse_ok = false;
-		}
-
-		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;
-		}
-
-		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);
-
-			/* Display result */
-			if (ret == sizeof(info)) {
-				printf("[boardinfo] Successfully wrote carrier board info\n");
-
-			} else {
-				fprintf(stderr, "[boardinfo] ERROR writing board info to carrier EEPROM (forgot to pull the WRITE_ENABLE line high?), aborting\n");
-				return ERROR;
 			}
+		};
+
+		result = I2C_TRANSFER(dev, msgv, 1);
+		if (result != OK) {
+			warnx("EEPROM write failed: %d", result);
+			goto out;
 		}
+		usleep(eeprom->page_write_delay);
+		address += count;
 	}
 
-	/* Print FMU information */
+out:
-	if (fmu_mode && !silent) {
+	if (dev != NULL)
-		struct fmu_board_info_s info;
+		up_i2cuninitialize(dev);
-		int ret = fmu_get_board_info(&info);
+	return result;
-
-		/* Print human readable name */
-		if (ret == sizeof(info)) {
-			printf("[boardinfo] Autopilot:\n\t%s\n", info.header);
-
-		} else {
-			fprintf(stderr, "[boardinfo] ERROR loading board info from FMU, aborting\n");
-			return ERROR;
-		}
-	}
-
-	/* print carrier information */
-	if (carrier_mode && !silent) {
-
-		struct carrier_board_info_s info;
-		int ret = carrier_get_board_info(&info);
-
-		/* 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;
-		}
-	}
-
-	/* 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);
-		}
-
-		if (info.board_id == test_boardid) {
-			printf("[boardinfo] Found carrier board with ID %d, test succeeded\n", info.board_id);
-			exit(0);
-
-		} else {
-			/* exit silently with an error so we can test for multiple boards quietly */
-			exit(1);
-		}
-	}
-
-	return 0;
 }
 
+static int
+eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
+{
+	int result = -1;
 
+	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;
+	}
+
+out:
+	if (dev != NULL)
+		up_i2cuninitialize(dev);
+	return result;
+}
+
+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;
+	}
+
+	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;
+
+fail:
+	if (buf != NULL)
+		free(buf);
+	return NULL;
+}
+
+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;
+
+	/* create the encoder and make a writable copy of the spec */
+	bson_encoder_init_buf(&encoder, NULL, 0);
+
+	for (i = 0, token = strtok_r(spec, ",", &state);
+	     token && (i < num_parameters);
+	     i++, token = strtok_r(NULL, ",", &state)) {
+
+		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;
+		}
+		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;
+	}
+
+out:
+	free(bson_encoder_buf_data(&encoder));
+
+	exit(result);
+}
+
+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;
+}
+
+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);
+}
+
+struct {
+	const char *property;
+	const char *value;
+} test_args;
+
+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;
+	}
+
+	case BSON_INT32: {
+		int32_t val = strtol(test_args.value, NULL, 0);
+
+		/* check for a match */
+		if (node->i == val) {
+			return 2;
+		}
+		break;
+	}
+
+	default:
+		break;
+	}
+
+	return 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);
+
+	/* 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'");
+}

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_CHECK(_c)		do { if (_c->dead) { debug("coder dead"); return -1; }} while(0)
-#define CODER_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->fd = -1; 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:
+		case BSON_BOOL:
-			if (read_int32(decoder, &decoder->node.i))
+			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 */
+	/* copy data */
-	if (decoder->pending == 0)
+	result = read_x(decoder, buf, decoder->pending);
-		return 0;
 
-	/* copy bytes per the node size */
+	if (result != 0)
-	result = read(decoder->fd, buf, decoder->pending);
-
-	if (result != decoder->pending)
 		CODER_KILL(decoder, "read error on copy_data");
 
 	/* pending count is discharged */
@@ -220,25 +295,57 @@ bson_decoder_data_pending(bson_decoder_t decoder)
 	return decoder->pending;
 }
 
+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_x(encoder, &b, sizeof(b));
-	return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
 }
 
 static int
 write_int32(bson_encoder_t encoder, int32_t i)
 {
-	debug("write_int32 %d", i);
+	return write_x(encoder, &i, sizeof(i));
-	return (write(encoder->fd, &i, sizeof(i)) == sizeof(i)) ? 0 : -1;
+}
+
+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_x(encoder, &d, sizeof(d));
-	return (write(encoder->fd, &d, sizeof(d)) == sizeof(d)) ? 0 : -1;
 }
 
 static int
@@ -247,16 +354,38 @@ 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_x(encoder, name, len + 1);
-	return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -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");
+
+	return 0;
+}
+
+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");
@@ -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;

apps/systemlib/bson/tinybson.h

@@ -59,9 +59,8 @@ typedef enum {
 	BSON_BOOL = 8,
 	BSON_DATE = 9,
 	BSON_NULL = 10,
-	BSON_INT = 16,
+	BSON_INT32 = 16,
-	BSON_TIMESTAMP = 17,
+	BSON_INT64 = 18
-	BSON_LONG = 18
 } bson_type_t;
 
 typedef enum bson_binary_subtype {
@@ -74,6 +73,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.
  */
@@ -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);
 
 /**
- * Append an integer to the encoded stream.
+ * Fetch the size of the encoded object; only valid for buffer operations.
  */
-__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int32_t value);
+__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, 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);
 

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;
 	}

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;
-}

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_ */