diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -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 -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 - diff --git a/main/RTCM32SBP.hs b/main/RTCM32SBP.hs --- a/main/RTCM32SBP.hs +++ b/main/RTCM32SBP.hs @@ -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 diff --git a/src/Data/RTCM3/SBP.hs b/src/Data/RTCM3/SBP.hs --- a/src/Data/RTCM3/SBP.hs +++ b/src/Data/RTCM3/SBP.hs @@ -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 -- 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 diff --git a/test/Test.hs b/test/Test.hs --- a/test/Test.hs +++ b/test/Test.hs @@ -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 +-- +-- 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 () diff --git a/test/Test/Data/RTCM3/SBP.hs b/test/Test/Data/RTCM3/SBP.hs new file mode 100644 --- /dev/null +++ b/test/Test/Data/RTCM3/SBP.hs @@ -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 +-- +-- 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 + ]