diff --git a/sbp.cabal b/sbp.cabal --- a/sbp.cabal +++ b/sbp.cabal @@ -1,5 +1,5 @@ name: sbp -version: 2.1.3 +version: 2.1.5 synopsis: SwiftNav's SBP Library homepage: https://github.com/swift-nav/libsbp license: LGPL-3 @@ -31,6 +31,7 @@ , SwiftNav.SBP.FileIo , SwiftNav.SBP.Flash , SwiftNav.SBP.Gnss + , SwiftNav.SBP.Imu , SwiftNav.SBP.Logging , SwiftNav.SBP.Navigation , SwiftNav.SBP.Observation diff --git a/src/SwiftNav/SBP.hs b/src/SwiftNav/SBP.hs --- a/src/SwiftNav/SBP.hs +++ b/src/SwiftNav/SBP.hs @@ -17,6 +17,7 @@ , module SwiftNav.SBP.FileIo , module SwiftNav.SBP.Flash , module SwiftNav.SBP.Gnss + , module SwiftNav.SBP.Imu , module SwiftNav.SBP.Logging , module SwiftNav.SBP.Navigation , module SwiftNav.SBP.Observation @@ -38,6 +39,7 @@ import SwiftNav.SBP.FileIo import SwiftNav.SBP.Flash import SwiftNav.SBP.Gnss +import SwiftNav.SBP.Imu import SwiftNav.SBP.Logging import SwiftNav.SBP.Navigation import SwiftNav.SBP.Observation @@ -106,6 +108,8 @@ | SBPMsgGroupDelay MsgGroupDelay Msg | SBPMsgHeartbeat MsgHeartbeat Msg | SBPMsgIarState MsgIarState Msg + | SBPMsgImuAux MsgImuAux Msg + | SBPMsgImuRaw MsgImuRaw Msg | SBPMsgInitBase MsgInitBase Msg | SBPMsgIono MsgIono Msg | SBPMsgLog MsgLog Msg @@ -123,6 +127,7 @@ | SBPMsgPosLlhDepA MsgPosLlhDepA Msg | SBPMsgPrintDep MsgPrintDep Msg | SBPMsgReset MsgReset Msg + | SBPMsgResetDep MsgResetDep Msg | SBPMsgResetFilters MsgResetFilters Msg | SBPMsgSetTime MsgSetTime Msg | SBPMsgSettingsReadByIndexDone MsgSettingsReadByIndexDone Msg @@ -218,6 +223,8 @@ | _msgSBPType == msgGroupDelay = SBPMsgGroupDelay (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgHeartbeat = SBPMsgHeartbeat (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgIarState = SBPMsgIarState (decode (fromStrict _msgSBPPayload)) m + | _msgSBPType == msgImuAux = SBPMsgImuAux (decode (fromStrict _msgSBPPayload)) m + | _msgSBPType == msgImuRaw = SBPMsgImuRaw (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgInitBase = SBPMsgInitBase (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgIono = SBPMsgIono (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgLog = SBPMsgLog (decode (fromStrict _msgSBPPayload)) m @@ -235,6 +242,7 @@ | _msgSBPType == msgPosLlhDepA = SBPMsgPosLlhDepA (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgPrintDep = SBPMsgPrintDep (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgReset = SBPMsgReset (decode (fromStrict _msgSBPPayload)) m + | _msgSBPType == msgResetDep = SBPMsgResetDep (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgResetFilters = SBPMsgResetFilters (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgSetTime = SBPMsgSetTime (decode (fromStrict _msgSBPPayload)) m | _msgSBPType == msgSettingsReadByIndexDone = SBPMsgSettingsReadByIndexDone (decode (fromStrict _msgSBPPayload)) m @@ -322,6 +330,8 @@ encode' (SBPMsgGroupDelay _ m) = put m encode' (SBPMsgHeartbeat _ m) = put m encode' (SBPMsgIarState _ m) = put m + encode' (SBPMsgImuAux _ m) = put m + encode' (SBPMsgImuRaw _ m) = put m encode' (SBPMsgInitBase _ m) = put m encode' (SBPMsgIono _ m) = put m encode' (SBPMsgLog _ m) = put m @@ -339,6 +349,7 @@ encode' (SBPMsgPosLlhDepA _ m) = put m encode' (SBPMsgPrintDep _ m) = put m encode' (SBPMsgReset _ m) = put m + encode' (SBPMsgResetDep _ m) = put m encode' (SBPMsgResetFilters _ m) = put m encode' (SBPMsgSetTime _ m) = put m encode' (SBPMsgSettingsReadByIndexDone _ m) = put m @@ -429,6 +440,8 @@ | msgType == msgGroupDelay = SBPMsgGroupDelay <$> parseJSON obj <*> parseJSON obj | msgType == msgHeartbeat = SBPMsgHeartbeat <$> parseJSON obj <*> parseJSON obj | msgType == msgIarState = SBPMsgIarState <$> parseJSON obj <*> parseJSON obj + | msgType == msgImuAux = SBPMsgImuAux <$> parseJSON obj <*> parseJSON obj + | msgType == msgImuRaw = SBPMsgImuRaw <$> parseJSON obj <*> parseJSON obj | msgType == msgInitBase = SBPMsgInitBase <$> parseJSON obj <*> parseJSON obj | msgType == msgIono = SBPMsgIono <$> parseJSON obj <*> parseJSON obj | msgType == msgLog = SBPMsgLog <$> parseJSON obj <*> parseJSON obj @@ -446,6 +459,7 @@ | msgType == msgPosLlhDepA = SBPMsgPosLlhDepA <$> parseJSON obj <*> parseJSON obj | msgType == msgPrintDep = SBPMsgPrintDep <$> parseJSON obj <*> parseJSON obj | msgType == msgReset = SBPMsgReset <$> parseJSON obj <*> parseJSON obj + | msgType == msgResetDep = SBPMsgResetDep <$> parseJSON obj <*> parseJSON obj | msgType == msgResetFilters = SBPMsgResetFilters <$> parseJSON obj <*> parseJSON obj | msgType == msgSetTime = SBPMsgSetTime <$> parseJSON obj <*> parseJSON obj | msgType == msgSettingsReadByIndexDone = SBPMsgSettingsReadByIndexDone <$> parseJSON obj <*> parseJSON obj @@ -538,6 +552,8 @@ toJSON (SBPMsgGroupDelay n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgHeartbeat n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgIarState n m) = toJSON n `mergeValues` toJSON m + toJSON (SBPMsgImuAux n m) = toJSON n `mergeValues` toJSON m + toJSON (SBPMsgImuRaw n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgInitBase n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgIono n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgLog n m) = toJSON n `mergeValues` toJSON m @@ -555,6 +571,7 @@ toJSON (SBPMsgPosLlhDepA n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgPrintDep n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgReset n m) = toJSON n `mergeValues` toJSON m + toJSON (SBPMsgResetDep n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgResetFilters n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgSetTime n m) = toJSON n `mergeValues` toJSON m toJSON (SBPMsgSettingsReadByIndexDone n m) = toJSON n `mergeValues` toJSON m @@ -641,6 +658,8 @@ msg f (SBPMsgGroupDelay n m) = SBPMsgGroupDelay n <$> f m msg f (SBPMsgHeartbeat n m) = SBPMsgHeartbeat n <$> f m msg f (SBPMsgIarState n m) = SBPMsgIarState n <$> f m + msg f (SBPMsgImuAux n m) = SBPMsgImuAux n <$> f m + msg f (SBPMsgImuRaw n m) = SBPMsgImuRaw n <$> f m msg f (SBPMsgInitBase n m) = SBPMsgInitBase n <$> f m msg f (SBPMsgIono n m) = SBPMsgIono n <$> f m msg f (SBPMsgLog n m) = SBPMsgLog n <$> f m @@ -658,6 +677,7 @@ msg f (SBPMsgPosLlhDepA n m) = SBPMsgPosLlhDepA n <$> f m msg f (SBPMsgPrintDep n m) = SBPMsgPrintDep n <$> f m msg f (SBPMsgReset n m) = SBPMsgReset n <$> f m + msg f (SBPMsgResetDep n m) = SBPMsgResetDep n <$> f m msg f (SBPMsgResetFilters n m) = SBPMsgResetFilters n <$> f m msg f (SBPMsgSetTime n m) = SBPMsgSetTime n <$> f m msg f (SBPMsgSettingsReadByIndexDone n m) = SBPMsgSettingsReadByIndexDone n <$> f m diff --git a/src/SwiftNav/SBP/Gnss.hs b/src/SwiftNav/SBP/Gnss.hs --- a/src/SwiftNav/SBP/Gnss.hs +++ b/src/SwiftNav/SBP/Gnss.hs @@ -55,7 +55,9 @@ -- Signal identifier containing constellation, band, and satellite identifier data GnssSignal = GnssSignal { _gnssSignal_sat :: Word16 - -- ^ Constellation-specific satellite identifier + -- ^ Constellation-specific satellite identifier. Note: unlike GnssSignal16, + -- GPS satellites are encoded as (PRN - 1). Other constellations do not + -- have this offset. , _gnssSignal_code :: Word8 -- ^ Signal constellation, band and code , _gnssSignal_reserved :: Word8 diff --git a/src/SwiftNav/SBP/Imu.hs b/src/SwiftNav/SBP/Imu.hs new file mode 100644 --- /dev/null +++ b/src/SwiftNav/SBP/Imu.hs @@ -0,0 +1,118 @@ +{-# OPTIONS_GHC -fno-warn-unused-imports #-} +-- | +-- Module: SwiftNav.SBP.Imu +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- Inertial Measurement Unit (IMU) messages. + +module SwiftNav.SBP.Imu where + +import BasicPrelude as P +import Control.Lens +import Control.Monad.Loops +import Data.Aeson.TH (defaultOptions, deriveJSON, fieldLabelModifier) +import Data.Binary +import Data.Binary.Get +import Data.Binary.IEEE754 +import Data.Binary.Put +import Data.ByteString +import Data.ByteString.Lazy hiding (ByteString) +import Data.Int +import Data.Word +import SwiftNav.SBP.Encoding +import SwiftNav.SBP.TH +import SwiftNav.SBP.Types + +msgImuRaw :: Word16 +msgImuRaw = 0x0900 + +-- | SBP class for message MSG_IMU_RAW (0x0900). +-- +-- Raw data from the Inertial Measurement Unit, containing accelerometer and +-- gyroscope readings. +data MsgImuRaw = MsgImuRaw + { _msgImuRaw_tow :: Word32 + -- ^ Milliseconds since start of GPS week. If the high bit is set, the time + -- is unknown or invalid. + , _msgImuRaw_tow_f :: Word8 + -- ^ Milliseconds since start of GPS week, fractional part + , _msgImuRaw_acc_x :: Int16 + -- ^ Acceleration in the body frame X axis + , _msgImuRaw_acc_y :: Int16 + -- ^ Acceleration in the body frame Y axis + , _msgImuRaw_acc_z :: Int16 + -- ^ Acceleration in the body frame Z axis + , _msgImuRaw_gyr_x :: Int16 + -- ^ Angular rate around the body frame X axis + , _msgImuRaw_gyr_y :: Int16 + -- ^ Angular rate around the body frame Y axis + , _msgImuRaw_gyr_z :: Int16 + -- ^ Angular rate around the body frame Z axis + } deriving ( Show, Read, Eq ) + +instance Binary MsgImuRaw where + get = do + _msgImuRaw_tow <- getWord32le + _msgImuRaw_tow_f <- getWord8 + _msgImuRaw_acc_x <- fromIntegral <$> getWord16le + _msgImuRaw_acc_y <- fromIntegral <$> getWord16le + _msgImuRaw_acc_z <- fromIntegral <$> getWord16le + _msgImuRaw_gyr_x <- fromIntegral <$> getWord16le + _msgImuRaw_gyr_y <- fromIntegral <$> getWord16le + _msgImuRaw_gyr_z <- fromIntegral <$> getWord16le + return MsgImuRaw {..} + + put MsgImuRaw {..} = do + putWord32le _msgImuRaw_tow + putWord8 _msgImuRaw_tow_f + putWord16le $ fromIntegral _msgImuRaw_acc_x + putWord16le $ fromIntegral _msgImuRaw_acc_y + putWord16le $ fromIntegral _msgImuRaw_acc_z + putWord16le $ fromIntegral _msgImuRaw_gyr_x + putWord16le $ fromIntegral _msgImuRaw_gyr_y + putWord16le $ fromIntegral _msgImuRaw_gyr_z + +$(deriveSBP 'msgImuRaw ''MsgImuRaw) + +$(deriveJSON defaultOptions {fieldLabelModifier = fromMaybe "_msgImuRaw_" . P.stripPrefix "_msgImuRaw_"} + ''MsgImuRaw) +$(makeLenses ''MsgImuRaw) + +msgImuAux :: Word16 +msgImuAux = 0x0901 + +-- | SBP class for message MSG_IMU_AUX (0x0901). +-- +-- Auxiliary data specific to a particular IMU. The `imu_type` field will +-- always be consistent but the rest of the payload is device specific and +-- depends on the value of `imu_type`. +data MsgImuAux = MsgImuAux + { _msgImuAux_imu_type :: Word8 + -- ^ IMU type + , _msgImuAux_temp :: Int16 + -- ^ Raw IMU temperature + , _msgImuAux_imu_conf :: Word8 + -- ^ IMU configuration + } deriving ( Show, Read, Eq ) + +instance Binary MsgImuAux where + get = do + _msgImuAux_imu_type <- getWord8 + _msgImuAux_temp <- fromIntegral <$> getWord16le + _msgImuAux_imu_conf <- getWord8 + return MsgImuAux {..} + + put MsgImuAux {..} = do + putWord8 _msgImuAux_imu_type + putWord16le $ fromIntegral _msgImuAux_temp + putWord8 _msgImuAux_imu_conf + +$(deriveSBP 'msgImuAux ''MsgImuAux) + +$(deriveJSON defaultOptions {fieldLabelModifier = fromMaybe "_msgImuAux_" . P.stripPrefix "_msgImuAux_"} + ''MsgImuAux) +$(makeLenses ''MsgImuAux) diff --git a/src/SwiftNav/SBP/Navigation.hs b/src/SwiftNav/SBP/Navigation.hs --- a/src/SwiftNav/SBP/Navigation.hs +++ b/src/SwiftNav/SBP/Navigation.hs @@ -215,7 +215,7 @@ , _msgPosEcef_z :: Double -- ^ ECEF Z coordinate , _msgPosEcef_accuracy :: Word16 - -- ^ Position accuracy estimate. Defaults to 0. + -- ^ Position accuracy estimate. Not implemented in SPP mode, defaults to 0. , _msgPosEcef_n_sats :: Word8 -- ^ Number of satellites used in solution , _msgPosEcef_flags :: Word8 @@ -270,9 +270,11 @@ , _msgPosLlh_height :: Double -- ^ Height above WGS84 ellipsoid , _msgPosLlh_h_accuracy :: Word16 - -- ^ Horizontal position accuracy estimate (not implemented). Defaults to 0. + -- ^ Horizontal position accuracy estimate. Not implemented in SPP mode, + -- defaults to 0. , _msgPosLlh_v_accuracy :: Word16 - -- ^ Vertical position accuracy estimate (not implemented). Defaults to 0. + -- ^ Vertical position accuracy estimate. Not implemented in SPP mode, + -- defaults to 0. , _msgPosLlh_n_sats :: Word8 -- ^ Number of satellites used in solution. , _msgPosLlh_flags :: Word8 @@ -326,7 +328,7 @@ , _msgBaselineEcef_z :: Int32 -- ^ Baseline ECEF Z coordinate , _msgBaselineEcef_accuracy :: Word16 - -- ^ Position accuracy estimate (not implemented). Defaults to 0. + -- ^ Position accuracy estimate , _msgBaselineEcef_n_sats :: Word8 -- ^ Number of satellites used in solution , _msgBaselineEcef_flags :: Word8 @@ -380,9 +382,9 @@ , _msgBaselineNed_d :: Int32 -- ^ Baseline Down coordinate , _msgBaselineNed_h_accuracy :: Word16 - -- ^ Horizontal position accuracy estimate (not implemented). Defaults to 0. + -- ^ Horizontal position accuracy estimate , _msgBaselineNed_v_accuracy :: Word16 - -- ^ Vertical position accuracy estimate (not implemented). Defaults to 0. + -- ^ Vertical position accuracy estimate , _msgBaselineNed_n_sats :: Word8 -- ^ Number of satellites used in solution , _msgBaselineNed_flags :: Word8 diff --git a/src/SwiftNav/SBP/Piksi.hs b/src/SwiftNav/SBP/Piksi.hs --- a/src/SwiftNav/SBP/Piksi.hs +++ b/src/SwiftNav/SBP/Piksi.hs @@ -77,26 +77,51 @@ $(makeLenses ''MsgSetTime) msgReset :: Word16 -msgReset = 0x00B2 +msgReset = 0x00B6 --- | SBP class for message MSG_RESET (0x00B2). +-- | SBP class for message MSG_RESET (0x00B6). -- -- This message from the host resets the Piksi back into the bootloader. data MsgReset = MsgReset - deriving ( Show, Read, Eq ) + { _msgReset_flags :: Word32 + -- ^ Reset flags + } deriving ( Show, Read, Eq ) instance Binary MsgReset where - get = - return MsgReset + get = do + _msgReset_flags <- getWord32le + return MsgReset {..} - put MsgReset = - return () + put MsgReset {..} = do + putWord32le _msgReset_flags $(deriveSBP 'msgReset ''MsgReset) $(deriveJSON defaultOptions {fieldLabelModifier = fromMaybe "_msgReset_" . P.stripPrefix "_msgReset_"} ''MsgReset) $(makeLenses ''MsgReset) + +msgResetDep :: Word16 +msgResetDep = 0x00B2 + +-- | SBP class for message MSG_RESET_DEP (0x00B2). +-- +-- This message from the host resets the Piksi back into the bootloader. +data MsgResetDep = MsgResetDep + deriving ( Show, Read, Eq ) + +instance Binary MsgResetDep where + get = + return MsgResetDep + + put MsgResetDep = + return () + +$(deriveSBP 'msgResetDep ''MsgResetDep) + +$(deriveJSON defaultOptions {fieldLabelModifier = fromMaybe "_msgResetDep_" . P.stripPrefix "_msgResetDep_"} + ''MsgResetDep) +$(makeLenses ''MsgResetDep) msgCwResults :: Word16 msgCwResults = 0x00C0