Improvements to MSG_VEL_COG [TES-129]

c/include/libsbp/legacy/navigation.h

@@ -653,26 +653,29 @@ typedef struct SBP_ATTR_PACKED {
  *
  * This message reports the receiver course over ground (COG) and speed over
  * ground (SOG) based on the horizontal (N-E) components of the NED velocity
- * vector. It also includes the vertical velocity in the form of the
- * D-component of the NED velocity vector. The NED coordinate system is
- * defined as the local WGS84 tangent  plane centered at the current position.
- * The full GPS time is given by the  preceding MSG_GPS_TIME with the matching
- * time-of-week (tow). Note: course over ground represents the receiver's
- * direction of travel,  but not necessarily the device heading.
- */
-
-typedef struct SBP_ATTR_PACKED {
-  u32 tow;   /**< GPS Time of Week [ms] */
-  u32 cog;   /**< Course over ground relative to local north [microdegrees] */
-  u32 sog;   /**< Speed over ground [mm/s] */
-  s32 vel_d; /**< Velocity Down coordinate [mm/s] */
-  u32 cog_accuracy;   /**< Course over ground estimated standard deviation
-                         [microdegrees] */
-  u32 sog_accuracy;   /**< Speed over ground estimated standard deviation [mm/s]
-                       */
-  u32 vel_d_accuracy; /**< Vertical velocity estimated standard deviation [mm/s]
-                       */
-  u8 flags;           /**< Status flags */
+ * vector. It also includes the vertical velocity coordinate. A flag is
+ * provided to indicate whether the COG value has been frozen. When  the flag
+ * is set to true, the COG field is set to its last valid value until  the
+ * system exceeds a minimum velocity threshold. No other fields are  affected
+ * by this flag.  The NED coordinate system is defined as the local WGS84
+ * tangent  plane centered at the current position. The full GPS time is given
+ * by the  preceding MSG_GPS_TIME with the matching time-of-week (tow). Note:
+ * course over ground represents the receiver's direction of travel,  but not
+ * necessarily the device heading.
+ */
+
+typedef struct SBP_ATTR_PACKED {
+  u32 tow; /**< GPS Time of Week [ms] */
+  u32 cog; /**< Course over ground relative to north direction [microdegrees] */
+  u32 sog; /**< Speed over ground (based on horizontal velocity) [mm/s] */
+  s32 v_up;          /**< Vertical velocity component (positive up) [mm/s] */
+  u32 cog_accuracy;  /**< Course over ground estimated standard deviation
+                        [microdegrees] */
+  u32 sog_accuracy;  /**< Speed over ground estimated standard deviation [mm/s]
+                      */
+  u32 v_up_accuracy; /**< Vertical velocity estimated standard deviation [mm/s]
+                      */
+  u16 flags;         /**< Status flags */
 } msg_vel_cog_t;
 
 /** Age of corrections

c/include/libsbp/navigation_macros.h

@@ -925,68 +925,96 @@
 #define SBP_MSG_VEL_BODY_ENCODED_LEN 42u
 
 #define SBP_MSG_VEL_COG 0x021C
+#define SBP_VEL_COG_COG_FROZEN_MASK (0x1)
+#define SBP_VEL_COG_COG_FROZEN_SHIFT (9u)
+#define SBP_VEL_COG_COG_FROZEN_GET(flags)            \
+  ((u16)(((flags) >> SBP_VEL_COG_COG_FROZEN_SHIFT) & \
+         SBP_VEL_COG_COG_FROZEN_MASK))
+#define SBP_VEL_COG_COG_FROZEN_SET(flags, val)                         \
+  do {                                                                 \
+    (flags) = (u16)((flags) | (((val) & (SBP_VEL_COG_COG_FROZEN_MASK)) \
+                               << (SBP_VEL_COG_COG_FROZEN_SHIFT)));    \
+  } while (0)
+
+#define SBP_VEL_COG_COG_FROZEN_NOT_FROZEN (0)
+#define SBP_VEL_COG_COG_FROZEN_FROZEN (1)
 #define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_MASK (0x1)
-#define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SHIFT (5u)
-#define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_GET(flags)           \
-  ((u8)(((flags) >> SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SHIFT) & \
-        SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_MASK))
-#define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SET(flags, val)              \
-  do {                                                                      \
-    (flags) = (u8)((flags) |                                                \
-                   (((val) & (SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_MASK)) \
-                    << (SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SHIFT)));    \
+#define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SHIFT (8u)
+#define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_GET(flags)            \
+  ((u16)(((flags) >> SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SHIFT) & \
+         SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_MASK))
+#define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SET(flags, val)               \
+  do {                                                                       \
+    (flags) = (u16)((flags) |                                                \
+                    (((val) & (SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_MASK)) \
+                     << (SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_SHIFT)));    \
   } while (0)
 
 #define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_INVALID (0)
 #define SBP_VEL_COG_VERTICAL_VELOCITY_VALIDITY_VERTICAL_VELOCITY_VALID (1)
 #define SBP_VEL_COG_SOG_VALIDITY_MASK (0x1)
-#define SBP_VEL_COG_SOG_VALIDITY_SHIFT (4u)
-#define SBP_VEL_COG_SOG_VALIDITY_GET(flags)           \
-  ((u8)(((flags) >> SBP_VEL_COG_SOG_VALIDITY_SHIFT) & \
-        SBP_VEL_COG_SOG_VALIDITY_MASK))
-#define SBP_VEL_COG_SOG_VALIDITY_SET(flags, val)                        \
-  do {                                                                  \
-    (flags) = (u8)((flags) | (((val) & (SBP_VEL_COG_SOG_VALIDITY_MASK)) \
-                              << (SBP_VEL_COG_SOG_VALIDITY_SHIFT)));    \
+#define SBP_VEL_COG_SOG_VALIDITY_SHIFT (7u)
+#define SBP_VEL_COG_SOG_VALIDITY_GET(flags)            \
+  ((u16)(((flags) >> SBP_VEL_COG_SOG_VALIDITY_SHIFT) & \
+         SBP_VEL_COG_SOG_VALIDITY_MASK))
+#define SBP_VEL_COG_SOG_VALIDITY_SET(flags, val)                         \
+  do {                                                                   \
+    (flags) = (u16)((flags) | (((val) & (SBP_VEL_COG_SOG_VALIDITY_MASK)) \
+                               << (SBP_VEL_COG_SOG_VALIDITY_SHIFT)));    \
   } while (0)
 
 #define SBP_VEL_COG_SOG_VALIDITY_INVALID (0)
 #define SBP_VEL_COG_SOG_VALIDITY_SOG_VALID (1)
 #define SBP_VEL_COG_COG_VALIDITY_MASK (0x1)
-#define SBP_VEL_COG_COG_VALIDITY_SHIFT (3u)
-#define SBP_VEL_COG_COG_VALIDITY_GET(flags)           \
-  ((u8)(((flags) >> SBP_VEL_COG_COG_VALIDITY_SHIFT) & \
-        SBP_VEL_COG_COG_VALIDITY_MASK))
-#define SBP_VEL_COG_COG_VALIDITY_SET(flags, val)                        \
-  do {                                                                  \
-    (flags) = (u8)((flags) | (((val) & (SBP_VEL_COG_COG_VALIDITY_MASK)) \
-                              << (SBP_VEL_COG_COG_VALIDITY_SHIFT)));    \
+#define SBP_VEL_COG_COG_VALIDITY_SHIFT (6u)
+#define SBP_VEL_COG_COG_VALIDITY_GET(flags)            \
+  ((u16)(((flags) >> SBP_VEL_COG_COG_VALIDITY_SHIFT) & \
+         SBP_VEL_COG_COG_VALIDITY_MASK))
+#define SBP_VEL_COG_COG_VALIDITY_SET(flags, val)                         \
+  do {                                                                   \
+    (flags) = (u16)((flags) | (((val) & (SBP_VEL_COG_COG_VALIDITY_MASK)) \
+                               << (SBP_VEL_COG_COG_VALIDITY_SHIFT)));    \
   } while (0)
 
 #define SBP_VEL_COG_COG_VALIDITY_INVALID (0)
 #define SBP_VEL_COG_COG_VALIDITY_COG_VALID (1)
-#define SBP_VEL_COG_INS_NAVIGATION_MODE_MASK (0x1)
-#define SBP_VEL_COG_INS_NAVIGATION_MODE_SHIFT (2u)
-#define SBP_VEL_COG_INS_NAVIGATION_MODE_GET(flags)           \
-  ((u8)(((flags) >> SBP_VEL_COG_INS_NAVIGATION_MODE_SHIFT) & \
-        SBP_VEL_COG_INS_NAVIGATION_MODE_MASK))
-#define SBP_VEL_COG_INS_NAVIGATION_MODE_SET(flags, val)                        \
-  do {                                                                         \
-    (flags) = (u8)((flags) | (((val) & (SBP_VEL_COG_INS_NAVIGATION_MODE_MASK)) \
-                              << (SBP_VEL_COG_INS_NAVIGATION_MODE_SHIFT)));    \
+#define SBP_VEL_COG_TYPE_OF_REPORTED_TOW_MASK (0x1)
+#define SBP_VEL_COG_TYPE_OF_REPORTED_TOW_SHIFT (5u)
+#define SBP_VEL_COG_TYPE_OF_REPORTED_TOW_GET(flags)            \
+  ((u16)(((flags) >> SBP_VEL_COG_TYPE_OF_REPORTED_TOW_SHIFT) & \
+         SBP_VEL_COG_TYPE_OF_REPORTED_TOW_MASK))
+#define SBP_VEL_COG_TYPE_OF_REPORTED_TOW_SET(flags, val)                   \
+  do {                                                                     \
+    (flags) =                                                              \
+        (u16)((flags) | (((val) & (SBP_VEL_COG_TYPE_OF_REPORTED_TOW_MASK)) \
+                         << (SBP_VEL_COG_TYPE_OF_REPORTED_TOW_SHIFT)));    \
+  } while (0)
+
+#define SBP_VEL_COG_TYPE_OF_REPORTED_TOW_TIME_OF_MEASUREMENT (0)
+#define SBP_VEL_COG_TYPE_OF_REPORTED_TOW_OTHER (1)
+#define SBP_VEL_COG_INS_NAVIGATION_MODE_MASK (0x3)
+#define SBP_VEL_COG_INS_NAVIGATION_MODE_SHIFT (3u)
+#define SBP_VEL_COG_INS_NAVIGATION_MODE_GET(flags)            \
+  ((u16)(((flags) >> SBP_VEL_COG_INS_NAVIGATION_MODE_SHIFT) & \
+         SBP_VEL_COG_INS_NAVIGATION_MODE_MASK))
+#define SBP_VEL_COG_INS_NAVIGATION_MODE_SET(flags, val)                   \
+  do {                                                                    \
+    (flags) =                                                             \
+        (u16)((flags) | (((val) & (SBP_VEL_COG_INS_NAVIGATION_MODE_MASK)) \
+                         << (SBP_VEL_COG_INS_NAVIGATION_MODE_SHIFT)));    \
   } while (0)
 
 #define SBP_VEL_COG_INS_NAVIGATION_MODE_NONE (0)
 #define SBP_VEL_COG_INS_NAVIGATION_MODE_INS_USED (1)
-#define SBP_VEL_COG_VELOCITY_MODE_MASK (0x3)
+#define SBP_VEL_COG_VELOCITY_MODE_MASK (0x7)
 #define SBP_VEL_COG_VELOCITY_MODE_SHIFT (0u)
-#define SBP_VEL_COG_VELOCITY_MODE_GET(flags)           \
-  ((u8)(((flags) >> SBP_VEL_COG_VELOCITY_MODE_SHIFT) & \
-        SBP_VEL_COG_VELOCITY_MODE_MASK))
-#define SBP_VEL_COG_VELOCITY_MODE_SET(flags, val)                        \
-  do {                                                                   \
-    (flags) = (u8)((flags) | (((val) & (SBP_VEL_COG_VELOCITY_MODE_MASK)) \
-                              << (SBP_VEL_COG_VELOCITY_MODE_SHIFT)));    \
+#define SBP_VEL_COG_VELOCITY_MODE_GET(flags)            \
+  ((u16)(((flags) >> SBP_VEL_COG_VELOCITY_MODE_SHIFT) & \
+         SBP_VEL_COG_VELOCITY_MODE_MASK))
+#define SBP_VEL_COG_VELOCITY_MODE_SET(flags, val)                         \
+  do {                                                                    \
+    (flags) = (u16)((flags) | (((val) & (SBP_VEL_COG_VELOCITY_MODE_MASK)) \
+                               << (SBP_VEL_COG_VELOCITY_MODE_SHIFT)));    \
   } while (0)
 
 #define SBP_VEL_COG_VELOCITY_MODE_INVALID (0)
@@ -997,7 +1025,7 @@
  * Encoded length of sbp_msg_vel_cog_t (V4 API) and
  * msg_vel_cog_t (legacy API)
  */
-#define SBP_MSG_VEL_COG_ENCODED_LEN 29u
+#define SBP_MSG_VEL_COG_ENCODED_LEN 30u
 
 #define SBP_MSG_AGE_CORRECTIONS 0x0210
 /**

c/include/libsbp/v4/navigation/MSG_VEL_COG.h

@@ -42,12 +42,15 @@ extern "C" {
  *
  * This message reports the receiver course over ground (COG) and speed over
  * ground (SOG) based on the horizontal (N-E) components of the NED velocity
- * vector. It also includes the vertical velocity in the form of the D-component
- * of the NED velocity vector. The NED coordinate system is defined as the local
- * WGS84 tangent  plane centered at the current position. The full GPS time is
- * given by the  preceding MSG_GPS_TIME with the matching time-of-week (tow).
- * Note: course over ground represents the receiver's direction of travel,  but
- * not necessarily the device heading.
+ * vector. It also includes the vertical velocity coordinate. A flag is provided
+ * to indicate whether the COG value has been frozen. When  the flag is set to
+ * true, the COG field is set to its last valid value until  the system exceeds
+ * a minimum velocity threshold. No other fields are  affected by this flag. The
+ * NED coordinate system is defined as the local WGS84 tangent  plane centered
+ * at the current position. The full GPS time is given by the  preceding
+ * MSG_GPS_TIME with the matching time-of-week (tow). Note: course over ground
+ * represents the receiver's direction of travel,  but not necessarily the
+ * device heading.
  */
 typedef struct {
   /**
@@ -56,19 +59,19 @@ typedef struct {
   u32 tow;
 
   /**
-   * Course over ground relative to local north [microdegrees]
+   * Course over ground relative to north direction [microdegrees]
    */
   u32 cog;
 
   /**
-   * Speed over ground [mm/s]
+   * Speed over ground (based on horizontal velocity) [mm/s]
    */
   u32 sog;
 
   /**
-   * Velocity Down coordinate [mm/s]
+   * Vertical velocity component (positive up) [mm/s]
    */
-  s32 vel_d;
+  s32 v_up;
 
   /**
    * Course over ground estimated standard deviation [microdegrees]
@@ -83,12 +86,12 @@ typedef struct {
   /**
    * Vertical velocity estimated standard deviation [mm/s]
    */
-  u32 vel_d_accuracy;
+  u32 v_up_accuracy;
 
   /**
    * Status flags
    */
-  u8 flags;
+  u16 flags;
 } sbp_msg_vel_cog_t;
 
 /**

c/include/libsbp/version.h

@@ -28,7 +28,7 @@
 #define SBP_PATCH_VERSION 2
 
 /** Full SBP version string. */
-#define SBP_VERSION "4.1.2"
+#define SBP_VERSION "4.1.3-alpha"
 
 /** \} */
 

c/src/v4/navigation.c

@@ -4122,7 +4122,7 @@ bool sbp_msg_vel_cog_encode_internal(sbp_encode_ctx_t *ctx,
   if (!sbp_u32_encode(ctx, &msg->sog)) {
     return false;
   }
-  if (!sbp_s32_encode(ctx, &msg->vel_d)) {
+  if (!sbp_s32_encode(ctx, &msg->v_up)) {
     return false;
   }
   if (!sbp_u32_encode(ctx, &msg->cog_accuracy)) {
@@ -4131,10 +4131,10 @@ bool sbp_msg_vel_cog_encode_internal(sbp_encode_ctx_t *ctx,
   if (!sbp_u32_encode(ctx, &msg->sog_accuracy)) {
     return false;
   }
-  if (!sbp_u32_encode(ctx, &msg->vel_d_accuracy)) {
+  if (!sbp_u32_encode(ctx, &msg->v_up_accuracy)) {
     return false;
   }
-  if (!sbp_u8_encode(ctx, &msg->flags)) {
+  if (!sbp_u16_encode(ctx, &msg->flags)) {
     return false;
   }
   return true;
@@ -4166,7 +4166,7 @@ bool sbp_msg_vel_cog_decode_internal(sbp_decode_ctx_t *ctx,
   if (!sbp_u32_decode(ctx, &msg->sog)) {
     return false;
   }
-  if (!sbp_s32_decode(ctx, &msg->vel_d)) {
+  if (!sbp_s32_decode(ctx, &msg->v_up)) {
     return false;
   }
   if (!sbp_u32_decode(ctx, &msg->cog_accuracy)) {
@@ -4175,10 +4175,10 @@ bool sbp_msg_vel_cog_decode_internal(sbp_decode_ctx_t *ctx,
   if (!sbp_u32_decode(ctx, &msg->sog_accuracy)) {
     return false;
   }
-  if (!sbp_u32_decode(ctx, &msg->vel_d_accuracy)) {
+  if (!sbp_u32_decode(ctx, &msg->v_up_accuracy)) {
     return false;
   }
-  if (!sbp_u8_decode(ctx, &msg->flags)) {
+  if (!sbp_u16_decode(ctx, &msg->flags)) {
     return false;
   }
   return true;
@@ -4230,7 +4230,7 @@ int sbp_msg_vel_cog_cmp(const sbp_msg_vel_cog_t *a,
     return ret;
   }
 
-  ret = sbp_s32_cmp(&a->vel_d, &b->vel_d);
+  ret = sbp_s32_cmp(&a->v_up, &b->v_up);
   if (ret != 0) {
     return ret;
   }
@@ -4245,12 +4245,12 @@ int sbp_msg_vel_cog_cmp(const sbp_msg_vel_cog_t *a,
     return ret;
   }
 
-  ret = sbp_u32_cmp(&a->vel_d_accuracy, &b->vel_d_accuracy);
+  ret = sbp_u32_cmp(&a->v_up_accuracy, &b->v_up_accuracy);
   if (ret != 0) {
     return ret;
   }
 
-  ret = sbp_u8_cmp(&a->flags, &b->flags);
+  ret = sbp_u16_cmp(&a->flags, &b->flags);
   if (ret != 0) {
     return ret;
   }