From 1219dc5ccbd140a673066381106f74fd8af72416 Mon Sep 17 00:00:00 2001 From: lucas Date: Tue, 7 Nov 2023 10:24:35 -0600 Subject: [PATCH] Removed RawIMUInput, refactored IMUInput --- python/fusion_engine_client/messages/defs.py | 1 - .../messages/measurements.py | 83 +------------------ src/point_one/fusion_engine/messages/defs.h | 4 - .../fusion_engine/messages/measurements.h | 55 ++---------- 4 files changed, 7 insertions(+), 136 deletions(-) diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index aad940f1..3c65d6b4 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -100,7 +100,6 @@ class MessageType(IntEnum): RAW_IMU_OUTPUT = 11002 HEADING_OUTPUT = 11003 IMU_INPUT = 11004 - RAW_IMU_INPUT = 11005 # Vehicle measurement messages. DEPRECATED_WHEEL_SPEED_MEASUREMENT = 11101 diff --git a/python/fusion_engine_client/messages/measurements.py b/python/fusion_engine_client/messages/measurements.py index 8a5cbaa5..fda034e6 100644 --- a/python/fusion_engine_client/messages/measurements.py +++ b/python/fusion_engine_client/messages/measurements.py @@ -150,93 +150,14 @@ def __str__(self): return construct_message_to_string(message=self, construct=self.Construct, title='Raw IMU Output', fields=['details', 'accel_mps2', 'gyro_rps', 'temperature_degc']) + class IMUInput(MessagePayload): """! - @brief IMU sensor measurement data. + @brief IMU sensor measurement input. """ MESSAGE_TYPE = MessageType.IMU_INPUT MESSAGE_VERSION = 0 - _STRUCT = struct.Struct('<3d 3d 3d 3d') - - def __init__(self): - self.p1_time = Timestamp() - - self.accel_mps2 = np.full((3,), np.nan) - self.accel_std_mps2 = np.full((3,), np.nan) - - self.gyro_rps = np.full((3,), np.nan) - self.gyro_std_rps = np.full((3,), np.nan) - - def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): - if buffer is None: - buffer = bytearray(self.calcsize()) - - initial_offset = offset - - offset += self.p1_time.pack(buffer, offset, return_buffer=False) - - offset += self.pack_values( - self._STRUCT, buffer, offset, - self.accel_mps2, - self.accel_std_mps2, - self.gyro_rps, - self.gyro_std_rps) - - if return_buffer: - return buffer - else: - return offset - initial_offset - - def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int: - initial_offset = offset - - offset += self.p1_time.unpack(buffer, offset) - - offset += self.unpack_values( - self._STRUCT, buffer, offset, - self.accel_mps2, - self.accel_std_mps2, - self.gyro_rps, - self.gyro_std_rps) - - return offset - initial_offset - - def __repr__(self): - return '%s @ %s' % (self.MESSAGE_TYPE.name, self.p1_time) - - def __str__(self): - return 'IMU Input @ %s' % str(self.p1_time) - - @classmethod - def to_numpy(cls, messages): - result = { - 'p1_time': np.array([float(m.p1_time) for m in messages]), - 'accel_mps2': np.array([m.accel_mps2 for m in messages]).T, - 'accel_std_mps2': np.array([m.accel_std_mps2 for m in messages]).T, - 'gyro_rps': np.array([m.gyro_rps for m in messages]).T, - 'gyro_std_rps': np.array([m.gyro_std_rps for m in messages]).T, - } - - # For convenience and consistency with raw measurement messages, we artificially create the following fields - # from MeasurementDetails: - result['measurement_time_source'] = np.full_like(result['p1_time'], SystemTimeSource.P1_TIME) - result['measurement_time'] = result['p1_time'] # No need to copy, reference is fine here. - - return result - - @classmethod - def calcsize(cls) -> int: - return Timestamp.calcsize() + cls._STRUCT.size - - -class RawIMUInput(MessagePayload): - """! - @brief Raw (uncorrected) IMU sensor measurement output. - """ - MESSAGE_TYPE = MessageType.RAW_IMU_INPUT - MESSAGE_VERSION = 0 - Construct = Struct( "details" / MeasurementDetailsConstruct, Padding(6), diff --git a/src/point_one/fusion_engine/messages/defs.h b/src/point_one/fusion_engine/messages/defs.h index 36ed7c81..cfa7c3ea 100644 --- a/src/point_one/fusion_engine/messages/defs.h +++ b/src/point_one/fusion_engine/messages/defs.h @@ -51,7 +51,6 @@ enum class MessageType : uint16_t { RAW_IMU_OUTPUT = 11002, ///< @ref RawIMUOutput HEADING_OUTPUT = 11003, ///< @ref HeadingOutput IMU_INPUT = 11004, ///< @ref ImuInput - RAW_IMU_INPUT = 11005, ///< @ref RawIMUInput // Vehicle measurement messages. DEPRECATED_WHEEL_SPEED_MEASUREMENT = @@ -160,9 +159,6 @@ P1_CONSTEXPR_FUNC const char* to_string(MessageType type) { case MessageType::IMU_INPUT: return "IMU Input"; - case MessageType::RAW_IMU_INPUT: - return "Raw IMU Input"; - case MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT: return "Wheel Speed Measurement"; diff --git a/src/point_one/fusion_engine/messages/measurements.h b/src/point_one/fusion_engine/messages/measurements.h index 1a31808d..df841b20 100644 --- a/src/point_one/fusion_engine/messages/measurements.h +++ b/src/point_one/fusion_engine/messages/measurements.h @@ -317,62 +317,17 @@ struct P1_ALIGNAS(4) RawIMUOutput : public MessagePayload { }; /** - * @brief IMU sensor measurement input with calibration and corrections applied - * (@ref MessageType::IMU_INPUT, version 1.0). - * @ingroup measurement_messages - * - * This message is an input from the device containing IMU acceleration and - * rotation rate measurements. The measurements been corrected for biases and - * scale factors, and have been rotated into the vehicle body frame from the - * original IMU orientation, including calibrated mounting error estimates. - * - * See also @ref RawIMUInput. - */ -struct P1_ALIGNAS(4) IMUInput : public MessagePayload { - static constexpr MessageType MESSAGE_TYPE = MessageType::IMU_INPUT; - static constexpr uint8_t MESSAGE_VERSION = 0; - - /** The time of the measurement, in P1 time (beginning at power-on). */ - Timestamp p1_time; - - /** - * Corrected vehicle x/y/z acceleration (in meters/second^2), resolved in the - * body frame. - */ - double accel_mps2[3] = {NAN, NAN, NAN}; - - /** - * Corrected vehicle x/y/z acceleration standard deviation (in - * meters/second^2), resolved in the body frame. - */ - double accel_std_mps2[3] = {NAN, NAN, NAN}; - - /** - * Corrected vehicle x/y/z rate of rotation (in radians/second), resolved in - * the body frame. - */ - double gyro_rps[3] = {NAN, NAN, NAN}; - - /** - * Corrected vehicle x/y/z rate of rotation standard deviation (in - * radians/second), resolved in the body frame. - */ - double gyro_std_rps[3] = {NAN, NAN, NAN}; -}; - -/** - * @brief Raw (uncorrected) IMU sensor measurement input (@ref - MessageType::RAW_IMU_INPUT, version 1.0). + * @brief IMU sensor measurement input (@ref MessageType::IMU_INPUT, + * version 1.0). * @ingroup measurement_messages * * This message is an input from the device containing raw IMU acceleration and - * rotation rate measurements. These measurements come directly from the sensor, - * and do not have any corrections or calibration applied. + * rotation rate measurements. * * See also @ref IMUInput. */ -struct P1_ALIGNAS(4) RawIMUInput : public MessagePayload { - static constexpr MessageType MESSAGE_TYPE = MessageType::RAW_IMU_INPUT; +struct P1_ALIGNAS(4) IMUInput : public MessagePayload { + static constexpr MessageType MESSAGE_TYPE = MessageType::IMU_INPUT; static constexpr uint8_t MESSAGE_VERSION = 0; /**