diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index c38c503f..253af2fa 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -145,6 +145,7 @@ class MessageType(IntEnum): IMPORT_DATA = 13110 EXPORT_DATA = 13111 PLATFORM_STORAGE_DATA = 13113 + INPUT_DATA_WRAPPER = 13120 SET_MESSAGE_RATE = 13220 GET_MESSAGE_RATE = 13221 diff --git a/python/fusion_engine_client/messages/measurements.py b/python/fusion_engine_client/messages/measurements.py index 86d87330..2847e8f5 100644 --- a/python/fusion_engine_client/messages/measurements.py +++ b/python/fusion_engine_client/messages/measurements.py @@ -1,7 +1,7 @@ import struct from typing import Sequence -from construct import Array, Struct, Padding, Float32l, Int16sl, Int32sl +from construct import Array, BytesInteger, GreedyBytes, Struct, Padding, Float32l, Int16ul, Int16sl, Int32sl import numpy as np from .defs import * @@ -1390,3 +1390,56 @@ def to_numpy(cls, messages: Sequence['RawHeadingOutput']): } result.update(MeasurementDetails.to_numpy([m.details for m in messages])) return result + + +class InputDataWrapperMessage(MessagePayload): + """! + @brief Wrapper for arbitrary data packets. + """ + MESSAGE_TYPE = MessageType.INPUT_DATA_WRAPPER + MESSAGE_VERSION = 0 + + CENTI_NANO_SCALE_FACTOR = 10_000_000 + + Construct = Struct( + # 5 byte, unsigned, little endian integer + "system_time_cs" / BytesInteger(5, swapped=True), + Padding(1), + "data_type" / Int16ul, + # NOTE: Since this message does no capture the expected data size, the Construct relies on the size of the + # Python buffer passed to `unpack`` to infer the size of the data. This is the behavior of @ref GreedyBytes. + "data" / GreedyBytes + ) + + def __init__(self): + self.system_time_ns = 0 + self.data_type = 0 + self.data = bytes() + + def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): + self.system_time_cs = int(self.system_time_ns / self.CENTI_NANO_SCALE_FACTOR) + ret = MessagePayload.pack(self, buffer, offset, return_buffer) + del self.__dict__['system_time_cs'] + return ret + + def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int: + ret = MessagePayload.unpack(self, buffer, offset, message_version) + self.system_time_ns = self.system_time_cs * self.CENTI_NANO_SCALE_FACTOR + del self.__dict__['system_time_cs'] + return ret + + def __repr__(self): + result = super().__repr__()[:-1] + result += f', data_type={self.data_type}, data_len={len(self.data)}]' + return result + + def __str__(self): + return construct_message_to_string(message=self, construct=self.Construct, + value_to_string={ + 'data': lambda x: f'{len(x)} B payload', + 'data_type': lambda x: f'{x} (0x{x:02x})', + }, + title=f'Data Wrapper') + + def calcsize(self) -> int: + return len(self.pack()) diff --git a/src/point_one/fusion_engine/messages/defs.h b/src/point_one/fusion_engine/messages/defs.h index 483b2baa..19a4d4e9 100644 --- a/src/point_one/fusion_engine/messages/defs.h +++ b/src/point_one/fusion_engine/messages/defs.h @@ -95,6 +95,7 @@ enum class MessageType : uint16_t { IMPORT_DATA = 13110, ///< @ref ImportDataMessage EXPORT_DATA = 13111, ///< @ref ExportDataMessage PLATFORM_STORAGE_DATA = 13113, ///< @ref PlatformStorageDataMessage + INPUT_DATA_WRAPPER = 13120, ///< @ref InputDataWrapperMessage SET_MESSAGE_RATE = 13220, ///< @ref SetMessageRate GET_MESSAGE_RATE = 13221, ///< @ref GetMessageRate @@ -266,6 +267,9 @@ P1_CONSTEXPR_FUNC const char* to_string(MessageType type) { case MessageType::PLATFORM_STORAGE_DATA: return "Platform Data Contents"; + case MessageType::INPUT_DATA_WRAPPER: + return "Wrapped Input Data"; + case MessageType::LBAND_FRAME: return "L-band Frame Contents"; } @@ -340,6 +344,7 @@ P1_CONSTEXPR_FUNC bool IsCommand(MessageType message_type) { case MessageType::DEVICE_ID: case MessageType::CONFIG_RESPONSE: case MessageType::PLATFORM_STORAGE_DATA: + case MessageType::INPUT_DATA_WRAPPER: case MessageType::MESSAGE_RATE_RESPONSE: case MessageType::SUPPORTED_IO_INTERFACES: case MessageType::LBAND_FRAME: diff --git a/src/point_one/fusion_engine/messages/measurements.h b/src/point_one/fusion_engine/messages/measurements.h index b43255a8..f414293e 100644 --- a/src/point_one/fusion_engine/messages/measurements.h +++ b/src/point_one/fusion_engine/messages/measurements.h @@ -1261,6 +1261,63 @@ struct P1_ALIGNAS(4) RawHeadingOutput : public MessagePayload { float baseline_distance_m = NAN; }; +/** + * @brief A block of incoming sensor data whose definition depends on the value + * of @ ref data_type. (@ref MessageType::INPUT_DATA_WRAPPER). + * @ingroup measurement_messages + * + * This message has the remainder of the payload_size_bytes filled with the + * wrapped data. The payload is not guaranteed to be aligned to a specific + * message boundary, or to contain complete messages. + * + * ``` + * {MessageHeader, InputDataWrapperMessage, [wrapped data]} + * ``` + */ +struct P1_ALIGNAS(4) InputDataWrapperMessage { + static constexpr MessageType MESSAGE_TYPE = MessageType::INPUT_DATA_WRAPPER; + static constexpr uint8_t MESSAGE_VERSION = 0; + +#if !defined(_MSC_VER) + // Default member initializers for bit-fields only available with c++20. + InputDataWrapperMessage() : system_time_cs(0) {} +#endif + +// The MSVC compiler does not allow unaligned bit fields: +// https://stackoverflow.com/questions/4310728/forcing-unaligned-bitfield-packing-in-msvc +// unlike Clang and GCC. This means that `uint64_t system_time_cs : 40;` is 5 +// bytes in GCC and 8 bytes in MSVC. On MSVC, you must cast +// @ref system_time_cs_bytes to read and write the timestamp. +#if defined(_MSC_VER) + /** + * 5 byte system wall-clock timestamp in centiseconds (hundredths of a + * second). Set to POSIX time (time since 1/1/1970) where available. + */ + uint8_t system_time_cs_bytes[5] = {0}; +#else + /** + * 5 byte system wall-clock timestamp in centiseconds (hundredths of a + * second). Set to POSIX time (time since 1/1/1970) where available. + */ + uint64_t system_time_cs : 40; +#endif + + uint8_t reserved[1] = {0}; + + /** Type identifier for the serialized message to follow. */ + uint16_t data_type = 0; + + /** + * The rest of this message contains the wrapped data. The size of the data is + * found by subtracting the size of the other fields in this message from the + * header `payload_size_bytes` (i.e. `size_t content_size = + * header->payload_size_bytes - sizeof(InputDataWrapperMessage)`). The data is + * interpreted based on the value of `data_type`. + */ +}; +static_assert(sizeof(InputDataWrapperMessage) == 8, + "InputDataWrapperMessage does not match expected packed size."); + #pragma pack(pop) } // namespace messages