diff --git a/rtcm.cabal b/rtcm.cabal --- a/rtcm.cabal +++ b/rtcm.cabal @@ -1,6 +1,6 @@ name: rtcm -version: 0.1.0 -synopsis: Haskell bindings for RTCM. +version: 0.1.2 +synopsis: RTCM Library. description: Haskell bindings for RTCM. homepage: http://github.com/swift-nav/librtcm license: BSD3 @@ -10,6 +10,11 @@ category: Network build-type: Simple cabal-version: >= 1.10 +description: + Haskell bindings for Radio Technical Commission For Maritime + Services (RTCM) standard, supporting GPS, GLONASS, Galileo and other + satellite-based position systems operation with one reference + station or a network. source-repository head type: git @@ -17,18 +22,36 @@ library hs-source-dirs: src - exposed-modules: Data.RTCM3 + exposed-modules: Data.CRC24Q + , Data.RTCM3 + , Data.RTCM3.Antennas + , Data.RTCM3.Internal + , Data.RTCM3.Observations + , Data.RTCM3.Types + other-modules: Data.RTCM3.Extras + , Data.RTCM3.TH ghc-options: -Wall - build-depends: base >= 4.7 && < 5 + build-depends: array + , base >= 4.7 && < 5 , basic-prelude + , binary + , binary-bits + , bytestring + , lens + , template-haskell + , word24 default-language: Haskell2010 default-extensions: NoImplicitPrelude OverloadedStrings + RecordWildCards + TemplateHaskell test-suite test type: exitcode-stdio-1.0 hs-source-dirs: test main-is: Test.hs + other-modules: Test.Data.CRC24Q + , Test.Data.RTCM3.Extras build-depends: base , basic-prelude , tasty diff --git a/src/Data/CRC24Q.hs b/src/Data/CRC24Q.hs new file mode 100644 --- /dev/null +++ b/src/Data/CRC24Q.hs @@ -0,0 +1,67 @@ +-- | +-- Module: Data.CRC24Q +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- Error detection functions. + +module Data.CRC24Q + ( crc24q + ) where + +import BasicPrelude hiding ( ByteString, foldl' ) +import Data.Array +import Data.Bits +import Data.ByteString.Lazy +import Data.Word.Word24 + +table :: Array Word24 Word24 +table = listArray (0, 255) + [ 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC + , 0x9F7F17, 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23 + , 0xB8B2D5, 0x3EFE2E, 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868 + , 0xD0E493, 0xDC7D65, 0x5A319E, 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646 + , 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, 0x0CD1E9, 0x8A9D12, 0x8604E4 + , 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, 0xAD50D0, 0x2B1C2B + , 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, 0xC99F60 + , 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077 + , 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5 + , 0xF7614E, 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8 + , 0x00903E, 0x86DCC5, 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A + , 0xAD88F1, 0xA11107, 0x275DFC, 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD + , 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, 0x7D6C62, 0xFB2099, 0xF7B96F + , 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, 0x15723B, 0x933EC0 + , 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, 0xB4F302 + , 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15 + , 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E + , 0x4F43A5, 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791 + , 0x688E67, 0xEEC29C, 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145 + , 0x26EDBE, 0x2A7448, 0xAC38B3, 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B + , 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, 0xF6092D, 0x7045D6, 0x7CDC20 + , 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, 0x578814, 0xD1C4EF + , 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, 0x3F964D + , 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A + , 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498 + , 0x016863, 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE + , 0xE3EB28, 0x65A7D3, 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C + , 0x4EF3E7, 0x426A11, 0xC426EA, 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80 + , 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, 0x8B654F, 0x0D29B4, 0x01B042 + , 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, 0xEFAAFF, 0x69E604 + , 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, 0x4E2BC6 + , 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1 + , 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673 + , 0xB94A88, 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC + , 0x9E874A, 0x18CBB1, 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7 + , 0xF6D10C, 0xFA48FA, 0x7C0401, 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9 + , 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 + ] + +-- | Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q). +crc24q :: ByteString -> Word24 +crc24q = + foldl' f 0 where + f ac v = (ac `shiftL` 8) `xor` (table ! idx) where + idx = (ac `shiftR` 16) `xor` fromIntegral v diff --git a/src/Data/RTCM3.hs b/src/Data/RTCM3.hs --- a/src/Data/RTCM3.hs +++ b/src/Data/RTCM3.hs @@ -6,8 +6,76 @@ -- Stability: experimental -- Portability: portable -- --- RTCM3 module. +-- RTCMv3 message containers. module Data.RTCM3 - ( + ( RTCM3Msg (..) + , module Data.RTCM3.Types + , module Data.RTCM3.Antennas + , module Data.RTCM3.Observations ) where + +import BasicPrelude +import Data.Binary +import Data.Binary.Get +import qualified Data.Binary.Bits.Get as B +import Data.ByteString.Lazy +import Data.Word.Word24 +import Data.RTCM3.Antennas +import Data.RTCM3.Extras +import Data.RTCM3.Observations +import Data.RTCM3.Types + +-- | An RTCM message ADT composed of all defined RTCM messages. +-- +-- Includes RTCMMsgUnknown for valid RTCM messages with undefined message +-- types and RTCMMsgBadCRC for RTCM messages with invalid CRC checksums. +data RTCM3Msg = + RTCM3Msg1001 Msg1001 Msg + | RTCM3Msg1002 Msg1002 Msg + | RTCM3Msg1003 Msg1003 Msg + | RTCM3Msg1004 Msg1004 Msg + | RTCM3Msg1005 Msg1005 Msg + | RTCM3Msg1006 Msg1006 Msg + | RTCM3Msg1007 Msg1007 Msg + | RTCM3Msg1008 Msg1008 Msg + | RTCM3MsgUnknown Word16 Msg + | RTCM3MsgBadCrc Word24 Msg + deriving ( Show, Read, Eq ) + +instance Binary RTCM3Msg where + get = do + preamble <- getWord8 + if preamble /= msgRTCM3Preamble then get else do + rtcm3 <- get + crc <- getWord24be + return $ decode' rtcm3 crc where + decode' rtcm3@Msg {..} crc + | checkCrc rtcm3 /= crc = RTCM3MsgBadCrc crc rtcm3 + | otherwise = flip runGet (fromStrict _msgRTCM3Payload) $ B.runBitGet $ do + num <- B.getWord16be 12 + return $ decode'' num where + decode'' num + | num == msg1001 = RTCM3Msg1001 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | num == msg1002 = RTCM3Msg1002 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | num == msg1003 = RTCM3Msg1003 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | num == msg1004 = RTCM3Msg1004 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | num == msg1005 = RTCM3Msg1005 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | num == msg1006 = RTCM3Msg1006 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | num == msg1007 = RTCM3Msg1007 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | num == msg1008 = RTCM3Msg1008 (decode $ fromStrict _msgRTCM3Payload) rtcm3 + | otherwise = RTCM3MsgUnknown num rtcm3 + + put msg = do + putWord8 msgRTCM3Preamble + encode' msg where + encode' (RTCM3Msg1001 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3Msg1002 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3Msg1003 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3Msg1004 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3Msg1005 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3Msg1006 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3Msg1007 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3Msg1008 _msg rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3MsgUnknown _num rtcm3) = put rtcm3 >> putWord24be (checkCrc rtcm3) + encode' (RTCM3MsgBadCrc crc rtcm3) = put rtcm3 >> putWord24be crc diff --git a/src/Data/RTCM3/Antennas.hs b/src/Data/RTCM3/Antennas.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/Antennas.hs @@ -0,0 +1,255 @@ +-- | +-- Module: Data.RTCM3.Antennas +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- RTCMv3 antenna messages for stationary references and descriptors. + +module Data.RTCM3.Antennas where + +import BasicPrelude +import Control.Lens +import Data.Binary +import Data.Binary.Bits +import qualified Data.Binary.Bits.Get as B +import qualified Data.Binary.Bits.Put as B +import Data.RTCM3.Extras +import Data.RTCM3.TH + +-- | AntennaReference. +-- +-- Stationary antenna reference point information. +data AntennaReference = AntennaReference + { _antennaReference_num :: Word16 + -- ^ Message number. + , _antennaReference_station :: Word16 + -- ^ Reference station id. + , _antennaReference_gps :: Bool + -- ^ GPS indicator. + , _antennaReference_glonass :: Bool + -- ^ GLONASS indicator. + , _antennaReference_galileo :: Bool + -- ^ Galileo indicator. + , _antennaReference_computed :: Bool + -- ^ Reference-station non-physical indicator. + , _antennaReference_ecef_x :: Int64 + -- ^ Antenna reference point ECEF-X. + , _antennaReference_oscillator :: Bool + -- ^ Single receiver oscillator indicator. + , _antennaReference_ecef_y :: Int64 + -- ^ Antenna reference point ECEF-Y. + , _antennaReference_ecef_z :: Int64 + -- ^ Antenna reference point ECEF-Z. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''AntennaReference) + +instance BinaryBit AntennaReference where + getBits _n = do + _antennaReference_num <- B.getWord16be 12 + _antennaReference_station <- B.getWord16be 12 + _reserved <- B.getWord8 6 + _antennaReference_gps <- B.getBool + _antennaReference_glonass <- B.getBool + _antennaReference_galileo <- B.getBool + _antennaReference_computed <- B.getBool + _antennaReference_ecef_x <- getInt64be 38 + _antennaReference_oscillator <- B.getBool + _reserved <- B.getBool + _antennaReference_ecef_y <- getInt64be 38 + _reserved <- B.getWord8 2 + _antennaReference_ecef_z <- getInt64be 38 + return AntennaReference {..} + + putBits _n AntennaReference {..} = do + B.putWord16be 12 _antennaReference_num + B.putWord16be 12 _antennaReference_station + B.putWord8 6 0 + B.putBool _antennaReference_gps + B.putBool _antennaReference_glonass + B.putBool _antennaReference_galileo + B.putBool _antennaReference_computed + putInt64be 38 _antennaReference_ecef_x + B.putBool False + putInt64be 38 _antennaReference_ecef_y + B.putWord8 2 0 + putInt64be 38 _antennaReference_ecef_z + +-- | ExtAntennaReference. +-- +-- Extended stationary antenna reference point information. +data ExtAntennaReference = ExtAntennaReference + { _extAntennaReference_height :: Word16 + -- ^ Antenna height. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''ExtAntennaReference) + +instance BinaryBit ExtAntennaReference where + getBits _n = do + _extAntennaReference_height <- B.getWord16be 16 + return ExtAntennaReference {..} + + putBits _n ExtAntennaReference {..} = + B.putWord16be 16 _extAntennaReference_height + +-- | AntennaDescriptor. +-- +-- Antenna description information. +data AntennaDescriptor = AntennaDescriptor + { _antennaDescriptor_num :: Word16 + -- ^ Message number. + , _antennaDescriptor_station :: Word16 + -- ^ Reference station id. + , _antennaDescriptor_n :: Word8 + -- ^ Number of antenna descriptors. + , _antennaDescriptor_descriptors :: [Word8] + -- ^ Antenna descriptors. + , _antennaDescriptor_setup :: Word8 + -- ^ Antenna setup id. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''AntennaDescriptor) + +instance BinaryBit AntennaDescriptor where + getBits _n = do + _antennaDescriptor_num <- B.getWord16be 12 + _antennaDescriptor_station <- B.getWord16be 12 + _antennaDescriptor_n <- B.getWord8 8 + _antennaDescriptor_descriptors <- replicateM (fromIntegral _antennaDescriptor_n) $ B.getWord8 8 + _antennaDescriptor_setup <- B.getWord8 8 + return AntennaDescriptor {..} + + putBits _n AntennaDescriptor {..} = do + B.putWord16be 12 _antennaDescriptor_num + B.putWord16be 12 _antennaDescriptor_station + B.putWord8 8 _antennaDescriptor_n + forM_ _antennaDescriptor_descriptors $ B.putWord8 8 + B.putWord8 8 _antennaDescriptor_setup + +-- | ExtAntennaDescriptor. +-- +-- Extended antenna description information. +data ExtAntennaDescriptor = ExtAntennaDescriptor + { _extAntennaDescriptor_n :: Word8 + -- ^ Number of antenna serial numbers. + , _extAntennaDescriptor_serialNumbers :: [Word8] + -- ^ Antenna serial numbers. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''ExtAntennaDescriptor) + +instance BinaryBit ExtAntennaDescriptor where + getBits _n = do + _extAntennaDescriptor_n <- B.getWord8 8 + _extAntennaDescriptor_serialNumbers <- replicateM (fromIntegral _extAntennaDescriptor_n) $ B.getWord8 8 + return ExtAntennaDescriptor {..} + + putBits _n ExtAntennaDescriptor {..} = do + B.putWord8 8 _extAntennaDescriptor_n + forM_ _extAntennaDescriptor_serialNumbers $ B.putWord8 8 + +msg1005 :: Word16 +msg1005 = 1005 + +-- | Msg1005. +-- +-- RTCMv3 message 1005. +data Msg1005 = Msg1005 + { _msg1005_reference :: AntennaReference + -- ^ Antenna reference. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1005) + +instance Binary Msg1005 where + get = B.runBitGet $ do + _msg1005_reference <- getBits 0 + return Msg1005 {..} + + put Msg1005 {..} = B.runBitPut $ + putBits 0 _msg1005_reference + +$(deriveRTCM3 ''Msg1005) + +msg1006 :: Word16 +msg1006 = 1006 + +-- | Msg1006. +-- +-- RTCMv3 message 1006. +data Msg1006 = Msg1006 + { _msg1006_reference :: AntennaReference + -- ^ Antenna reference. + , _msg1006_extReference :: ExtAntennaReference + -- ^ Antenna extended reference. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1006) + +instance Binary Msg1006 where + get = B.runBitGet $ do + _msg1006_reference <- getBits 0 + _msg1006_extReference <- getBits 0 + return Msg1006 {..} + + put Msg1006 {..} = B.runBitPut $ do + putBits 0 _msg1006_reference + putBits 0 _msg1006_extReference + +$(deriveRTCM3 ''Msg1006) + +msg1007 :: Word16 +msg1007 = 1007 + +-- | Msg1007. +-- +-- RTCMv3 message 1007. +data Msg1007 = Msg1007 + { _msg1007_descriptor :: AntennaDescriptor + -- ^ Antenna descriptor. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1007) + +instance Binary Msg1007 where + get = B.runBitGet $ do + _msg1007_descriptor <- getBits 0 + return Msg1007 {..} + + put Msg1007 {..} = B.runBitPut $ + putBits 0 _msg1007_descriptor + +$(deriveRTCM3 ''Msg1007) + +msg1008 :: Word16 +msg1008 = 1008 + +-- | Msg1008. +-- +-- RTCMv3 message 1008. +data Msg1008 = Msg1008 + { _msg1008_descriptor :: AntennaDescriptor + -- ^ Antenna descriptor. + , _msg1008_extDescriptor :: ExtAntennaDescriptor + -- ^ Antenna extended descriptor. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1008) + +instance Binary Msg1008 where + get = B.runBitGet $ do + _msg1008_descriptor <- getBits 0 + -- ^ Antenna descriptor. + _msg1008_extDescriptor <- getBits 0 + -- ^ Antenna extended descriptor. + return Msg1008 {..} + + put Msg1008 {..} = B.runBitPut $ do + putBits 0 _msg1008_descriptor + putBits 0 _msg1008_extDescriptor + +$(deriveRTCM3 ''Msg1008) diff --git a/src/Data/RTCM3/Extras.hs b/src/Data/RTCM3/Extras.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/Extras.hs @@ -0,0 +1,81 @@ +-- | +-- Module: Data.RTCM3.Extras +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- Extra stuff. + +module Data.RTCM3.Extras + ( getInt8 + , getInt16be + , getInt32be + , getInt64be + , putInt8 + , putInt16be + , putInt32be + , putInt64be + , getWord24be + , putWord24be + ) where + +import BasicPrelude +import Data.Binary +import qualified Data.Binary.Bits.Get as B +import qualified Data.Binary.Bits.Put as B +import Data.Bits +import Data.Int +import Data.Word.Word24 + +signExtend8 :: Int -> Word8 -> Int8 +signExtend8 n x = (fromIntegral x `shiftL` (8 - n)) `shiftR` (8 - n) + +signExtend16 :: Int -> Word16 -> Int16 +signExtend16 n x = (fromIntegral x `shiftL` (16 - n)) `shiftR` (16 - n) + +signExtend32 :: Int -> Word32 -> Int32 +signExtend32 n x = (fromIntegral x `shiftL` (32 - n)) `shiftR` (32 - n) + +signExtend64 :: Int -> Word64 -> Int64 +signExtend64 n x = (fromIntegral x `shiftL` (64 - n)) `shiftR` (64 - n) + +getInt8 :: Int -> B.BitGet Int8 +getInt8 n = signExtend8 n <$> B.getWord8 n + +getInt16be :: Int -> B.BitGet Int16 +getInt16be n = signExtend16 n <$> B.getWord16be n + +getInt32be :: Int -> B.BitGet Int32 +getInt32be n = signExtend32 n <$> B.getWord32be n + +getInt64be :: Int -> B.BitGet Int64 +getInt64be n = signExtend64 n <$> B.getWord64be n + +putInt8 :: Int -> Int8 -> B.BitPut () +putInt8 n = B.putWord8 n . fromIntegral + +putInt16be :: Int -> Int16 -> B.BitPut () +putInt16be n = B.putWord16be n . fromIntegral + +putInt32be :: Int -> Int32 -> B.BitPut () +putInt32be n = B.putWord32be n . fromIntegral + +putInt64be :: Int -> Int64 -> B.BitPut () +putInt64be n = B.putWord64be n . fromIntegral + +getWord24be :: Get Word24 +getWord24be = do + b1 <- fromIntegral <$> getWord8 + b2 <- fromIntegral <$> getWord8 + b3 <- fromIntegral <$> getWord8 + return $ fromInteger $ shiftL b1 16 .|. shiftL b2 8 .|. b3 +{-# INLINE getWord24be #-} + +putWord24be :: Word24 -> Put +putWord24be w = do + putWord8 $ fromIntegral $ shiftR w 16 + putWord8 $ fromIntegral $ shiftR w 8 + putWord8 $ fromIntegral w +{-# INLINE putWord24be #-} diff --git a/src/Data/RTCM3/Internal.hs b/src/Data/RTCM3/Internal.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/Internal.hs @@ -0,0 +1,15 @@ +-- | +-- Module: Data.RTCM3.Internal +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- Internal stuff. + +module Data.RTCM3.Internal + ( module Data.RTCM3.Extras + ) where + +import Data.RTCM3.Extras diff --git a/src/Data/RTCM3/Observations.hs b/src/Data/RTCM3/Observations.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/Observations.hs @@ -0,0 +1,380 @@ +-- | +-- Module: Data.RTCM3.Observations +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- RTCMv3 Observations. + +module Data.RTCM3.Observations where + +import BasicPrelude +import Control.Lens +import Data.Binary +import Data.Binary.Bits +import qualified Data.Binary.Bits.Get as B +import qualified Data.Binary.Bits.Put as B +import Data.Int +import Data.RTCM3.Extras +import Data.RTCM3.TH + +-- | GpsObservationHeader. +-- +-- GPS RTK observation header. +data GpsObservationHeader = GpsObservationHeader + { _gpsObservationHeader_num :: Word16 + -- ^ Message number. + , _gpsObservationHeader_station :: Word16 + -- ^ Reference station id. + , _gpsObservationHeader_tow :: Word32 + -- ^ GPS epoch time. + , _gpsObservationHeader_synchronous :: Bool + -- ^ Synchronous GNSS flag. + , _gpsObservationHeader_n :: Word8 + -- ^ Number of GPS satellite observations. + , _gpsObservationHeader_smoothing :: Bool + -- ^ GPS divergence-free smoothing indicator. + , _gpsObservationHeader_smoothingInterval :: Word8 + -- ^ GPS smoothing interval. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''GpsObservationHeader) + +instance BinaryBit GpsObservationHeader where + getBits _n = do + _gpsObservationHeader_num <- B.getWord16be 12 + _gpsObservationHeader_station <- B.getWord16be 12 + _gpsObservationHeader_tow <- B.getWord32be 30 + _gpsObservationHeader_synchronous <- B.getBool + _gpsObservationHeader_n <- B.getWord8 5 + _gpsObservationHeader_smoothing <- B.getBool + _gpsObservationHeader_smoothingInterval <- B.getWord8 3 + return GpsObservationHeader {..} + + putBits _n GpsObservationHeader {..} = do + B.putWord16be 12 _gpsObservationHeader_num + B.putWord16be 12 _gpsObservationHeader_station + B.putWord32be 30 _gpsObservationHeader_tow + B.putWord32be 30 _gpsObservationHeader_tow + B.putBool _gpsObservationHeader_synchronous + B.putWord8 5 _gpsObservationHeader_n + B.putBool _gpsObservationHeader_smoothing + B.putWord8 3 _gpsObservationHeader_smoothingInterval + +-- | GpsL1Observation. +-- +-- GPS RTK L1 observation. +data GpsL1Observation = GpsL1Observation + { _gpsL1Observation_code :: Bool + -- ^ GPS L1 code indicator. + , _gpsL1Observation_pseudorange :: Word32 + -- ^ GPS L1 pseudorange. + , _gpsL1Observation_carrierMinusCode :: Int32 + -- ^ GPS L1 phaserange - pseudorange. + , _gpsL1Observation_lockTime :: Word8 + -- ^ GPS L1 lock time indicator. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''GpsL1Observation) + +instance BinaryBit GpsL1Observation where + getBits _n = do + _gpsL1Observation_code <- B.getBool + _gpsL1Observation_pseudorange <- B.getWord32be 24 + _gpsL1Observation_carrierMinusCode <- getInt32be 20 + _gpsL1Observation_lockTime <- B.getWord8 7 + return GpsL1Observation {..} + + putBits _n GpsL1Observation {..} = do + B.putBool _gpsL1Observation_code + B.putWord32be 24 _gpsL1Observation_pseudorange + putInt32be 20 _gpsL1Observation_carrierMinusCode + B.putWord8 7 _gpsL1Observation_lockTime + +-- | GpsL1ExtObservation. +-- +-- GPS RTK L1 extended observation. +data GpsL1ExtObservation = GpsL1ExtObservation + { _gpsL1ExtObservation_ambiguity :: Word8 + -- ^ GPS L1 pseudorange ambiguity. + , _gpsL1ExtObservation_cnr :: Word8 + -- ^ GPS L1 carrier-to-noise ratio. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''GpsL1ExtObservation) + +instance BinaryBit GpsL1ExtObservation where + getBits _n = do + _gpsL1ExtObservation_ambiguity <- B.getWord8 8 + _gpsL1ExtObservation_cnr <- B.getWord8 8 + return GpsL1ExtObservation {..} + + putBits _n GpsL1ExtObservation {..} = do + B.putWord8 8 _gpsL1ExtObservation_ambiguity + B.putWord8 8 _gpsL1ExtObservation_cnr + +-- | GpsL2Observation. +-- +-- GPS RTK L2 observation. +data GpsL2Observation = GpsL2Observation + { _gpsL2Observation_code :: Word8 + -- ^ GPS L2 code indicator. + , _gpsL2Observation_pseudorangeDifference :: Int16 + -- ^ GPS L2-L1 pseudorange difference. + , _gpsL2Observation_carrierMinusCode :: Int32 + -- ^ GPS L2 phaserange - L1 pseudorange. + , _gpsL2Observation_lockTime :: Word8 + -- ^ GPS L2 lock time indicator. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''GpsL2Observation) + +instance BinaryBit GpsL2Observation where + getBits _n = do + _gpsL2Observation_code <- B.getWord8 2 + _gpsL2Observation_pseudorangeDifference <- getInt16be 14 + _gpsL2Observation_carrierMinusCode <- getInt32be 20 + _gpsL2Observation_lockTime <- B.getWord8 7 + return GpsL2Observation {..} + + putBits _n GpsL2Observation {..} = do + B.putWord8 2 _gpsL2Observation_code + putInt16be 14 _gpsL2Observation_pseudorangeDifference + putInt32be 20 _gpsL2Observation_carrierMinusCode + B.putWord8 7 _gpsL2Observation_lockTime + +-- | GpsL2ExtObservation. +-- +-- GPS RTK L2 extended observation. +data GpsL2ExtObservation = GpsL2ExtObservation + { _gpsL2ExtObservation_cnr :: Word8 + -- ^ GPS L2 carrier-to-noise ratio. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''GpsL2ExtObservation) + +instance BinaryBit GpsL2ExtObservation where + getBits _n = do + _gpsL2ExtObservation_cnr <- B.getWord8 8 + return GpsL2ExtObservation {..} + + putBits _n GpsL2ExtObservation {..} = + B.putWord8 8 _gpsL2ExtObservation_cnr + +msg1001 :: Word16 +msg1001 = 1001 + +-- | Observation1001. +-- +-- GPS RTK L1 observation for message 1001. +data Observation1001 = Observation1001 + { _observation1001_sat :: Word8 + -- ^ GPS satellite id. + , _observation1001_l1 :: GpsL1Observation + -- ^ GPS RTK L1 observation. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Observation1001) + +instance BinaryBit Observation1001 where + getBits n = do + _observation1001_sat <- B.getWord8 6 + _observation1001_l1 <- getBits n + return Observation1001 {..} + + putBits n Observation1001 {..} = do + B.putWord8 6 _observation1001_sat + putBits n _observation1001_l1 + +-- | Msg1001. +-- +-- RTCMv3 message 1001. +data Msg1001 = Msg1001 + { _msg1001_header :: GpsObservationHeader + -- ^ GPS observation header. + , _msg1001_observations :: [Observation1001] + -- ^ GPS RTK L1 observations. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1001) + +instance Binary Msg1001 where + get = B.runBitGet $ do + _msg1001_header <- getBits 0 + _msg1001_observations <- replicateM (fromIntegral $ _msg1001_header ^. gpsObservationHeader_n) $ getBits 0 + return Msg1001 {..} + + put Msg1001 {..} = B.runBitPut $ do + putBits 0 _msg1001_header + forM_ _msg1001_observations $ putBits 0 + +$(deriveRTCM3 ''Msg1001) + +msg1002 :: Word16 +msg1002 = 1002 + +-- | Observation1002. +-- +-- GPS RTK L1 extended observation for message 1002. +data Observation1002 = Observation1002 + { _observation1002_sat :: Word8 + -- ^ GPS satellite id. + , _observation1002_l1 :: GpsL1Observation + -- ^ GPS RTK L1 observation. + , _observation1002_l1e :: GpsL1ExtObservation + -- ^ GPS RTK L1 extended observation. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Observation1002) + +instance BinaryBit Observation1002 where + getBits n = do + _observation1002_sat <- B.getWord8 6 + _observation1002_l1 <- getBits n + _observation1002_l1e <- getBits n + return Observation1002 {..} + + putBits n Observation1002 {..} = do + B.putWord8 6 _observation1002_sat + putBits n _observation1002_l1 + putBits n _observation1002_l1e + +-- | Msg1002. +-- +-- RTCMv3 message 1002. +data Msg1002 = Msg1002 + { _msg1002_header :: GpsObservationHeader + -- ^ GPS observation header. + , _msg1002_observations :: [Observation1002] + -- ^ GPS RTK L1 extended observations. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1002) + +instance Binary Msg1002 where + get = B.runBitGet $ do + _msg1002_header <- getBits 0 + _msg1002_observations <- replicateM (fromIntegral $ _msg1002_header ^. gpsObservationHeader_n) $ getBits 0 + return Msg1002 {..} + + put Msg1002 {..} = B.runBitPut $ do + putBits 0 _msg1002_header + forM_ _msg1002_observations $ putBits 0 + +$(deriveRTCM3 ''Msg1002) + +msg1003 :: Word16 +msg1003 = 1003 + +-- | Observation1003. +-- +-- GPS RTK L1, L2 observation for message 1003. +data Observation1003 = Observation1003 + { _observation1003_sat :: Word8 + -- ^ GPS satellite id. + , _observation1003_l1 :: GpsL1Observation + -- ^ GPS RTK L1 observation. + , _observation1003_l2 :: GpsL2Observation + -- ^ GPS RTK L2 observation. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Observation1003) + +instance BinaryBit Observation1003 where + getBits n = do + _observation1003_sat <- B.getWord8 6 + _observation1003_l1 <- getBits n + _observation1003_l2 <- getBits n + return Observation1003 {..} + + putBits n Observation1003 {..} = do + B.putWord8 6 _observation1003_sat + putBits n _observation1003_l1 + putBits n _observation1003_l2 + +-- | Msg1003. +-- +-- RTCMv3 message 1003. +data Msg1003 = Msg1003 + { _msg1003_header :: GpsObservationHeader + -- ^ GPS observation header. + , _msg1003_observations :: [Observation1003] + -- ^ GPS RTK L1, L2 observations. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1003) + +instance Binary Msg1003 where + get = B.runBitGet $ do + _msg1003_header <- getBits 0 + _msg1003_observations <- replicateM (fromIntegral $ _msg1003_header ^. gpsObservationHeader_n) $ getBits 0 + return Msg1003 {..} + + put Msg1003 {..} = B.runBitPut $ do + putBits 0 _msg1003_header + forM_ _msg1003_observations $ putBits 0 + +$(deriveRTCM3 ''Msg1003) + +msg1004 :: Word16 +msg1004 = 1004 + +-- | Observation1004. +-- +-- GPS RTK L1, L2 extended observation for message 1004. +data Observation1004 = Observation1004 + { _observation1004_sat :: Word8 + -- ^ GPS satellite id. + , _observation1004_l1 :: GpsL1Observation + -- ^ GPS RTK L1 observation. + , _observation1004_l1e :: GpsL1ExtObservation + -- ^ GPS RTK L1 extended observation. + , _observation1004_l2 :: GpsL2Observation + -- ^ GPS RTK L2 observation. + , _observation1004_l2e :: GpsL2ExtObservation + -- ^ GPS RTK L2 extended observation. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Observation1004) + +instance BinaryBit Observation1004 where + getBits n = do + _observation1004_sat <- B.getWord8 6 + _observation1004_l1 <- getBits n + _observation1004_l1e <- getBits n + _observation1004_l2 <- getBits n + _observation1004_l2e <- getBits n + return Observation1004 {..} + + putBits n Observation1004 {..} = do + B.putWord8 6 _observation1004_sat + putBits n _observation1004_l1 + putBits n _observation1004_l1e + putBits n _observation1004_l2 + putBits n _observation1004_l2e + +-- | Msg1004. +-- +-- RTCMv3 message 1004. +data Msg1004 = Msg1004 + { _msg1004_header :: GpsObservationHeader + -- ^ GPS observation header. + , _msg1004_observations :: [Observation1004] + -- ^ GPS RTK L1, L2 extended observations. + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg1004) + +instance Binary Msg1004 where + get = B.runBitGet $ do + _msg1004_header <- getBits 0 + _msg1004_observations <- replicateM (fromIntegral $ _msg1004_header ^. gpsObservationHeader_n) $ getBits 0 + return Msg1004 {..} + + put Msg1004 {..} = B.runBitPut $ do + putBits 0 _msg1004_header + forM_ _msg1004_observations $ putBits 0 + +$(deriveRTCM3 ''Msg1004) diff --git a/src/Data/RTCM3/TH.hs b/src/Data/RTCM3/TH.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/TH.hs @@ -0,0 +1,26 @@ +-- | +-- Module: Data.RTCM3.TH +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- Templated generation of RTCM3 interfaces. + +module Data.RTCM3.TH where + +import BasicPrelude hiding ( length ) +import Data.Binary +import Data.ByteString.Lazy +import Data.RTCM3.Types +import Language.Haskell.TH + +-- | Derive ToRTCM3 typeclass, given an RTCM3 message number name and the +-- name of the implemented type. +deriveRTCM3 :: Name -> Q [Dec] +deriveRTCM3 name = + [d|instance ToRTCM3 $(conT name) where + toRTCM3 msg = Msg (fromIntegral $ length payload) (toStrict payload) where + payload = encode msg + |] diff --git a/src/Data/RTCM3/Types.hs b/src/Data/RTCM3/Types.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/Types.hs @@ -0,0 +1,55 @@ +-- | +-- Module: Data.RTCM3.Types +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- Common RTCMv3 type requirements, containers, and serialization +-- utilities. + +module Data.RTCM3.Types where + +import BasicPrelude +import Control.Lens +import Data.Binary +import qualified Data.Binary.Bits.Get as B +import qualified Data.Binary.Bits.Put as B +import Data.Binary.Get +import Data.Binary.Put +import Data.CRC24Q +import Data.Word.Word24 + +msgRTCM3Preamble :: Word8 +msgRTCM3Preamble = 0xD3 + +data Msg = Msg + { _msgRTCM3Len :: Word16 + , _msgRTCM3Payload :: !ByteString + } deriving ( Show, Read, Eq ) + +$(makeLenses ''Msg) + +instance Binary Msg where + get = do + _msgRTCM3Len <- B.runBitGet $ do + _reserved <- B.getWord16be 6 + B.getWord16be 10 + _msgRTCM3Payload <- getByteString $ fromIntegral _msgRTCM3Len + return Msg {..} + + put Msg {..} = do + B.runBitPut $ do + B.putWord16be 6 0 + B.putWord16be 10 _msgRTCM3Len + putByteString _msgRTCM3Payload + +checkCrc :: Msg -> Word24 +checkCrc msg = + crc24q $ runPut $ do + putWord8 msgRTCM3Preamble + put msg + +class Binary a => ToRTCM3 a where + toRTCM3 :: a -> Msg diff --git a/test/Test.hs b/test/Test.hs --- a/test/Test.hs +++ b/test/Test.hs @@ -1,10 +1,14 @@ -import BasicPrelude -import Test.Tasty +import BasicPrelude +import qualified Test.Data.CRC24Q as CRC24Q +import qualified Test.Data.RTCM3.Extras as Extras +import Test.Tasty tests :: TestTree -tests = testGroup "Tests" - [ - ] +tests = + testGroup "Tests" + [ CRC24Q.tests + , Extras.tests + ] main :: IO () main = defaultMain tests diff --git a/test/Test/Data/CRC24Q.hs b/test/Test/Data/CRC24Q.hs new file mode 100644 --- /dev/null +++ b/test/Test/Data/CRC24Q.hs @@ -0,0 +1,23 @@ +module Test.Data.CRC24Q + ( tests + ) where + +import BasicPrelude +import Data.CRC24Q +import Test.Tasty +import Test.Tasty.HUnit + +testEmpty :: TestTree +testEmpty = testCase "Empty test" $ + crc24q "" @?= 0 + +testSimple :: TestTree +testSimple = testCase "Simple test" $ + crc24q "123456789" @?= 0xCDE703 + +tests :: TestTree +tests = + testGroup "CRC24Q tests" + [ testEmpty + , testSimple + ] diff --git a/test/Test/Data/RTCM3/Extras.hs b/test/Test/Data/RTCM3/Extras.hs new file mode 100644 --- /dev/null +++ b/test/Test/Data/RTCM3/Extras.hs @@ -0,0 +1,11 @@ +module Test.Data.RTCM3.Extras + ( tests + ) where + +import Test.Tasty + +tests :: TestTree +tests = + testGroup "Extras tests" + [ + ]