gnss-converters 0.3.32 → 0.3.35
raw patch · 6 files changed
+501/−58 lines, 6 filesdep ~rtcmdep ~sbp
Dependency ranges changed: rtcm, sbp
Files
- gnss-converters.cabal +5/−4
- src/Data/RTCM3/SBP.hs +25/−0
- src/Data/RTCM3/SBP/MSM.hs +384/−0
- src/Data/RTCM3/SBP/Observations.hs +14/−36
- src/Data/RTCM3/SBP/Time.hs +62/−7
- test/Test/Data/RTCM3/SBP/Time.hs +11/−11
gnss-converters.cabal view
@@ -1,6 +1,6 @@ cabal-version: >=1.22 name: gnss-converters-version: 0.3.32+version: 0.3.35 license: BSD3 copyright: Copyright (C) 2016, 2017 Swift Navigation, Inc. maintainer: Swift Navigation <dev@swiftnav.com>@@ -23,6 +23,7 @@ Data.RTCM3.SBP.Biases Data.RTCM3.SBP.Ephemerides Data.RTCM3.SBP.Logging+ Data.RTCM3.SBP.MSM Data.RTCM3.SBP.Observations Data.RTCM3.SBP.Positions Data.RTCM3.SBP.SSR@@ -42,8 +43,8 @@ monad-control >=1.0.2.3, mtl >=2.2.2, resourcet >=1.1.11,- rtcm >=0.2.16,- sbp >=2.3.15,+ rtcm >=0.2.17,+ sbp >=2.3.16, time >=1.8.0.2, transformers-base >=0.4.4, vector >=0.12.0.1@@ -107,7 +108,7 @@ conduit-extra >=1.2.3.2, gnss-converters -any, lens >=4.15.4,- sbp >=2.3.15,+ sbp >=2.3.16, tasty >=0.11.3, tasty-golden >=2.3.1.2, tasty-hunit >=0.9.2,
src/Data/RTCM3/SBP.hs view
@@ -26,6 +26,7 @@ import qualified Data.RTCM3.SBP.Biases as Biases import qualified Data.RTCM3.SBP.Ephemerides as Ephemerides import qualified Data.RTCM3.SBP.Logging as Logging+import qualified Data.RTCM3.SBP.MSM as MSM import qualified Data.RTCM3.SBP.Observations as Observations import qualified Data.RTCM3.SBP.Positions as Positions import qualified Data.RTCM3.SBP.SSR as SSR@@ -46,6 +47,30 @@ (RTCM3Msg1019 m _rtcm3) -> Ephemerides.gpsConverter m (RTCM3Msg1020 m _rtcm3) -> Ephemerides.glonassConverter m (RTCM3Msg1029 m _rtcm3) -> Logging.converter m+ (RTCM3Msg1074 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1075 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1076 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1077 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1084 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1085 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1086 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1087 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1094 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1095 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1096 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1097 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1104 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1105 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1106 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1107 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1114 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1115 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1116 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1117 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1124 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1125 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1126 m _rtcm3) -> MSM.converter m+ (RTCM3Msg1127 m _rtcm3) -> MSM.converter m (RTCM3Msg1230 m _rtcm3) -> Biases.converter m (RTCM3Msg1060 m _rtcm3) -> SSR.gpsOrbitClockConverter m (RTCM3Msg1066 m _rtcm3) -> SSR.glonassOrbitClockConverter m
+ src/Data/RTCM3/SBP/MSM.hs view
@@ -0,0 +1,384 @@+{-# LANGUAGE NoImplicitPrelude #-}++-- |+-- Module: Data.RTCM3.SBP.MSM+-- Copyright: Copyright (C) 2017 Swift Navigation, Inc.+-- License: LGPL-3+-- Maintainer: Swift Navigation <dev@swiftnav.com>+-- Stability: experimental+-- Portability: portable+--+-- RTCMv3 MSM to SBP Observations.++module Data.RTCM3.SBP.MSM+ ( converter+ ) where++import BasicPrelude hiding (null)+import Control.Lens+import Data.Bits+import Data.Conduit+import Data.IORef+import Data.List.Extra hiding (filter, null, zip)+import Data.RTCM3+import Data.RTCM3.SBP.Time+import Data.RTCM3.SBP.Types+import Data.Vector hiding (filter, length, zip)+import Data.Word+import SwiftNav.SBP++-- | Derive sender from station.+--+toSender :: Word16 -> Word16+toSender station = station .|. 61440++masks :: Bits a => Int -> a -> [b] -> [b]+masks n a bs = fst <$> filter snd (zip bs $ testBit a <$> [n,n-1..0])++mask :: FiniteBits a => a -> [Word8]+mask n = masks (finiteBitSize n-1) n [(1::Word8)..]++toCells :: MsmHeader -> [((Int, Word8), (Int, Word8))]+toCells hdr =+ masks (popCount (hdr ^. msmHeader_satelliteMask) * popCount (hdr ^. msmHeader_signalMask)) (hdr ^. msmHeader_cellMask) $ do+ sat <- zip [0..] $ mask (hdr ^. msmHeader_satelliteMask)+ sig <- zip [0..] $ mask (hdr ^. msmHeader_signalMask)+ pure (sat, sig)++toGpsSat :: Word8 -> Maybe Word8+toGpsSat sat+ | sat >= 64 = Nothing+ | otherwise = Just sat++toGpsCode :: Word8 -> Maybe Word8+toGpsCode sig+ | sig == 2 = Just 0+ | sig == 3 = Just 5+ | sig == 9 = Just 6+ | sig == 15 = Just 1+ | sig == 16 = Just 7+ | sig == 17 = Just 8+ | sig == 22 = Just 9+ | sig == 23 = Just 10+ | sig == 24 = Just 11+ | otherwise = Nothing++toGpsSignal :: Word8 -> Word8 -> Maybe GnssSignal+toGpsSignal sat sig = do+ sat' <- toGpsSat sat+ code <- toGpsCode sig+ Just $ GnssSignal sat' code++-- | Convert to SBP pseudorange.+--+toP :: Double -> Word32+toP p = round $ 50 * p++-- | Convert to SBP carrier-phase measurement.+--+toL :: Double -> CarrierPhase+toL l = if f /= 256 then CarrierPhase i (fromIntegral f) else CarrierPhase (i + 1) 0+ where+ i = floor l+ f = (round $ (l - fromIntegral i) * 256) :: Word16++-- | GPS pseudorange unit.+--+gpsPseudorange :: Double+gpsPseudorange = 299792.458++toD :: Doppler+toD = Doppler 0 0++toGpsFrequency :: Word8 -> Maybe Double+toGpsFrequency sig+ | sig == 2 = Just 1.57542e9+ | sig == 3 = Just 1.57542e9+ | sig == 9 = Just 1.22760e9+ | sig == 15 = Just 1.22760e9+ | sig == 16 = Just 1.22760e9+ | sig == 17 = Just 1.22760e9+ | sig == 22 = Just (115 * 10.23e6)+ | sig == 23 = Just (115 * 10.23e6)+ | sig == 24 = Just (115 * 10.23e6)+ | otherwise = Nothing++-- | Convert from RTCMv3 lock indicator.+--+lock :: Word16 -> Word32+lock t+ | t < 64 = 1000 * fromIntegral t+ | t < 96 = 1000 * fromIntegral t * 2 - 64+ | t < 128 = 1000 * fromIntegral t * 4 - 256+ | t < 160 = 1000 * fromIntegral t * 8 - 768+ | t < 192 = 1000 * fromIntegral t * 16 - 2048+ | t < 224 = 1000 * fromIntegral t * 32 - 5120+ | t < 256 = 1000 * fromIntegral t * 64 - 12288+ | t < 288 = 1000 * fromIntegral t * 128 - 28672+ | t < 320 = 1000 * fromIntegral t * 256 - 65536+ | t < 352 = 1000 * fromIntegral t * 512 - 147456+ | t < 384 = 1000 * fromIntegral t * 1024 - 327680+ | t < 416 = 1000 * fromIntegral t * 2048 - 720896+ | t < 448 = 1000 * fromIntegral t * 4096 - 1572864+ | t < 480 = 1000 * fromIntegral t * 8192 - 3407872+ | t < 512 = 1000 * fromIntegral t * 16384 - 7340032+ | t < 544 = 1000 * fromIntegral t * 32768 - 15728640+ | t < 576 = 1000 * fromIntegral t * 65536 - 33554432+ | t < 608 = 1000 * fromIntegral t * 131072 - 71303168+ | t < 640 = 1000 * fromIntegral t * 262144 - 150994944+ | t < 672 = 1000 * fromIntegral t * 524288 - 318767104+ | t < 704 = 1000 * fromIntegral t * 1048576 - 671088640+ | otherwise = 1000 * 67108864++-- | Convert to SBP lock time.+--+toLock :: Word32 -> Word8+toLock t+ | t < 32 = 0+ | t < 64 = 1+ | t < 128 = 2+ | t < 256 = 3+ | t < 512 = 4+ | t < 1024 = 5+ | t < 2048 = 6+ | t < 4096 = 7+ | t < 8192 = 8+ | t < 16384 = 9+ | t < 32768 = 10+ | t < 65536 = 11+ | t < 131072 = 12+ | t < 262144 = 13+ | t < 524288 = 14+ | otherwise = 15++toPackedObsContent1074 :: Msm46SatelliteData -> Msm4SignalData -> (Int, Word8) -> (Int, Word8) -> Maybe PackedObsContent+toPackedObsContent1074 satData sigData (satIndex, sat) (sigIndex, sig) = do+ sid <- toGpsSignal sat sig+ freq <- toGpsFrequency sig+ Just PackedObsContent+ { _packedObsContent_P = toP (roughPseudorange + finePseudorange)+ , _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0))+ , _packedObsContent_D = toD+ , _packedObsContent_cn0 = (sigData ^. msm4SignalData_cnrs) !! sigIndex+ , _packedObsContent_lock = (sigData ^. msm4SignalData_lockTimes) !! sigIndex+ , _packedObsContent_sid = sid+ , _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved+ }+ where+ pseudorangeValid = 1+ phaseValid = 2+ halfCycleResolved = bool 0 4 $ (sigData ^. msm4SignalData_halfCycles) !! sigIndex+ roughPseudorange = fromIntegral ((satData ^. msm46SatelliteData_ranges) !! satIndex) * gpsPseudorange ++ fromIntegral ((satData ^. msm46SatelliteData_rangesModulo) !! satIndex) / (1024 * gpsPseudorange)+ finePseudorange = fromIntegral ((sigData ^. msm4SignalData_pseudoranges) !! sigIndex) * gpsPseudorange * (2 ** (-24))+ finePhaserange = fromIntegral ((sigData ^. msm4SignalData_phaseranges) !! sigIndex) * gpsPseudorange * (2 ** (-29))++toPackedObsContent1077 :: Msm57SatelliteData -> Msm7SignalData -> (Int, Word8) -> (Int, Word8) -> Maybe PackedObsContent+toPackedObsContent1077 satData sigData (satIndex, sat) (sigIndex, sig) = do+ sid <- toGpsSignal sat sig+ freq <- toGpsFrequency sig+ Just PackedObsContent+ { _packedObsContent_P = toP (roughPseudorange + finePseudorange)+ , _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0))+ , _packedObsContent_D = toD+ , _packedObsContent_cn0 = round (fromIntegral ((sigData ^. msm7SignalData_cnrs) !! sigIndex) * ((2 :: Double) ** (-4)))+ , _packedObsContent_lock = toLock $ lock ((sigData ^. msm7SignalData_lockTimes) !! sigIndex)+ , _packedObsContent_sid = sid+ , _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved+ }+ where+ pseudorangeValid = 1+ phaseValid = 2+ halfCycleResolved = bool 0 4 $ (sigData ^. msm7SignalData_halfCycles) !! sigIndex+ roughPseudorange = fromIntegral ((satData ^. msm57SatelliteData_ranges) !! satIndex) * gpsPseudorange ++ fromIntegral ((satData ^. msm57SatelliteData_rangesModulo) !! satIndex) / (1024 * gpsPseudorange)+ finePseudorange = fromIntegral ((sigData ^. msm7SignalData_pseudoranges) !! sigIndex) * gpsPseudorange * (2 ** (-29))+ finePhaserange = fromIntegral ((sigData ^. msm7SignalData_phaseranges) !! sigIndex) * gpsPseudorange * (2 ** (-31))+++class FromObservations a where+ gpsTime :: MonadStore e m => a -> m (GpsTime, GpsTime)+ packedObsContents :: a -> [PackedObsContent]+ sender :: a -> Word16+ multiple :: a -> Bool++instance FromObservations Msg1074 where+ gpsTime m = toGpsTime (m ^. msg1074_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1074_header . msmHeader_epoch)+ packedObsContents m = catMaybes $ uncurry (toPackedObsContent1074 (m ^. msg1074_satelliteData) (m ^. msg1074_signalData)) <$> toCells (m ^. msg1074_header)+ sender = toSender . view (msg1074_header . msmHeader_station)+ multiple = view (msg1074_header . msmHeader_multiple)++instance FromObservations Msg1075 where+ gpsTime m = toGpsTime (m ^. msg1075_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1075_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1075_header . msmHeader_station)+ multiple = view (msg1075_header . msmHeader_multiple)++instance FromObservations Msg1076 where+ gpsTime m = toGpsTime (m ^. msg1076_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1076_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1076_header . msmHeader_station)+ multiple = view (msg1076_header . msmHeader_multiple)++instance FromObservations Msg1077 where+ gpsTime m = toGpsTime (m ^. msg1077_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1077_header . msmHeader_epoch)+ packedObsContents m = catMaybes $ uncurry (toPackedObsContent1077 (m ^. msg1077_satelliteData) (m ^. msg1077_signalData)) <$> toCells (m ^. msg1077_header)+ sender = toSender . view (msg1077_header . msmHeader_station)+ multiple = view (msg1077_header . msmHeader_multiple)++instance FromObservations Msg1084 where+ gpsTime m = toGpsTime (m ^. msg1084_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1084_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1084_header . msmHeader_station)+ multiple = view (msg1084_header . msmHeader_multiple)++instance FromObservations Msg1085 where+ gpsTime m = toGpsTime (m ^. msg1085_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1085_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1085_header . msmHeader_station)+ multiple = view (msg1085_header . msmHeader_multiple)++instance FromObservations Msg1086 where+ gpsTime m = toGpsTime (m ^. msg1086_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1086_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1086_header . msmHeader_station)+ multiple = view (msg1086_header . msmHeader_multiple)++instance FromObservations Msg1087 where+ gpsTime m = toGpsTime (m ^. msg1087_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1087_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1087_header . msmHeader_station)+ multiple = view (msg1087_header . msmHeader_multiple)++instance FromObservations Msg1094 where+ gpsTime m = toGpsTime (m ^. msg1094_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1094_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1094_header . msmHeader_station)+ multiple = view (msg1094_header . msmHeader_multiple)++instance FromObservations Msg1095 where+ gpsTime m = toGpsTime (m ^. msg1095_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1095_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1095_header . msmHeader_station)+ multiple = view (msg1095_header . msmHeader_multiple)++instance FromObservations Msg1096 where+ gpsTime m = toGpsTime (m ^. msg1096_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1096_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1096_header . msmHeader_station)+ multiple = view (msg1096_header . msmHeader_multiple)++instance FromObservations Msg1097 where+ gpsTime m = toGpsTime (m ^. msg1097_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1097_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1097_header . msmHeader_station)+ multiple = view (msg1097_header . msmHeader_multiple)++instance FromObservations Msg1104 where+ gpsTime m = toGpsTime (m ^. msg1104_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1104_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1104_header . msmHeader_station)+ multiple = view (msg1104_header . msmHeader_multiple)++instance FromObservations Msg1105 where+ gpsTime m = toGpsTime (m ^. msg1105_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1105_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1105_header . msmHeader_station)+ multiple = view (msg1105_header . msmHeader_multiple)++instance FromObservations Msg1106 where+ gpsTime m = toGpsTime (m ^. msg1106_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1106_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1106_header . msmHeader_station)+ multiple = view (msg1106_header . msmHeader_multiple)++instance FromObservations Msg1107 where+ gpsTime m = toGpsTime (m ^. msg1107_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1107_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1107_header . msmHeader_station)+ multiple = view (msg1107_header . msmHeader_multiple)++instance FromObservations Msg1114 where+ gpsTime m = toGpsTime (m ^. msg1114_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1114_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1114_header . msmHeader_station)+ multiple = view (msg1114_header . msmHeader_multiple)++instance FromObservations Msg1115 where+ gpsTime m = toGpsTime (m ^. msg1115_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1115_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1115_header . msmHeader_station)+ multiple = view (msg1115_header . msmHeader_multiple)++instance FromObservations Msg1116 where+ gpsTime m = toGpsTime (m ^. msg1116_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1116_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1116_header . msmHeader_station)+ multiple = view (msg1116_header . msmHeader_multiple)++instance FromObservations Msg1117 where+ gpsTime m = toGpsTime (m ^. msg1117_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1117_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1117_header . msmHeader_station)+ multiple = view (msg1117_header . msmHeader_multiple)++instance FromObservations Msg1124 where+ gpsTime m = toGpsTime (m ^. msg1124_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1124_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1124_header . msmHeader_station)+ multiple = view (msg1124_header . msmHeader_multiple)++instance FromObservations Msg1125 where+ gpsTime m = toGpsTime (m ^. msg1125_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1125_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1125_header . msmHeader_station)+ multiple = view (msg1125_header . msmHeader_multiple)++instance FromObservations Msg1126 where+ gpsTime m = toGpsTime (m ^. msg1126_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1126_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1126_header . msmHeader_station)+ multiple = view (msg1126_header . msmHeader_multiple)++instance FromObservations Msg1127 where+ gpsTime m = toGpsTime (m ^. msg1127_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1127_header . msmHeader_epoch)+ packedObsContents = const mempty+ sender = toSender . view (msg1127_header . msmHeader_station)+ multiple = view (msg1127_header . msmHeader_multiple)++-- | Convert RTCMv3 observation(s) to SBP observations in chunks.+--+toMsgObs :: Applicative f => GpsTime -> [PackedObsContent] -> Word16 -> f [SBPMsg]+toMsgObs t obs s = do+ let chunks = chunksOf maxObs obs+ ifor chunks $ \i obs' -> do+ let n = length chunks `shiftL` 4 .|. i+ m = MsgObs (ObservationHeader t (fromIntegral n)) obs'+ pure $ SBPMsgObs m $ toSBP m s+ where+ maxObs = (maxSize - hdrSize) `div` obsSize+ maxSize = 255+ hdrSize = 11+ obsSize = 17++-- | Convert RTCMv3 observation message to SBP observations message(s).+--+converter :: (MonadStore e m, FromObservations a) => a -> Conduit i m [SBPMsg]+converter m = do+ (t, t') <- gpsTime m+ observations <- view storeObservations+ obs <- liftIO $ readIORef observations+ when (t' /= t) $+ unless (null obs) $ do+ liftIO $ writeIORef observations mempty+ ms <- toMsgObs t (toList obs) $ sender m+ yield ms+ let obs' = fromList $ packedObsContents m+ if multiple m then liftIO $ modifyIORef' observations (obs' <>) else do+ obs'' <- liftIO $ readIORef observations+ unless (null obs'') $+ liftIO $ writeIORef observations mempty+ let obs''' = obs' <> obs''+ unless (null obs''') $ do+ ms <- toMsgObs t' (toList obs''') $ sender m+ yield ms
src/Data/RTCM3/SBP/Observations.hs view
@@ -28,26 +28,6 @@ import Data.Word import SwiftNav.SBP --- | Monadic IORef modify.----modifyIORefM :: MonadIO m => IORef a -> (a -> m (a, b)) -> m b-modifyIORefM ref f = do- x <- liftIO $ readIORef ref- (y, z) <- f x- liftIO $ writeIORef ref y- pure z---- | Update and convert stored and incoming GPS times.----toGpsTime :: MonadStore e m => Word16 -> (GpsTime -> GpsTime) -> m (GpsTime, GpsTime)-toGpsTime station rollover = do- timeMap <- view storeGpsTimeMap- modifyIORefM timeMap $ \timeMap' -> do- time <- view storeCurrentGpsTime- t <- maybe (liftIO time) pure (timeMap' ^. at station)- let t' = rollover t- pure (timeMap' & at station ?~ t', (t, t'))- -- | Default observation doppler. -- obsDoppler :: Doppler@@ -165,17 +145,17 @@ -- | Map GLONASS L1 signal. ---glonassL1Signal :: Word8 -> Bool -> Maybe GnssSignal+glonassL1Signal :: Word8 -> Bool -> GnssSignal glonassL1Signal sat code- | code = Just $ GnssSignal sat 3- | otherwise = Just $ GnssSignal sat 3+ | code = GnssSignal sat 3+ | otherwise = GnssSignal sat 3 -- | Map GLONASS L2 signal. ---glonassL2Signal :: Word8 -> Word8 -> Maybe GnssSignal+glonassL2Signal :: Word8 -> Word8 -> GnssSignal glonassL2Signal sat code- | code == 0 = Just $ GnssSignal sat 4- | otherwise = Just $ GnssSignal sat 4+ | code == 0 = GnssSignal sat 4+ | otherwise = GnssSignal sat 4 -- | Max GPS satellite number. --@@ -231,15 +211,14 @@ toGlonassL1PackedObsContents :: Word8 -> GlonassL1Observation -> GlonassL1ExtObservation -> Maybe PackedObsContent toGlonassL1PackedObsContents sat l1o l1eo | no = Nothing- | otherwise = do- sid <- glonassL1Signal sat (l1o ^. glonassL1Observation_code)+ | otherwise = Just PackedObsContent { _packedObsContent_P = toP p1 , _packedObsContent_L = toL l1 , _packedObsContent_D = obsDoppler , _packedObsContent_cn0 = l1eo ^. glonassL1ExtObservation_cnr , _packedObsContent_lock = toLock $ lock (l1o ^. glonassL1Observation_lockTime)- , _packedObsContent_sid = sid+ , _packedObsContent_sid = glonassL1Signal sat (l1o ^. glonassL1Observation_code) , _packedObsContent_flags = obsFlags } where@@ -252,15 +231,14 @@ toGlonassL2PackedObsContents :: Word8 -> GlonassL1Observation -> GlonassL1ExtObservation -> GlonassL2Observation -> GlonassL2ExtObservation -> Maybe PackedObsContent toGlonassL2PackedObsContents sat l1o l1eo l2o l2eo | no = Nothing- | otherwise = do- sid <- glonassL2Signal sat (l2o ^. glonassL2Observation_code)+ | otherwise = Just PackedObsContent { _packedObsContent_P = toP p2 , _packedObsContent_L = toL l2 , _packedObsContent_D = obsDoppler , _packedObsContent_cn0 = l2eo ^. glonassL2ExtObservation_cnr , _packedObsContent_lock = toLock $ lock (l2o ^. glonassL2Observation_lockTime)- , _packedObsContent_sid = sid+ , _packedObsContent_sid = glonassL2Signal sat (l2o ^. glonassL2Observation_code) , _packedObsContent_flags = obsFlags } where@@ -310,25 +288,25 @@ synchronous :: a -> Bool instance FromObservations Msg1002 where- gpsTime m = toGpsTime (m ^. msg1002_header . gpsObservationHeader_station) $ rolloverTowGpsTime (m ^. msg1002_header . gpsObservationHeader_tow)+ gpsTime m = toGpsTime (m ^. msg1002_header . gpsObservationHeader_station) $ gpsRolloverGpsTime (m ^. msg1002_header . gpsObservationHeader_tow) packedObsContents = toPackedObsContent . view msg1002_observations sender = toSender . view (msg1002_header . gpsObservationHeader_station) synchronous = view (msg1002_header . gpsObservationHeader_synchronous) instance FromObservations Msg1004 where- gpsTime m = toGpsTime (m ^. msg1004_header . gpsObservationHeader_station) $ rolloverTowGpsTime (m ^. msg1004_header . gpsObservationHeader_tow)+ gpsTime m = toGpsTime (m ^. msg1004_header . gpsObservationHeader_station) $ gpsRolloverGpsTime (m ^. msg1004_header . gpsObservationHeader_tow) packedObsContents = toPackedObsContent . view msg1004_observations sender = toSender . view (msg1004_header . gpsObservationHeader_station) synchronous = view (msg1004_header . gpsObservationHeader_synchronous) instance FromObservations Msg1010 where- gpsTime m = toGpsTime (m ^. msg1010_header . glonassObservationHeader_station) $ rolloverEpochGpsTime (m ^. msg1010_header . glonassObservationHeader_epoch)+ gpsTime m = toGpsTime (m ^. msg1010_header . glonassObservationHeader_station) $ glonassRolloverGpsTime (m ^. msg1010_header . glonassObservationHeader_epoch) packedObsContents = toPackedObsContent . view msg1010_observations sender = toSender . view (msg1010_header . glonassObservationHeader_station) synchronous = view (msg1010_header . glonassObservationHeader_synchronous) instance FromObservations Msg1012 where- gpsTime m = toGpsTime (m ^. msg1012_header . glonassObservationHeader_station) $ rolloverEpochGpsTime (m ^. msg1012_header . glonassObservationHeader_epoch)+ gpsTime m = toGpsTime (m ^. msg1012_header . glonassObservationHeader_station) $ glonassRolloverGpsTime (m ^. msg1012_header . glonassObservationHeader_epoch) packedObsContents = toPackedObsContent . view msg1012_observations sender = toSender . view (msg1012_header . glonassObservationHeader_station) synchronous = view (msg1012_header . glonassObservationHeader_synchronous)
src/Data/RTCM3/SBP/Time.hs view
@@ -21,12 +21,19 @@ , toStartDate , toTow , currentGpsTime- , rolloverTowGpsTime- , rolloverEpochGpsTime+ , gpsRolloverGpsTime+ , glonassRolloverGpsTime+ , glonassRolloverGpsTime'+ , beidouRolloverGpsTime+ , modifyIORefM+ , toGpsTime ) where import BasicPrelude import Control.Lens+import Data.Bits+import Data.IORef+import Data.RTCM3.SBP.Types import Data.Time import Data.Time.Calendar.WeekDate import Data.Word@@ -47,6 +54,16 @@ gpsLeapMillis :: Integer gpsLeapMillis = 1000 * gpsLeapSeconds +-- | Number of BeiDou offset seconds.+--+beidouOffsetSeconds :: Integer+beidouOffsetSeconds = 14++-- | Number of BeiDou offet milliseconds.+--+beidouOffsetMillis :: Integer+beidouOffsetMillis = 1000 * beidouOffsetSeconds+ -- | Minute seconds -- minuteSeconds :: Integer@@ -124,8 +141,8 @@ -- | Update GPS time based on GPS time of week, handling week rollover. ---rolloverTowGpsTime :: Word32 -> GpsTime -> GpsTime-rolloverTowGpsTime tow t = t & gpsTime_tow .~ tow & rollover+gpsRolloverGpsTime :: Word32 -> GpsTime -> GpsTime+gpsRolloverGpsTime tow t = t & gpsTime_tow .~ tow & rollover where rollover | increment = gpsTime_wn +~ 1@@ -138,8 +155,8 @@ -- | Update GPS time based on GLONASS epoch, handling week rollover. ---rolloverEpochGpsTime :: Word32 -> GpsTime -> GpsTime-rolloverEpochGpsTime epoch t = rolloverTowGpsTime tow t+glonassRolloverGpsTime :: Word32 -> GpsTime -> GpsTime+glonassRolloverGpsTime epoch t = gpsRolloverGpsTime tow t where epoch' = fromIntegral epoch - 3 * hourMillis + gpsLeapMillis epoch''@@ -153,4 +170,42 @@ | otherwise = dow increment = epoch'' > tod && epoch'' - tod > dayMillis `div` 2 decrement = tod > epoch'' && tod - epoch'' > dayMillis `div` 2- tow = fromIntegral $ dow' * dayMillis + epoch''+ tow = fromIntegral $ dow' * dayMillis + epoch''++-- | Update GPS time based on MSM GLONASS epoch, handling week rollover.+--+glonassRolloverGpsTime' :: Word32 -> GpsTime -> GpsTime+glonassRolloverGpsTime' epoch = glonassRolloverGpsTime epoch'+ where+ epoch' = epoch .&. 134217727++-- | Update GPS time based on MSM BeiDou epoch, handling week rollover.+--+beidouRolloverGpsTime :: Word32 -> GpsTime -> GpsTime+beidouRolloverGpsTime epoch = gpsRolloverGpsTime tow+ where+ epoch' = fromIntegral epoch + beidouOffsetMillis+ epoch''+ | epoch' >= dayMillis = epoch' - dayMillis+ | otherwise = epoch'+ tow = fromIntegral epoch''++-- | Monadic IORef modify.+--+modifyIORefM :: MonadIO m => IORef a -> (a -> m (a, b)) -> m b+modifyIORefM ref f = do+ x <- liftIO $ readIORef ref+ (y, z) <- f x+ liftIO $ writeIORef ref y+ pure z++-- | Update and convert stored and incoming GPS times.+--+toGpsTime :: MonadStore e m => Word16 -> (GpsTime -> GpsTime) -> m (GpsTime, GpsTime)+toGpsTime station rollover = do+ timeMap <- view storeGpsTimeMap+ modifyIORefM timeMap $ \timeMap' -> do+ time <- view storeCurrentGpsTime+ t <- maybe (liftIO time) pure (timeMap' ^. at station)+ let t' = rollover t+ pure (timeMap' & at station ?~ t', (t, t'))
test/Test/Data/RTCM3/SBP/Time.hs view
@@ -17,19 +17,19 @@ testRolloverGpsTime = testGroup "Rollover GPS time tests" [ testCase "No rollover" $ do- rolloverTowGpsTime 0 (GpsTime 0 0 1960) @?= GpsTime 0 0 1960- rolloverTowGpsTime 302400000 (GpsTime 302400000 0 1960) @?= GpsTime 302400000 0 1960- rolloverTowGpsTime 604800000 (GpsTime 604800000 0 1960) @?= GpsTime 604800000 0 1960- rolloverTowGpsTime 0 (GpsTime 302400000 0 1960) @?= GpsTime 0 0 1960- rolloverTowGpsTime 302400000 (GpsTime 604800000 0 1960) @?= GpsTime 302400000 0 1960+ gpsRolloverGpsTime 0 (GpsTime 0 0 1960) @?= GpsTime 0 0 1960+ gpsRolloverGpsTime 302400000 (GpsTime 302400000 0 1960) @?= GpsTime 302400000 0 1960+ gpsRolloverGpsTime 604800000 (GpsTime 604800000 0 1960) @?= GpsTime 604800000 0 1960+ gpsRolloverGpsTime 0 (GpsTime 302400000 0 1960) @?= GpsTime 0 0 1960+ gpsRolloverGpsTime 302400000 (GpsTime 604800000 0 1960) @?= GpsTime 302400000 0 1960 , testCase "Positive rollover" $ do- rolloverTowGpsTime 0 (GpsTime 302400001 0 1960) @?= GpsTime 0 0 1961- rolloverTowGpsTime 0 (GpsTime 604800000 0 1960) @?= GpsTime 0 0 1961- rolloverTowGpsTime 302399999 (GpsTime 604800000 0 1960) @?= GpsTime 302399999 0 1961+ gpsRolloverGpsTime 0 (GpsTime 302400001 0 1960) @?= GpsTime 0 0 1961+ gpsRolloverGpsTime 0 (GpsTime 604800000 0 1960) @?= GpsTime 0 0 1961+ gpsRolloverGpsTime 302399999 (GpsTime 604800000 0 1960) @?= GpsTime 302399999 0 1961 , testCase "Negative rollover" $ do- rolloverTowGpsTime 604800000 (GpsTime 0 0 1961) @?= GpsTime 604800000 0 1960- rolloverTowGpsTime 604800000 (GpsTime 302399999 0 1961) @?= GpsTime 604800000 0 1960- rolloverTowGpsTime 302400001 (GpsTime 0 0 1961) @?= GpsTime 302400001 0 1960+ gpsRolloverGpsTime 604800000 (GpsTime 0 0 1961) @?= GpsTime 604800000 0 1960+ gpsRolloverGpsTime 604800000 (GpsTime 302399999 0 1961) @?= GpsTime 604800000 0 1960+ gpsRolloverGpsTime 302400001 (GpsTime 0 0 1961) @?= GpsTime 302400001 0 1960 ] testStartDate :: TestTree