Removed RawIMUInput, refactored IMUInput

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

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),

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

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;
 
   /**