diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -1,100 +1,93 @@ -name: gnss-converters -version: 0.3.11 -cabal-version: >=1.22 -build-type: Simple -license: BSD3 -copyright: Copyright (C) 2016, 2017 Swift Navigation, Inc. -maintainer: Swift Navigation -homepage: http://github.com/swift-nav/gnss-converters -synopsis: GNSS Converters. -description: - Haskell bindings for GNSS converters. -category: Network -author: Mark Fine , Joshua Gross +name: gnss-converters +version: 0.3.13 +synopsis: GNSS Converters. +description: Haskell bindings for GNSS converters. +homepage: http://github.com/swift-nav/gnss-converters +license: BSD3 +author: Mark Fine , Joshua Gross +maintainer: Swift Navigation +copyright: Copyright (C) 2016, 2017 Swift Navigation, Inc. +category: Network +build-type: Simple +cabal-version: >= 1.22 source-repository head - type: git - location: https://github.com/swift-nav/gnss-converters + type: git + location: https://github.com/swift-nav/gnss-converters library - exposed-modules: - Data.RTCM3.SBP - Data.RTCM3.SBP.Ephemerides - Data.RTCM3.SBP.Observations - Data.RTCM3.SBP.Positions - Data.RTCM3.SBP.Time - Data.RTCM3.SBP.Types - SwiftNav.SBP.RTCM3 - build-depends: - base >=4.8 && <5, - basic-prelude >=0.5.2, - conduit >=1.2.10, - exceptions >=0.8.3, - extra >=1.4.10, - lens >=4.13, - monad-control >=1.0.1.0, - mtl >=2.2.1, - resourcet >=1.1.9, - rtcm >=0.2.5, - sbp >=2.2.12, - time >=1.5.0.1, - transformers-base >=0.4.4, - vector >=0.11.0.0 - default-language: Haskell2010 - hs-source-dirs: src - ghc-options: -Wall + hs-source-dirs: src + exposed-modules: Data.RTCM3.SBP + , Data.RTCM3.SBP.Ephemerides + , Data.RTCM3.SBP.Observations + , Data.RTCM3.SBP.Positions + , Data.RTCM3.SBP.Time + , Data.RTCM3.SBP.Types + , SwiftNav.SBP.RTCM3 + ghc-options: -Wall + build-depends: base >= 4.8 && < 5 + , basic-prelude + , conduit + , exceptions + , extra + , lens + , monad-control + , mtl + , resourcet + , rtcm + , sbp + , time + , transformers-base + , vector + default-language: Haskell2010 executable sbp2rtcm3 - main-is: SBP2RTCM3.hs - build-depends: - base >=4.8.2.0, - basic-prelude >=0.5.2, - binary-conduit >=1.2.4.1, - conduit >=1.2.10, - conduit-extra >=1.1.16, - gnss-converters - default-language: Haskell2010 - hs-source-dirs: main - ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + hs-source-dirs: main + main-is: SBP2RTCM3.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , binary-conduit + , conduit + , conduit-extra + , gnss-converters + default-language: Haskell2010 executable rtcm32sbp - main-is: RTCM32SBP.hs - build-depends: - base >=4.8.2.0, - basic-prelude >=0.5.2, - binary-conduit >=1.2.4.1, - conduit >=1.2.10, - conduit-extra >=1.1.16, - gnss-converters - default-language: Haskell2010 - hs-source-dirs: main - ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + hs-source-dirs: main + main-is: RTCM32SBP.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , binary-conduit + , conduit + , conduit-extra + , gnss-converters + default-language: Haskell2010 test-suite test - type: exitcode-stdio-1.0 - main-is: Test.hs - build-depends: - aeson >=0.11.3.0, - aeson-pretty >=0.7.2, - base >=4.8.2.0, - basic-prelude >=0.5.2, - binary >=0.7.5.0, - binary-conduit >=1.2.4.1, - bytestring >=0.10.6.0, - conduit >=1.2.10, - conduit-extra >=1.1.16, - gnss-converters, - lens >=4.13, - resourcet >=1.1.9, - rtcm >=0.2.5, - sbp >=2.2.12, - tasty >=0.11.2.1, - tasty-golden >=2.3.1.1, - tasty-hunit >=0.9.2, - text >=1.2.2.2, - unordered-containers >=0.2.8.0 - default-language: Haskell2010 - hs-source-dirs: test - other-modules: - Test.Data.RTCM3.SBP - ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + type: exitcode-stdio-1.0 + hs-source-dirs: test + main-is: Test.hs + other-modules: Test.Data.RTCM3.SBP + , Test.Data.RTCM3.SBP.Time + build-depends: aeson + , aeson-pretty + , base + , basic-prelude + , binary + , binary-conduit + , bytestring + , conduit + , conduit-extra + , gnss-converters + , lens + , rtcm + , sbp + , tasty + , tasty-golden + , tasty-hunit + , text + , unordered-containers + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + default-language: Haskell2010 diff --git a/main/RTCM32SBP.hs b/main/RTCM32SBP.hs --- a/main/RTCM32SBP.hs +++ b/main/RTCM32SBP.hs @@ -15,13 +15,11 @@ import Data.Conduit.Binary import Data.Conduit.Serialization.Binary import Data.RTCM3.SBP -import Data.RTCM3.SBP.Types import System.IO main :: IO () -main = do - s <- newStore - runConvertT s $ runConduitRes $ +main = + runConverter $ runConduitRes $ sourceHandle stdin =$= conduitDecode =$= awaitForever converter 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 @@ -14,15 +14,19 @@ module Data.RTCM3.SBP ( converter , newStore + , runConvertT + , runConverter ) where import BasicPrelude +import Control.Monad.Reader import Data.Conduit import Data.IORef import Data.RTCM3 import qualified Data.RTCM3.SBP.Ephemerides as Ephemerides import qualified Data.RTCM3.SBP.Observations as Observations import qualified Data.RTCM3.SBP.Positions as Positions +import Data.RTCM3.SBP.Time import Data.RTCM3.SBP.Types import SwiftNav.SBP @@ -42,4 +46,16 @@ -- | Setup new storage for converter. -- newStore :: MonadIO m => m Store -newStore = liftIO $ Store <$> newIORef mempty <*> newIORef mempty +newStore = liftIO $ Store <$> pure currentGpsTime <*> newIORef mempty <*> newIORef mempty + +-- | Run converter. +-- +runConvertT :: HasStore e => e -> ConvertT e m a -> m a +runConvertT e (ConvertT m) = runReaderT m e + +-- | Run converter with default store. +-- +runConverter :: MonadIO m => ConvertT Store m a -> m a +runConverter action = do + s <- newStore + runConvertT s action diff --git a/src/Data/RTCM3/SBP/Ephemerides.hs b/src/Data/RTCM3/SBP/Ephemerides.hs --- a/src/Data/RTCM3/SBP/Ephemerides.hs +++ b/src/Data/RTCM3/SBP/Ephemerides.hs @@ -19,7 +19,7 @@ import Data.Bits import Data.Conduit import Data.RTCM3 -import Data.RTCM3.SBP.Time +import Data.RTCM3.SBP.Types import Data.Word import SwiftNav.SBP @@ -29,9 +29,10 @@ -- | Produce GPS time from RTCM time (week-number % 1024, tow in seconds, scaled 2^4). -- Stateful but non-side-effecting. The week number is mod-1024 - add in the base -- wn from the stored wn masked to 10 bits. -toGpsTimeSec :: MonadIO m => Word16 -> Word16 -> m GpsTimeSec +toGpsTimeSec :: MonadStore e m => Word16 -> Word16 -> m GpsTimeSec toGpsTimeSec wn tow = do - wn' <- view gpsTimeNano_wn <$> currentGpsTime + time <- view storeCurrentGpsTime + wn' <- view gpsTimeNano_wn <$> liftIO time pure GpsTimeSec { _gpsTimeSec_tow = 16 * fromIntegral tow , _gpsTimeSec_wn = wn' `shiftR` 10 `shiftL` 10 + wn @@ -75,7 +76,7 @@ | otherwise = 6 * 60 * 60 -- | Construct an EphemerisCommonContent from an RTCM 1019 message. -toGpsEphemerisCommonContent :: MonadIO m => Msg1019 -> m EphemerisCommonContent +toGpsEphemerisCommonContent :: MonadStore e m => Msg1019 -> m EphemerisCommonContent toGpsEphemerisCommonContent m = do toe <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toe) pure EphemerisCommonContent @@ -95,7 +96,7 @@ -- 1005 (antenna position), 1006 (antenna position). -- | Convert an RTCM 1019 GPS ephemeris message into an SBP MsgEphemerisGps. -converter :: MonadIO m => Msg1019 -> Conduit i m [SBPMsg] +converter :: MonadStore e m => Msg1019 -> Conduit i m [SBPMsg] converter m = do common <- toGpsEphemerisCommonContent m toc <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toc) diff --git a/src/Data/RTCM3/SBP/Observations.hs b/src/Data/RTCM3/SBP/Observations.hs --- a/src/Data/RTCM3/SBP/Observations.hs +++ b/src/Data/RTCM3/SBP/Observations.hs @@ -43,7 +43,8 @@ toGpsTimeNano station rollover = do timeMap <- view storeGpsTimeMap modifyIORefM timeMap $ \timeMap' -> do - t <- maybe currentGpsTime pure (timeMap' ^. at station) + time <- view storeCurrentGpsTime + t <- maybe (liftIO time) pure (timeMap' ^. at station) let t' = rollover t pure (timeMap' & at station ?~ t', (t, t')) diff --git a/src/Data/RTCM3/SBP/Time.hs b/src/Data/RTCM3/SBP/Time.hs --- a/src/Data/RTCM3/SBP/Time.hs +++ b/src/Data/RTCM3/SBP/Time.hs @@ -1,3 +1,4 @@ +{-# OPTIONS -fno-warn-overflowed-literals #-} {-# LANGUAGE NoImplicitPrelude #-} -- | @@ -101,9 +102,10 @@ | increment = gpsTimeNano_wn +~ 1 | decrement = gpsTimeNano_wn +~ -1 | otherwise = gpsTimeNano_wn +~ 0 - diff = fromIntegral tow - fromIntegral (t ^. gpsTimeNano_tow) - increment = diff < -weekMillis `div` 2 - decrement = diff > weekMillis `div` 2 + new = fromIntegral tow + old = fromIntegral (t ^. gpsTimeNano_tow) + increment = old > new && old - new > weekMillis `div` 2 + decrement = new > old && new - old > weekMillis `div` 2 -- | Update GPS time based on GLONASS epoch, handling week rollover. -- @@ -120,6 +122,6 @@ | increment = dow + 1 `mod` 7 | decrement = dow - 1 `mod` 7 | otherwise = dow - increment = epoch'' - tod > dayMillis `div` 2 - decrement = tod - epoch'' > dayMillis `div` 2 + increment = epoch'' > tod && epoch'' - tod > dayMillis `div` 2 + decrement = tod > epoch'' && tod - epoch'' > dayMillis `div` 2 tow = fromIntegral $ dow' * dayMillis + epoch'' diff --git a/src/Data/RTCM3/SBP/Types.hs b/src/Data/RTCM3/SBP/Types.hs --- a/src/Data/RTCM3/SBP/Types.hs +++ b/src/Data/RTCM3/SBP/Types.hs @@ -68,14 +68,12 @@ {-# INLINE liftBase #-} data Store = Store - { _storeGpsTimeMap :: IORef GpsTimeNanoMap - , _storeObservations :: IORef (Vector PackedObsContent) - } deriving ( Eq ) + { _storeCurrentGpsTime :: IO GpsTimeNano + , _storeGpsTimeMap :: IORef GpsTimeNanoMap + , _storeObservations :: IORef (Vector PackedObsContent) + } $(makeClassy ''Store) - -runConvertT :: HasStore e => e -> ConvertT e m a -> m a -runConvertT e (ConvertT m) = runReaderT m e type MonadStore e m = ( MonadIO m diff --git a/test/Test.hs b/test/Test.hs --- a/test/Test.hs +++ b/test/Test.hs @@ -10,12 +10,14 @@ -- Test module for GNSS converters import BasicPrelude -import qualified Test.Data.RTCM3.SBP as SBP +import qualified Test.Data.RTCM3.SBP as SBP +import qualified Test.Data.RTCM3.SBP.Time as Time import Test.Tasty tests :: TestTree tests = testGroup "Tests" [ SBP.tests + , Time.tests ] main :: IO () diff --git a/test/Test/Data/RTCM3/SBP.hs b/test/Test/Data/RTCM3/SBP.hs --- a/test/Test/Data/RTCM3/SBP.hs +++ b/test/Test/Data/RTCM3/SBP.hs @@ -6,7 +6,7 @@ ) where import BasicPrelude hiding (map) -import Control.Monad.Trans.Resource +import Control.Lens import Data.Aeson import Data.Aeson.Encode.Pretty import Data.ByteString.Lazy hiding (ByteString, map) @@ -16,28 +16,33 @@ import Data.Conduit.Serialization.Binary import Data.RTCM3.SBP import Data.RTCM3.SBP.Types +import Data.Word +import SwiftNav.SBP import Test.Tasty import Test.Tasty.Golden +testGpsTime :: Applicative f => Word32 -> Word16 -> f GpsTimeNano +testGpsTime tow wn = pure $ GpsTimeNano tow 0 wn + encodeLine :: ToJSON a => a -> ByteString encodeLine v = toStrict $ encodePretty' defConfig { confCompare = compare } v <> "\n" -runConverter :: FilePath -> FilePath -> IO () -runConverter f t = do - s <- newStore - runResourceT $ runConvertT s $ - sourceFile f =$= - conduitDecode =$= - awaitForever converter =$= - map encodeLine $$ - sinkFile t +testConverter :: FilePath -> FilePath -> IO () +testConverter f t = do + s <- newStore <&> storeCurrentGpsTime .~ testGpsTime 510191000 1961 + runConvertT s $ runConduitRes $ + sourceFile f + =$= conduitDecode + =$= awaitForever converter + =$= map encodeLine + $$ sinkFile t goldenFileTest :: TestName -> FilePath -> FilePath -> TestTree goldenFileTest name s d = do let n = ("test/golden" ) f = "from_" <> d t = "to_" <> d - goldenVsFile name (n f) (n t) $ runConverter (n s) (n t) + goldenVsFile name (n f) (n t) $ testConverter (n s) (n t) testObservations :: TestTree testObservations = diff --git a/test/Test/Data/RTCM3/SBP/Time.hs b/test/Test/Data/RTCM3/SBP/Time.hs new file mode 100644 --- /dev/null +++ b/test/Test/Data/RTCM3/SBP/Time.hs @@ -0,0 +1,37 @@ +{-# LANGUAGE NoImplicitPrelude #-} +{-# LANGUAGE OverloadedStrings #-} + +module Test.Data.RTCM3.SBP.Time + ( tests + ) where + +import BasicPrelude +import Test.Tasty +import Test.Tasty.HUnit +import Data.RTCM3.SBP.Time +import SwiftNav.SBP + +testRolloverGpsTime :: TestTree +testRolloverGpsTime = + testGroup "Rollover GPS time tests" + [ testCase "No rollover" $ do + rolloverTowGpsTime 0 (GpsTimeNano 0 0 1960) @?= (GpsTimeNano 0 0 1960) + rolloverTowGpsTime 302400000 (GpsTimeNano 302400000 0 1960) @?= (GpsTimeNano 302400000 0 1960) + rolloverTowGpsTime 604800000 (GpsTimeNano 604800000 0 1960) @?= (GpsTimeNano 604800000 0 1960) + rolloverTowGpsTime 0 (GpsTimeNano 302400000 0 1960) @?= (GpsTimeNano 0 0 1960) + rolloverTowGpsTime 302400000 (GpsTimeNano 604800000 0 1960) @?= (GpsTimeNano 302400000 0 1960) + , testCase "Positive rollover" $ do + rolloverTowGpsTime 0 (GpsTimeNano 302400001 0 1960) @?= (GpsTimeNano 0 0 1961) + rolloverTowGpsTime 0 (GpsTimeNano 604800000 0 1960) @?= (GpsTimeNano 0 0 1961) + rolloverTowGpsTime 302399999 (GpsTimeNano 604800000 0 1960) @?= (GpsTimeNano 302399999 0 1961) + , testCase "Negative rollover" $ do + rolloverTowGpsTime 604800000 (GpsTimeNano 0 0 1961) @?= (GpsTimeNano 604800000 0 1960) + rolloverTowGpsTime 604800000 (GpsTimeNano 302399999 0 1961) @?= (GpsTimeNano 604800000 0 1960) + rolloverTowGpsTime 302400001 (GpsTimeNano 0 0 1961) @?= (GpsTimeNano 302400001 0 1960) + ] + +tests :: TestTree +tests = + testGroup "RTCM3 to SBP time tests" + [ testRolloverGpsTime + ]