diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -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 @@ -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, 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 @@ -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 diff --git a/src/Data/RTCM3/SBP/MSM.hs b/src/Data/RTCM3/SBP/MSM.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/SBP/MSM.hs @@ -0,0 +1,384 @@ +{-# LANGUAGE NoImplicitPrelude #-} + +-- | +-- Module: Data.RTCM3.SBP.MSM +-- Copyright: Copyright (C) 2017 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Swift Navigation +-- 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 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 @@ -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) 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 @@ -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')) 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 @@ -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