gnss-converters 0.3.6 → 0.3.7

raw patch · 5 files changed

+136/−96 lines, 5 filesdep +vectorPVP: major bump suggested

API removals or changes: PVP suggests a major version bump

Dependencies added: vector

API changes (from Hackage documentation)

Files

• gnss-converters.cabal +4/−2
• main/RTCM32SBP.hs +12/−13
• src/Data/RTCM3/SBP.hs +63/−56
• src/Data/RTCM3/SBP/Observations.hs +45/−20
• src/Data/RTCM3/SBP/Types.hs +12/−5

@@ -1,5 +1,5 @@ name: gnss-converters-version: 0.3.6+version: 0.3.7 cabal-version: >=1.22 build-type: Simple license: BSD3@@ -26,6 +26,7 @@     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,@@ -35,7 +36,8 @@         rtcm >=0.2.2,         sbp >=2.2.11,         time >=1.5.0.1,-        transformers-base >=0.4.4+        transformers-base >=0.4.4,+        vector >=0.11.0.0     default-language: Haskell2010     hs-source-dirs: src     ghc-options: -Wall

@@ -10,22 +10,21 @@ -- -- RTCM3 to SBP tool. -import           BasicPrelude-import           Control.Monad.Trans.Resource-import           Data.Conduit-import           Data.Conduit.Binary-import qualified Data.Conduit.List                 as CL-import           Data.Conduit.Serialization.Binary-import           Data.RTCM3.SBP-import           Data.RTCM3.SBP.Types-import           System.IO+import BasicPrelude+import Control.Monad.Trans.Resource+import Data.Conduit+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   runResourceT $ runConvertT s $-    sourceHandle stdin    =$=-    conduitDecode         =$=-    CL.concatMapM convert =$=-    conduitEncode         $$+    sourceHandle stdin =$=+    conduitDecode      =$=+    converter          =$=+    conduitEncode      $$     sinkHandle stdout

