Added IMU measurement input support. (#282)

# New Features
- Added new `IMUInput` message definition

# Fixes
- Corrected `RAW_IMU_OUTPUT` enum name in Python
Merge PR #282

python/fusion_engine_client/messages/defs.py

@@ -97,8 +97,9 @@ class MessageType(IntEnum):
     # Sensor measurement messages.
     IMU_OUTPUT = 11000
     RAW_HEADING_OUTPUT = 11001
-    RAW_IMU_MEASUREMENT = 11002
+    RAW_IMU_OUTPUT = 11002
     HEADING_OUTPUT = 11003
+    IMU_INPUT = 11004
 
     # Vehicle measurement messages.
     DEPRECATED_WHEEL_SPEED_MEASUREMENT = 11101

python/fusion_engine_client/messages/measurements.py

@@ -12,6 +12,62 @@
 # IMU Measurements
 ################################################################################
 
+class IMUInput(MessagePayload):
+    """!
+    @brief IMU sensor measurement input.
+    """
+    MESSAGE_TYPE = MessageType.IMU_INPUT
+    MESSAGE_VERSION = 0
+
+    Construct = Struct(
+        "details" / MeasurementDetailsConstruct,
+        Padding(6),
+        "temperature_degc" / FixedPointAdapter(2 ** -7, Int16sl, invalid=0x7FFF),
+        "accel_mps2" / Array(3, FixedPointAdapter(2 ** -16, Int32sl, invalid=0x7FFFFFFF)),
+        "gyro_rps" / Array(3, FixedPointAdapter(2 ** -20, Int32sl, invalid=0x7FFFFFFF)),
+    )
+
+    def __init__(self):
+        self.details = MeasurementDetails()
+        self.temperature_degc = np.nan
+        self.accel_mps2 = np.full((3,), np.nan)
+        self.gyro_rps = np.full((3,), np.nan)
+
+    def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int):
+        values = dict(self.__dict__)
+        packed_data = self.Construct.build(values)
+        return PackedDataToBuffer(packed_data, buffer, offset, return_buffer)
+
+    def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int:
+        parsed = self.Construct.parse(buffer[offset:])
+        self.__dict__.update(parsed)
+        del self.__dict__['_io']
+        return parsed._io.tell()
+
+    @classmethod
+    def calcsize(cls) -> int:
+        return cls.Construct.sizeof()
+
+    @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,
+            'gyro_rps': np.array([m.gyro_rps for m in messages]).T,
+            'temperature_degc': np.array([m.temperature_degc for m in messages]),
+        }
+        result.update(MeasurementDetails.to_numpy([m.details for m in messages]))
+        return result
+
+    def __getattr__(self, item):
+        if item == 'p1_time':
+            return self.details.p1_time
+        else:
+            return super().__getattr__(item)
+
+    def __str__(self):
+        return construct_message_to_string(message=self, construct=self.Construct, title='IMU Input',
+                                           fields=['details', 'accel_mps2', 'gyro_rps', 'temperature_degc'])
 
 class IMUOutput(MessagePayload):
     """!
@@ -97,7 +153,7 @@ class RawIMUOutput(MessagePayload):
     """!
     @brief Raw (uncorrected) IMU sensor measurement output.
     """
-    MESSAGE_TYPE = MessageType.RAW_IMU_MEASUREMENT
+    MESSAGE_TYPE = MessageType.RAW_IMU_OUTPUT
     MESSAGE_VERSION = 0
 
     Construct = Struct(

src/point_one/fusion_engine/messages/defs.h

@@ -50,6 +50,7 @@ enum class MessageType : uint16_t {
   RAW_HEADING_OUTPUT = 11001, ///< @ref RawHeadingOutput
   RAW_IMU_OUTPUT = 11002, ///< @ref RawIMUOutput
   HEADING_OUTPUT = 11003, ///< @ref HeadingOutput
+  IMU_INPUT = 11004, ///< @ref IMUInput
 
   // Vehicle measurement messages.
   DEPRECATED_WHEEL_SPEED_MEASUREMENT =
@@ -155,6 +156,9 @@ P1_CONSTEXPR_FUNC const char* to_string(MessageType type) {
     case MessageType::HEADING_OUTPUT:
       return "Heading Output";
 
+    case MessageType::IMU_INPUT:
+      return "IMU Input";
+
     case MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT:
       return "Wheel Speed Measurement";
 

src/point_one/fusion_engine/messages/measurements.h

@@ -231,6 +231,46 @@ struct P1_ALIGNAS(4) MeasurementDetails {
 // IMU Measurements
 ////////////////////////////////////////////////////////////////////////////////
 
+/**
+ * @brief IMU sensor measurement input (@ref MessageType::IMU_INPUT,
+ *        version 1.0).
+ * @ingroup measurement_messages
+ *
+ * This message is an input to the device containing raw IMU acceleration and
+ * rotation rate measurements.
+ *
+ * See also @ref IMUOutput.
+ */
+struct P1_ALIGNAS(4) IMUInput : public MessagePayload {
+  static constexpr MessageType MESSAGE_TYPE = MessageType::IMU_INPUT;
+  static constexpr uint8_t MESSAGE_VERSION = 0;
+
+  /**
+   * Measurement timestamp and additional information, if available. See @ref
+   * MeasurementDetails for details.
+   */
+  MeasurementDetails details;
+
+  uint8_t reserved[6] = {0};
+
+  /**
+   * The IMU temperature (in deg Celcius * 2^-7). Set to 0x7FFF if invalid.
+   */
+  int16_t temperature = INT16_MAX;
+
+  /**
+   * Measured x/y/z acceleration (in meters/second^2 * 2^-16), resolved in the
+   * sensor measurement frame. Set to 0x7FFFFFFF if invalid.
+   */
+  int32_t accel[3] = {INT32_MAX, INT32_MAX, INT32_MAX};
+
+  /**
+   * Measured x/y/z rate of rotation (in radians/second * 2^-20), resolved in
+   * the sensor measurement frame. Set to 0x7FFFFFFF if invalid.
+   */
+  int32_t gyro[3] = {INT32_MAX, INT32_MAX, INT32_MAX};
+};
+
 /**
  * @brief IMU sensor measurement output with calibration and corrections applied
  *        (@ref MessageType::IMU_OUTPUT, version 1.0).