TEST test_entry_write_multiple(void)
{
uint32_t pos_a = test_logger.flash_write_pos;
cmp_ctx_t *ctx;
ctx = _log_entry_create(&test_logger, "a");
cmp_write_integer(ctx, 42);
_log_entry_write_to_flash(&test_logger);
uint32_t pos_b = test_logger.flash_write_pos;
ASSERT(pos_a < pos_b);
ctx = _log_entry_create(&test_logger, "b");
cmp_write_integer(ctx, 23);
_log_entry_write_to_flash(&test_logger);
cmp_ctx_t reader;
cmp_mem_access_t cma;
cmp_mem_access_ro_init(&reader, &cma, &flash_array[pos_a + LOG_ENTRY_HEADER_LEN], LOG_ENTRY_DATA_LEN);
uint32_t size = 0;
uint64_t timestamp = 0;
int64_t val = 0;
char name[10];
bool ok;
size = sizeof(name);
ok = true;
ok &= cmp_read_array(&reader, &size);
ok &= cmp_read_uinteger(&reader, ×tamp);
ok &= cmp_read_str(&reader, name, &size);
ok &= cmp_read_integer(&reader, &val);
ASSERT(ok);
ASSERT_EQ(val, 42);
size_t pos = cmp_mem_access_get_pos(&cma);
ASSERT_EQ(pos, flash_array[0]);
ASSERT_EQ(pos + LOG_ENTRY_HEADER_LEN, pos_b);
cmp_mem_access_ro_init(&reader, &cma, &flash_array[pos_b + LOG_ENTRY_HEADER_LEN], LOG_ENTRY_DATA_LEN);
size = sizeof(name);
ok = true;
ok &= cmp_read_array(&reader, &size);
ok &= cmp_read_uinteger(&reader, ×tamp);
ok &= cmp_read_str(&reader, name, &size);
ok &= cmp_read_integer(&reader, &val);
ASSERT(ok);
ASSERT_EQ(val, 23);
ASSERT_EQ(flash_array[pos_b], cmp_mem_access_get_pos(&cma));
PASS();
}
void scene_deserialize(Scene* scene, cmp_ctx_t* context) {
uint32_t key_count;
char key[32];
uint32_t key_len;
cmp_read_map(context, &key_count);
int k;
for(k = 0 ; k < key_count ; ++k) {
key_len = sizeof(key);
cmp_read_str(context, key, &key_len);
key[key_len] = 0;
if(strcmp("path", key) == 0) {
uint32_t path_len = 65;
cmp_read_str(context, scene->path, &path_len);
scene->path[path_len] = 0;
} else if (strcmp("entities", key) == 0) {
uint32_t entities_size;
cmp_read_array(context, &entities_size);
int i;
for(i = 0; i < entities_size ; ++i) {
Entity* entity = entitypool_add(scene->entities);
entity_deserialize(entity, scene, context);
}
} else if(strcmp("source", key) == 0) {
uint32_t source_len = 65;
char source[source_len];
cmp_read_str(context, source, &source_len);
}
}
}
int main(void) {
FILE *fh = NULL;
cmp_ctx_t cmp;
uint32_t array_size = 0;
uint32_t str_size = 0;
char hello[6] = {0, 0, 0, 0, 0, 0};
char message_pack[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
fh = fopen("cmp_data.dat", "w+b");
if (fh == NULL)
error_and_exit("Error opening data.dat");
cmp_init(&cmp, fh, file_reader, file_writer);
if (!cmp_write_array(&cmp, 2))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_str(&cmp, "Hello", 5))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_str(&cmp, "MessagePack", 11))
error_and_exit(cmp_strerror(&cmp));
rewind(fh);
if (!cmp_read_array(&cmp, &array_size))
error_and_exit(cmp_strerror(&cmp));
/* You can read the str byte size and then read str bytes... */
if (!cmp_read_str_size(&cmp, &str_size))
error_and_exit(cmp_strerror(&cmp));
if (str_size > (sizeof(hello) - 1))
error_and_exit("Packed 'hello' length too long\n");
if (!read_bytes(hello, str_size, fh))
error_and_exit(cmp_strerror(&cmp));
/*
* ...or you can set the maximum number of bytes to read and do it all in
* one call
*/
str_size = sizeof(message_pack);
if (!cmp_read_str(&cmp, message_pack, &str_size))
error_and_exit(cmp_strerror(&cmp));
printf("Array Length: %zu.\n", array_size);
printf("[\"%s\", \"%s\"]\n", hello, message_pack);
fclose(fh);
return EXIT_SUCCESS;
}
static void can_bridge_set_filter(cmp_ctx_t *ctx)
{
uint32_t id, mask, size;
bool rtr, ide;
if (!cmp_read_array(ctx, &size) || size != 6) {
return;
}
if (!cmp_read_bool(ctx, &ide)) {
return;
}
if (!cmp_read_bool(ctx, &rtr)) {
return;
}
if (!cmp_read_uint(ctx, &id)) {
return;
}
if (ide) {
id = (id & CAN_FRAME_EXT_ID_MASK) | CAN_FRAME_EXT_FLAG;
} else {
id = id & CAN_FRAME_STD_ID_MASK;
}
if (rtr) {
id |= CAN_FRAME_RTR_FLAG;
}
if (!cmp_read_bool(ctx, &ide)) {
return;
}
if (!cmp_read_bool(ctx, &rtr)) {
return;
}
if (!cmp_read_uint(ctx, &mask)) {
return;
}
if (ide) {
mask = (mask & CAN_FRAME_EXT_ID_MASK) | CAN_FRAME_EXT_FLAG;
} else {
mask = mask & CAN_FRAME_STD_ID_MASK;
}
if (rtr) {
mask |= CAN_FRAME_RTR_FLAG;
}
can_bridge_filter_id = id;
can_bridge_filter_mask = mask;
}
const void *rcservo_load_calibration(rcservo_t *s, const void *calib)
{
cmp_ctx_t cmp;
const void *readp = calib;
cmp_init(&cmp, &readp, cmp_read, NULL);
bool err = false;
uint32_t array_size;
err = err || !cmp_read_array(&cmp, &array_size);
err = err || (array_size != 6);
err = err || !cmp_read_u16(&cmp, &s->calib.gain_pos);
err = err || !cmp_read_u16(&cmp, &s->calib.gain_neg);
err = err || !cmp_read_u16(&cmp, &s->calib.zero);
err = err || !cmp_read_u16(&cmp, &s->calib.min);
err = err || !cmp_read_u16(&cmp, &s->calib.max);
err = err || !cmp_read_u8(&cmp, &s->calib.update_period);
if (err || (char *)calib + RC_SERVO_CALIBRATION_BUFFER_SIZE != readp) {
return NULL;
}
return readp;
}
TEST test_entry_header(void)
{
_log_entry_create(&test_logger, "entry");
cmp_ctx_t reader;
cmp_mem_access_t cma;
cmp_mem_access_ro_init(&reader, &cma, &test_logger.buffer[LOG_ENTRY_HEADER_LEN], LOG_ENTRY_DATA_LEN);
uint32_t size = 0;
if (!cmp_read_array(&reader, &size) || size != 3) {
FAIL();
}
uint64_t timestamp = 0;
if (!cmp_read_uinteger(&reader, ×tamp) || timestamp != logger_timestamp_sec()) {
FAIL();
}
char name[10];
size = sizeof(name);
if (!cmp_read_str(&reader, name, &size) || strcmp(name, "entry") != 0) {
FAIL();
}
PASS();
}
/** This callback shows how to read the several arguments. */
bool arg_read_cb(void *p, cmp_ctx_t *args_ctx, cmp_ctx_t *output_ctx)
{
bool result;
int arg;
char arg_name[64];
unsigned int arg_len;
uint32_t array_size = 0;
MockActualCall& state = mock().actualCall("arg_read_cb");
result = cmp_read_array(args_ctx, &array_size);
CHECK_TRUE(result);
CHECK_EQUAL(2, array_size);
result = cmp_read_int(args_ctx, &arg);
CHECK_TRUE(result);
state = state.withIntParameter("x", arg);
result = cmp_read_int(args_ctx, &arg);
CHECK_TRUE(result);
state = state.withIntParameter("y", arg);
return true;
}
TEST test_entry_write_and_readback(void)
{
cmp_ctx_t *ctx = _log_entry_create(&test_logger, "entry");
cmp_write_integer(ctx, 42);
_log_entry_write_to_flash(&test_logger);
uint8_t buf[LOG_ENTRY_DATA_LEN];
uint32_t next_entry = 0;
size_t len = 0;
if (!log_read_entry(0, buf, &len, &next_entry)) {
FAILm("CRC missmatch");
}
ASSERT_EQ(test_logger.flash_write_pos, next_entry);
// readback entry
cmp_ctx_t reader;
cmp_mem_access_t cma;
cmp_mem_access_ro_init(&reader, &cma, buf, sizeof(buf));
uint32_t size = 0;
if (!cmp_read_array(&reader, &size) || size != 3) {
FAIL();
}
uint64_t timestamp = 0;
if (!cmp_read_uinteger(&reader, ×tamp) || timestamp != logger_timestamp_sec()) {
FAIL();
}
char name[10];
size = sizeof(name);
if (!cmp_read_str(&reader, name, &size) || strcmp(name, "entry") != 0) {
FAIL();
}
int64_t data = 0;
if (!cmp_read_integer(&reader, &data) || data != 42) {
FAIL();
}
PASS();
}
int main(void) {
FILE *fh = NULL;
cmp_ctx_t cmp;
uint16_t year = 1983;
uint8_t month = 5;
uint8_t day = 28;
int64_t sint = 0;
uint64_t uint = 0;
float flt = 0.0f;
double dbl = 0.0;
bool boolean = false;
uint8_t fake_bool = 0;
uint32_t string_size = 0;
uint32_t array_size = 0;
uint32_t binary_size = 0;
uint32_t map_size = 0;
int8_t ext_type = 0;
uint32_t ext_size = 0;
char sbuf[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
fh = fopen("cmp_data.dat", "w+b");
if (fh == NULL)
error_and_exit("Error opening data.dat");
cmp_init(&cmp, fh, file_reader, file_skipper, file_writer);
/*
* When you write an array, you first specify the number of array
* elements, then you write that many elements.
*/
if (!cmp_write_array(&cmp, 9))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_sint(&cmp, -14))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_uint(&cmp, 38))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_float(&cmp, 1.8f))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_double(&cmp, 300.4))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_nil(&cmp))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_true(&cmp))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_false(&cmp))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_bool(&cmp, false))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_u8_as_bool(&cmp, 1))
error_and_exit(cmp_strerror(&cmp));
/* Array full */
/*
* Maps work similar to arrays, but the length is in "pairs", so this
* writes 2 pairs to the map. Subsequently, pairs are written in key,
* value order.
*/
if (!cmp_write_map(&cmp, 2))
error_and_exit(cmp_strerror(&cmp));
/* You can write string data all at once... */
if (!cmp_write_str(&cmp, "Greeting", 8))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_str(&cmp, "Hello", 5))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_str(&cmp, "Name", 4))
error_and_exit(cmp_strerror(&cmp));
/* ...or in chunks */
if (!cmp_write_str_marker(&cmp, 5))
error_and_exit(cmp_strerror(&cmp));
if (file_writer(&cmp, "Li", 2) != 2)
error_and_exit(strerror(errno));
if (file_writer(&cmp, "nus", 3) != 3)
error_and_exit(strerror(errno));
/* Map full */
/* Binary data functions the same as string data */
if (!cmp_write_bin(&cmp, "MessagePack", 11))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_write_bin_marker(&cmp, 8))
error_and_exit(cmp_strerror(&cmp));
if (file_writer(&cmp, "is ", 3) != 3)
error_and_exit(strerror(errno));
if (file_writer(&cmp, "great", 5) != 5)
error_and_exit(strerror(errno));
/*
* With extended types, you can create your own custom types. Here we
* create a simple date type.
*/
/* cmp_write_ext_marker(type, size) */
if (!cmp_write_ext_marker(&cmp, 1, 4))
error_and_exit(cmp_strerror(&cmp));
file_writer(&cmp, &year, sizeof(uint16_t));
file_writer(&cmp, &month, sizeof(uint8_t));
file_writer(&cmp, &day, sizeof(uint8_t));
/* Now we can read the data back just as easily */
rewind(fh);
if (!cmp_read_array(&cmp, &array_size))
error_and_exit(cmp_strerror(&cmp));
if (array_size != 9)
error_and_exit("Array size was not 9");
if (!cmp_read_sinteger(&cmp, &sint))
error_and_exit(cmp_strerror(&cmp));
if (sint != -14)
error_and_exit("Signed int was not -14");
if (!cmp_read_uinteger(&cmp, &uint))
error_and_exit(cmp_strerror(&cmp));
if (uint != 38)
error_and_exit("Unsigned int was not 38");
if (!cmp_read_float(&cmp, &flt))
error_and_exit(cmp_strerror(&cmp));
if (flt != 1.8f)
error_and_exit("Float was not 1.8f");
if (!cmp_read_double(&cmp, &dbl))
error_and_exit(cmp_strerror(&cmp));
if (dbl != 300.4)
error_and_exit("Double was not 300.f");
if (!cmp_read_nil(&cmp))
error_and_exit(cmp_strerror(&cmp));
if (!cmp_read_bool(&cmp, &boolean))
error_and_exit(cmp_strerror(&cmp));
if (boolean != true)
error_and_exit("First boolean was not true");
if (!cmp_read_bool(&cmp, &boolean))
error_and_exit(cmp_strerror(&cmp));
if (boolean != false)
error_and_exit("Second boolean was not false");
if (!cmp_read_bool(&cmp, &boolean))
error_and_exit(cmp_strerror(&cmp));
if (boolean != false)
error_and_exit("Third boolean was not false");
if (!cmp_read_bool_as_u8(&cmp, &fake_bool))
error_and_exit(cmp_strerror(&cmp));
if (fake_bool != 1) {
fprintf(stderr, "%u.\n", fake_bool);
error_and_exit("Third boolean (u8) was not 1");
}
if (!cmp_read_map(&cmp, &map_size))
error_and_exit(cmp_strerror(&cmp));
if (map_size != 2)
error_and_exit("Map size was not 2");
/*
* String reading here. Note that normally strings are encoded using
* UTF-8. I have cleverly restricted this example to ASCII, which overlaps
* UTF-8 encoding, but this must not be assumed in real-world code.
*
* You can read strings in two ways. Either you can read the string's size
* in bytes and then read the bytes manually...
*/
if (!cmp_read_str_size(&cmp, &string_size))
error_and_exit(cmp_strerror(&cmp));
if (string_size != 8)
error_and_exit("Greeting string key size was not 8");
if (!read_bytes(sbuf, 8, fh))
error_and_exit(strerror(errno));
sbuf[string_size] = 0;
if (strncmp(sbuf, "Greeting", 8) != 0)
error_and_exit("Greeting string key name was not 'Greeting'");
/*
* ...or you can set the maximum number of bytes to read and do it all in
* one call. cmp_read_str will write no more than "size" bytes, including
* the terminating NULL, to the passed buffer. If the string's size
* exceeds the passed buffer size, the "size" input is set to the number of
* bytes necessary, not including the terminating NULL. Otherwise, the
* "size" input is set to the number of bytes written, not including the
* terminating NULL.
*/
string_size = sizeof(sbuf);
if (!cmp_read_str(&cmp, sbuf, &string_size))
error_and_exit(cmp_strerror(&cmp));
if (strncmp(sbuf, "Hello", 5) != 0)
error_and_exit("Greeting string value was not 'Hello'");
string_size = sizeof(sbuf);
if (!cmp_read_str(&cmp, sbuf, &string_size))
error_and_exit(cmp_strerror(&cmp));
if (strncmp(sbuf, "Name", 4) != 0)
error_and_exit("Name key name was not 'Name'");
string_size = sizeof(sbuf);
if (!cmp_read_str(&cmp, sbuf, &string_size))
error_and_exit(cmp_strerror(&cmp));
if (strncmp(sbuf, "Linus", 5) != 0)
error_and_exit("Name key value was not 'Linus'");
memset(sbuf, 0, sizeof(sbuf));
binary_size = sizeof(sbuf);
if (!cmp_read_bin(&cmp, &sbuf, &binary_size))
error_and_exit(cmp_strerror(&cmp));
if (memcmp(sbuf, "MessagePack", 11) != 0)
error_and_exit("1st binary value was not 'MessagePack'");
memset(sbuf, 0, sizeof(sbuf));
binary_size = sizeof(sbuf);
if (!cmp_read_bin(&cmp, &sbuf, &binary_size))
error_and_exit(cmp_strerror(&cmp));
if (memcmp(sbuf, "is great", 8) != 0)
error_and_exit("2nd binary value was not 'is great'");
if (!cmp_read_ext_marker(&cmp, &ext_type, &ext_size))
error_and_exit(cmp_strerror(&cmp));
if (!read_bytes(&year, sizeof(uint16_t), fh))
error_and_exit(strerror(errno));
if (!read_bytes(&month, sizeof(uint8_t), fh))
error_and_exit(strerror(errno));
if (!read_bytes(&day, sizeof(uint8_t), fh))
error_and_exit(strerror(errno));
if (year != 1983)
error_and_exit("Year was not 1983");
if (month != 5)
error_and_exit("Month was not 5");
if (day != 28)
error_and_exit("Day was not 28");
rewind(fh);
/* Alternately, you can read objects until the stream is empty */
while (1) {
cmp_object_t obj;
if (!cmp_read_object(&cmp, &obj)) {
if (feof(fh))
break;
error_and_exit(cmp_strerror(&cmp));
}
switch (obj.type) {
case CMP_TYPE_POSITIVE_FIXNUM:
case CMP_TYPE_UINT8:
printf("Unsigned Integer: %u\n", obj.as.u8);
break;
case CMP_TYPE_FIXMAP:
case CMP_TYPE_MAP16:
case CMP_TYPE_MAP32:
printf("Map: %u\n", obj.as.map_size);
break;
case CMP_TYPE_FIXARRAY:
case CMP_TYPE_ARRAY16:
case CMP_TYPE_ARRAY32:
printf("Array: %u\n", obj.as.array_size);
break;
case CMP_TYPE_FIXSTR:
case CMP_TYPE_STR8:
case CMP_TYPE_STR16:
case CMP_TYPE_STR32:
if (!read_bytes(sbuf, obj.as.str_size, fh))
error_and_exit(strerror(errno));
sbuf[obj.as.str_size] = 0;
printf("String: %s\n", sbuf);
break;
case CMP_TYPE_BIN8:
case CMP_TYPE_BIN16:
case CMP_TYPE_BIN32:
memset(sbuf, 0, sizeof(sbuf));
if (!read_bytes(sbuf, obj.as.bin_size, fh))
error_and_exit(strerror(errno));
printf("Binary: %s\n", sbuf);
break;
case CMP_TYPE_NIL:
printf("NULL\n");
break;
case CMP_TYPE_BOOLEAN:
if (obj.as.boolean)
printf("Boolean: true\n");
else
printf("Boolean: false\n");
break;
case CMP_TYPE_EXT8:
case CMP_TYPE_EXT16:
case CMP_TYPE_EXT32:
case CMP_TYPE_FIXEXT1:
case CMP_TYPE_FIXEXT2:
case CMP_TYPE_FIXEXT4:
case CMP_TYPE_FIXEXT8:
case CMP_TYPE_FIXEXT16:
if (obj.as.ext.type == 1) { /* Date object */
if (!read_bytes(&year, sizeof(uint16_t), fh))
error_and_exit(strerror(errno));
if (!read_bytes(&month, sizeof(uint8_t), fh))
error_and_exit(strerror(errno));
if (!read_bytes(&day, sizeof(uint8_t), fh))
error_and_exit(strerror(errno));
printf("Date: %u/%u/%u\n", year, month, day);
}
else {
printf("Extended type {%d, %u}: ",
obj.as.ext.type, obj.as.ext.size
);
while (obj.as.ext.size--) {
read_bytes(sbuf, sizeof(uint8_t), fh);
printf("%02x ", sbuf[0]);
}
printf("\n");
}
break;
case CMP_TYPE_FLOAT:
printf("Float: %f\n", obj.as.flt);
break;
case CMP_TYPE_DOUBLE:
printf("Double: %f\n", obj.as.dbl);
break;
case CMP_TYPE_UINT16:
printf("Unsigned Integer: %u\n", obj.as.u16);
break;
case CMP_TYPE_UINT32:
printf("Unsigned Integer: %u\n", obj.as.u32);
break;
case CMP_TYPE_UINT64:
printf("Unsigned Integer: %" PRIu64 "\n", obj.as.u64);
break;
case CMP_TYPE_NEGATIVE_FIXNUM:
case CMP_TYPE_SINT8:
printf("Signed Integer: %d\n", obj.as.s8);
break;
case CMP_TYPE_SINT16:
printf("Signed Integer: %d\n", obj.as.s16);
break;
case CMP_TYPE_SINT32:
printf("Signed Integer: %d\n", obj.as.s32);
break;
case CMP_TYPE_SINT64:
printf("Signed Integer: %" PRId64 "\n", obj.as.s64);
break;
default:
printf("Unrecognized object type %u\n", obj.type);
break;
}
}
fclose(fh);
return EXIT_SUCCESS;
}
int main(int argc, char const *argv[])
{
zmq::context_t context(1);
zmq::socket_t subscriber (context, ZMQ_SUB);
subscriber.connect("tcp://localhost:5556");
subscriber.setsockopt(ZMQ_SUBSCRIBE, NULL, 0);
EKFGyroAccMag kalman;
bool mag_calibration_ready = false;
sleep(3);
float prev_timestamp = 0;
// int i = 1000;
// int skip = 10;
while (1) {
zmq::message_t update;
subscriber.recv(&update);
// if (i > 0) {
// i--;
// continue;
// }
cmp_ctx_t cmp;
cmp_mem_access_t mem;
cmp_mem_access_ro_init(&cmp, &mem, update.data(), update.size());
char strbuf[100];
bool err = false;
uint32_t strsize = sizeof(strbuf);
err = err || !cmp_read_str(&cmp, &strbuf[0], &strsize);
if (err) {
continue;
}
// std::cerr << "received: " << strbuf << std::endl;
if (strcmp("imu", strbuf) == 0) {
uint32_t mapsize = 0;
err = err | !cmp_read_map(&cmp, &mapsize);
bool gyro_ok = false;
bool acc_ok = false;
bool time_ok = false;
float gyro[3];
float acc[3];
float timestamp;
for (int i = 0; i < mapsize; i++) {
char strbuf[100];
uint32_t strsize = sizeof(strbuf);
err = err || !cmp_read_str(&cmp, strbuf, &strsize);
uint32_t arr_sz;
if (!err && strcmp("gyro", strbuf) == 0) {
err = err || !cmp_read_array(&cmp, &arr_sz);
err = err || (arr_sz != 3);
err = err || !cmp_read_float(&cmp, &gyro[0]);
err = err || !cmp_read_float(&cmp, &gyro[1]);
err = err || !cmp_read_float(&cmp, &gyro[2]);
gyro_ok = !err;
} else if (!err && strcmp("acc", strbuf) == 0) {
err = err || !cmp_read_array(&cmp, &arr_sz);
err = err || (arr_sz != 3);
err = err || !cmp_read_float(&cmp, &acc[0]);
err = err || !cmp_read_float(&cmp, &acc[1]);
err = err || !cmp_read_float(&cmp, &acc[2]);
acc_ok = !err;
} else if (!err && strcmp("time", strbuf) == 0) {
uint64_t int_timestamp;
err = err || !cmp_read_uinteger(&cmp, &int_timestamp);
if (!err) {
timestamp = ((float) int_timestamp) / 1000000;
}
time_ok = !err;
}
}
// skip--;
// if (skip > 0) {
// continue;
// } else {
// skip = 10;
// }
if (!err && gyro_ok && acc_ok && time_ok) {
acc[0] = acc[0]/1;
acc[1] = acc[1]/1;
acc[2] = acc[2]/1;
if (prev_timestamp == 0 || !mag_calibration_ready) {
prev_timestamp = timestamp;
continue;
}
float delta_t = timestamp - prev_timestamp;
prev_timestamp = timestamp;
kalman.update_imu(gyro, acc, delta_t);
std::cout << kalman.get_attitude().w() << ", "
<< kalman.get_attitude().x() << ", "
<< kalman.get_attitude().y() << ", "
<< kalman.get_attitude().z() << std::endl;
// std::cerr << "q: " << kalman.get_attitude().w() << ", "
// << kalman.get_attitude().x() << ", "
// << kalman.get_attitude().y() << ", "
// << kalman.get_attitude().z() << std::endl;
// std::cerr << kalman.P << std::endl;
// std::cerr << "time: " << timestamp << std::endl;
// std::cerr << kalman.P(0, 0) << ", "
// << kalman.P(1, 1) << ", "
// << kalman.P(2, 2) << ", "
// << kalman.P(3, 3) << std::endl;
// std::cerr << "imu: gyro: " << gyro[0] << ", " << gyro[1] << ", " << gyro[2];
// std::cerr << " acc " << acc[0] << ", " << acc[1] << ", " << acc[2] << std::endl;
// std::cerr << " time" << timestamp << std::endl;
} else {
std::cerr << "imu: msgpack read error : " << cmp_strerror(&cmp) << std::endl;
}
}
if (strcmp("mag", strbuf) == 0) {
static float mag_max[3] = {-1000, -1000, -1000};
static float mag_min[3] = {1000, 1000, 1000};
uint32_t mapsize = 0;
err = err | !cmp_read_map(&cmp, &mapsize);
bool field_ok = false;
bool time_ok = false;
float mag[3];
uint64_t timestamp;
for (int i = 0; i < mapsize; i++) {
char strbuf[100];
uint32_t strsize = sizeof(strbuf);
err = err || !cmp_read_str(&cmp, strbuf, &strsize);
uint32_t arr_sz;
if (!err && strcmp("field", strbuf) == 0) {
err = err || !cmp_read_array(&cmp, &arr_sz);
err = err || (arr_sz != 3);
err = err || !cmp_read_float(&cmp, &mag[0]);
err = err || !cmp_read_float(&cmp, &mag[1]);
err = err || !cmp_read_float(&cmp, &mag[2]);
field_ok = !err;
} else if (!err && strcmp("time", strbuf) == 0) {
err = err || !cmp_read_uinteger(&cmp, ×tamp);
time_ok = !err;
}
}
if (!err && field_ok && time_ok) {
// std::cerr << "mag field: " << mag[0] << ", " << mag[1] << ", " << mag[2] << std::endl;
for (int i = 0; i < 3; i++) {
if (mag[i] > mag_max[i]) {
mag_max[i] = mag[i];
}
if (mag[i] < mag_min[i]) {
mag_min[i] = mag[i];
}
}
const float EARTH_MAG_FIELD_TH = 0.8*0.4;
if (mag_max[0] - mag_min[0] > 2*EARTH_MAG_FIELD_TH
&& mag_max[1] - mag_min[1] > EARTH_MAG_FIELD_TH
&& mag_max[2] - mag_min[2] > EARTH_MAG_FIELD_TH) {
mag_calibration_ready = true;
std::cerr << "mag calibration ready" << std::endl;
}
float mag_zero[3];
float mag_calib[3];
for (int i = 0; i < 3; i++) {
mag_zero[i] = (mag_min[i] + mag_max[i]) / 2;
mag_calib[i] = mag[i] - mag_zero[i];
}
if (mag_calibration_ready) {
std::cerr << "calibrated mag field: " << mag_calib[0] << ", " << mag_calib[1] << ", " << mag_calib[2] << std::endl;
kalman.update_mag(&mag_calib[0]);
float mag_inertial[3];
ekf_mag::z_inertial(kalman.x[0], kalman.x[1], kalman.x[2], kalman.x[3], mag_calib[0], mag_calib[1], mag_calib[2], &mag_inertial[0]);
std::cerr << "meas " << mag_inertial[0] << ", " << mag_inertial[1] << ", " << mag_inertial[2] << std::endl;
}
}
}
}
return 0;
}
int stor_lookup_next_rsp_unmash(void *ptr, cmp_reader reader, cmp_writer writer, struct _stor_lookup_next_rsp **out){
struct _stor_lookup_next_rsp *o;
cmp_ctx_t cmp;
cmp_init(&cmp, ptr, reader, writer);
o = (struct _stor_lookup_next_rsp*)osapi_malloc(sizeof(*o));
if(o == NULL) return -ENOMEM;
memset(o, 0, sizeof(*o));
cmp_read_long(&cmp,&o->luid);
{
stor_fstat_rsp_t *obj;
uint32_t size;
int i;
cmp_read_array(&cmp, &size);
fixarray_create(size, &o->rfiles);
for (i = 0; i < size; i++){
obj = osapi_malloc(sizeof(*obj));
if(obj == NULL) {
osapi_free(o);
return -ENOMEM;
}
cmp_read_long(&cmp,&obj->fid);
{
uint32_t size;
cmp_read_str_size(&cmp, &size);
if(size == 0){
obj->fname = NULL;
}else{
obj->fname = (char*)osapi_malloc(size+1);
if(obj->fname == NULL){
osapi_free(o);
return -ENOMEM;
}
reader(&cmp, obj->fname, size);
obj->fname[size] = 0;
}
}
{
uint32_t size;
cmp_read_str_size(&cmp, &size);
if(size == 0){
obj->folder = NULL;
}else{
obj->folder = (char*)osapi_malloc(size+1);
if(obj->folder == NULL){
osapi_free(o);
return -ENOMEM;
}
reader(&cmp, obj->folder, size);
obj->folder[size] = 0;
}
}
cmp_read_int(&cmp, &obj->type);
cmp_read_long(&cmp,&obj->fatime);
cmp_read_long(&cmp,&obj->fmtime);
cmp_read_long(&cmp,&obj->fctime);
cmp_read_long(&cmp,&obj->fsize);
cmp_read_bin_size(&cmp, &obj->snap_len);
if(obj->snap_len == 0){
obj->snap = NULL; }else{
obj->snap = osapi_malloc(obj->snap_len);
if(obj->snap == NULL){
return -ENOMEM;
}
reader(ptr, obj->snap, obj->snap_len);
}
fixarray_set(o->rfiles, i, obj);
}
}
*out = o;
return 0;
}
