forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
Lorenz Meier
commit
01932a2dc3
|
|
@ -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 ]
|
||||
|
|
|
|||
|
|
@ -32,7 +32,9 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Character device base class.
|
||||
* @file cdev.cpp
|
||||
*
|
||||
* Character device base class.
|
||||
*/
|
||||
|
||||
#include "device.h"
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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?
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -1045,6 +1045,7 @@ int HMC5883::check_calibration()
|
|||
_calibrated = false;
|
||||
// XXX Notify system via uORB
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int HMC5883::set_excitement(unsigned enable)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -39,6 +39,7 @@
|
|||
#include <nuttx/config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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";
|
||||
unsigned bus;
|
||||
unsigned address;
|
||||
unsigned page_size;
|
||||
unsigned page_count;
|
||||
unsigned page_write_delay;
|
||||
};
|
||||
|
||||
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;
|
||||
/* XXX currently code below only supports 8-bit addressing */
|
||||
const struct eeprom_info_s eeprom_info[] = {
|
||||
{3, 0x57, 8, 16, 3300},
|
||||
};
|
||||
|
||||
while ((ch = getopt(argc, argv, "cft:w:v")) != -1) {
|
||||
switch (ch) {
|
||||
case 'c':
|
||||
carrier_mode = true;
|
||||
break;
|
||||
struct board_parameter_s {
|
||||
const char *name;
|
||||
bson_type_t type;
|
||||
};
|
||||
|
||||
case 'f':
|
||||
fmu_mode = true;
|
||||
break;
|
||||
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 */
|
||||
};
|
||||
|
||||
case 't':
|
||||
test_enabled = true;
|
||||
test_boardid = strtol(optarg, NULL, 10);
|
||||
silent = true;
|
||||
break;
|
||||
const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]);
|
||||
|
||||
case 'w':
|
||||
write_mode = true;
|
||||
write_string = optarg;
|
||||
break;
|
||||
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;
|
||||
|
||||
/* 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;
|
||||
struct i2c_msg_s msgv[1] = {
|
||||
{
|
||||
.addr = eeprom->address,
|
||||
.flags = 0,
|
||||
.buffer = pagebuf,
|
||||
.length = count + 1
|
||||
}
|
||||
};
|
||||
|
||||
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 */
|
||||
if (fmu_mode && !silent) {
|
||||
struct fmu_board_info_s info;
|
||||
int ret = fmu_get_board_info(&info);
|
||||
|
||||
/* 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;
|
||||
out:
|
||||
if (dev != NULL)
|
||||
up_i2cuninitialize(dev);
|
||||
return result;
|
||||
}
|
||||
|
||||
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'");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 data */
|
||||
result = read_x(decoder, buf, decoder->pending);
|
||||
|
||||
/* copy bytes per the node size */
|
||||
result = read(decoder->fd, buf, decoder->pending);
|
||||
|
||||
if (result != decoder->pending)
|
||||
if (result != 0)
|
||||
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(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");
|
||||
|
|
@ -264,6 +372,27 @@ bson_encoder_init(bson_encoder_t encoder, int fd)
|
|||
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");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
bson_encoder_fini(bson_encoder_t 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;
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue