Add new SBP message [SEN-611]

c/include/libsbp/cpp/message_traits.h

@@ -4828,6 +4828,41 @@ struct MessageTraits<sbp_msg_pos_llh_t> {
   }
 };
 
+template <>
+struct MessageTraits<sbp_msg_pose_relative_t> {
+  static constexpr sbp_msg_type_t id = SbpMsgPoseRelative;
+  static const sbp_msg_pose_relative_t &get(const sbp_msg_t &msg) {
+    return msg.pose_relative;
+  }
+  static sbp_msg_pose_relative_t &get(sbp_msg_t &msg) {
+    return msg.pose_relative;
+  }
+  static void to_sbp_msg(const sbp_msg_pose_relative_t &msg,
+                         sbp_msg_t *sbp_msg) {
+    sbp_msg->pose_relative = msg;
+  }
+  static sbp_msg_t to_sbp_msg(const sbp_msg_pose_relative_t &msg) {
+    sbp_msg_t sbp_msg;
+    sbp_msg.pose_relative = msg;
+    return sbp_msg;
+  }
+  static s8 send(sbp_state_t *state, u16 sender_id,
+                 const sbp_msg_pose_relative_t &msg, sbp_write_fn_t write) {
+    return sbp_msg_pose_relative_send(state, sender_id, &msg, write);
+  }
+  static s8 encode(uint8_t *buf, uint8_t len, uint8_t *n_written,
+                   const sbp_msg_pose_relative_t &msg) {
+    return sbp_msg_pose_relative_encode(buf, len, n_written, &msg);
+  }
+  static s8 decode(const uint8_t *buf, uint8_t len, uint8_t *n_read,
+                   sbp_msg_pose_relative_t *msg) {
+    return sbp_msg_pose_relative_decode(buf, len, n_read, msg);
+  }
+  static size_t encoded_len(const sbp_msg_pose_relative_t &msg) {
+    return sbp_msg_pose_relative_encoded_len(&msg);
+  }
+};
+
 template <>
 struct MessageTraits<sbp_msg_pps_time_t> {
   static constexpr sbp_msg_type_t id = SbpMsgPpsTime;

c/include/libsbp/legacy/cpp/message_traits.h

@@ -926,6 +926,12 @@ struct MessageTraits<msg_reference_frame_param_t> {
 };
 
 
+template<>
+struct MessageTraits<msg_pose_relative_t> {
+  static constexpr u16 id = 581;
+};
+
+
 template<>
 struct MessageTraits<msg_ndb_event_t> {
   static constexpr u16 id = 1024;

c/include/libsbp/legacy/navigation.h

@@ -969,6 +969,47 @@ typedef struct SBP_ATTR_PACKED {
   s16 dot_scale; /**< Rate of change of scale correction. [0.0000002 ppm/yr] */
 } msg_reference_frame_param_t;
 
+/** Relative Pose
+ *
+ * This solution message reports the relative pose of a sensor between two
+ * time instances. The relative pose comprises of a rotation and a translation
+ * which relates the sensor (e.g. camera) frame at a given time (first
+ * keyframe) to the sensor frame at another time (second key frame). The
+ * relative translations is a 3x1 vector described in the first keyframe.
+ * Relative rotation is described by a quaternion from second keyframe to the
+ * first keyframe.
+ */
+
+typedef struct SBP_ATTR_PACKED {
+  u32 tow;         /**< GPS Time of Week [ms] */
+  u8 sensor_id;    /**< ID of the sensor producing this message */
+  u32 timestamp_1; /**< Timestamp of first keyframe [ms] */
+  u32 timestamp_2; /**< Timestamp of second keyframe [ms] */
+  s32 trans[3];    /**< Relative translation [x,y,z] described in first
+                        keyframe [mm] */
+  s32 w;           /**< Real component of quaternion to describe relative
+                        rotation (second to first keyframe) [2^-31] */
+  s32 x;           /**< 1st imaginary component of quaternion to describe
+                        relative rotation (second to first keyframe) [2^-31] */
+  s32 y;           /**< 2nd imaginary component of quaternion to describe
+                        relative rotation (second to first keyframe) [2^-31] */
+  s32 z;           /**< 3rd imaginary component of quaternion to describe
+                        relative rotation (second to first keyframe) [2^-31] */
+  float cov_r_x_x; /**< Estimated variance of x (relative translation) [m^2] */
+  float cov_r_x_y; /**< Covariance of x and y (relative translation) [m^2] */
+  float cov_r_x_z; /**< Covariance of x and z (relative translation) [m^2] */
+  float cov_r_y_y; /**< Estimated variance of y (relative translation) [m^2] */
+  float cov_r_y_z; /**< Covariance of y and z (relative translation) [m^2] */
+  float cov_r_z_z; /**< Estimated variance of z (relative translation) [m^2] */
+  float cov_c_x_x; /**< Estimated variance of x (relative rotation) [rad^2] */
+  float cov_c_x_y; /**< Covariance of x and y (relative rotation) [rad^2] */
+  float cov_c_x_z; /**< Covariance of x and z (relative rotation) [rad^2] */
+  float cov_c_y_y; /**< Estimated variance of y (relative rotation) [rad^2] */
+  float cov_c_y_z; /**< Covariance of y and z (relative rotation) [rad^2] */
+  float cov_c_z_z; /**< Estimated variance of z (relative rotation) [rad^2] */
+  u8 flags;        /**< Status flags of relative translation and rotation */
+} msg_pose_relative_t;
+
 /** \} */
 
 SBP_PACK_END

c/include/libsbp/navigation_macros.h

@@ -1766,4 +1766,67 @@
  */
 #define SBP_MSG_REFERENCE_FRAME_PARAM_ENCODED_LEN 124u
 
+#define SBP_MSG_POSE_RELATIVE 0x0245
+/**
+ * The maximum number of items that can be stored in
+ * sbp_msg_pose_relative_t::trans (V4 API) or msg_pose_relative_t::trans (legacy
+ * API) before the maximum SBP message size is exceeded
+ */
+#define SBP_MSG_POSE_RELATIVE_TRANS_MAX 3u
+
+#define SBP_POSE_RELATIVE_TIME_SOURCE_MASK (0x3u)
+#define SBP_POSE_RELATIVE_TIME_SOURCE_SHIFT (4u)
+#define SBP_POSE_RELATIVE_TIME_SOURCE_GET(flags)               \
+  ((u8)((u8)((flags) >> SBP_POSE_RELATIVE_TIME_SOURCE_SHIFT) & \
+        SBP_POSE_RELATIVE_TIME_SOURCE_MASK))
+#define SBP_POSE_RELATIVE_TIME_SOURCE_SET(flags, val)                      \
+  do {                                                                     \
+    (flags) = (u8)((flags & (~(SBP_POSE_RELATIVE_TIME_SOURCE_MASK          \
+                               << SBP_POSE_RELATIVE_TIME_SOURCE_SHIFT))) | \
+                   (((val) & (SBP_POSE_RELATIVE_TIME_SOURCE_MASK))         \
+                    << (SBP_POSE_RELATIVE_TIME_SOURCE_SHIFT)));            \
+  } while (0)
+
+#define SBP_POSE_RELATIVE_TIME_SOURCE_NONE (0)
+#define SBP_POSE_RELATIVE_TIME_SOURCE_GNSS_SOLUTION (1)
+#define SBP_POSE_RELATIVE_TIME_SOURCE_LOCAL_CPU_TIME (2)
+#define SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_MASK (0x3u)
+#define SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_SHIFT (2u)
+#define SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_GET(flags)               \
+  ((u8)((u8)((flags) >> SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_SHIFT) & \
+        SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_MASK))
+#define SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_SET(flags, val)        \
+  do {                                                                       \
+    (flags) =                                                                \
+        (u8)((flags &                                                        \
+              (~(SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_MASK          \
+                 << SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_SHIFT))) | \
+             (((val) & (SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_MASK)) \
+              << (SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_SHIFT)));    \
+  } while (0)
+
+#define SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_INVALID (0)
+#define SBP_POSE_RELATIVE_RELATIVE_TRANSLATION_STATUS_VALID (1)
+#define SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_MASK (0x3u)
+#define SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_SHIFT (0u)
+#define SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_GET(flags)               \
+  ((u8)((u8)((flags) >> SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_SHIFT) & \
+        SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_MASK))
+#define SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_SET(flags, val)           \
+  do {                                                                       \
+    (flags) = (u8)(                                                          \
+        (flags & (~(SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_MASK          \
+                    << SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_SHIFT))) | \
+        (((val) & (SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_MASK))         \
+         << (SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_SHIFT)));            \
+  } while (0)
+
+#define SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_INVALID (0)
+#define SBP_POSE_RELATIVE_RELATIVE_ROTATION_STATUS_VALID (1)
+/**
+ * Encoded length of sbp_msg_pose_relative_t (V4 API) and
+ * msg_pose_relative_t (legacy API)
+ */
+#define SBP_MSG_POSE_RELATIVE_ENCODED_LEN 90u
+
 #endif /* LIBSBP_NAVIGATION_MACROS_H */

c/include/libsbp/sbp_msg_type.h

@@ -188,6 +188,7 @@ typedef enum {
   SbpMsgPosLlhDepA = SBP_MSG_POS_LLH_DEP_A,
   SbpMsgPosLlhGnss = SBP_MSG_POS_LLH_GNSS,
   SbpMsgPosLlh = SBP_MSG_POS_LLH,
+  SbpMsgPoseRelative = SBP_MSG_POSE_RELATIVE,
   SbpMsgPpsTime = SBP_MSG_PPS_TIME,
   SbpMsgPrintDep = SBP_MSG_PRINT_DEP,
   SbpMsgProtectionLevelDepA = SBP_MSG_PROTECTION_LEVEL_DEP_A,

c/include/libsbp/v4/navigation.h

@@ -29,6 +29,7 @@
 #include <libsbp/v4/navigation/MSG_GPS_TIME.h>
 #include <libsbp/v4/navigation/MSG_GPS_TIME_DEP_A.h>
 #include <libsbp/v4/navigation/MSG_GPS_TIME_GNSS.h>
+#include <libsbp/v4/navigation/MSG_POSE_RELATIVE.h>
 #include <libsbp/v4/navigation/MSG_POS_ECEF.h>
 #include <libsbp/v4/navigation/MSG_POS_ECEF_COV.h>
 #include <libsbp/v4/navigation/MSG_POS_ECEF_COV_GNSS.h>