diff --git a/sbp.cabal b/sbp.cabal --- a/sbp.cabal +++ b/sbp.cabal @@ -1,5 +1,5 @@ name: sbp -version: 6.3.3 +version: 6.5.0 synopsis: SwiftNav's SBP Library homepage: https://github.com/swift-nav/libsbp license: MIT diff --git a/src/SwiftNav/SBP/Msg.hs b/src/SwiftNav/SBP/Msg.hs --- a/src/SwiftNav/SBP/Msg.hs +++ b/src/SwiftNav/SBP/Msg.hs @@ -193,6 +193,7 @@ | SBPMsgOdometry MsgOdometry Msg | SBPMsgOrientEuler MsgOrientEuler Msg | SBPMsgOrientQuat MsgOrientQuat Msg + | SBPMsgOrientQuatCov MsgOrientQuatCov Msg | SBPMsgOsr MsgOsr Msg | SBPMsgPosEcef MsgPosEcef Msg | SBPMsgPosEcefCov MsgPosEcefCov Msg @@ -208,6 +209,7 @@ | SBPMsgPoseRelative MsgPoseRelative Msg | SBPMsgPpsTime MsgPpsTime Msg | SBPMsgPrintDep MsgPrintDep Msg + | SBPMsgProfilingQueueInfo MsgProfilingQueueInfo Msg | SBPMsgProfilingResourceCounter MsgProfilingResourceCounter Msg | SBPMsgProfilingSystemInfo MsgProfilingSystemInfo Msg | SBPMsgProfilingThreadInfo MsgProfilingThreadInfo Msg @@ -446,6 +448,7 @@ | _msgSBPType == msgOdometry = SBPMsgOdometry (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgOrientEuler = SBPMsgOrientEuler (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgOrientQuat = SBPMsgOrientQuat (decode (fromStrict (unBytes _msgSBPPayload))) m + | _msgSBPType == msgOrientQuatCov = SBPMsgOrientQuatCov (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgOsr = SBPMsgOsr (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgPosEcef = SBPMsgPosEcef (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgPosEcefCov = SBPMsgPosEcefCov (decode (fromStrict (unBytes _msgSBPPayload))) m @@ -461,6 +464,7 @@ | _msgSBPType == msgPoseRelative = SBPMsgPoseRelative (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgPpsTime = SBPMsgPpsTime (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgPrintDep = SBPMsgPrintDep (decode (fromStrict (unBytes _msgSBPPayload))) m + | _msgSBPType == msgProfilingQueueInfo = SBPMsgProfilingQueueInfo (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgProfilingResourceCounter = SBPMsgProfilingResourceCounter (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgProfilingSystemInfo = SBPMsgProfilingSystemInfo (decode (fromStrict (unBytes _msgSBPPayload))) m | _msgSBPType == msgProfilingThreadInfo = SBPMsgProfilingThreadInfo (decode (fromStrict (unBytes _msgSBPPayload))) m @@ -691,6 +695,7 @@ encoder (SBPMsgOdometry _ m) = put m encoder (SBPMsgOrientEuler _ m) = put m encoder (SBPMsgOrientQuat _ m) = put m + encoder (SBPMsgOrientQuatCov _ m) = put m encoder (SBPMsgOsr _ m) = put m encoder (SBPMsgPosEcef _ m) = put m encoder (SBPMsgPosEcefCov _ m) = put m @@ -706,6 +711,7 @@ encoder (SBPMsgPoseRelative _ m) = put m encoder (SBPMsgPpsTime _ m) = put m encoder (SBPMsgPrintDep _ m) = put m + encoder (SBPMsgProfilingQueueInfo _ m) = put m encoder (SBPMsgProfilingResourceCounter _ m) = put m encoder (SBPMsgProfilingSystemInfo _ m) = put m encoder (SBPMsgProfilingThreadInfo _ m) = put m @@ -940,6 +946,7 @@ | msgType == msgOdometry = SBPMsgOdometry <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgOrientEuler = SBPMsgOrientEuler <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgOrientQuat = SBPMsgOrientQuat <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj + | msgType == msgOrientQuatCov = SBPMsgOrientQuatCov <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgOsr = SBPMsgOsr <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgPosEcef = SBPMsgPosEcef <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgPosEcefCov = SBPMsgPosEcefCov <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj @@ -955,6 +962,7 @@ | msgType == msgPoseRelative = SBPMsgPoseRelative <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgPpsTime = SBPMsgPpsTime <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgPrintDep = SBPMsgPrintDep <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj + | msgType == msgProfilingQueueInfo = SBPMsgProfilingQueueInfo <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgProfilingResourceCounter = SBPMsgProfilingResourceCounter <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgProfilingSystemInfo = SBPMsgProfilingSystemInfo <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj | msgType == msgProfilingThreadInfo = SBPMsgProfilingThreadInfo <$> pure (decode (fromStrict (unBytes payload))) <*> parseJSON obj @@ -1190,6 +1198,7 @@ toJSON (SBPMsgOdometry n m) = toJSON n <<>> toJSON m toJSON (SBPMsgOrientEuler n m) = toJSON n <<>> toJSON m toJSON (SBPMsgOrientQuat n m) = toJSON n <<>> toJSON m + toJSON (SBPMsgOrientQuatCov n m) = toJSON n <<>> toJSON m toJSON (SBPMsgOsr n m) = toJSON n <<>> toJSON m toJSON (SBPMsgPosEcef n m) = toJSON n <<>> toJSON m toJSON (SBPMsgPosEcefCov n m) = toJSON n <<>> toJSON m @@ -1205,6 +1214,7 @@ toJSON (SBPMsgPoseRelative n m) = toJSON n <<>> toJSON m toJSON (SBPMsgPpsTime n m) = toJSON n <<>> toJSON m toJSON (SBPMsgPrintDep n m) = toJSON n <<>> toJSON m + toJSON (SBPMsgProfilingQueueInfo n m) = toJSON n <<>> toJSON m toJSON (SBPMsgProfilingResourceCounter n m) = toJSON n <<>> toJSON m toJSON (SBPMsgProfilingSystemInfo n m) = toJSON n <<>> toJSON m toJSON (SBPMsgProfilingThreadInfo n m) = toJSON n <<>> toJSON m @@ -1434,6 +1444,7 @@ msg f (SBPMsgOdometry n m) = SBPMsgOdometry n <$> f m msg f (SBPMsgOrientEuler n m) = SBPMsgOrientEuler n <$> f m msg f (SBPMsgOrientQuat n m) = SBPMsgOrientQuat n <$> f m + msg f (SBPMsgOrientQuatCov n m) = SBPMsgOrientQuatCov n <$> f m msg f (SBPMsgOsr n m) = SBPMsgOsr n <$> f m msg f (SBPMsgPosEcef n m) = SBPMsgPosEcef n <$> f m msg f (SBPMsgPosEcefCov n m) = SBPMsgPosEcefCov n <$> f m @@ -1449,6 +1460,7 @@ msg f (SBPMsgPoseRelative n m) = SBPMsgPoseRelative n <$> f m msg f (SBPMsgPpsTime n m) = SBPMsgPpsTime n <$> f m msg f (SBPMsgPrintDep n m) = SBPMsgPrintDep n <$> f m + msg f (SBPMsgProfilingQueueInfo n m) = SBPMsgProfilingQueueInfo n <$> f m msg f (SBPMsgProfilingResourceCounter n m) = SBPMsgProfilingResourceCounter n <$> f m msg f (SBPMsgProfilingSystemInfo n m) = SBPMsgProfilingSystemInfo n <$> f m msg f (SBPMsgProfilingThreadInfo n m) = SBPMsgProfilingThreadInfo n <$> f m diff --git a/src/SwiftNav/SBP/Orientation.hs b/src/SwiftNav/SBP/Orientation.hs --- a/src/SwiftNav/SBP/Orientation.hs +++ b/src/SwiftNav/SBP/Orientation.hs @@ -239,3 +239,88 @@ $(makeJSON "_msgAngularRate_" ''MsgAngularRate) $(makeLenses ''MsgAngularRate) +msgOrientQuatCov :: Word16 +msgOrientQuatCov = 0x0223 + +-- | SBP class for message MSG_ORIENT_QUAT_COV (0x0223). +-- +-- This message reports the orientation as a unit quaternion together with the +-- upper triangle of the symmetric 3x3 attitude covariance matrix and a GPS +-- time-of-week time-tag. The reference frame of the quaternion and the +-- parameterization of the covariance matrix are both encoded in the flags +-- field, allowing additional frames or parameterizations to be added later +-- without introducing a new message. By default the quaternion describes the +-- orientation of the vehicle body frame with respect to a local-level NED +-- frame (matching MSG_ORIENT_QUAT) and the covariance is expressed as small- +-- angle rotation errors about the axes of that NED frame; in this default +-- case the cov_x_x, cov_y_y, cov_z_z diagonal entries correspond to the +-- variance of the rotation error about North, East, and Down respectively. +-- The components of the quaternion sum to a unit vector assuming that the LSB +-- of each component has a value of 2^-31. +data MsgOrientQuatCov = MsgOrientQuatCov + { _msgOrientQuatCov_tow :: !Word32 + -- ^ GPS Time of Week + , _msgOrientQuatCov_w :: !Int32 + -- ^ Real component + , _msgOrientQuatCov_x :: !Int32 + -- ^ 1st imaginary component + , _msgOrientQuatCov_y :: !Int32 + -- ^ 2nd imaginary component + , _msgOrientQuatCov_z :: !Int32 + -- ^ 3rd imaginary component + , _msgOrientQuatCov_cov_x_x :: !Float + -- ^ Estimated variance of the rotation error about the 1st axis of the + -- covariance frame + , _msgOrientQuatCov_cov_x_y :: !Float + -- ^ Estimated covariance of the rotation errors about the 1st and 2nd axes + -- of the covariance frame + , _msgOrientQuatCov_cov_x_z :: !Float + -- ^ Estimated covariance of the rotation errors about the 1st and 3rd axes + -- of the covariance frame + , _msgOrientQuatCov_cov_y_y :: !Float + -- ^ Estimated variance of the rotation error about the 2nd axis of the + -- covariance frame + , _msgOrientQuatCov_cov_y_z :: !Float + -- ^ Estimated covariance of the rotation errors about the 2nd and 3rd axes + -- of the covariance frame + , _msgOrientQuatCov_cov_z_z :: !Float + -- ^ Estimated variance of the rotation error about the 3rd axis of the + -- covariance frame + , _msgOrientQuatCov_flags :: !Word8 + -- ^ Status flags + } deriving ( Show, Read, Eq ) + +instance Binary MsgOrientQuatCov where + get = do + _msgOrientQuatCov_tow <- getWord32le + _msgOrientQuatCov_w <- (fromIntegral <$> getWord32le) + _msgOrientQuatCov_x <- (fromIntegral <$> getWord32le) + _msgOrientQuatCov_y <- (fromIntegral <$> getWord32le) + _msgOrientQuatCov_z <- (fromIntegral <$> getWord32le) + _msgOrientQuatCov_cov_x_x <- getFloat32le + _msgOrientQuatCov_cov_x_y <- getFloat32le + _msgOrientQuatCov_cov_x_z <- getFloat32le + _msgOrientQuatCov_cov_y_y <- getFloat32le + _msgOrientQuatCov_cov_y_z <- getFloat32le + _msgOrientQuatCov_cov_z_z <- getFloat32le + _msgOrientQuatCov_flags <- getWord8 + pure MsgOrientQuatCov {..} + + put MsgOrientQuatCov {..} = do + putWord32le _msgOrientQuatCov_tow + (putWord32le . fromIntegral) _msgOrientQuatCov_w + (putWord32le . fromIntegral) _msgOrientQuatCov_x + (putWord32le . fromIntegral) _msgOrientQuatCov_y + (putWord32le . fromIntegral) _msgOrientQuatCov_z + putFloat32le _msgOrientQuatCov_cov_x_x + putFloat32le _msgOrientQuatCov_cov_x_y + putFloat32le _msgOrientQuatCov_cov_x_z + putFloat32le _msgOrientQuatCov_cov_y_y + putFloat32le _msgOrientQuatCov_cov_y_z + putFloat32le _msgOrientQuatCov_cov_z_z + putWord8 _msgOrientQuatCov_flags + +$(makeSBP 'msgOrientQuatCov ''MsgOrientQuatCov) +$(makeJSON "_msgOrientQuatCov_" ''MsgOrientQuatCov) +$(makeLenses ''MsgOrientQuatCov) + diff --git a/src/SwiftNav/SBP/Profiling.hs b/src/SwiftNav/SBP/Profiling.hs --- a/src/SwiftNav/SBP/Profiling.hs +++ b/src/SwiftNav/SBP/Profiling.hs @@ -98,7 +98,7 @@ -- | SBP class for message MSG_PROFILING_SYSTEM_INFO (0xCF01). -- -- Contains basic information about system resource usage. System is defined --- in terms of the source of this message and may vary from sender to sender. +-- in terms of the source of this message and may vary from sender to sender. -- Refer to product documentation to understand the exact scope and meaning of -- this message. data MsgProfilingSystemInfo = MsgProfilingSystemInfo @@ -259,4 +259,76 @@ $(makeSBP 'msgProfilingResourceCounter ''MsgProfilingResourceCounter) $(makeJSON "_msgProfilingResourceCounter_" ''MsgProfilingResourceCounter) $(makeLenses ''MsgProfilingResourceCounter) + +-- | QueueInfo. +-- +-- Profiling information for a single swiftlet internal message queue type. +data QueueInfo = QueueInfo + { _queueInfo_timestamp :: !Word64 + -- ^ Timestamp in milliseconds + , _queueInfo_name :: !Text + -- ^ Queue type name + , _queueInfo_size :: !Word16 + -- ^ Total number of slots in the queue + , _queueInfo_current_fill :: !Word16 + -- ^ Number of slots currently in use + , _queueInfo_peak_fill :: !Word16 + -- ^ Peak number of slots used since init + , _queueInfo_drop_count :: !Word16 + -- ^ Number of messages dropped since init + } deriving ( Show, Read, Eq ) + +instance Binary QueueInfo where + get = do + _queueInfo_timestamp <- getWord64le + _queueInfo_name <- decodeUtf8 <$> getByteString 40 + _queueInfo_size <- getWord16le + _queueInfo_current_fill <- getWord16le + _queueInfo_peak_fill <- getWord16le + _queueInfo_drop_count <- getWord16le + pure QueueInfo {..} + + put QueueInfo {..} = do + putWord64le _queueInfo_timestamp + putByteString $ encodeUtf8 _queueInfo_name + putWord16le _queueInfo_size + putWord16le _queueInfo_current_fill + putWord16le _queueInfo_peak_fill + putWord16le _queueInfo_drop_count + +$(makeJSON "_queueInfo_" ''QueueInfo) +$(makeLenses ''QueueInfo) + +msgProfilingQueueInfo :: Word16 +msgProfilingQueueInfo = 0xCF04 + +-- | SBP class for message MSG_PROFILING_QUEUE_INFO (0xCF04). +-- +-- Contains profiling information for swiftlet internal message queues. Refer +-- to product documentation to understand the meaning and values in this +-- message. +data MsgProfilingQueueInfo = MsgProfilingQueueInfo + { _msgProfilingQueueInfo_seq_no :: !Word8 + -- ^ Message number in complete sequence + , _msgProfilingQueueInfo_seq_len :: !Word8 + -- ^ Length of message sequence + , _msgProfilingQueueInfo_queues :: ![QueueInfo] + -- ^ List of queue stats + } deriving ( Show, Read, Eq ) + +instance Binary MsgProfilingQueueInfo where + get = do + _msgProfilingQueueInfo_seq_no <- getWord8 + _msgProfilingQueueInfo_seq_len <- getWord8 + _msgProfilingQueueInfo_queues <- whileM (not <$> isEmpty) get + pure MsgProfilingQueueInfo {..} + + put MsgProfilingQueueInfo {..} = do + putWord8 _msgProfilingQueueInfo_seq_no + putWord8 _msgProfilingQueueInfo_seq_len + mapM_ put _msgProfilingQueueInfo_queues + +$(makeSBP 'msgProfilingQueueInfo ''MsgProfilingQueueInfo) +$(makeJSON "_msgProfilingQueueInfo_" ''MsgProfilingQueueInfo) +$(makeLenses ''MsgProfilingQueueInfo)