gnss-converters 0.1.9 → 0.1.10
raw patch · 5 files changed
+545/−73 lines, 5 filesdep +HUnit-approxdep +bytestringdep +extranew-uploader
Dependencies added: HUnit-approx, bytestring, extra, unordered-containers
Files
- gnss-converters.cabal +15/−5
- main/RTCM32SBP.hs +2/−1
- src/Data/RTCM3/SBP.hs +308/−64
- test/Test.hs +15/−3
- test/Test/Data/RTCM3/SBP.hs +205/−0
gnss-converters.cabal view
@@ -1,12 +1,12 @@ name: gnss-converters-version: 0.1.9+version: 0.1.10 synopsis: GNSS Converters. description: Haskell bindings for GNSS converters. homepage: http://github.com/swift-nav/gnss-converters license: BSD3 author: Swift Navigation Inc. maintainer: Mark Fine <dev@swiftnav.com>-copyright: Copyright (C) 2015 Swift Navigation, Inc.+copyright: Copyright (C) 2016 Swift Navigation, Inc. category: Network build-type: Simple cabal-version: >= 1.10@@ -23,10 +23,12 @@ build-depends: base >= 4.7 && < 5 , basic-prelude , conduit-extra+ , extra , lens , rtcm , sbp , time+ , unordered-containers default-language: Haskell2010 default-extensions: NoImplicitPrelude OverloadedStrings@@ -61,15 +63,24 @@ type: exitcode-stdio-1.0 hs-source-dirs: test main-is: Test.hs+ other-modules: Test.Data.RTCM3.SBP build-depends: base , basic-prelude+ , binary-conduit+ , bytestring+ , conduit+ , conduit-extra+ , gnss-converters+ , HUnit-approx+ , lens+ , resourcet+ , sbp , tasty , tasty-hunit- , gnss-converters+ , unordered-containers ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall default-language: Haskell2010 default-extensions: NoImplicitPrelude- OverloadedStrings benchmark bench type: exitcode-stdio-1.0@@ -83,4 +94,3 @@ default-language: Haskell2010 default-extensions: NoImplicitPrelude OverloadedStrings-
main/RTCM32SBP.hs view
@@ -9,7 +9,7 @@ -- RTCM3 to SBP tool. import BasicPrelude-import Control.Monad.Trans.Resource+import Control.Monad.Trans.Resource import Data.Conduit import Data.Conduit.Binary import qualified Data.Conduit.List as CL@@ -22,5 +22,6 @@ sourceHandle stdin =$= conduitDecode =$= CL.mapMaybeM convert =$=+ CL.concat =$= conduitEncode $$ sinkHandle stdout
src/Data/RTCM3/SBP.hs view
@@ -2,7 +2,7 @@ -- | -- Module: Data.RTCM3.SBP--- Copyright: Copyright (C) 2015 Swift Navigation, Inc.+-- Copyright: Copyright (C) 2016 Swift Navigation, Inc. -- License: LGPL-3 -- Maintainer: Mark Fine <dev@swiftnav.com> -- Stability: experimental@@ -10,17 +10,117 @@ -- -- RTCMv3 to SBP Conversions. -module Data.RTCM3.SBP- ( convert- ) where+module Data.RTCM3.SBP where import BasicPrelude import Control.Lens+import Data.Bits+import Data.HashMap.Strict+import Data.List.Extra hiding (concat, map) import Data.Time import Data.Word import Data.RTCM3 import SwiftNav.SBP ++--------------------------------------------------------------------------------+-- SBP, GNSS, RTCM constant definitions+++-- | Two centimeters+--+-- SBP pseudoranges are in units of 2cm, and there are 50 of those in a meter.+twoCM :: Double+twoCM = 0.02++sbpCMinM :: Double+sbpCMinM = 50++-- | Speed of light (meters/msec)+lightSpeedMMSEC :: Double+lightSpeedMMSEC = 299792.458++-- | Speed of light (meters/sec)+lightSpeedMS :: Double+lightSpeedMS = 299792458.0++-- | L1 GPS center frequency (Hz)+l1frequency :: Double+l1frequency = 1.57542e9++-- | L2 GPS center frequency (Hz)+l2frequency :: Double+l2frequency = 1.22760e9++-- | We only support PRNS 1 - 32+maxSats :: Word8+maxSats = 32++-- | Q32.8 carrier phase representation width+q32Width :: Double+q32Width = 256++-- | RTCM phase range resolution.+--+-- See DF018, pg. 3-19 of the RTCM3 spec+phaseRangeRes :: Double+phaseRangeRes = 0.0005++-- | SBP L1 GNSS signal value+--+-- See: https://github.com/swift-nav/libswiftnav/blob/master/include/libswiftnav/signal.h#L65+l1CSidCode :: Word8+l1CSidCode = 0++-- | SBP L2CM GNSS signal value+--+-- See: https://github.com/swift-nav/libswiftnav/blob/master/include/libswiftnav/signal.h#L65+l2CMSidCode :: Word8+l2CMSidCode = 1++-- | SBP L1P and L2P GNSS signal values+--+-- Note that libswiftnav currently does not support L1P or L2P observations,+-- just L1C and L2C. This is a stop gap definition so that we can properly+-- serialize out SBP observations with L1P and L2P observations from external+-- receivers.+l1PSidCode :: Word8+l1PSidCode = 5++l2PSidCode :: Word8+l2PSidCode = 6++-- | L2C code indicator value+--+-- See DF016, pg. 3-17 of the RTCM3 spec+codeIndicator_L2C :: Word8+codeIndicator_L2C = 0++-- | L2P code indicator value+--+-- See DF016, pg. 3-17 of the RTCM3 spec+codeIndicator_L2PD :: Word8+codeIndicator_L2PD = 1++-- | Map L2 codes to SBP GnssSignal codes+--+l2codeToSBPSignalCode :: HashMap Word8 Word8+l2codeToSBPSignalCode = fromList [(0, l2CMSidCode), (1, l2PSidCode)]++-- | Maximum number of packed observations to allow in a single SBP message.+--+maxObsPerMessage :: Int+maxObsPerMessage = floor $ (maxPayloadSize - headerSize) / packedObsSize+ where+ maxPayloadSize = 255+ headerSize = 7+ packedObsSize = 16.0+++--------------------------------------------------------------------------------+-- GNSS RTCM observation reconstruction utilities++ fromEcefVal :: Int64 -> Double fromEcefVal x = fromIntegral x / 10000 @@ -32,90 +132,226 @@ , _obsGPSTime_wn = fromIntegral $ div (diffDays today (fromGregorian 1980 1 6)) 7 } -fromGpsObservationHeader :: MonadIO m => GpsObservationHeader -> m ObservationHeader-fromGpsObservationHeader hdr = do- t <- toGPSTime hdr- return ObservationHeader- { _observationHeader_t = t- , _observationHeader_n_obs = 0x10- }+-- | Construct metric pseudorange (meters!) from L1 RTCM observation.+--+-- See DF011, pg. 3-16 of the RTCM3 spec+metricPseudorange :: GpsL1Observation -> GpsL1ExtObservation -> Double+metricPseudorange l1 l1e =+ twoCM * fromIntegral (l1 ^. gpsL1Observation_pseudorange) ++ lightSpeedMMSEC * fromIntegral (l1e ^. gpsL1ExtObservation_ambiguity) -toP :: GpsL1Observation -> GpsL1ExtObservation -> Word32-toP l1 l1e = round $ p * 50 where- p = (0.02 :: Double) * fromIntegral (l1 ^. gpsL1Observation_pseudorange) +- 299792.458 * fromIntegral (l1e ^. gpsL1ExtObservation_ambiguity)+-- | Construct L1 SBP pseudorange for L1 RTCM observation+--+-- See DF011, pg. 3-16 of the RTCM3 spec+toP_L1 :: GpsL1Observation -> GpsL1ExtObservation -> Word32+toP_L1 l1 l1e = round $ sbpCMinM * metricPseudorange l1 l1e -toL :: GpsL1Observation -> GpsL1ExtObservation -> CarrierPhase-toL l1 l1e = CarrierPhase+-- | Construct L2 SBP pseudorange from L1/L2 RTCM observations+--+-- See DF017, pg. 3-18 of the RTCM3 spec+toP_L2 :: GpsL1Observation -> GpsL1ExtObservation -> GpsL2Observation -> Word32+toP_L2 l1 l1e l2 = round $ p * sbpCMinM where+ p = metricPseudorange l1 l1e ++ twoCM * fromIntegral (l2 ^. gpsL2Observation_pseudorangeDifference)++-- | Construct SBP L1 GPS carrier phase from L1 RTCM observation+--+-- See DF012, pg. 3-16 of the RTCM3 spec+toL_L1 :: GpsL1Observation -> GpsL1ExtObservation -> CarrierPhase+toL_L1 l1 l1e = CarrierPhase { _carrierPhase_i = fromIntegral li , _carrierPhase_f = fromIntegral lf } where- p = 0.02 * fromIntegral (l1 ^. gpsL1Observation_pseudorange) +- 299792.458 * fromIntegral (l1e ^. gpsL1ExtObservation_ambiguity)+ p = metricPseudorange l1 l1e+ -- Convert to SBP carrier phase representation per+ -- https://github.com/swift-nav/libsbp/blob/master/spec/yaml/swiftnav/sbp/observation.yaml#L39 lm :: Double- lm = p + 0.0005 * fromIntegral (l1 ^. gpsL1Observation_carrierMinusCode)- l = lm / (299792458.0 / 1.57542e9)+ lm = p + phaseRangeRes * fromIntegral (l1 ^. gpsL1Observation_carrierMinusCode)+ l = lm / (lightSpeedMS / l1frequency) li :: Int32 li = floor (l) lf :: Word8- lf = truncate ((l - fromIntegral li) * 256)+ lf = truncate ((l - fromIntegral li) * q32Width) +-- | Construct SBP L2 GPS carrier phase from L2 RTCM observation+--+-- See DF018, pg. 3-18 of the RTCM3 spec+toL_L2 :: GpsL1Observation+ -> GpsL1ExtObservation+ -> GpsL2Observation+ -> GpsL2ExtObservation+ -> CarrierPhase+toL_L2 l1 l1e l2 l2e = CarrierPhase+ { _carrierPhase_i = fromIntegral li+ , _carrierPhase_f = fromIntegral lf+ } where+ p = metricPseudorange l1 l1e+ -- Convert to SBP carrier phase representation per+ -- https://github.com/swift-nav/libsbp/blob/master/spec/yaml/swiftnav/sbp/observation.yaml#L39+ lm :: Double+ lm = p + phaseRangeRes * fromIntegral (l2 ^. gpsL2Observation_carrierMinusCode)+ l = lm / (lightSpeedMS / l2frequency)+ li :: Int32+ li = floor (l)+ lf :: Word8+ lf = truncate ((l - fromIntegral li) * q32Width) -toCn0 :: GpsL1ExtObservation -> Word8-toCn0 = (^. gpsL1ExtObservation_cnr)+toCn0_L1 :: GpsL1ExtObservation -> Word8+toCn0_L1 = (^. gpsL1ExtObservation_cnr) -toLock :: GpsL1Observation -> Word16-toLock _l1 = 0+toCn0_L2 :: GpsL2ExtObservation -> Word8+toCn0_L2 = (^. gpsL2ExtObservation_cnr) -toSid :: Word8 -> GnssSignal-toSid sat = GnssSignal- { _gnssSignal_sat = fromIntegral $ sat - 1- , _gnssSignal_code = 0- , _gnssSignal_reserved = 0- }+toLock_L1 :: GpsL1Observation -> Word16+toLock_L1 _l1 = 0 +toLock_L2 :: GpsL2Observation -> Word16+toLock_L2 _l2 = 0++-- | Construct sequenced SBP observation header+--+fromGpsObservationHeader :: MonadIO m+ => Word8 -- ^ Total messages+ -> Word8 -- ^ Message in sequence+ -> GpsObservationHeader -- ^ RTCM observation header+ -> m ObservationHeader+fromGpsObservationHeader totalMsgs n hdr = do+ t <- toGPSTime hdr+ return ObservationHeader+ { _observationHeader_t = t+ -- First nibble is the size of the sequence (n), second nibble is the+ -- zero-indexed counter (ith packet of n). See observation header packing+ -- https://github.com/swift-nav/libsbp/blob/master/spec/yaml/swiftnav/sbp/observation.yaml#L63+ , _observationHeader_n_obs = totalMsgs `shiftL` 4 .|. n+ }++-- | Construct an L1 SBP PackedObsContent an RTCM satellite vehicle observation+--+fromL1SatelliteObservation :: Word8 -- ^ Satellite PRN+ -> GpsL1Observation+ -> GpsL1ExtObservation+ -> PackedObsContent+fromL1SatelliteObservation sat l1 l1e =+ -- Checks GPS L1 code indicator for RTCM message 1002.+ -- See DF016, pg. 3-17 of the RTCM3 spec.+ if l1 ^. gpsL1Observation_code then+ PackedObsContent+ { _packedObsContent_P = toP_L1 l1 l1e+ , _packedObsContent_L = toL_L1 l1 l1e+ , _packedObsContent_cn0 = toCn0_L1 l1e+ , _packedObsContent_lock = toLock_L1 l1+ , _packedObsContent_sid = GnssSignal+ { _gnssSignal_sat = fromIntegral $ sat - 1+ , _gnssSignal_code = l1PSidCode+ , _gnssSignal_reserved = 0+ }+ }+ else+ PackedObsContent+ { _packedObsContent_P = toP_L1 l1 l1e+ , _packedObsContent_L = toL_L1 l1 l1e+ , _packedObsContent_cn0 = toCn0_L1 l1e+ , _packedObsContent_lock = toLock_L1 l1+ , _packedObsContent_sid = GnssSignal+ { _gnssSignal_sat = fromIntegral $ sat - 1+ , _gnssSignal_code = l1CSidCode+ , _gnssSignal_reserved = 0+ }+ }++-- | Construct SBP GPS observation message (possibly chunked).+--+chunkToMsgObs :: MonadIO m+ => Msg1004 -- ^ RTCM 1004 observation message+ -> Word8 -- ^ Total messages+ -> Word8 -- ^ Message in sequence+ -> [PackedObsContent]+ -> m MsgObs+chunkToMsgObs m totalMsgs n packed = do+ header <- fromGpsObservationHeader totalMsgs n $ m ^. msg1004_header+ return MsgObs+ { _msgObs_header = header+ , _msgObs_obs = packed+ }++-- | Sender Id is Station Id with high byte or'd in+--+toSender :: Word16 -> Word16+toSender = (.|. 0xf00)+++--------------------------------------------------------------------------------+-- RTCM to SBP conversion utilities: RTCM Msgs. 1002 (L1 RTK), 1004 (L1+L2 RTK),+-- 1005 (antenna position), 1006 (antenna position).+++-- | Construct an L1 SBP PackedObsContent from an RTCM Msg 1002.+-- fromObservation1002 :: Observation1002 -> Maybe PackedObsContent fromObservation1002 obs =- if obs ^. observation1002_l1 ^. gpsL1Observation_code then Nothing else Just PackedObsContent- { _packedObsContent_P = toP l1 l1e- , _packedObsContent_L = toL l1 l1e- , _packedObsContent_cn0 = toCn0 l1e- , _packedObsContent_lock = toLock l1- , _packedObsContent_sid = toSid sat- } where+ -- Only lower set of PRN numbers (1-32) are supported+ if sat > maxSats then Nothing else Just (fromL1SatelliteObservation sat l1 l1e)+ where sat = obs ^. observation1002_sat l1 = obs ^. observation1002_l1 l1e = obs ^. observation1002_l1e -fromObservation1004 :: Observation1004 -> Maybe PackedObsContent-fromObservation1004 obs =- if obs ^. observation1004_l1 ^. gpsL1Observation_code then Nothing else Just PackedObsContent- { _packedObsContent_P = toP l1 l1e- , _packedObsContent_L = toL l1 l1e- , _packedObsContent_cn0 = toCn0 l1e- , _packedObsContent_lock = toLock l1- , _packedObsContent_sid = toSid sat- } where- sat = obs ^. observation1004_sat- l1 = obs ^. observation1004_l1- l1e = obs ^. observation1004_l1e-+-- | Convert an RTCM L1 1002 observation into an SBP MsgObs.+-- fromMsg1002 :: MonadIO m => Msg1002 -> m MsgObs fromMsg1002 m = do- header <- fromGpsObservationHeader $ m ^. msg1002_header+ header <- fromGpsObservationHeader 1 0 (m ^. msg1002_header) return MsgObs { _msgObs_header = header , _msgObs_obs = mapMaybe fromObservation1002 $ m ^. msg1002_observations } -fromMsg1004 :: MonadIO m => Msg1004 -> m MsgObs+-- | Construct an L1/L2 SBP PackedObsContent from an RTCM Msg 1004.+--+fromObservation1004 :: Observation1004 -> Maybe [PackedObsContent]+fromObservation1004 obs =+ -- Only lower set of PRN numbers (1-32) are supported+ if sat > maxSats then Nothing else mapM id [obs1, obs2]+ where+ sat = obs ^. observation1004_sat+ l1 = obs ^. observation1004_l1+ l1e = obs ^. observation1004_l1e+ -- Checks GPS L1 code indicator for RTCM message 1004+ -- See DF016, pg. 3-17 of the RTCM3 spec.+ obs1 = Just (fromL1SatelliteObservation sat l1 l1e)+ l2 = obs ^. observation1004_l2+ l2e = obs ^. observation1004_l2e+ code = l2 ^. gpsL2Observation_code+ -- Checks GPS L2 code indicator.+ -- See DF016, pg. 3-17 of the RTCM3 spec.+ obs2 = if member code l2codeToSBPSignalCode then+ Just PackedObsContent+ { _packedObsContent_P = toP_L2 l1 l1e l2+ , _packedObsContent_L = toL_L2 l1 l1e l2 l2e+ , _packedObsContent_cn0 = toCn0_L2 l2e+ , _packedObsContent_lock = toLock_L2 l2+ , _packedObsContent_sid = GnssSignal+ { _gnssSignal_sat = fromIntegral $ sat - 1+ , _gnssSignal_code = l2codeToSBPSignalCode ! code+ , _gnssSignal_reserved = 0+ }+ }+ else+ Nothing++-- | Convert an RTCM L1+L2 1004 observation into multiple SBP MsgObs.+--+-- This chunking takes places because the number of observations in a given 1004+-- may very well exceed the maximum SBP supported payload size of 255 bytes.+fromMsg1004 :: MonadIO m => Msg1004 -> m [MsgObs] fromMsg1004 m = do- header <- fromGpsObservationHeader $ m ^. msg1004_header- return MsgObs- { _msgObs_header = header- , _msgObs_obs = mapMaybe fromObservation1004 $ m ^. msg1004_observations- }+ let obs = concat $ mapMaybe fromObservation1004 $ m ^. msg1004_observations+ chunks = zip [0..] $ chunksOf maxObsPerMessage obs+ totalMsgs = fromIntegral $ length chunks+ mapM (\(n, packed) -> chunkToMsgObs m totalMsgs n packed) chunks +-- | Convert an RTCM 1005 antenna reference position message into an SBP+-- MsgBasePosEcef. fromMsg1005 :: MonadIO m => Msg1005 -> m MsgBasePosEcef fromMsg1005 m = return MsgBasePosEcef@@ -124,6 +360,8 @@ , _msgBasePosEcef_z = fromEcefVal $ m ^. msg1005_reference ^. antennaReference_ecef_z } +-- | Convert an RTCM 1006 antenna reference position message into an SBP+-- MsgBasePosEcef. fromMsg1006 :: MonadIO m => Msg1006 -> m MsgBasePosEcef fromMsg1006 m = return MsgBasePosEcef@@ -132,18 +370,24 @@ , _msgBasePosEcef_z = fromEcefVal $ m ^. msg1006_reference ^. antennaReference_ecef_z } -convert :: MonadIO m => RTCM3Msg -> m (Maybe SBPMsg)+-- | Convert an RTCM message into possibly multiple SBP messages.+--+convert :: MonadIO m => RTCM3Msg -> m (Maybe [SBPMsg]) convert = \case (RTCM3Msg1002 m _rtcm3) -> do+ let sender = m ^. msg1002_header ^. gpsObservationHeader_station m' <- fromMsg1002 m- return $ Just $ SBPMsgObs m' $ toSBP m' defaultSender+ return $ Just [SBPMsgObs m' $ toSBP m' $ toSender sender] (RTCM3Msg1004 m _rtcm3) -> do+ let sender = m ^. msg1004_header ^. gpsObservationHeader_station m' <- fromMsg1004 m- return $ Just $ SBPMsgObs m' $ toSBP m' defaultSender+ return $ Just (fmap (\x -> SBPMsgObs x $ toSBP x $ toSender sender) m') (RTCM3Msg1005 m _rtcm3) -> do+ let sender = m ^. msg1005_reference ^. antennaReference_station m' <- fromMsg1005 m- return $ Just $ SBPMsgBasePosEcef m' $ toSBP m' defaultSender+ return $ Just [SBPMsgBasePosEcef m' $ toSBP m' $ toSender sender] (RTCM3Msg1006 m _rtcm3) -> do+ let sender = m ^. msg1006_reference ^. antennaReference_station m' <- fromMsg1006 m- return $ Just $ SBPMsgBasePosEcef m' $ toSBP m' defaultSender+ return $ Just [SBPMsgBasePosEcef m' $ toSBP m' $ toSender sender] _rtcm3Msg -> return Nothing
test/Test.hs view
@@ -1,9 +1,21 @@-import BasicPrelude-import Test.Tasty+{-# LANGUAGE NoImplicitPrelude #-}+{-# LANGUAGE OverloadedStrings #-} +-- |+-- Module: Test+-- Copyright: (c) 2016 Swift Navigation Inc.+-- License: AllRightsReserved+-- Maintainer: Skylark Team <skylark@swift-nav.com>+--+-- Test module for GNSS converters++import BasicPrelude+import qualified Test.Data.RTCM3.SBP as SBP+import Test.Tasty+ tests :: TestTree tests = testGroup "Tests"- [+ [ SBP.tests ] main :: IO ()
+ test/Test/Data/RTCM3/SBP.hs view
@@ -0,0 +1,205 @@+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE LambdaCase #-}+{-# LANGUAGE NoImplicitPrelude #-}+{-# LANGUAGE OverloadedStrings #-}+{-# LANGUAGE RecordWildCards #-}++-- |+-- Module: Test.Data.RTCM3.SBP+-- Copyright: (c) 2016 Swift Navigation Inc.+-- License: LGPL-3+-- Maintainer: Skylark Team <skylark@swift-nav.com>+--+-- Test RTCMv3 to SBP Conversions.++module Test.Data.RTCM3.SBP+ ( tests+ ) where++import BasicPrelude+import Control.Lens+import Control.Monad.Trans.Resource+import Data.Conduit+import Data.Conduit.Binary+import Data.Conduit.Serialization.Binary+import Data.HashMap.Strict+import Data.RTCM3.SBP+import qualified Data.Conduit.List as CL+import Data.Word+import SwiftNav.SBP+import Test.HUnit.Approx+import Test.Tasty+import Test.Tasty.HUnit++decodeRTCMFile :: (MonadIO m, MonadBaseControl IO m, MonadThrow m)+ => FilePath+ -> m [SBPMsg]+decodeRTCMFile filename =+ runResourceT . runConduit $+ sourceFile filename =$=+ conduitDecode =$=+ CL.mapMaybeM convert =$=+ CL.concat $$+ CL.consume++basePosition :: MsgBasePosEcef -> (Double, Double, Double)+basePosition msg' = ( msg' ^. msgBasePosEcef_x+ , msg' ^. msgBasePosEcef_y+ , msg' ^. msgBasePosEcef_z+ )++isL1 :: PackedObsContent -> Bool+isL1 obs = (obs ^. packedObsContent_sid ^. gnssSignal_code) == l1CSidCode++isL2 :: PackedObsContent -> Bool+isL2 obs = code == l2CMSidCode || code == l2PSidCode+ where+ code = obs ^. packedObsContent_sid ^. gnssSignal_code++sat :: PackedObsContent -> Word16+sat obs = obs ^. packedObsContent_sid ^. gnssSignal_sat++cn0 :: PackedObsContent -> Double+cn0 obs = 0.25 * fromIntegral (obs ^. packedObsContent_cn0)++pseudorange :: PackedObsContent -> Double+pseudorange obs = fromIntegral (obs ^. packedObsContent_P) / sbpCMinM++carrierPhase :: PackedObsContent -> Double+carrierPhase obs = whole + fraction+ where+ phase = obs ^. packedObsContent_L+ whole = fromIntegral (phase ^. carrierPhase_i)+ fraction = fromIntegral (phase ^. carrierPhase_f) / q32Width++assertExpectedBase :: SBPMsg -> (Double, Double, Double) -> Assertion+assertExpectedBase (SBPMsgBasePosEcef posEcef _) pos =+ assertEqual "Base station position error!" pos $ basePosition posEcef+assertExpectedBase _ _ = assertFailure "Invalid message type!"++assertObsHeader :: SBPMsg -> ObservationHeader -> Assertion+assertObsHeader (SBPMsgObs obs _) header =+ assertEqual "Observation header is not equal" header $ obs ^. msgObs_header+assertObsHeader _ _ = assertFailure "Invalid message type!"++assertObs :: HashMap Word16 (Double, Double, Double)+ -> PackedObsContent+ -> Double -- ^ Pseudorange tolerance (meters)+ -> Double -- ^ Carrier phase tolerance (cycles)+ -> Word8 -- ^ Satellite band code+ -> Assertion+assertObs truth obs ptol ctol sig = do+ let prn = 1 + sat obs+ def = (0, 0, 0)+ (p, c, snr) = lookupDefault def prn truth+ code = obs ^. packedObsContent_sid ^. gnssSignal_code+ msg' = textToString $ "PRN=" ++ show prn ++ " CODE=" ++ show code+ -- Pseudorange representation error+ assertApproxEqual ("Incorrect pseudorange" ++ msg') ptol p $ pseudorange obs+ -- Carrier phase representation error+ assertApproxEqual ("Incorrect carrier phase" ++ msg') ctol c $ carrierPhase obs+ -- SNRs/cn0 should be exact+ assertEqual ("Incorrect SNR" ++ msg') snr $ cn0 obs+ assertEqual ("Incorrect code" ++ msg') sig code++assertMsgObs :: SBPMsg -> Assertion+assertMsgObs (SBPMsgObs obs' _) = do+ let ptol = 0.020 -- 2cm representation errorn+ ctol_L1 = 0.005 -- carrier phase / L1 wavelength = 0.001m/0.190m cycles+ ctol_L2 = 0.004 -- carrier phase / L2 wavelength = 0.001m/0.250m cycles+ forM_ (obs' ^. msgObs_obs) $ \packed ->+ case () of+ _ | isL1 packed -> assertObs testObs_L1 packed ptol ctol_L1 l1CSidCode+ | isL2 packed -> assertObs testObs_L2 packed ptol ctol_L2 l2PSidCode+ | otherwise -> assertFailure "Not L1 or L2!"+assertMsgObs _ = assertFailure "Invalid message type!"++assertMsgObsLength :: SBPMsg -> Int -> Assertion+assertMsgObsLength (SBPMsgObs obs' _) len = length (obs' ^. msgObs_obs) @?= len+assertMsgObsLength _ _ = assertFailure "Invalid message type!"++-- | L1 observations: PRN => Pseudorange, Carrier Phase, SNR+--+-- From fixtures/rinex/ucsf_bard_four_seconds.obs RINEX+testObs_L1 :: HashMap Word16 (Double, Double, Double)+testObs_L1 = fromList [ (5, (20982568.242, 110264218.428, 50.500))+ , (31, (24390286.418, 128171817.525, 43.000))+ , (25, (20560669.144, 108047101.226, 51.750))+ , (12, (21678581.616, 113921748.286, 49.500))+ , (29, (20679317.864, 108670633.460, 50.000))+ , (21, (23719676.742, 124647833.324, 41.000))+ , (2, (22612474.630, 118829414.562, 44.000))+ , (20, (21815089.336, 114639133.794, 48.000))+ ]++-- | L2 observations: PRN => Pseudorange, Carrier Phase, SNR+--+-- From fixtures/rinex/ucsf_bard_four_seconds.obs RINEX+testObs_L2 :: HashMap Word16 (Double, Double, Double)+testObs_L2 = fromList [ (5, (20982564.742, 85920194.806, 40.000))+ , (31, (24390281.338, 99874145.710, 22.750))+ , (25, (20560668.244, 84192554.659, 42.750))+ , (12, (21678578.556, 88770214.224, 38.500))+ , (29, (20679314.544, 84678442.345, 42.750))+ , (21, (23719674.482, 97128180.775, 23.250))+ , (2, (22612468.690, 92594355.238, 30.750))+ , (20, (21815086.356, 89329221.086, 34.500))+ ]++testMsg1004 :: TestTree+testMsg1004 =+ testGroup "Msg1004 conversion to SBP"+ [ testCase "Four seconds of RTCM3" $ do+ msgs <- decodeRTCMFile "fixtures/rtcm3/ucsf_bard_four_seconds.rtcm3"+ -- There are eight messages+ length msgs @?= 8+ -- Check message 0 and 5 against values in RINEX file:+ -- fixtures/rinex/ucsf_bard_four_seconds.obs+ let expectedBasePos = (-2709557.0660, -4260015.9160, 3884773.0630)+ assertExpectedBase (msgs !! 0) expectedBasePos+ assertExpectedBase (msgs !! 5) expectedBasePos+ -- Message 1+ assertObsHeader (msgs !! 1) ObservationHeader {+ _observationHeader_t = ObsGPSTime 86354000 1905+ , _observationHeader_n_obs = 32+ }+ assertMsgObsLength (msgs !! 1) 15+ assertMsgObs (msgs !! 1)+ -- Message 2+ assertObsHeader (msgs !! 2) ObservationHeader {+ _observationHeader_t = ObsGPSTime 86354000 1905+ , _observationHeader_n_obs = 33+ }+ assertMsgObs (msgs !! 2)+ assertMsgObsLength (msgs !! 2) 1+ -- Message 3+ assertObsHeader (msgs !! 3) ObservationHeader {+ _observationHeader_t = ObsGPSTime 86355000 1905+ , _observationHeader_n_obs = 32+ }+ assertMsgObsLength (msgs !! 3) 15+ -- Message 4+ assertObsHeader (msgs !! 4) ObservationHeader {+ _observationHeader_t = ObsGPSTime 86355000 1905+ , _observationHeader_n_obs = 33+ }+ assertMsgObsLength (msgs !! 4) 1+ -- Message 6+ assertObsHeader (msgs !! 6) ObservationHeader {+ _observationHeader_t = ObsGPSTime 86356000 1905+ , _observationHeader_n_obs = 32+ }+ assertMsgObsLength (msgs !! 6) 15+ -- Message 7+ assertObsHeader (msgs !! 7) ObservationHeader {+ _observationHeader_t = ObsGPSTime 86356000 1905+ , _observationHeader_n_obs = 33+ }+ assertMsgObsLength (msgs !! 7) 1+ ]++tests :: TestTree+tests =+ testGroup "RTCM3 to SBP conversion tests"+ [ testMsg1004+ ]