diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 04d02fa87..62e60efc2 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -58,32 +58,39 @@ macro(add_mip_example name sources) endmacro() -# C++ examples need either serial or TCP support -if(MICROSTRAIN_ENABLE_CPP AND (MICROSTRAIN_ENABLE_SERIAL OR MICROSTRAIN_ENABLE_TCP)) - - # Generic examples - add_mip_example(DeviceInfo "${EXAMPLE_DIR}/device_info.cpp") - add_mip_example(WatchImu "${EXAMPLE_DIR}/watch_imu.cpp") - add_mip_example(Threads "${EXAMPLE_DIR}/threading.cpp") - if(UNIX) - target_link_libraries(Threads PUBLIC pthread) - endif() - if(MIP_ENABLE_METADATA) - add_mip_example(CsvExample "${EXAMPLE_SOURCES};${EXAMPLE_DIR}/CSV/csv.cpp;${EXAMPLE_DIR}/CSV/stringify.cpp;${EXAMPLE_DIR}/CSV/stringify.hpp") - target_link_libraries(CsvExample PRIVATE ${MIP_METADATA_LIBRARY}) - endif() - - # Product-specific examples - add_mip_example(GQ7_Example "${EXAMPLE_DIR}/GQ7/GQ7_example.cpp") - add_mip_example(CV7_Example "${EXAMPLE_DIR}/CV7/CV7_example.cpp") - add_mip_example(CV7_INS_Simple_Example "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_example.cpp") - add_mip_example(CV7_INS_Simple_Ublox_Example "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp") - add_mip_example(CX5_GX5_45_Example "${EXAMPLE_DIR}/CX5_GX5_45/CX5_GX5_45_example.cpp") - add_mip_example(CX5_GX5_CV5_15_25_Example "${EXAMPLE_DIR}/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp") +add_mip_example(MipPacketC "${EXAMPLE_DIR}/mip_packet_example.c") + +if(MICROSTRAIN_ENABLE_CPP) + + add_mip_example(MipPacket "${EXAMPLE_DIR}/mip_packet_example.cpp") + + # C++ examples that need either serial or TCP support + if(MICROSTRAIN_ENABLE_SERIAL OR MICROSTRAIN_ENABLE_TCP) + + # Generic examples + add_mip_example(DeviceInfo "${EXAMPLE_DIR}/device_info.cpp") + add_mip_example(WatchImu "${EXAMPLE_DIR}/watch_imu.cpp") + add_mip_example(Threads "${EXAMPLE_DIR}/threading.cpp") + if(UNIX) + target_link_libraries(Threads PUBLIC pthread) + endif() + if(MIP_ENABLE_METADATA) + add_mip_example(CsvExample "${EXAMPLE_SOURCES};${EXAMPLE_DIR}/CSV/csv.cpp;${EXAMPLE_DIR}/CSV/stringify.cpp;${EXAMPLE_DIR}/CSV/stringify.hpp") + target_link_libraries(CsvExample PRIVATE ${MIP_METADATA_LIBRARY}) + endif() + + # Product-specific examples + add_mip_example(GQ7_Example "${EXAMPLE_DIR}/GQ7/GQ7_example.cpp") + add_mip_example(CV7_Example "${EXAMPLE_DIR}/CV7/CV7_example.cpp") + add_mip_example(CV7_INS_Simple_Example "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_example.cpp") + add_mip_example(CV7_INS_Simple_Ublox_Example "${EXAMPLE_DIR}/CV7_INS/CV7_INS_simple_ublox_example.cpp") + add_mip_example(CX5_GX5_45_Example "${EXAMPLE_DIR}/CX5_GX5_45/CX5_GX5_45_example.cpp") + add_mip_example(CX5_GX5_CV5_15_25_Example "${EXAMPLE_DIR}/CX5_GX5_CV5_15_25/CX5_GX5_CV5_15_25_example.cpp") + endif() endif() -# C examples need serial support +# C examples that need serial support if(MICROSTRAIN_ENABLE_SERIAL) add_mip_example(WatchImuC "${EXAMPLE_DIR}/watch_imu.c") diff --git a/examples/mip_packet_example.c b/examples/mip_packet_example.c new file mode 100644 index 000000000..416db23fd --- /dev/null +++ b/examples/mip_packet_example.c @@ -0,0 +1,405 @@ + +#include "mip/mip_packet.h" +#include "mip/mip_serialization.h" + +#include "mip/definitions/commands_base.h" +#include "mip/definitions/commands_3dm.h" +#include "mip/definitions/data_sensor.h" +#include "mip/definitions/data_shared.h" + +#include +#include + + +// This function demonstrates how to inspect and iterate fields in +// a MIP packet. These functions do not modify the packet and can +// be used on any packet as long as it has valid structure. +void print_packet(const mip_packet_view* packet, const char* name) +{ + // Ensure the packet structure is correct, otherwise a crash may result. + // This checks the following: + // 1. The packet buffer is not NULL. + // 2. The buffer size is at least MIP_PACKET_LENGTH_MIN. + // 3. The payload length does not exceed the buffer size. + // There's generally no need to check this if you're getting packets + // directly from mip_packet_create (with sufficient buffer space) or + // from the mip parser. If you're reading packets from a file, etc., + // without parsing, use this as a cheap validation step. + // In this example, it should always pass. + assert(mip_packet_is_sane(packet)); + + // Roughly equivalent to mip_packet_checksum_value(packet) == mip_packet_compute_checksum(packet). + const char* checksum_str = mip_packet_is_valid(packet) ? "valid" : "invalid"; + + // Print descriptor set, total length, and payload length. + printf( + "%s: desc_set=0x%02X tot_len=%u pay_len=%u chk=%s [", + name, mip_packet_descriptor_set(packet), mip_packet_total_length(packet), mip_packet_payload_length(packet), checksum_str + ); + + // Print each byte in the packet, including header and checksum. + for(size_t i = 0; i < mip_packet_total_length(packet); i++) + { + printf("%02X", mip_packet_pointer(packet)[i]); + } + + puts("]"); + + // Iterate each field in the packet and print it. + mip_field_view field; + mip_field_init_empty(&field); + while(mip_field_next_in_packet(&field, packet)) + { + // Print descriptors (descriptor set always matches the packet). + printf(" (%02X,%02X): payload=[", mip_field_descriptor_set(&field), mip_field_field_descriptor(&field)); + + // Print field payload bytes. + for(size_t i = 0; i < mip_field_payload_length(&field); i++) + { + printf("%02X", mip_field_payload(&field)[i]); + } + + puts("]"); + } +} + +// This function demonstrates how to build mip packets from scratch. +// It starts with simple cases and moves to more complex ones. It +// also shows some of the low-level details to help you understand +// what's happening "under the hood". +void create_packet_from_scratch() +{ + puts("\nCreate packet from scratch"); + + // Create a mip packet and storage buffer. + mip_packet_view packet; + uint8_t buffer[MIP_PACKET_LENGTH_MAX]; + mip_serializer serializer; + + // If the descriptor set is specified (even if it's invalid, i.e. 0x00) + // the constructor initializes a new MIP packet in the buffer. + // Otherwise, it assumes the buffer already contains a valid MIP packet. + mip_packet_create(&packet, buffer, sizeof(buffer), MIP_BASE_CMD_DESC_SET); + + // The packet isn't ready yet, but print it to demonstrate what the raw bytes look like after each step. + print_packet(&packet, "Empty, un-finalized packet"); + + // Write the checksum. + mip_packet_finalize(&packet); + print_packet(&packet, "Empty packet with checksum"); + + // + // FIELD #1 + // + // A Ping command, with no payload data. + // + + mip_packet_add_field(&packet, MIP_CMD_DESC_BASE_PING, NULL, 0); + + // + // FIELD #2 + // + // A Base Comm Speed command using a fixed/pre-serialized payload. + // + // If you already have the payload data in a buffer, this is the + // most efficient way to add a field. + // + + const uint8_t payload2[] = { 0x01, 0x01, 0x00, 0x01, 0xC2, 0x00 }; + mip_packet_add_field(&packet, MIP_CMD_DESC_BASE_COMM_SPEED, payload2, sizeof(payload2)); + + print_packet(&packet, "Unfinished packet with 2 fields"); + + // + // FIELD #3 + // + // A Base Comm Speed command, using the C struct definition. + // + // Generally, the C struct definitions are the easiest and most readable way to + // create a field in a packet from its parameter values. It's also the most efficient + // method offered by the MIP SDK because the data is not copied through any intermediate + // buffers; it's serialized directly from the command struct to the packet. + // + + mip_base_comm_speed_command cmd3; + cmd3.function = MIP_FUNCTION_WRITE; + cmd3.port = 0x01; + cmd3.baud = 115200; + + // Create a new field with a serializer. + mip_serializer_init_new_field(&serializer, &packet, MIP_CMD_DESC_BASE_COMM_SPEED); + // Serialize the command directly into the field's payload. + insert_mip_base_comm_speed_command(&serializer, &cmd3); + // Update the field length byte. + mip_serializer_finish_new_field(&serializer, &packet); + + print_packet(&packet, "Unfinished packet with 3 fields"); + + // + // FIELD #4 + // + // A Base Comm Speed command, by manually serializing the parameters. + // + // This is exactly the same as field #3, but without the mip_serializer_*_field helper + // functions. This is intended to show what happens "behind the scenes". + // + + // This part is analogous to mip_serializer_init_new_field from field #3. + // -------- + uint8_t* payload4 = NULL; + // Create a field and obtain the payload pointer. + // The return value is the number of bytes remaining after allocating this field. + // Note that in this particular case, we know the field length will be 6 bytes. + // This allows us to save going back and updating the field length after serialization (see field #6). + int leftover3 = mip_packet_create_field(&packet, MIP_CMD_DESC_BASE_COMM_SPEED, 6, &payload4); + + // If less than 0 bytes are left over, then allocation failed by this many bytes. + assert(leftover3 >= 0 && payload4 != NULL); + + // Show what the packet looks like after allocating a field. + // The header will exist but the payload will not be valid yet. + print_packet(&packet, "Field #4 allocated, not written"); + + // Initialize the serializer for the payload. + microstrain_serializer_init_insertion(&serializer, payload4, 6); + // -------- + + // This part is analogous to insert_mip_base_comm_speed_command + // -------- + // Write parameters to the payload. + microstrain_insert_u8(&serializer, 0x01); + microstrain_insert_u8(&serializer, 0x01); + microstrain_insert_u32(&serializer, 115200); + // Make sure serialization was successful (not out of space). + assert(microstrain_serializer_is_ok(&serializer)); + // -------- + + // The equivalent to mip_serializer_finish_new_field isn't required in this case + // because we specified an exact payload length to mip_packet_create_field. + + + // + // Complete packet #1 + // + + // Update the checksum. + mip_packet_finalize(&packet); + + // "Send" the packet + print_packet(&packet, "Finished packet with 4 fields"); + + // + // Packet #2 + // + + // Start over with a new descriptor set. + mip_packet_reset(&packet, MIP_3DM_CMD_DESC_SET); + + // Payload length has been zeroed out. Checksum is garbage. + print_packet(&packet, "Empty packet after reset"); + + // + // FIELD #5 (second packet) + // + // A 3DM Message Format command field using the C struct definition. + // + // Similar to #3 except that we have a variable-length payload. + // Again, this is the recommended method for field creation. + // + + mip_3dm_message_format_command cmd5; + cmd5.function = MIP_FUNCTION_WRITE; + cmd5.desc_set = MIP_SENSOR_DATA_DESC_SET; + cmd5.num_descriptors = 3; // This determines how many descriptors follow. + cmd5.descriptors[0].descriptor = MIP_DATA_DESC_SHARED_REFERENCE_TIME; + cmd5.descriptors[1].descriptor = MIP_DATA_DESC_SENSOR_ACCEL_SCALED; + cmd5.descriptors[2].descriptor = MIP_DATA_DESC_SENSOR_GYRO_SCALED; + cmd5.descriptors[0].decimation = 10; + cmd5.descriptors[1].decimation = 10; + cmd5.descriptors[2].decimation = 10; + + // Create the field and init the serializer in one step. + mip_serializer_init_new_field(&serializer, &packet, MIP_CMD_DESC_3DM_MESSAGE_FORMAT); + // Serialize the command. + insert_mip_3dm_message_format_command(&serializer, &cmd5); + // Update the field length. + mip_serializer_finish_new_field(&serializer, &packet); + + mip_packet_finalize(&packet); + print_packet(&packet, "3DM Message Format command"); + mip_packet_reset(&packet, MIP_3DM_CMD_DESC_SET); + + // + // Packet #3 + // + // FIELD #6 (third packet) + // + + // A 3DM Message Format command field, by manually serializing parameters + // + // Similar to #4 except that with a variable-length payload. + // Similar to #5, but with lower level function calls and slightly different command. + // + + // This part is analogous to mip_serializer_init_new_field from field #3. + // It's almost the same as in field #4, except we use a zero-length field. + // -------- + // Get a pointer to the payload buffer and the maximum number of bytes that can fit. + // + // Start by creating a zero-payload-length field and determining the remaining space. + uint8_t* payload5 = NULL; + int available5 = mip_packet_create_field(&packet, MIP_CMD_DESC_3DM_POLL_DATA, 0, &payload5); + // If less than 0 bytes are available, then there wasn't even space for the field header. + assert(available5 > 0 && payload5 != NULL); + // Initialize the serializer, giving it all available space. + microstrain_serializer_init_insertion(&serializer, payload5, available5); + // -------- + + // This part is analogous to insert_mip_3dm_poll_command + // -------- + // Build a 3DM Poll Data command: + // 1. Suppress_ack + microstrain_insert_bool(&serializer, false); + // 2. Descriptor set + microstrain_insert_u8(&serializer, MIP_SENSOR_DATA_DESC_SET); + // 3. Number of data quantities. + unsigned int num_data = 3; // Configure two descriptors + microstrain_insert_u8(&serializer, num_data); + // 4. Array of uint8_t descriptors. + // Descriptor 1 - Reference timestamp + microstrain_insert_u8(&serializer, MIP_DATA_DESC_SHARED_REFERENCE_TIME); + // Descriptor 2 - Scaled Accel + microstrain_insert_u8(&serializer, MIP_DATA_DESC_SENSOR_ACCEL_SCALED); + // Descriptor 3 - Scaled Gyro + microstrain_insert_u8(&serializer, MIP_DATA_DESC_SENSOR_GYRO_SCALED); + // -------- + + // This part is analogous to mip_serializer_finish_new_field + // -------- + // Check that everything fit in the field payload. + if(microstrain_serializer_is_ok(&serializer)) + { + // Now we know how many bytes were written, so update the field length in the packet. + mip_packet_update_last_field_length(&packet, payload5, microstrain_serializer_length(&serializer)); + } + else // Not enough space + { + assert(false); // This shouldn't happen in this example (note, there's no assertion in mip_serializer_finish_new_field). + // Cancel the field to ensure the packet is still valid. + mip_packet_cancel_last_field(&packet, payload5); + } + // -------- + + // + // Complete packet #3 + // + + mip_packet_finalize(&packet); + print_packet(&packet, "3DM Poll Data command"); +} + +// This function demonstrates how to construct a packet view from a buffer with existing data. +// For simplicity, the packet is hardcoded. +void create_packet_from_buffer() +{ + puts("\nCreate packet from buffer"); + + const uint8_t buffer[] = { + 0x75, 0x65, 0x80, 0x4c, 0x0a, 0xd5, 0x00, 0x00, 0x00, 0x05, 0x5e, 0xe6, 0x7c, 0xc0, 0x0a, 0xd6, + 0x00, 0x00, 0x00, 0x01, 0x4e, 0x43, 0x4a, 0x00, 0x0e, 0x04, 0x3d, 0x9e, 0xe8, 0x8d, 0x38, 0x7f, + 0xdb, 0x00, 0xbf, 0x7a, 0xaf, 0x03, 0x0e, 0x05, 0xbb, 0x0c, 0x1e, 0x30, 0xbb, 0x57, 0x2e, 0x68, + 0xbb, 0xaa, 0x24, 0xae, 0x0e, 0x07, 0xbc, 0x8a, 0xac, 0x80, 0xbc, 0x72, 0xc5, 0x0e, 0xbc, 0xc4, + 0xe2, 0xc1, 0x0e, 0x08, 0x3e, 0xee, 0x3d, 0x9f, 0xbd, 0x66, 0xda, 0xdd, 0xc0, 0xaf, 0xde, 0xf5, + 0x91, 0x96 + }; + + // Create a view of the packet in the buffer. + // Note: The buffer must not be modified, so do not call functions that manipulate the packet. + // E.g. finalize(), addField, createField, reset, etc. + mip_packet_view packet1; + mip_packet_from_buffer(&packet1, buffer, sizeof(buffer)); + + // Ensure the packet is valid before inspecting it. + // This is the combination of checking: + // 1. mip_packet_is_sane (buffer size and payload length checks) + // 2. The packet has a non-zero descriptor set. + // 3. The checksum is valid. + if(!mip_packet_is_valid(&packet1)) + { + assert(false); // The packet should be valid in this example. + return; + } + + // Print the packet. + print_packet(&packet1, "Packet from buffer"); + + // Example of what an application might do to parse specific data. + // This example is for demonstration of the techniques used with the MIP SDK; + // consider using the "dispatch" system (see the documentation) instead. + + // Is this a sensor data packet? + if(mip_packet_descriptor_set(&packet1) == MIP_SENSOR_DATA_DESC_SET) + { + puts("Sensor Data packet:"); + + // Iterate all fields in the packet. + mip_field_view field; + mip_field_init_empty(&field); + while(mip_field_next_in_packet(&field, &packet1)) + { + // Create a deserializer for the field. + mip_serializer serializer; + microstrain_serializer_init_extraction(&serializer, mip_field_payload(&field), mip_field_payload_length(&field)); + + // Check what data the field contains. + switch(mip_field_field_descriptor(&field)) + { + case MIP_DATA_DESC_SHARED_REFERENCE_TIME: + { + uint64_t nanoseconds; + microstrain_extract_u64(&serializer, &nanoseconds); + + if(microstrain_serializer_is_complete(&serializer)) + printf(" Ref Time = %lu\n", nanoseconds); + + break; + } + case MIP_DATA_DESC_SENSOR_ACCEL_SCALED: + { + // Extract the 3-vector. + float x,y,z; + microstrain_extract_float(&serializer, &x); + microstrain_extract_float(&serializer, &y); + microstrain_extract_float(&serializer, &z); + + // Check if the entire field was deserialized (validity check). + if(microstrain_serializer_is_complete(&serializer)) + printf(" Scaled Accel = (%f, %f, %f)\n", x, y, z); + + break; + } + case MIP_DATA_DESC_SENSOR_GYRO_SCALED: + { + // Same as scaled accel except using the C data structure. + // This is the recommended method. + mip_sensor_scaled_gyro_data data; + extract_mip_sensor_scaled_gyro_data(&serializer, &data); + + if(microstrain_serializer_is_complete(&serializer)) + printf(" Scaled Gyro = (%f, %f, %f)\n", data.scaled_gyro[0], data.scaled_gyro[1], data.scaled_gyro[2]); + + break; + } + } + } + puts(""); + } +} + +int main() +{ + create_packet_from_scratch(); + create_packet_from_buffer(); + + return 0; +} diff --git a/examples/mip_packet_example.cpp b/examples/mip_packet_example.cpp new file mode 100644 index 000000000..7f8db90ee --- /dev/null +++ b/examples/mip_packet_example.cpp @@ -0,0 +1,359 @@ + +#include + +#include +#include +#include +#include + +#include + +// This function demonstrates how to inspect and iterate fields in +// a MIP packet. These functions do not modify the packet and can +// be used on any packet as long as it has valid structure. +void print_packet(const mip::PacketView& packet, const char* name) +{ + // Ensure the packet structure is correct, otherwise a crash may result. + // This checks the following: + // 1. The packet buffer is not NULL. + // 2. The buffer size is at least MIP_PACKET_LENGTH_MIN. + // 3. The payload length does not exceed the buffer size. + // There's generally no need to check this if you're getting packets + // directly from mip_packet_create (with sufficient buffer space) or + // from the mip parser. If you're reading packets from a file, etc., + // without parsing, use this as a cheap validation step. + // In this example, it should always pass. + assert(packet.isSane()); + + // Roughly equivalent to packet.checksumValue() == packet.computeChecksum() + const char* checksum_str = packet.isValid() ? "valid" : "invalid"; + + // Print descriptor set, total length, and payload length. + std::printf( + "%s: desc_set=0x%02X tot_len=%u pay_len=%u chk=%s [", + name, packet.descriptorSet(), packet.totalLength(), packet.payloadLength(), checksum_str + ); + + // Print each byte in the packet, including header and checksum. + for(size_t i=0; i(0x01); + serialize4.insert(0x01); + serialize4.insert(115200); +#endif + + // + // Complete packet #1 + // + + // Update the checksum. + packet.finalize(); + + // "Send" the packet + print_packet(packet, "Finished packet with 4 fields"); + + // + // Packet #2 + // + + // Start over with a new descriptor set. + packet.reset(mip::commands_3dm::DESCRIPTOR_SET); + + // Payload length has been zeroed out. Checksum is garbage. + print_packet(packet, "Empty packet after reset"); + + // + // FIELD #5 (second packet) + // + // A 3DM Message Format command field using the C struct definition. + // + // Similar to #3 except that we have a variable-length payload. + // Again, this is the recommended method for field creation. + // + + mip::commands_3dm::MessageFormat cmd5; + cmd5.function = mip::FunctionSelector::WRITE; + cmd5.desc_set = mip::data_sensor::DESCRIPTOR_SET; + cmd5.num_descriptors = 3; // This determines how many descriptors follow. + cmd5.descriptors[0].descriptor = mip::data_shared::ReferenceTimestamp::FIELD_DESCRIPTOR; + cmd5.descriptors[1].descriptor = mip::data_sensor::ScaledAccel::FIELD_DESCRIPTOR; + cmd5.descriptors[2].descriptor = mip::data_sensor::DATA_GYRO_SCALED; + cmd5.descriptors[0].decimation = 10; + cmd5.descriptors[1].decimation = 10; + cmd5.descriptors[2].decimation = 10; + + packet.addField(cmd5); + + packet.finalize(); + print_packet(packet, "3DM Message Format command"); + mip_packet_reset(&packet, mip::commands_3dm::DESCRIPTOR_SET); + + // + // Packet #3 + // + + // FIELD #6 (third packet) + // + // A 3DM Message Format command field, by manually serializing parameters + // + // Similar to #4 except that with a variable-length payload. + // Similar to #5, but with lower level function calls and slightly different command. + // + + // Create a field of unknown length. + // This is equivalent to creating a field with length 0 and a serializer covering + // all of the remaining space. After the field is serialized, the field length is + // updated based on how many bytes were used. + mip::PacketView::AllocatedField field6 = packet.createField(mip::commands_3dm::PollData::FIELD_DESCRIPTOR); + + field6.insert( + false, // suppress_ack + uint8_t(mip::data_sensor::DESCRIPTOR_SET), + uint8_t(3) + ); + + // Arrays can also be directly serialized if their size is known. +#if 1 + std::array descriptors6 = {{ + mip::data_shared::ReferenceTimestamp::FIELD_DESCRIPTOR, + mip::data_sensor::ScaledAccel::FIELD_DESCRIPTOR, + mip::data_sensor::DATA_GYRO_SCALED, + }}; + field6.insert(descriptors6); +#else + uint8_t descriptors6[] = { + mip::data_shared::ReferenceTimestamp::FIELD_DESCRIPTOR, + mip::data_sensor::ScaledAccel::FIELD_DESCRIPTOR, + mip::data_sensor::DATA_GYRO_SCALED, + }; + field6.insert(descriptors6); + // Equivalent to + //field6.insert(descriptors6, 3); +#endif + + // Update the field length. + // Note that if the field would exceed the remaining space in the packet, this will + // instead remove the field entirely and return false. + bool ok6 = field6.commit(); + assert(ok6); // Shouldn't happen in this example. + + // + // Complete packet #3 + // + + mip_packet_finalize(&packet); + print_packet(&packet, "3DM Poll Data command"); +} + +void create_packet_from_buffer() +{ + puts("\nCreate packet from buffer"); + + const uint8_t buffer[] = { + 0x75, 0x65, 0x80, 0x4c, 0x0a, 0xd5, 0x00, 0x00, 0x00, 0x05, 0x5e, 0xe6, 0x7c, 0xc0, 0x0a, 0xd6, + 0x00, 0x00, 0x00, 0x01, 0x4e, 0x43, 0x4a, 0x00, 0x0e, 0x04, 0x3d, 0x9e, 0xe8, 0x8d, 0x38, 0x7f, + 0xdb, 0x00, 0xbf, 0x7a, 0xaf, 0x03, 0x0e, 0x05, 0xbb, 0x0c, 0x1e, 0x30, 0xbb, 0x57, 0x2e, 0x68, + 0xbb, 0xaa, 0x24, 0xae, 0x0e, 0x07, 0xbc, 0x8a, 0xac, 0x80, 0xbc, 0x72, 0xc5, 0x0e, 0xbc, 0xc4, + 0xe2, 0xc1, 0x0e, 0x08, 0x3e, 0xee, 0x3d, 0x9f, 0xbd, 0x66, 0xda, 0xdd, 0xc0, 0xaf, 0xde, 0xf5, + 0x91, 0x96 + }; + + // Create a view of the packet in the buffer. + // Note: The buffer must not be modified, so do not call functions that manipulate the packet. + // E.g. finalize(), addField, createField, reset, etc. + mip::PacketView packet1(buffer, sizeof(buffer)); + + // Ensure the packet is valid before inspecting it. + // This is the combination of checking: + // 1. mip_packet_is_sane (buffer size and payload length checks) + // 2. The packet has a non-zero descriptor set. + // 3. The checksum is valid. + if(!packet1.isValid()) + { + assert(false); // The packet should be valid in this example. + return; + } + + // Print the packet. + print_packet(packet1, "Packet from buffer"); + + // Create a view from a span. + microstrain::Span span(buffer, sizeof(buffer)); + mip::PacketView packet2(span); + print_packet(packet2, "Packet from span"); + + // Create a C++ PacketView from the C equivalent. + mip::C::mip_packet_view packet3c; + mip::C::mip_packet_from_buffer(&packet3c, buffer, sizeof(buffer)); + mip::PacketView packet3(packet3c); + print_packet(packet3, "Packet from C packet"); + + // Example of what an application might do to parse specific data. + // This example is for demonstration of the techniques used with the MIP SDK; + // consider using the "dispatch" system (see the documentation) instead. + + // Is this a sensor data packet? + if(packet1.descriptorSet() == mip::data_sensor::DESCRIPTOR_SET) + { + puts("Sensor Data packet:"); + + for(mip::FieldView field : packet1) + { + mip::Serializer serializer(field.payloadSpan()); + + switch(field.fieldDescriptor()) + { + case mip::data_shared::ReferenceTimestamp::FIELD_DESCRIPTOR: + { + uint64_t nanoseconds; + + if(serializer.extract(nanoseconds)) + printf(" Ref Time = %lu\n", nanoseconds); + + break; + } + case mip::data_sensor::ScaledAccel::FIELD_DESCRIPTOR: + { + // Extract the 3-vector. + float x,y,z; + + if(serializer.extract(x,y,z)) + printf(" Scaled Accel = (%f, %f, %f)\n", x, y, z); + + break; + } + case mip::data_sensor::ScaledGyro::FIELD_DESCRIPTOR: + { + mip::data_sensor::ScaledGyro data; + + if(serializer.extract(data)) + printf(" Scaled Gyro = (%f, %f, %f)\n", data.scaled_gyro[0], data.scaled_gyro[1], data.scaled_gyro[2]); + + break; + } + } + } + } +} + +int main() +{ + create_packet_from_scratch(); + create_packet_from_buffer(); +} diff --git a/src/c/mip/mip_packet.c b/src/c/mip/mip_packet.c index 2d38cc353..d5d3c9353 100644 --- a/src/c/mip/mip_packet.c +++ b/src/c/mip/mip_packet.c @@ -35,7 +35,7 @@ /// MIP_PACKET_LENGTH_MIN bytes, calling the accessor functions is undefined /// behavior. /// -void mip_packet_from_buffer(mip_packet_view* packet, uint8_t* buffer, size_t length) +void mip_packet_from_buffer(mip_packet_view* packet, const uint8_t* buffer, size_t length) { assert(buffer != NULL); @@ -43,7 +43,7 @@ void mip_packet_from_buffer(mip_packet_view* packet, uint8_t* buffer, size_t len if( length > MIP_PACKET_LENGTH_MAX ) length = MIP_PACKET_LENGTH_MAX; - packet->_buffer = buffer; + packet->_buffer = (uint8_t*)buffer; packet->_buffer_length = (uint_least16_t)length; } @@ -291,7 +291,7 @@ bool mip_packet_is_data(const mip_packet_view* packet) bool mip_packet_add_field(mip_packet_view* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length) { uint8_t* payload_buffer; - int remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer); + int remaining = mip_packet_create_field(packet, field_descriptor, payload_length, &payload_buffer); if( remaining < 0 ) return false; @@ -330,7 +330,7 @@ bool mip_packet_add_field(mip_packet_view* packet, uint8_t field_descriptor, con /// is negative, the field could not be allocated and the payload must /// not be written. /// -int mip_packet_alloc_field(mip_packet_view* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out) +int mip_packet_create_field(mip_packet_view* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** payload_ptr_out) { assert(payload_ptr_out != NULL); assert( payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); @@ -359,7 +359,7 @@ int mip_packet_alloc_field(mip_packet_view* packet, uint8_t field_descriptor, ui //////////////////////////////////////////////////////////////////////////////// ///@brief Changes the size of the last field in the packet. /// -/// Use this in conjunction with mip_packet_alloc_field() when the size of the +/// Use this in conjunction with mip_packet_create_field() when the size of the /// field is not known in advance. Pass a payload size of 0 to alloc_field and /// check that the returned available space is sufficient, then write the /// payload and call this function with the actual space used. @@ -377,7 +377,7 @@ int mip_packet_alloc_field(mip_packet_view* packet, uint8_t field_descriptor, ui ///@returns The space remaining in the packet after changing the field size. /// This will be negative if the new length did not fit. /// -int mip_packet_realloc_last_field(mip_packet_view* packet, uint8_t* payload_ptr, uint8_t new_payload_length) +int mip_packet_update_last_field_length(mip_packet_view* packet, uint8_t* payload_ptr, uint8_t new_payload_length) { assert(payload_ptr != NULL); assert( new_payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX ); @@ -402,7 +402,7 @@ int mip_packet_realloc_last_field(mip_packet_view* packet, uint8_t* payload_ptr, //////////////////////////////////////////////////////////////////////////////// ///@brief Removes the last field from the packet after having allocated it. /// -/// Use only after allocating a field with mip_packet_alloc_field to cancel it. +/// Use only after allocating a field with mip_packet_create_field to cancel it. /// E.g. if it turns out that there isn't enough buffer space to write the /// payload. /// diff --git a/src/c/mip/mip_packet.h b/src/c/mip/mip_packet.h index e2ad918f7..46accb802 100644 --- a/src/c/mip/mip_packet.h +++ b/src/c/mip/mip_packet.h @@ -67,8 +67,8 @@ typedef struct mip_packet_view void mip_packet_create(mip_packet_view* packet, uint8_t* buffer, size_t buffer_size, uint8_t descriptor_set); bool mip_packet_add_field(mip_packet_view* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length); -int mip_packet_alloc_field(mip_packet_view* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** payload_ptr_out); -int mip_packet_realloc_last_field(mip_packet_view* packet, uint8_t* payload_ptr, uint8_t new_payload_length); +int mip_packet_create_field(mip_packet_view* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** payload_ptr_out); +int mip_packet_update_last_field_length(mip_packet_view* packet, uint8_t* payload_ptr, uint8_t new_payload_length); int mip_packet_cancel_last_field(mip_packet_view* packet, uint8_t* payload_ptr); void mip_packet_finalize(mip_packet_view* packet); @@ -89,7 +89,7 @@ void mip_packet_reset(mip_packet_view* packet, uint8_t descriptor_set); /// ///@{ -void mip_packet_from_buffer(mip_packet_view* packet, uint8_t* buffer, size_t length); +void mip_packet_from_buffer(mip_packet_view* packet, const uint8_t* buffer, size_t length); uint8_t mip_packet_descriptor_set(const mip_packet_view* packet); uint_least16_t mip_packet_total_length(const mip_packet_view* packet); @@ -111,15 +111,7 @@ int mip_packet_remaining_space(const mip_packet_view* packet); bool mip_packet_is_data(const mip_packet_view* packet); ///@} -//////////////////////////////////////////////////////////////////////////////// -///@defgroup Serialization Serializers - Functions for serializing a MIP packet. -/// -///@{ -//void microstrain_serializer_init_new_field(microstrain_serializer* serializer, mip_packet_view* packet, uint8_t field_descriptor); -//void microstrain_serializer_finish_new_field(const microstrain_serializer* serializer, mip_packet_view* packet); - -///@} ///@} ///@} //////////////////////////////////////////////////////////////////////////////// diff --git a/src/c/mip/mip_serialization.c b/src/c/mip/mip_serialization.c index 1a55932f4..58a855c5f 100644 --- a/src/c/mip/mip_serialization.c +++ b/src/c/mip/mip_serialization.c @@ -9,7 +9,7 @@ ///@brief Initialize a serialization struct for creation of a new field at the /// end of the packet. /// -///@note Call microstrain_serializer_finish_new_field after the data has been serialized. +///@note Call mip_serializer_finish_new_field after the data has been serialized. /// ///@note Only one new field per packet can be in progress at a time. /// @@ -19,7 +19,7 @@ ///@param field_descriptor /// Field descriptor of the new field. /// -void microstrain_serializer_init_new_field(microstrain_serializer* serializer, mip_packet_view* packet, uint8_t field_descriptor) +void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet_view* packet, uint8_t field_descriptor) { assert(packet); @@ -27,29 +27,29 @@ void microstrain_serializer_init_new_field(microstrain_serializer* serializer, m serializer->_buffer_size = 0; serializer->_offset = 0; - const int length = mip_packet_alloc_field(packet, field_descriptor, 0, &serializer->_buffer); + const int length = mip_packet_create_field(packet, field_descriptor, 0, &serializer->_buffer); if( length >= 0 ) serializer->_buffer_size = length; } //////////////////////////////////////////////////////////////////////////////// -///@brief Call this after a new field allocated by microstrain_serializer_init_new_field +///@brief Call this after a new field allocated by mip_serializer_init_new_field /// has been written. /// /// This will either finish the field, or abort it if the serializer failed. /// -///@param serializer Must be created from microstrain_serializer_init_new_field. +///@param serializer Must be created from mip_serializer_init_new_field. ///@param packet Must be the original packet. /// -void microstrain_serializer_finish_new_field(const microstrain_serializer* serializer, mip_packet_view* packet) +void mip_serializer_finish_new_field(const mip_serializer* serializer, mip_packet_view* packet) { assert(packet); if(microstrain_serializer_is_ok(serializer) ) { assert(serializer->_offset <= MIP_FIELD_LENGTH_MAX); // Payload too long! - mip_packet_realloc_last_field(packet, serializer->_buffer, (uint8_t) serializer->_offset); + mip_packet_update_last_field_length(packet, serializer->_buffer, (uint8_t) serializer->_offset); } else if( serializer->_buffer ) mip_packet_cancel_last_field(packet, serializer->_buffer); @@ -61,7 +61,7 @@ void microstrain_serializer_finish_new_field(const microstrain_serializer* seria ///@param serializer ///@param field /// -void microstrain_serializer_init_from_field(microstrain_serializer* serializer, const mip_field_view* field) +void microstrain_serializer_init_from_field(mip_serializer* serializer, const mip_field_view* field) { microstrain_serializer_init_extraction(serializer, mip_field_payload(field), mip_field_payload_length(field)); } diff --git a/src/c/mip/mip_serialization.h b/src/c/mip/mip_serialization.h index ed314d21d..a9162a46c 100644 --- a/src/c/mip/mip_serialization.h +++ b/src/c/mip/mip_serialization.h @@ -11,9 +11,11 @@ namespace C { extern "C" { #endif -void microstrain_serializer_init_new_field(microstrain_serializer* serializer, mip_packet_view* packet, uint8_t field_descriptor); -void microstrain_serializer_finish_new_field(const microstrain_serializer* serializer, mip_packet_view* packet); -void microstrain_serializer_init_from_field(microstrain_serializer* serializer, const mip_field_view* field); +typedef microstrain_serializer mip_serializer; + +void mip_serializer_init_new_field(mip_serializer* serializer, mip_packet_view* packet, uint8_t field_descriptor); +void mip_serializer_finish_new_field(const mip_serializer* serializer, mip_packet_view* packet); +void microstrain_serializer_init_from_field(mip_serializer* serializer, const mip_field_view* field); #ifdef __cplusplus } // extern "C" diff --git a/src/cpp/microstrain/common/serialization/serializer.hpp b/src/cpp/microstrain/common/serialization/serializer.hpp index 4dbcb7cd9..d9f575bd6 100644 --- a/src/cpp/microstrain/common/serialization/serializer.hpp +++ b/src/cpp/microstrain/common/serialization/serializer.hpp @@ -952,7 +952,9 @@ bool Serializer::extract(Ts&... values) // https://stackoverflow.com/questions/13407205/calling-nonmember-instead-of-member-function using microstrain::extract; - return extract(*this, values...); + extract(*this, values...); + + return isOk(); } //////////////////////////////////////////////////////////////////////////////// diff --git a/src/cpp/mip/mip_packet.hpp b/src/cpp/mip/mip_packet.hpp index 3f8297fdf..c5301a016 100644 --- a/src/cpp/mip/mip_packet.hpp +++ b/src/cpp/mip/mip_packet.hpp @@ -45,12 +45,21 @@ class PacketView : public C::mip_packet_view ///@copydoc mip::C::mip_packet_create PacketView(uint8_t* buffer, size_t bufferSize, uint8_t descriptorSet) { C::mip_packet_create(this, buffer, bufferSize, descriptorSet); } ///@copydoc mip_packet_from_buffer - PacketView(uint8_t* buffer, size_t length) { C::mip_packet_from_buffer(this, buffer, length); } + PacketView(const uint8_t* buffer, size_t length) { C::mip_packet_from_buffer(this, const_cast(buffer), length); } /// Constructs a C++ %PacketRef class from the base C object. PacketView(const C::mip_packet_view* other) { std::memcpy(static_cast(this), other, sizeof(*this)); } /// Constructs a C++ %PacketRef class from the base C object. PacketView(const C::mip_packet_view& other) { std::memcpy(static_cast(this), &other, sizeof(*this)); } + ///@brief Create a new MIP packet in an existing buffer. + ///@param buffer Place to store the MIP packet bytes. + ///@param descriptorSet Initializes the packet to this descriptor set. + PacketView(microstrain::Span buffer, uint8_t descriptorSet) { C::mip_packet_create(this, buffer.data(), buffer.size(), descriptorSet); } + + ///@brief Create a reference to an existing MIP packet. + ///@param buffer Buffer containing an existing MIP packet. + ///@caution Do not call functions which modify the packet (addField, finalize, reset, etc) unless you know the buffer is not const. + PacketView(microstrain::Span buffer) { C::mip_packet_from_buffer(this, const_cast(buffer.data()), buffer.size()); } // // C function wrappers @@ -76,7 +85,7 @@ class PacketView : public C::mip_packet_view int remainingSpace() const { return C::mip_packet_remaining_space(this); } ///<@copydoc mip::C::mip_packet_remaining_space bool addField(uint8_t fieldDescriptor, const uint8_t* payload, uint8_t payloadLength) { return C::mip_packet_add_field(this, fieldDescriptor, payload, payloadLength); } ///<@copydoc mip::C::mip_packet_add_field - Serializer createField(uint8_t fieldDescriptor, uint8_t length) { uint8_t * ptr; if(C::mip_packet_alloc_field(this, fieldDescriptor, length, &ptr) < 0) length =0; return Serializer{ptr, length}; } + Serializer createField(uint8_t fieldDescriptor, uint8_t length) { uint8_t * ptr; if(C::mip_packet_create_field(this, fieldDescriptor, length, &ptr) < 0) length =0; return Serializer{ptr, length}; } //std::tuple createField(uint8_t fieldDescriptor) { uint8_t* ptr; int max_size = C::mip_packet_alloc_field(this, fieldDescriptor, 0, &ptr); return max_size >= 0 ? std::make_tuple(ptr, max_size) : std::make_tuple(nullptr, 0); } ///<@copydoc mip::C::mip_packet_alloc_field //int finishLastField(uint8_t* payloadPtr, uint8_t newPayloadLength) { return C::mip_packet_realloc_last_field(this, payloadPtr, newPayloadLength); } ///<@copydoc mip::C::mip_packet_realloc_last_field //int cancelLastField(uint8_t* payloadPtr) { return C::mip_packet_cancel_last_field(this, payloadPtr); } ///<@copydoc mip::C::mip_packet_cancel_last_field @@ -95,7 +104,7 @@ class PacketView : public C::mip_packet_view bool ok = isOk(); if(ok) - ok &= C::mip_packet_realloc_last_field(&m_packet, basePointer(), (uint8_t)usedLength()) >= 0; + ok &= C::mip_packet_update_last_field_length(&m_packet, basePointer(), (uint8_t) usedLength()) >= 0; if(!ok) C::mip_packet_cancel_last_field(&m_packet, basePointer()); @@ -107,7 +116,7 @@ class PacketView : public C::mip_packet_view PacketView& m_packet; }; - AllocatedField createField(uint8_t fieldDescriptor) { uint8_t* ptr; size_t max_size = std::max(0, C::mip_packet_alloc_field(this, fieldDescriptor, 0, &ptr)); return {*this, ptr, max_size}; } + AllocatedField createField(uint8_t fieldDescriptor) { uint8_t* ptr; size_t max_size = std::max(0, C::mip_packet_create_field(this, fieldDescriptor, 0, &ptr)); return {*this, ptr, max_size}; } void finalize() { C::mip_packet_finalize(this); } ///<@copydoc mip::C::mip_packet_finalize @@ -273,7 +282,13 @@ class SizedPacketBuf : public PacketView ///@param fieldDescriptor If specified (not INVALID_FIELD_DESCRIPTOR), overrides the field descriptor. /// template - SizedPacketBuf(const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR) : PacketView(mData, sizeof(mData)) { createFromField(mData, sizeof(mData), field, fieldDescriptor); } + SizedPacketBuf( + const typename std::enable_if::value>::type& field, + uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR + ) : PacketView(mData, sizeof(mData)) + { + createFromField(mData, sizeof(mData), field, fieldDescriptor); + } ///@brief Explicitly obtains a reference to the packet data. diff --git a/test/mip/test_mip_fields.c b/test/mip/test_mip_fields.c index ee0a5f6c1..9fa409197 100644 --- a/test/mip/test_mip_fields.c +++ b/test/mip/test_mip_fields.c @@ -20,7 +20,7 @@ bool add_random_field() const uint8_t field_desc = (rand() % 255) + 1; uint8_t* payload; - if( !mip_packet_alloc_field(&packet, field_desc, length, &payload) ) + if( !mip_packet_create_field(&packet, field_desc, length, &payload) ) return false; for(unsigned int i=0; i