Add compensated IMU message [AP-1475] (#1446)

# Description

Adding a new MSG_IMU_COMP according to the corresponding design doc. 

<img width="938" alt="Screenshot 2024-10-01 at 8 57 33 PM"
src="https://github.com/user-attachments/assets/796be85d-99e9-46df-a9c3-420895d042c7">

# API compatibility

No, this is a new message

## API compatibility plan

n/a

# JIRA Reference

https://swift-nav.atlassian.net/browse/AP-1475

---------

Co-authored-by: Ivan Smolyakov <[email protected]>
Co-authored-by: Ivan Smolyakov <[email protected]>
Co-authored-by: Ivan Smolyakov <[email protected]>
Co-authored-by: Rodrigo Reichert <[email protected]>

Author: 5 people

c/include/libsbp/cpp/message_traits.h

@@ -3527,6 +3527,39 @@ struct MessageTraits<sbp_msg_imu_aux_t> {
   }
 };
 
+template <>
+struct MessageTraits<sbp_msg_imu_comp_t> {
+  static constexpr sbp_msg_type_t id = SbpMsgImuComp;
+  static constexpr const char *name = "MSG_IMU_COMP";
+  static const sbp_msg_imu_comp_t &get(const sbp_msg_t &msg) {
+    return msg.imu_comp;
+  }
+  static sbp_msg_imu_comp_t &get(sbp_msg_t &msg) { return msg.imu_comp; }
+  static void to_sbp_msg(const sbp_msg_imu_comp_t &msg, sbp_msg_t *sbp_msg) {
+    sbp_msg->imu_comp = msg;
+  }
+  static sbp_msg_t to_sbp_msg(const sbp_msg_imu_comp_t &msg) {
+    sbp_msg_t sbp_msg;
+    sbp_msg.imu_comp = msg;
+    return sbp_msg;
+  }
+  static s8 send(sbp_state_t *state, u16 sender_id,
+                 const sbp_msg_imu_comp_t &msg, sbp_write_fn_t write) {
+    return sbp_msg_imu_comp_send(state, sender_id, &msg, write);
+  }
+  static s8 encode(uint8_t *buf, uint8_t len, uint8_t *n_written,
+                   const sbp_msg_imu_comp_t &msg) {
+    return sbp_msg_imu_comp_encode(buf, len, n_written, &msg);
+  }
+  static s8 decode(const uint8_t *buf, uint8_t len, uint8_t *n_read,
+                   sbp_msg_imu_comp_t *msg) {
+    return sbp_msg_imu_comp_decode(buf, len, n_read, msg);
+  }
+  static size_t encoded_len(const sbp_msg_imu_comp_t &msg) {
+    return sbp_msg_imu_comp_encoded_len(&msg);
+  }
+};
+
 template <>
 struct MessageTraits<sbp_msg_imu_raw_t> {
   static constexpr sbp_msg_type_t id = SbpMsgImuRaw;

c/include/libsbp/imu.h

@@ -18,6 +18,7 @@
 #ifndef LIBSBP_IMU_MESSAGES_H
 #define LIBSBP_IMU_MESSAGES_H
 #include <libsbp/imu/MSG_IMU_AUX.h>
+#include <libsbp/imu/MSG_IMU_COMP.h>
 #include <libsbp/imu/MSG_IMU_RAW.h>
 
 #endif /* LIBSBP_IMU_MESSAGES_H */

c/include/libsbp/imu/MSG_IMU_COMP.h

@@ -0,0 +1,215 @@
+/*
+ * Copyright (C) 2015-2021 Swift Navigation Inc.
+ * Contact: https://support.swiftnav.com
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+/*****************************************************************************
+ * Automatically generated from yaml/swiftnav/sbp/imu.yaml
+ * with generate.py. Please do not hand edit!
+ *****************************************************************************/
+
+#ifndef LIBSBP_IMU_MSG_IMU_COMP_H
+#define LIBSBP_IMU_MSG_IMU_COMP_H
+
+#include <math.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <libsbp/common.h>
+#include <libsbp/imu_macros.h>
+#include <libsbp/string/sbp_string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/******************************************************************************
+ *
+ * SBP_MSG_IMU_COMP
+ *
+ *****************************************************************************/
+/** Compensated IMU data
+ *
+ * Data from the Inertial Measurement Unit, containing accelerometer and
+ * gyroscope readings compensated for estimated errors and constant  physical
+ * effects. The output is valid for inertially referenced center  of navigation
+ * (IMU body frame) represented in vehicle body frame.
+ */
+typedef struct {
+  /**
+   * Microseconds since reference epoch [microseconds]
+   */
+  u64 time;
+
+  /**
+   * Contains the applied compensation parameters and time synchronization mode
+   */
+  u16 flags;
+
+  /**
+   * Compensated acceleration X axis [1e-6 m/s^2]
+   */
+  s32 acc_comp_x;
+
+  /**
+   * Compensated acceleration Y axis [1e-6 m/s^2]
+   */
+  s32 acc_comp_y;
+
+  /**
+   * Compensated acceleration Z axis [1e-6 m/s^2]
+   */
+  s32 acc_comp_z;
+
+  /**
+   * Compensated angular rate X axis [1e-6 deg/s]
+   */
+  s32 gyr_comp_x;
+
+  /**
+   * Compensated angular rate Y axis [1e-6 deg/s]
+   */
+  s32 gyr_comp_y;
+
+  /**
+   * Compensated angular rate Z axis [1e-6 deg/s]
+   */
+  s32 gyr_comp_z;
+} sbp_msg_imu_comp_t;
+
+/**
+ * Get encoded size of an instance of sbp_msg_imu_comp_t
+ *
+ * @param msg sbp_msg_imu_comp_t instance
+ * @return Length of on-wire representation
+ */
+static inline size_t sbp_msg_imu_comp_encoded_len(
+    const sbp_msg_imu_comp_t *msg) {
+  (void)msg;
+  return SBP_MSG_IMU_COMP_ENCODED_LEN;
+}
+
+/**
+ * Encode an instance of sbp_msg_imu_comp_t to wire representation
+ *
+ * This function encodes the given instance in to the user provided buffer. The
+ * buffer provided to this function must be large enough to store the encoded
+ * message otherwise it will return SBP_ENCODE_ERROR without writing anything to
+ * the buffer.
+ *
+ * Specify the length of the destination buffer in the \p len parameter. If
+ * non-null the number of bytes written to the buffer will be returned in \p
+ * n_written.
+ *
+ * @param buf Destination buffer
+ * @param len Length of \p buf
+ * @param n_written If not null, on success will be set to the number of bytes
+ * written to \p buf
+ * @param msg Instance of sbp_msg_imu_comp_t to encode
+ * @return SBP_OK on success, or other libsbp error code
+ */
+SBP_EXPORT s8 sbp_msg_imu_comp_encode(uint8_t *buf, uint8_t len,
+                                      uint8_t *n_written,
+                                      const sbp_msg_imu_comp_t *msg);
+
+/**
+ * Decode an instance of sbp_msg_imu_comp_t from wire representation
+ *
+ * This function decodes the wire representation of a sbp_msg_imu_comp_t message
+ * to the given instance. The caller must specify the length of the buffer in
+ * the \p len parameter. If non-null the number of bytes read from the buffer
+ * will be returned in \p n_read.
+ *
+ * @param buf Wire representation of the sbp_msg_imu_comp_t instance
+ * @param len Length of \p buf
+ * @param n_read If not null, on success will be set to the number of bytes read
+ * from \p buf
+ * @param msg Destination
+ * @return SBP_OK on success, or other libsbp error code
+ */
+SBP_EXPORT s8 sbp_msg_imu_comp_decode(const uint8_t *buf, uint8_t len,
+                                      uint8_t *n_read, sbp_msg_imu_comp_t *msg);
+/**
+ * Send an instance of sbp_msg_imu_comp_t with the given write function
+ *
+ * An equivalent of #sbp_message_send which operates specifically on
+ * sbp_msg_imu_comp_t
+ *
+ * The given message will be encoded to wire representation and passed in to the
+ * given write function callback. The write callback will be called several
+ * times for each invocation of this function.
+ *
+ * @param s SBP state
+ * @param sender_id SBP sender id
+ * @param msg Message to send
+ * @param write Write function
+ * @return SBP_OK on success, or other libsbp error code
+ */
+SBP_EXPORT s8 sbp_msg_imu_comp_send(sbp_state_t *s, u16 sender_id,
+                                    const sbp_msg_imu_comp_t *msg,
+                                    sbp_write_fn_t write);
+
+/**
+ * Compare two instances of sbp_msg_imu_comp_t
+ *
+ * The two instances will be compared and a value returned consistent with the
+ * return codes of comparison functions from the C standard library
+ *
+ * 0 will be returned if \p a and \p b are considered equal
+ * A value less than 0 will be returned if \p a is considered to be less than \p
+ * b A value greater than 0 will be returned if \p b is considered to be greater
+ * than \p b
+ *
+ * @param a sbp_msg_imu_comp_t instance
+ * @param b sbp_msg_imu_comp_t instance
+ * @return 0, <0, >0
+ */
+SBP_EXPORT int sbp_msg_imu_comp_cmp(const sbp_msg_imu_comp_t *a,
+                                    const sbp_msg_imu_comp_t *b);
+
+#ifdef __cplusplus
+}
+
+static inline bool operator==(const sbp_msg_imu_comp_t &lhs,
+                              const sbp_msg_imu_comp_t &rhs) {
+  return sbp_msg_imu_comp_cmp(&lhs, &rhs) == 0;
+}
+
+static inline bool operator!=(const sbp_msg_imu_comp_t &lhs,
+                              const sbp_msg_imu_comp_t &rhs) {
+  return sbp_msg_imu_comp_cmp(&lhs, &rhs) != 0;
+}
+
+static inline bool operator<(const sbp_msg_imu_comp_t &lhs,
+                             const sbp_msg_imu_comp_t &rhs) {
+  return sbp_msg_imu_comp_cmp(&lhs, &rhs) < 0;
+}
+
+static inline bool operator<=(const sbp_msg_imu_comp_t &lhs,
+                              const sbp_msg_imu_comp_t &rhs) {
+  return sbp_msg_imu_comp_cmp(&lhs, &rhs) <= 0;
+}
+
+static inline bool operator>(const sbp_msg_imu_comp_t &lhs,
+                             const sbp_msg_imu_comp_t &rhs) {
+  return sbp_msg_imu_comp_cmp(&lhs, &rhs) > 0;
+}
+
+static inline bool operator>=(const sbp_msg_imu_comp_t &lhs,
+                              const sbp_msg_imu_comp_t &rhs) {
+  return sbp_msg_imu_comp_cmp(&lhs, &rhs) >= 0;
+}
+
+#endif  // ifdef __cplusplus
+
+#endif /* LIBSBP_IMU_MSG_IMU_COMP_H */

