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