@@ -14,22 +14,23 @@ module Data.RTCM3.SBP   ( toWn   , mjdEpoch-  , convert+  , converter   , newStore   , validateIodcIode   , gpsUriToUra   ) where -import BasicPrelude-import Control.Lens-import Data.Bits-import Data.IORef-import Data.RTCM3-import Data.RTCM3.SBP.Observations-import Data.RTCM3.SBP.Types-import Data.Time-import Data.Word-import SwiftNav.SBP+import           BasicPrelude+import           Control.Lens+import           Data.Bits+import           Data.Conduit+import           Data.IORef+import           Data.RTCM3+import qualified Data.RTCM3.SBP.Observations as Observations+import           Data.RTCM3.SBP.Time+import           Data.RTCM3.SBP.Types+import           Data.Word+import           SwiftNav.SBP  {-# ANN module ("HLint: ignore Redundant if"::String) #-} @@ -44,8 +45,8 @@ -------------------------------------------------------------------------------- -- General utilities -applyScaleFactor :: (Floating a, Integral b) => a -> a -> b -> a-applyScaleFactor n p x = (n ** p)  * fromIntegral x+applyScaleFactor :: (Floating a, Integral b) => a -> b -> a+applyScaleFactor p x = (2 ** p)  * fromIntegral x  -------------------------------------------------------------------------------- -- GNSS RTCM observation reconstruction utilities@@ -165,54 +166,60 @@   toc           <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toc)   return MsgEphemerisGps     { _msgEphemerisGps_common   = commonContent-    , _msgEphemerisGps_tgd      =         applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_tgd-    , _msgEphemerisGps_c_rs     =         applyScaleFactor 2 (-5)  $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rs-    , _msgEphemerisGps_c_rc     =         applyScaleFactor 2 (-5)  $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rc-    , _msgEphemerisGps_c_uc     =         applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_uc-    , _msgEphemerisGps_c_us     =         applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_us-    , _msgEphemerisGps_c_ic     =         applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_ic-    , _msgEphemerisGps_c_is     =         applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_is-    , _msgEphemerisGps_dn       = gpsPi * applyScaleFactor 2 (-43)  (m ^. msg1019_ephemeris ^. gpsEphemeris_dn)-    , _msgEphemerisGps_m0       = gpsPi * applyScaleFactor 2 (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_m0)-    , _msgEphemerisGps_ecc      =         applyScaleFactor 2 (-33) $ m ^. msg1019_ephemeris ^. gpsEphemeris_ecc-    , _msgEphemerisGps_sqrta    =         applyScaleFactor 2 (-19) $ m ^. msg1019_ephemeris ^. gpsEphemeris_sqrta-    , _msgEphemerisGps_omega0   = gpsPi * applyScaleFactor 2 (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_omega0)-    , _msgEphemerisGps_omegadot = gpsPi * applyScaleFactor 2 (-43)  (m ^. msg1019_ephemeris ^. gpsEphemeris_omegadot)-    , _msgEphemerisGps_w        = gpsPi * applyScaleFactor 2 (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_w)-    , _msgEphemerisGps_inc      = gpsPi * applyScaleFactor 2 (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_i0)-    , _msgEphemerisGps_inc_dot  = gpsPi * applyScaleFactor 2 (-43)  (m ^. msg1019_ephemeris ^. gpsEphemeris_idot)-    , _msgEphemerisGps_af0      =         applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af0-    , _msgEphemerisGps_af1      =         applyScaleFactor 2 (-43) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af1-    , _msgEphemerisGps_af2      =         applyScaleFactor 2 (-55) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af2-    , _msgEphemerisGps_iodc     =                                    m ^. msg1019_ephemeris ^. gpsEphemeris_iodc-    , _msgEphemerisGps_iode     =                                    m ^. msg1019_ephemeris ^. gpsEphemeris_iode+    , _msgEphemerisGps_tgd      =         applyScaleFactor (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_tgd+    , _msgEphemerisGps_c_rs     =         applyScaleFactor (-5)  $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rs+    , _msgEphemerisGps_c_rc     =         applyScaleFactor (-5)  $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rc+    , _msgEphemerisGps_c_uc     =         applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_uc+    , _msgEphemerisGps_c_us     =         applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_us+    , _msgEphemerisGps_c_ic     =         applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_ic+    , _msgEphemerisGps_c_is     =         applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_is+    , _msgEphemerisGps_dn       = gpsPi * applyScaleFactor (-43)  (m ^. msg1019_ephemeris ^. gpsEphemeris_dn)+    , _msgEphemerisGps_m0       = gpsPi * applyScaleFactor (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_m0)+    , _msgEphemerisGps_ecc      =         applyScaleFactor (-33) $ m ^. msg1019_ephemeris ^. gpsEphemeris_ecc+    , _msgEphemerisGps_sqrta    =         applyScaleFactor (-19) $ m ^. msg1019_ephemeris ^. gpsEphemeris_sqrta+    , _msgEphemerisGps_omega0   = gpsPi * applyScaleFactor (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_omega0)+    , _msgEphemerisGps_omegadot = gpsPi * applyScaleFactor (-43)  (m ^. msg1019_ephemeris ^. gpsEphemeris_omegadot)+    , _msgEphemerisGps_w        = gpsPi * applyScaleFactor (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_w)+    , _msgEphemerisGps_inc      = gpsPi * applyScaleFactor (-31)  (m ^. msg1019_ephemeris ^. gpsEphemeris_i0)+    , _msgEphemerisGps_inc_dot  = gpsPi * applyScaleFactor (-43)  (m ^. msg1019_ephemeris ^. gpsEphemeris_idot)+    , _msgEphemerisGps_af0      =         applyScaleFactor (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af0+    , _msgEphemerisGps_af1      =         applyScaleFactor (-43) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af1+    , _msgEphemerisGps_af2      =         applyScaleFactor (-55) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af2+    , _msgEphemerisGps_iodc     =                                  m ^. msg1019_ephemeris ^. gpsEphemeris_iodc+    , _msgEphemerisGps_iode     =                                  m ^. msg1019_ephemeris ^. gpsEphemeris_iode     , _msgEphemerisGps_toc      = toc     } +msg1005Converter :: MonadStore e m => Msg1005 -> Conduit i m [SBPMsg]+msg1005Converter m = do+  m' <- fromMsg1005 m+  yield [SBPMsgBasePosEcef m' $ toSBP m' $ toSender $ m ^. msg1005_reference ^. antennaReference_station]++msg1006Converter :: MonadStore e m => Msg1006 -> Conduit i m [SBPMsg]+msg1006Converter m = do+  m' <- fromMsg1006 m+  yield [SBPMsgBasePosEcef m' $ toSBP m' $ toSender $ m ^. msg1006_reference ^. antennaReference_station]++msg1019Converter :: MonadStore e m => Msg1019 -> Conduit i m [SBPMsg]+msg1019Converter m = do+  m' <- fromMsg1019 m+  yield [SBPMsgEphemerisGps m' $ toSBP m' $ toSender 0]+ -- | Convert an RTCM message into possibly multiple SBP messages. ---convert :: MonadStore e m => RTCM3Msg -> m [SBPMsg]-convert = \case-  (RTCM3Msg1002 m _rtcm3) -> toSBPMsgObs m-  (RTCM3Msg1004 m _rtcm3) -> toSBPMsgObs m-  (RTCM3Msg1005 m _rtcm3) -> do-    let sender =  m ^. msg1005_reference ^. antennaReference_station-    m' <- fromMsg1005 m-    return [SBPMsgBasePosEcef m' $ toSBP m' $ toSender sender]-  (RTCM3Msg1006 m _rtcm3) -> do-    let sender = m ^. msg1006_reference ^. antennaReference_station-    m' <- fromMsg1006 m-    return [SBPMsgBasePosEcef m' $ toSBP m' $ toSender sender]-  (RTCM3Msg1010 m _rtcm3) -> toSBPMsgObs m-  (RTCM3Msg1012 m _rtcm3) -> toSBPMsgObs m-  (RTCM3Msg1019 m _rtcm3) -> do-    let sender = 0-    m' <- fromMsg1019 m-    return [SBPMsgEphemerisGps m' $ toSBP m' $ toSender sender]-  _rtcm3Msg -> return mempty+converter :: MonadStore e m => Conduit RTCM3Msg m [SBPMsg]+converter =+  awaitForever $ \case+    (RTCM3Msg1002 m _rtcm3) -> Observations.converter m+    (RTCM3Msg1004 m _rtcm3) -> Observations.converter m+    (RTCM3Msg1010 m _rtcm3) -> Observations.converter m+    (RTCM3Msg1012 m _rtcm3) -> Observations.converter m+    (RTCM3Msg1005 m _rtcm3) -> msg1005Converter m+    (RTCM3Msg1006 m _rtcm3) -> msg1006Converter m+    (RTCM3Msg1019 m _rtcm3) -> msg1019Converter m+    _rtcm3Msg -> mempty  newStore :: IO Store newStore = do-  day <- utctDay <$> getCurrentTime-  let wn = fromIntegral $ div (diffDays day (fromGregorian 1980 1 6)) 7-  Store <$> newIORef wn <*> newIORef mempty+  t <- currentGpsTime+  Store <$> newIORef (t ^. gpsTimeNano_wn) <*> newIORef t <*> newIORef mempty <*> newIORef mempty

@@ -11,18 +11,20 @@ -- RTCMv3 to SBP Observation Conversions.  module Data.RTCM3.SBP.Observations-  ( toSBPMsgObs+  ( converter   ) where -import BasicPrelude+import BasicPrelude         hiding (null) import Control.Lens import Data.Bits+import Data.Conduit import Data.Int import Data.IORef-import Data.List.Extra+import Data.List.Extra      hiding (null) import Data.RTCM3 import Data.RTCM3.SBP.Time import Data.RTCM3.SBP.Types+import Data.Vector          hiding (concatMap, length) import Data.Word import SwiftNav.SBP @@ -39,10 +41,10 @@ -- toGpsTimeNano :: MonadStore e m => Word16 -> (GpsTimeNano -> GpsTimeNano) -> m GpsTimeNano toGpsTimeNano station rollover = do-  gpsTimeMap <- view storeGpsTimeMap-  modifyIORefM gpsTimeMap $ \gpsTimeMap' -> do-    t <- rollover <$> maybe currentGpsTime return (gpsTimeMap' ^. at station)-    return (gpsTimeMap' & at station ?~ t, t)+  timeMap <- view storeGpsTimeMap+  modifyIORefM timeMap $ \timeMap' -> do+    t <- rollover <$> maybe currentGpsTime return (timeMap' ^. at station)+    return (timeMap' & at station ?~ t, t)  -- | Default observation doppler. --@@ -303,36 +305,41 @@   gpsTimeNano       :: MonadStore e m => a -> m GpsTimeNano   packedObsContents :: a -> [PackedObsContent]   sender            :: a -> Word16+  synchronous       :: a -> Bool  instance FromObservations Msg1002 where   gpsTimeNano m     = toGpsTimeNano (m ^. msg1002_header . gpsObservationHeader_station) $ rolloverTowGpsTime (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   gpsTimeNano m     = toGpsTimeNano (m ^. msg1004_header . gpsObservationHeader_station) $ rolloverTowGpsTime (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   gpsTimeNano m     = toGpsTimeNano (m ^. msg1010_header . glonassObservationHeader_station) $ rolloverEpochGpsTime (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   gpsTimeNano m     = toGpsTimeNano (m ^. msg1012_header . glonassObservationHeader_station) $ rolloverEpochGpsTime (m ^. msg1012_header . glonassObservationHeader_epoch)   packedObsContents = toPackedObsContent . view msg1012_observations   sender            = toSender . view (msg1012_header . glonassObservationHeader_station)+  synchronous       = view (msg1012_header . glonassObservationHeader_synchronous) --- | Convert RTCMv3 observations to SBP observations in chunks.+-- | Convert RTCMv3 observation(s) to SBP observations in chunks. ---toMsgObs :: (MonadStore e m, FromObservations a) => a -> m [MsgObs]-toMsgObs m = do-  let obs = chunksOf maxObs $ packedObsContents m-  iforM obs $ \i obs' -> do-    t <- gpsTimeNano m-    let n = length obs `shiftL` 4 .|. i-    return $ MsgObs (ObservationHeader t (fromIntegral n)) obs'+toMsgObs :: Applicative f => GpsTimeNano -> [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@@ -341,8 +348,26 @@  -- | Convert RTCMv3 observation message to SBP observations message(s). ---toSBPMsgObs :: (MonadStore e m, FromObservations a) => a -> m [SBPMsg]-toSBPMsgObs m = do-  ms <- toMsgObs m-  return $ for ms $ \m' ->-    SBPMsgObs m' $ toSBP m' $ sender m+converter :: (MonadStore e m, FromObservations a) => a -> Conduit i m [SBPMsg]+converter m = do+  time <- view storeGpsTime+  t    <- liftIO $ readIORef time+  t'   <- gpsTimeNano m+  when (t' >= t) $ do+    observations <- view storeObservations+    obs          <- liftIO $ readIORef observations+    when (t' > t) $ do+      liftIO $ writeIORef time t'+      unless (null obs) $ do+        liftIO $ writeIORef observations mempty+        ms <- toMsgObs t (toList obs) $ sender m+        yield ms+    let obs' = fromList $ packedObsContents m+    if synchronous 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

@@ -31,10 +31,15 @@ import Data.Word import SwiftNav.SBP -type GpsTimeMap = HashMap Word16 GpsTimeNano+type GpsTimeNanoMap = HashMap Word16 GpsTimeNano -instance Hashable GnssSignal where-  hashWithSalt s g = hashWithSalt s (g ^. gnssSignal_sat, g ^. gnssSignal_code, g ^. gnssSignal_reserved)+instance Ord GpsTimeNano where+  compare (GpsTimeNano tow _ns wn) (GpsTimeNano tow' _ns' wn')+    | wn > wn'   = GT+    | wn < wn'   = LT+    | tow > tow' = GT+    | tow < tow' = LT+    | otherwise  = EQ  newtype ConvertT e m a = ConvertT { unConvertT :: ReaderT e m a }   deriving@@ -66,8 +71,10 @@     liftBase = liftBaseDefault  data Store = Store-  { _storeWn         :: IORef Word16-  , _storeGpsTimeMap :: IORef GpsTimeMap+  { _storeWn           :: IORef Word16+  , _storeGpsTime      :: IORef GpsTimeNano+  , _storeGpsTimeMap   :: IORef GpsTimeNanoMap+  , _storeObservations :: IORef (Vector PackedObsContent)   } deriving ( Eq )  $(makeClassy ''Store)