c/include/libsbp/imu_macros.h

@@ -129,4 +129,97 @@
  */
 #define SBP_MSG_IMU_AUX_ENCODED_LEN 4u
 
+#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK (0x1u)
+#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT (6u)
+#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_GET(flags)                 \
+  ((u16)((u16)((flags) >> SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT) & \
+         SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK))
+#define SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SET(flags, val)           \
+  do {                                                                    \
+    (flags) = (u16)(                                                      \
+        (flags & (~(SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK          \
+                    << SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT))) | \
+        (((val) & (SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_MASK))         \
+         << (SBP_IMU_COMP_GYROSCOPEERRORSCOMPENSATED_SHIFT)));            \
+  } while (0)
+
+#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK (0x1u)
+#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT (5u)
+#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_GET(flags)                 \
+  ((u16)((u16)((flags) >> SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT) & \
+         SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK))
+#define SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SET(flags, val)           \
+  do {                                                                        \
+    (flags) = (u16)(                                                          \
+        (flags & (~(SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK          \
+                    << SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT))) | \
+        (((val) & (SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_MASK))         \
+         << (SBP_IMU_COMP_ACCELEROMETERERRORSCOMPENSATED_SHIFT)));            \
+  } while (0)
+
+#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK (0x1u)
+#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT (4u)
+#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_GET(flags)                 \
+  ((u16)((u16)((flags) >> SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT) & \
+         SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK))
+#define SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SET(flags, val)                 \
+  do {                                                                         \
+    (flags) =                                                                  \
+        (u16)((flags & (~(SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK          \
+                          << SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT))) | \
+              (((val) & (SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_MASK))         \
+               << (SBP_IMU_COMP_CORIOLISEFFECTCOMPENSATED_SHIFT)));            \
+  } while (0)
+
+#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK (0x1u)
+#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT (3u)
+#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_GET(flags)                 \
+  ((u16)((u16)((flags) >> SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT) & \
+         SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK))
+#define SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SET(flags, val)           \
+  do {                                                                      \
+    (flags) = (u16)(                                                        \
+        (flags & (~(SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK          \
+                    << SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT))) | \
+        (((val) & (SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_MASK))         \
+         << (SBP_IMU_COMP_EARTHRORATIONRATECOMPENSATED_SHIFT)));            \
+  } while (0)
+
+#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK (0x1u)
+#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT (2u)
+#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_GET(flags)                 \
+  ((u16)((u16)((flags) >> SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT) & \
+         SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK))
+#define SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SET(flags, val)                 \
+  do {                                                                       \
+    (flags) =                                                                \
+        (u16)((flags & (~(SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK          \
+                          << SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT))) | \
+              (((val) & (SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_MASK))         \
+               << (SBP_IMU_COMP_EARTHGRAVITYCOMPENSATED_SHIFT)));            \
+  } while (0)
+
+#define SBP_IMU_COMP_TIME_STATUS_MASK (0x3u)
+#define SBP_IMU_COMP_TIME_STATUS_SHIFT (0u)
+#define SBP_IMU_COMP_TIME_STATUS_GET(flags)                 \
+  ((u16)((u16)((flags) >> SBP_IMU_COMP_TIME_STATUS_SHIFT) & \
+         SBP_IMU_COMP_TIME_STATUS_MASK))
+#define SBP_IMU_COMP_TIME_STATUS_SET(flags, val)                       \
+  do {                                                                 \
+    (flags) = (u16)((flags & (~(SBP_IMU_COMP_TIME_STATUS_MASK          \
+                                << SBP_IMU_COMP_TIME_STATUS_SHIFT))) | \
+                    (((val) & (SBP_IMU_COMP_TIME_STATUS_MASK))         \
+                     << (SBP_IMU_COMP_TIME_STATUS_SHIFT)));            \
+  } while (0)
+
+#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_START_OF_CURRENT_GPS_WEEK \
+  (0)
+#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_TIME_OF_SYSTEM_STARTUP (1)
+#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_UNKNOWN (2)
+#define SBP_IMU_COMP_TIME_STATUS_REFERENCE_EPOCH_IS_LAST_PPS (3)
+/**
+ * Encoded length of sbp_msg_imu_comp_t
+ */
+#define SBP_MSG_IMU_COMP_ENCODED_LEN 34u
+
 #endif /* LIBSBP_IMU_MACROS_H */