diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -1,5 +1,5 @@ name: gnss-converters -version: 0.3.13 +version: 0.3.14 synopsis: GNSS Converters. description: Haskell bindings for GNSS converters. homepage: http://github.com/swift-nav/gnss-converters @@ -17,7 +17,8 @@ library hs-source-dirs: src - exposed-modules: Data.RTCM3.SBP + exposed-modules: Data.RTCM3.Replay + , Data.RTCM3.SBP , Data.RTCM3.SBP.Ephemerides , Data.RTCM3.SBP.Observations , Data.RTCM3.SBP.Positions @@ -56,6 +57,18 @@ executable rtcm32sbp 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 + +executable rtcm32rtcm3 + hs-source-dirs: main + main-is: RTCM32RTCM3.hs ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall build-depends: base , basic-prelude diff --git a/main/RTCM32RTCM3.hs b/main/RTCM32RTCM3.hs new file mode 100644 --- /dev/null +++ b/main/RTCM32RTCM3.hs @@ -0,0 +1,27 @@ +{-# LANGUAGE NoImplicitPrelude #-} + +-- | +-- Module: RTCM32RTCM3 +-- Copyright: (c) 2015 Mark Fine +-- License: BSD3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- RTCM3 to RTCM3 tool. + +import BasicPrelude +import Data.Conduit +import Data.Conduit.Binary +import Data.Conduit.Serialization.Binary +import Data.RTCM3.Replay +import System.IO + +main :: IO () +main = + runConduitRes $ + sourceHandle stdin + =$= conduitDecode + =$= replay + =$= conduitEncode + $$ sinkHandle stdout diff --git a/src/Data/RTCM3/Replay.hs b/src/Data/RTCM3/Replay.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/Replay.hs @@ -0,0 +1,106 @@ +{-# LANGUAGE LambdaCase #-} +{-# LANGUAGE NoImplicitPrelude #-} + +module Data.RTCM3.Replay + ( replay + , replay' + ) where + +import BasicPrelude hiding (mapM) +import Control.Concurrent hiding (yield) +import Control.Lens +import Data.Conduit +import Data.Conduit.List +import Data.RTCM3 +import Data.Time +import Data.Time.Calendar.WeekDate + +-- | Produce GPS time of week from a GPS UTC time. +-- +toTow :: UTCTime -> Word32 +toTow t = floor since + where + (y, w, _d) = toWeekDate (utctDay t) + begin = addDays (-1) $ fromWeekDate y w 1 + since = 1000 * diffUTCTime t (UTCTime begin 0) + +-- | Produce current GPS time of week. +-- +currentTow :: MonadIO m => m Word32 +currentTow = + liftIO $ toTow . addUTCTime (fromIntegral gpsLeapSeconds) <$> getCurrentTime + where + gpsLeapSeconds = 18 :: Int + +-- | Update time of week on replayed messages to be current. +-- +postDelay :: MonadIO m => Conduit RTCM3Msg m RTCM3Msg +postDelay = awaitForever $ \case + (RTCM3Msg1002 m _rtcm3) -> do + tow <- currentTow + let n = set (msg1002_header . gpsObservationHeader_tow) tow m + yield (RTCM3Msg1002 n (toRTCM3 n)) + (RTCM3Msg1004 m _rtcm3) -> do + tow <- currentTow + let n = set (msg1004_header . gpsObservationHeader_tow) tow m + yield (RTCM3Msg1004 n (toRTCM3 n)) + _rtcm3Msg -> + pure () + +-- | Delay between two time of week values - differences over 15 seconds are ignored. +-- +delayTow :: MonadIO m => Word32 -> Word32 -> m () +delayTow tow tow' = + when (tow' > tow) $ do + let diff = tow' - tow + unless (diff > diffMilliseconds) $ + liftIO $ threadDelay $ fromIntegral $ diff * 1000 + where + diffMilliseconds = 15 * 1000 + +-- | Delay between observations. +-- +delay :: MonadIO m => Conduit (Word32, RTCM3Msg) m RTCM3Msg +delay = awaitForever $ uncurry $ \tow -> \case + (RTCM3Msg1002 m rtcm3) -> peek >>= delayTow tow . fromMaybe maxBound . (fst <$>) >> yield (RTCM3Msg1002 m rtcm3) + (RTCM3Msg1004 m rtcm3) -> peek >>= delayTow tow . fromMaybe maxBound . (fst <$>) >> yield (RTCM3Msg1004 m rtcm3) + _rtcm3Msg -> pure () + +-- | Setup observations with their time of weeks. +-- +preDelay :: Monad m => Conduit RTCM3Msg m (Word32, RTCM3Msg) +preDelay = awaitForever $ \case + (RTCM3Msg1002 m rtcm3) -> yield (m ^. msg1002_header ^. gpsObservationHeader_tow, RTCM3Msg1002 m rtcm3) + (RTCM3Msg1004 m rtcm3) -> yield (m ^. msg1004_header ^. gpsObservationHeader_tow, RTCM3Msg1004 m rtcm3) + _rtcm3Msg -> pure () + +-- | Replay observations. +-- +replay :: MonadIO m => Conduit RTCM3Msg m RTCM3Msg +replay = preDelay =$= delay =$= postDelay + +dog :: MonadIO m => Word32 -> RTCM3Msg -> ConduitM i RTCM3Msg m Word32 +dog tow = \case + (RTCM3Msg1002 m _rtcm3) -> do + let tow' = m ^. msg1002_header ^. gpsObservationHeader_tow + delayTow tow tow' + tow'' <- currentTow + let n = set (msg1002_header . gpsObservationHeader_tow) tow'' m + yield (RTCM3Msg1002 n (toRTCM3 n)) + pure tow' + (RTCM3Msg1004 m _rtcm3) -> do + let tow' = m ^. msg1004_header ^. gpsObservationHeader_tow + delayTow tow tow' + tow'' <- currentTow + let n = set (msg1004_header . gpsObservationHeader_tow) tow'' m + yield (RTCM3Msg1004 n (toRTCM3 n)) + pure tow' + rtcm3Msg -> do + yield rtcm3Msg + pure tow + +replay' :: MonadIO m => Conduit RTCM3Msg m RTCM3Msg +replay' = loop maxBound + where + loop tow = + await >>= maybe (pure tow) (dog tow) >>= loop diff --git a/test/Test/Data/RTCM3/SBP/Time.hs b/test/Test/Data/RTCM3/SBP/Time.hs --- a/test/Test/Data/RTCM3/SBP/Time.hs +++ b/test/Test/Data/RTCM3/SBP/Time.hs @@ -6,28 +6,28 @@ ) where import BasicPrelude -import Test.Tasty -import Test.Tasty.HUnit import Data.RTCM3.SBP.Time import SwiftNav.SBP +import Test.Tasty +import Test.Tasty.HUnit 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) + 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) + 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) + 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