packages feed

gnss-converters 0.1.12 → 0.1.13

raw patch · 5 files changed

+222/−60 lines, 5 filesdep +random

Dependencies added: random

Files

gnss-converters.cabal view
@@ -1,5 +1,5 @@ name:                  gnss-converters-version:               0.1.12+version:               0.1.13 synopsis:              GNSS Converters. description:           Haskell bindings for GNSS converters. homepage:              http://github.com/swift-nav/gnss-converters@@ -29,6 +29,7 @@                      , lens                      , monad-control                      , mtl+                     , random                      , resourcet                      , rtcm                      , sbp@@ -80,6 +81,7 @@                      , gnss-converters                      , lens                      , resourcet+                     , rtcm                      , sbp                      , tasty                      , tasty-hunit
main/RTCM32SBP.hs view
@@ -20,7 +20,7 @@  main :: IO () main = do-  s <- liftIO $ newStore+  s <- newStore   runResourceT $ runConvertT s $     sourceHandle stdin    =$=     conduitDecode         =$=
src/Data/RTCM3/SBP.hs view
@@ -14,6 +14,7 @@  import           BasicPrelude import           Control.Lens+import           Control.Monad.Extra import           Data.Bits import qualified Data.HashMap.Strict  as M import           Data.IORef@@ -23,6 +24,7 @@ import           Data.Time import           Data.Word import           SwiftNav.SBP+import           System.Random   --------------------------------------------------------------------------------@@ -123,20 +125,65 @@   --------------------------------------------------------------------------------+-- General utilities+++modifyMap :: (Eq k, Hashable k) => IORef (HashMap k v) -> v -> k -> (v -> v) -> IO v+modifyMap r d k a = do+  m <- readIORef r+  let v = a $ M.lookupDefault d k m+      n = M.insert k v m+  writeIORef r n+  n `seq` return v++maybe' :: Maybe a -> b -> (a -> b) -> b+maybe' m b a = maybe b a m+++-------------------------------------------------------------------------------- -- GNSS RTCM observation reconstruction utilities   fromEcefVal :: Int64 -> Double fromEcefVal x = fromIntegral x / 10000 -toGPSTime :: MonadStore e m => GpsObservationHeader -> m ObsGPSTime-toGPSTime hdr = do+newGPSTime :: MonadStore e m => Word32 -> m ObsGPSTime+newGPSTime tow = do   wn <- view storeWn >>= liftIO . readIORef   return ObsGPSTime-    { _obsGPSTime_tow = hdr ^. gpsObservationHeader_tow+    { _obsGPSTime_tow = tow     , _obsGPSTime_wn  = wn     } +-- | If incoming TOW is less than stored TOW, rollover WN.+--+updateGPSTime :: Word32 -> ObsGPSTime -> ObsGPSTime+updateGPSTime tow gpsTime =+  gpsTime & obsGPSTime_tow .~ tow &+    if tow >= gpsTime ^. obsGPSTime_tow then id else+      obsGPSTime_wn %~ (+ 1)++-- | Produce GPS Time from Observation header, handling WN rollover.+--+toGPSTime :: MonadStore e m => GpsObservationHeader -> m ObsGPSTime+toGPSTime hdr = do+  let tow      = hdr ^. gpsObservationHeader_tow+      station  = hdr ^. gpsObservationHeader_station+  gpsTime      <- newGPSTime tow+  gpsTimeMap   <- view storeGPSTimeMap+  liftIO $ modifyMap gpsTimeMap gpsTime station $ updateGPSTime tow++-- | MJD GPS Epoch - First day in GPS week 0. See DF051 of the RTCM3 spec+--+mjdEpoch :: Word16+mjdEpoch = 44244++-- | Convert from MJD to GPS week number+--+-- See DF051 of the RTCM3 spec+toWn :: Word16 -> Word16+toWn mjd = (mjd - mjdEpoch) `div` 7+ -- | Construct metric pseudorange (meters!) from L1 RTCM observation. -- -- See DF011, pg. 3-16 of the RTCM3 spec@@ -207,12 +254,31 @@ toCn0_L2 :: GpsL2ExtObservation -> Word8 toCn0_L2 = (^. gpsL2ExtObservation_cnr) -toLock_L1 :: GpsL1Observation -> Word16-toLock_L1 _l1 = 0+newLock :: IO Lock+newLock = do+  counter <- randomIO+  return Lock+    { _lockTime    = 0+    , _lockCounter = counter+    } -toLock_L2 :: GpsL2Observation -> Word16-toLock_L2 _l2 = 0+-- | If incoming time is less than stored time, increment lock counter.+--+updateLock :: Word8 -> Lock -> Lock+updateLock time lock =+  lock & lockTime .~ time &+    if time >= lock ^. lockTime then id else+      lockCounter %~ (+1) +-- | Produce Lock Counter from Lock Time, handling cycle slips.+--+toLock :: MonadStore e m => Word16 -> GnssSignal -> Word8 -> m Word16+toLock station sid time = do+  lockMap <- view storeLockMap+  lock    <- liftIO $ newLock+  liftIO $ fmap (^. lockCounter) $+    modifyMap lockMap lock (station, sid) $ updateLock time+ -- | Construct sequenced SBP observation header -- fromGpsObservationHeader :: MonadStore e m@@ -230,50 +296,70 @@     , _observationHeader_n_obs = totalMsgs `shiftL` 4 .|. n     } +-- | Construct an L1 GnssSignal+--+toL1GnssSignal :: Word8 -> GpsL1Observation -> GnssSignal+toL1GnssSignal sat l1 =+  GnssSignal+    { _gnssSignal_sat      = fromIntegral $ sat - 1+    , _gnssSignal_code     = if l1 ^. gpsL1Observation_code then l1PSidCode else l1CSidCode+    , _gnssSignal_reserved = 0+    }+ -- | Construct an L1 SBP PackedObsContent an RTCM satellite vehicle observation ---fromL1SatelliteObservation :: Word8                -- ^ Satellite PRN+fromL1SatelliteObservation :: MonadStore e m+                           => Word16              -- ^ Station ID+                           -> Word8               -- ^ Satellite PRN                            -> GpsL1Observation                            -> GpsL1ExtObservation-                           -> PackedObsContent-fromL1SatelliteObservation sat l1 l1e =+                           -> m PackedObsContent+fromL1SatelliteObservation station sat l1 l1e = do   -- Checks GPS L1 code indicator for RTCM message 1002.   -- See DF016, pg. 3-17 of the RTCM3 spec.-  PackedObsContent+  let sid = toL1GnssSignal sat l1+  lock    <- toLock station sid $ l1 ^. gpsL1Observation_lockTime+  return PackedObsContent     { _packedObsContent_P    = toP_L1 l1 l1e     , _packedObsContent_L    = toL_L1 l1 l1e     , _packedObsContent_cn0  = toCn0_L1 l1e-    , _packedObsContent_lock = toLock_L1 l1-    , _packedObsContent_sid  = GnssSignal-      { _gnssSignal_sat      = fromIntegral $ sat - 1-      , _gnssSignal_code     = if l1 ^. gpsL1Observation_code then l1PSidCode else l1CSidCode-      , _gnssSignal_reserved = 0-      }+    , _packedObsContent_lock = lock+    , _packedObsContent_sid  = sid     } +-- | Construct an L2 GnssSignal+--+toL2GnssSignal :: Word8 -> GpsL2Observation -> Maybe GnssSignal+toL2GnssSignal sat l2 = do+  code <- M.lookup (l2 ^. gpsL2Observation_code) l2codeToSBPSignalCode+  return GnssSignal+    { _gnssSignal_sat      = fromIntegral $ sat - 1+    , _gnssSignal_code     = code+    , _gnssSignal_reserved = 0+    }+ -- | Construct an L2 SBP PackedObsContent an RTCM satellite vehicle observation ---fromL2SatelliteObservation :: Word8                  -- ^ Satellite PRN+fromL2SatelliteObservation :: MonadStore e m+                           => Word16                 -- ^ Station ID+                           -> Word8                  -- ^ Satellite PRN                            -> GpsL1Observation                            -> GpsL1ExtObservation                            -> GpsL2Observation                            -> GpsL2ExtObservation-                           -> Maybe PackedObsContent-fromL2SatelliteObservation sat l1 l1e l2 l2e = do+                           -> m (Maybe PackedObsContent)+fromL2SatelliteObservation station sat l1 l1e l2 l2e =   -- Checks GPS L2 code indicator.   -- See DF016, pg. 3-17 of the RTCM3 spec.-  code <- M.lookup (l2 ^. gpsL2Observation_code) l2codeToSBPSignalCode-  return PackedObsContent-    { _packedObsContent_P    = toP_L2 l1 l1e l2-    , _packedObsContent_L    = toL_L2 l1 l1e l2 l2e-    , _packedObsContent_cn0  = toCn0_L2 l2e-    , _packedObsContent_lock = toLock_L2 l2-    , _packedObsContent_sid  = GnssSignal-      { _gnssSignal_sat      = fromIntegral $ sat - 1-      , _gnssSignal_code     = code-      , _gnssSignal_reserved = 0+  maybe' (toL2GnssSignal sat l2) (return Nothing) $ \sid -> do+    lock <- toLock station sid $ l2 ^. gpsL2Observation_lockTime+    return $ Just PackedObsContent+      { _packedObsContent_P    = toP_L2 l1 l1e l2+      , _packedObsContent_L    = toL_L2 l1 l1e l2 l2e+      , _packedObsContent_cn0  = toCn0_L2 l2e+      , _packedObsContent_lock = lock+      , _packedObsContent_sid  = sid       }-    }  -- | Construct SBP GPS observation message (possibly chunked). --@@ -303,47 +389,45 @@  -- | Construct an L1 SBP PackedObsContent from an RTCM Msg 1002. ---fromObservation1002 :: Observation1002 -> [PackedObsContent]-fromObservation1002 obs =+fromObservation1002 :: MonadStore e m => Word16 -> Observation1002 -> m [PackedObsContent]+fromObservation1002 station obs =   -- Only lower set of PRN numbers (1-32) are supported-  if sat > maxSats then mempty else [fromL1SatelliteObservation sat l1 l1e]+  if sat > maxSats then return mempty else do+    obs1 <- fromL1SatelliteObservation station sat l1 l1e+    return [obs1]     where       sat = obs ^. observation1002_sat       l1  = obs ^. observation1002_l1       l1e = obs ^. observation1002_l1e - -- | Convert an RTCM L1 1002 observation into an SBP MsgObs. -- -- This chunking takes places because the number of observations in a given 1002 -- may very well exceed the maximum SBP supported payload size of 255 bytes. fromMsg1002 :: MonadStore e m => Msg1002 -> m [MsgObs] fromMsg1002 m = do-  let hdr       = m ^. msg1002_header-      obs       = concatMap fromObservation1002 $ m ^. msg1002_observations-      chunks    = zip [0..] $ chunksOf maxObsPerMessage obs+  let hdr     = m ^. msg1002_header+      station = hdr ^. gpsObservationHeader_station+  obs <- concatMapM (fromObservation1002 station) $ m ^. msg1002_observations+  let chunks    = zip [0..] $ chunksOf maxObsPerMessage obs       totalMsgs = fromIntegral $ length chunks   forM chunks $ uncurry $ chunkToMsgObs hdr totalMsgs  -- | Construct an L1/L2 SBP PackedObsContent from an RTCM Msg 1004. ---fromObservation1004 :: Observation1004 -> [PackedObsContent]-fromObservation1004 obs =+fromObservation1004 :: MonadStore e m => Word16 -> Observation1004 -> m [PackedObsContent]+fromObservation1004 station obs =   -- Only lower set of PRN numbers (1-32) are supported-  if sat > maxSats then mempty else-    maybe [obs1] (: [obs1]) obs2+  if sat > maxSats then return mempty else do+    obs1 <- fromL1SatelliteObservation station sat l1 l1e+    obs2 <- fromL2SatelliteObservation station sat l1 l1e l2 l2e+    return $ maybe [obs1] (: [obs1]) obs2     where       sat = obs ^. observation1004_sat       l1  = obs ^. observation1004_l1       l1e = obs ^. observation1004_l1e-      -- Checks GPS L1 code indicator for RTCM message 1004-      -- See DF016, pg. 3-17 of the RTCM3 spec.-      obs1 = fromL1SatelliteObservation sat l1 l1e-      l2   = obs ^. observation1004_l2-      l2e  = obs ^. observation1004_l2e-      -- Checks GPS L2 code indicator.-      -- See DF016, pg. 3-17 of the RTCM3 spec.-      obs2 = fromL2SatelliteObservation sat l1 l1e l2 l2e+      l2  = obs ^. observation1004_l2+      l2e = obs ^. observation1004_l2e  -- | Convert an RTCM L1+L2 1004 observation into multiple SBP MsgObs. --@@ -351,9 +435,10 @@ -- may very well exceed the maximum SBP supported payload size of 255 bytes. fromMsg1004 :: MonadStore e m => Msg1004 -> m [MsgObs] fromMsg1004 m = do-  let hdr       = m ^. msg1004_header-      obs       = concatMap fromObservation1004 $ m ^. msg1004_observations-      chunks    = zip [0..] $ chunksOf maxObsPerMessage obs+  let hdr     = m ^. msg1004_header+      station = hdr ^. gpsObservationHeader_station+  obs <- concatMapM (fromObservation1004 station) $ m ^. msg1004_observations+  let chunks    = zip [0..] $ chunksOf maxObsPerMessage obs       totalMsgs = fromIntegral $ length chunks   forM chunks $ uncurry $ chunkToMsgObs hdr totalMsgs @@ -397,11 +482,15 @@     let sender = m ^. msg1006_reference ^. antennaReference_station     m' <- fromMsg1006 m     return [SBPMsgBasePosEcef m' $ toSBP m' $ toSender sender]+  (RTCM3Msg1013 m _rtcm3) -> do+    wn <- view storeWn+    liftIO $ writeIORef wn $ toWn $ m ^. msg1013_header ^. messageHeader_mjd+    return mempty   _rtcm3Msg -> return mempty  newStore :: IO Store newStore = do   day <- utctDay <$> getCurrentTime   let wn = fromIntegral $ div (diffDays day (fromGregorian 1980 1 6)) 7-  Store <$> newIORef wn+  Store <$> newIORef wn <*> newIORef mempty <*> newIORef mempty
src/Data/RTCM3/SBP/Types.hs view
@@ -1,3 +1,4 @@+{-# OPTIONS  -fno-warn-orphans          #-} {-# LANGUAGE ConstraintKinds            #-} {-# LANGUAGE FlexibleInstances          #-} {-# LANGUAGE GeneralizedNewtypeDeriving #-}@@ -27,7 +28,21 @@ import Control.Monad.Trans.Resource import Data.IORef import Data.Word+import SwiftNav.SBP +data Lock = Lock+  { _lockTime    :: Word8+  , _lockCounter :: Word16+  } deriving ( Eq )++$(makeLenses ''Lock)++type GPSTimeMap = HashMap Word16 ObsGPSTime+type LockMap    = HashMap (Word16, GnssSignal) Lock++instance Hashable GnssSignal where+  hashWithSalt s g = hashWithSalt s (g ^. gnssSignal_sat, g ^. gnssSignal_code, g ^. gnssSignal_reserved)+ newtype ConvertT e m a = ConvertT { unConvertT :: ReaderT e m a }   deriving     ( Functor@@ -58,7 +73,9 @@     liftBase = liftBaseDefault  data Store = Store-  { _storeWn :: IORef Word16+  { _storeWn         :: IORef Word16+  , _storeLockMap    :: IORef LockMap+  , _storeGPSTimeMap :: IORef GPSTimeMap   } deriving ( Eq )  $(makeClassy ''Store)
test/Test/Data/RTCM3/SBP.hs view
@@ -31,8 +31,8 @@  decodeRTCMFile :: FilePath -> IO [SBPMsg] decodeRTCMFile filename = do-  wn <- liftIO $ newIORef 1906-  runResourceT $ runConvertT (Store wn) $ runConduit  $+  s <- Store <$> newIORef 1906 <*> newIORef mempty <*> newIORef mempty+  runResourceT $ runConvertT s $ runConduit  $     sourceFile filename   =$=     conduitDecode         =$=     CL.concatMapM convert $$@@ -42,7 +42,7 @@ basePosition msg' = ( msg' ^. msgBasePosEcef_x                     , msg' ^. msgBasePosEcef_y                     , msg' ^. msgBasePosEcef_z-                   )+                    )  isL1 :: PackedObsContent -> Bool isL1 obs = (obs ^. packedObsContent_sid ^. gnssSignal_code) == l1CSidCode@@ -194,8 +194,62 @@         assertMsgObsLength (msgs !! 7) 1      ] +testToWn :: TestTree+testToWn =+  testGroup "MJD to GPS week number"+    [ testCase "Day 0, Week 0" $+       toWn mjdEpoch @?= 0+    , testCase "Day 4, Week 1906" $+       toWn 57590 @?= 1906+    , testCase "Day 0, Week 1906" $+       toWn 57586 @?= 1906+    , testCase "Day 7, Week 1905" $+       toWn 57585 @?= 1905+    , testCase "Day 7, Week 1906" $+       toWn 57592 @?= 1906+    , testCase "Day 0, Week 1907" $+       toWn 57593 @?= 1907+    ]++testUpdateGPSTime :: TestTree+testUpdateGPSTime =+  testGroup "Update GPS Time"+    [ testCase "old TOW < new TOW" $ do+        let old = ObsGPSTime 1 10+            new = updateGPSTime 2 old+        new ^. obsGPSTime_wn @?= 10+    , testCase "old TOW = new TOW" $ do+        let old = ObsGPSTime 1 10+            new = updateGPSTime 1 old+        new ^. obsGPSTime_wn @?= 10+    , testCase "old TOW > new TOW" $ do+        let old = ObsGPSTime 1 10+            new = updateGPSTime 0 old+        new ^. obsGPSTime_wn @?= 11+    ]++testUpdateLock :: TestTree+testUpdateLock =+  testGroup "Update Lock"+    [ testCase "old time < new time" $ do+        let old = Lock 1 10+            new = updateLock 2 old+        new ^. lockCounter @?= 10+    , testCase "old time = new time" $ do+        let old = Lock 1 10+            new = updateLock 1 old+        new ^. lockCounter @?= 10+    , testCase "old time > new time" $ do+        let old = Lock 1 10+            new = updateLock 0 old+        new ^. lockCounter @?= 11+    ]+ tests :: TestTree tests =   testGroup "RTCM3 to SBP conversion tests"     [ testMsg1004+    , testToWn+    , testUpdateGPSTime+    , testUpdateLock     ]