gnss-converters 0.3.3 → 0.3.5

raw patch · 6 files changed

+544/−550 lines, 6 filesPVP: major bump suggested

API removals or changes: PVP suggests a major version bump

API changes (from Hackage documentation)

Files

• gnss-converters.cabal +4/−4
• src/Data/RTCM3/SBP.hs +56/−501
• src/Data/RTCM3/SBP/Observations.hs +348/−0
• src/Data/RTCM3/SBP/Time.hs +125/−0
• src/SwiftNav/SBP/RTCM3.hs +2/−19
• test/Test/Data/RTCM3/SBP.hs +9/−26

@@ -1,5 +1,5 @@ name: gnss-converters-version: 0.3.3+version: 0.3.5 cabal-version: >=1.22 build-type: Simple license: BSD3@@ -19,12 +19,13 @@ library     exposed-modules:         Data.RTCM3.SBP+        Data.RTCM3.SBP.Observations+        Data.RTCM3.SBP.Time         Data.RTCM3.SBP.Types         SwiftNav.SBP.RTCM3     build-depends:         base >=4.8 && <5,         basic-prelude >=0.5.2,-        conduit-extra >=1.1.16,         exceptions >=0.8.3,         extra >=1.4.10,         lens >=4.13,@@ -34,8 +35,7 @@         rtcm >=0.2.2,         sbp >=2.2.11,         time >=1.5.0.1,-        transformers-base >=0.4.4,-        unordered-containers >=0.2.8.0+        transformers-base >=0.4.4     default-language: Haskell2010     hs-source-dirs: src     ghc-options: -Wall

@@ -12,144 +12,30 @@ -- RTCMv3 to SBP Conversions.  module Data.RTCM3.SBP-  ( l1CSidCode-  , l2CMSidCode-  , l2PSidCode-  , sbpCMinM-  , q32Width-  , toWn+  ( toWn   , mjdEpoch-  , updateGpsTimeNano   , convert   , newStore   , validateIodcIode   , gpsUriToUra   ) where -import           BasicPrelude-import           Control.Lens-import           Control.Monad.Extra-import           Data.Bits-import qualified Data.HashMap.Strict  as M-import           Data.IORef-import           Data.List.Extra      hiding (concat, map)-import           Data.RTCM3-import           Data.RTCM3.SBP.Types-import           Data.Time-import           Data.Word-import           SwiftNav.SBP+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 +{-# ANN module ("HLint: ignore Redundant if"::String) #-}  -------------------------------------------------------------------------------- -- SBP, GNSS, RTCM constant definitions ---- | Two centimeters------ SBP pseudoranges are in units of 2cm, and there are 50 of those in a meter.-twoCM :: Double-twoCM = 0.02--sbpCMinM :: Double-sbpCMinM = 50---- | Speed of light (meters/msec)-lightSpeedMMSEC :: Double-lightSpeedMMSEC = 299792.458---- | Speed of light (meters/sec)-lightSpeedMS :: Double-lightSpeedMS = 299792458.0---- | L1 GPS center frequency (Hz)-l1frequency :: Double-l1frequency = 1.57542e9---- | L2 GPS center frequency (Hz)-l2frequency :: Double-l2frequency = 1.22760e9---- | We only support PRNS 1 - 32-maxSats :: Word8-maxSats = 32---- | Q32.8 carrier phase representation width-q32Width :: Double-q32Width = 256---- | RTCM phase range resolution.------ See DF018, pg. 3-19 of the RTCM3 spec-phaseRangeRes :: Double-phaseRangeRes = 0.0005---- | SBP L1 GNSS signal value------ See: https://github.com/swift-nav/libswiftnav/blob/master/include/libswiftnav/signal.h#L65-l1CSidCode :: Word8-l1CSidCode = 0---- | SBP L2CM GNSS signal value------ See: https://github.com/swift-nav/libswiftnav/blob/master/include/libswiftnav/signal.h#L65-l2CMSidCode :: Word8-l2CMSidCode = 1---- | SBP L1P and L2P GNSS signal values------ Note that libswiftnav currently does not support L1P or L2P observations,--- just L1C and L2C. This is a stop gap definition so that we can properly--- serialize out SBP observations with L1P and L2P observations from external--- receivers.-l1PSidCode :: Word8-l1PSidCode = 5--l2PSidCode :: Word8-l2PSidCode = 6---- | L2C code indicator value------ See DF016, pg. 3-17 of the RTCM3 spec-codeIndicator_L2C :: Word8-codeIndicator_L2C = 0---- | L2P code direct indicator value------ See DF016, pg. 3-17 of the RTCM3 spec-codeIndicator_L2P :: Word8-codeIndicator_L2P = 1---- | L2P code cross-correlated indicator value------ See DF016, pg. 3-17 of the RTCM3 spec-codeIndicator_L2D :: Word8-codeIndicator_L2D = 2---- | L2P code correlated indicator value------ See DF016, pg. 3-17 of the RTCM3 spec-codeIndicator_L2W :: Word8-codeIndicator_L2W = 3---- | Map L2 codes to SBP GnssSignal codes----l2codeToSBPSignalCode :: HashMap Word8 Word8-l2codeToSBPSignalCode = M.fromList-  [ (codeIndicator_L2C, l2CMSidCode)-  , (codeIndicator_L2P, l2PSidCode)-  , (codeIndicator_L2D, l2PSidCode)-  , (codeIndicator_L2W, l2PSidCode)-  ]---- | Maximum number of packed observations to allow in a single SBP message.----maxObsPerMessage :: Int-maxObsPerMessage = (maxPayloadSize - headerSize) `div` packedObsSize-  where-    maxPayloadSize = 255-    headerSize     = 11-    packedObsSize  = 17- -- | The official GPS value of Pi. This is the value used by the CS to curve -- fit ephemeris parameters and should be used in all ephemeris calculations. gpsPi :: Double@@ -158,18 +44,6 @@ -------------------------------------------------------------------------------- -- 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- applyScaleFactor :: (Floating a, Integral b) => a -> a -> b -> a applyScaleFactor n p x = (n ** p)  * fromIntegral x @@ -180,40 +54,13 @@ fromEcefVal :: Int64 -> Double fromEcefVal x = fromIntegral x / 10000 -newGpsTime :: MonadStore e m => Word32 -> m GpsTimeNano-newGpsTime tow = do-  wn <- view storeWn >>= liftIO . readIORef-  return GpsTimeNano-    { _gpsTimeNano_tow         = tow-    , _gpsTimeNano_ns_residual = 0-    , _gpsTimeNano_wn          = wn-    }---- | If incoming TOW is less than stored TOW, rollover WN.----updateGpsTimeNano :: Word32 -> GpsTimeNano -> GpsTimeNano-updateGpsTimeNano tow gpsTime =-  gpsTime & gpsTimeNano_tow .~ tow &-    if tow >= gpsTime ^. gpsTimeNano_tow then id else-      gpsTimeNano_wn %~ (+ 1)---- | Produce GPS Time from Observation header, handling WN rollover.----toGpsTimeNano :: MonadStore e m => GpsObservationHeader -> m GpsTimeNano-toGpsTimeNano hdr = do-  let tow      = hdr ^. gpsObservationHeader_tow-      station  = hdr ^. gpsObservationHeader_station-  gpsTime      <- newGpsTime tow-  gpsTimeMap   <- view storeGpsTimeMap-  liftIO $ modifyMap gpsTimeMap gpsTime station $ updateGpsTimeNano tow- -- | Produce GPS time from RTCM time (week-number % 1024, tow in seconds, scaled 2^4). -- Stateful but non-side-effecting. The week number is mod-1024 - add in the base -- wn from the stored wn masked to 10 bits. toGpsTimeSec :: MonadStore e m => Word16 -> Word16 -> m GpsTimeSec toGpsTimeSec wn tow = do   stateWn <- view storeWn >>= liftIO . readIORef-  return $ GpsTimeSec+  return GpsTimeSec     { _gpsTimeSec_tow = 16 * fromIntegral tow     , _gpsTimeSec_wn  = stateWn `shiftR` 10 `shiftL` 10 + wn     }@@ -229,213 +76,6 @@ toWn :: Word16 -> Word16 toWn mjd = (mjd - mjdEpoch) `div` 7 --- | Determine whether an L1 RTCM observation is invalid.------ See DF011 and DF012 of the RTCM3 spec-invalid_L1 :: GpsL1Observation -> Bool-invalid_L1 l1 =-  l1 ^. gpsL1Observation_pseudorange == 524288 ||-  l1 ^. gpsL1Observation_carrierMinusCode == -524288---- | Determine whether an L1 + L2 RTCM observation is invalid.------ See DF011, DF012, and DF018 of the RTCM3 spec-invalid_L2 :: GpsL2Observation -> Bool-invalid_L2 l2 =-  l2 ^. gpsL2Observation_pseudorangeDifference == -8192 ||-  l2 ^. gpsL2Observation_carrierMinusCode == -524288---- | Construct metric pseudorange (meters!) from L1 RTCM observation.------ See DF011, pg. 3-16 of the RTCM3 spec-metricPseudorange :: GpsL1Observation -> GpsL1ExtObservation -> Double-metricPseudorange l1 l1e =-  twoCM * fromIntegral (l1 ^. gpsL1Observation_pseudorange) +-  lightSpeedMMSEC * fromIntegral (l1e ^. gpsL1ExtObservation_ambiguity)---- | Construct L1 SBP pseudorange for L1 RTCM observation------ See DF011, pg. 3-16 of the RTCM3 spec-toP_L1 :: GpsL1Observation -> GpsL1ExtObservation -> Word32-toP_L1 l1 l1e = round $ sbpCMinM * metricPseudorange l1 l1e---- | Construct L2 SBP pseudorange from L1/L2 RTCM observations------ See DF017, pg. 3-18 of the RTCM3 spec-toP_L2 :: GpsL1Observation -> GpsL1ExtObservation -> GpsL2Observation -> Word32-toP_L2 l1 l1e l2 = round $ p * sbpCMinM where-  p = metricPseudorange l1 l1e +-      twoCM * fromIntegral (l2 ^. gpsL2Observation_pseudorangeDifference)---- | Construct SBP L1 GPS carrier phase from L1 RTCM observation------ See DF012, pg. 3-16 of the RTCM3 spec-toL_L1 :: GpsL1Observation -> GpsL1ExtObservation -> CarrierPhase-toL_L1 l1 l1e = CarrierPhase-  { _carrierPhase_i = fromIntegral li'-  , _carrierPhase_f = fromIntegral lf'-  } where-    p = metricPseudorange l1 l1e-    -- Convert to SBP carrier phase representation per-    -- https://github.com/swift-nav/libsbp/blob/master/spec/yaml/swiftnav/sbp/observation.yaml#L39-    lm :: Double-    lm = p + phaseRangeRes * fromIntegral (l1 ^. gpsL1Observation_carrierMinusCode)-    l = lm / (lightSpeedMS / l1frequency)-    li :: Int32-    li = floor l-    lf :: Word16-    lf = round ((l - fromIntegral li) * q32Width)-    li' :: Int32-    li' = if lf == 256 then li + 1 else li-    lf' :: Word8-    lf' = if lf == 256 then 0 else fromIntegral lf--- | Construct SBP L2 GPS carrier phase from L2 RTCM observation------ See DF018, pg. 3-18 of the RTCM3 spec-toL_L2 :: GpsL1Observation-       -> GpsL1ExtObservation-       -> GpsL2Observation-       -> GpsL2ExtObservation-       -> CarrierPhase-toL_L2 l1 l1e l2 _l2e = CarrierPhase-  { _carrierPhase_i = fromIntegral li-  , _carrierPhase_f = fromIntegral lf-  } where-    p = metricPseudorange l1 l1e-    -- Convert to SBP carrier phase representation per-    -- https://github.com/swift-nav/libsbp/blob/master/spec/yaml/swiftnav/sbp/observation.yaml#L39-    lm :: Double-    lm = p + phaseRangeRes * fromIntegral (l2 ^. gpsL2Observation_carrierMinusCode)-    l = lm / (lightSpeedMS / l2frequency)-    li :: Int32-    li = floor l-    lf :: Word8-    lf = round ((l - fromIntegral li) * q32Width)--toCn0_L1 :: GpsL1ExtObservation -> Word8-toCn0_L1 = (^. gpsL1ExtObservation_cnr)--toCn0_L2 :: GpsL2ExtObservation -> Word8-toCn0_L2 = (^. gpsL2ExtObservation_cnr)---- | Convert between DF013 and DF019 lock time to DF402 lock time----toLock :: Word8 -> Word8-toLock t =-  if t' < 32 then 0 else-    if t' < 64 then 1 else-      if t' < 128 then 2 else-        if t' < 256 then 3 else-          if t' < 512 then 4 else-            if t' < 1024 then 5 else-              if t' < 2048 then 6 else-                if t' < 4096 then 7 else-                  if t' < 8192 then 8 else-                    if t' < 16384 then 9 else-                      if t' < 32768 then 10 else-                        if t' < 65536 then 11 else-                          if t' < 131072 then 12 else-                            if t' < 262144 then 13 else-                              if t' < 524288 then 14 else-                                15-  where-    t' :: Word32-    t' =-      1000 *-        if t <= 23 then fromIntegral t else-          if t <= 47 then fromIntegral t * 2 - 24 else-            if t <= 71 then fromIntegral t * 4 - 120 else-              if t <= 95 then fromIntegral t * 8 - 408 else-                if t <= 119 then fromIntegral t * 16 - 1176 else-                  if t <= 126 then fromIntegral t * 32 - 3096 else-                    937---- | Construct sequenced SBP observation header----fromGpsObservationHeader :: MonadStore e m-                         => Word8                -- ^ Total messages-                         -> Word8                -- ^ Message in sequence-                         -> GpsObservationHeader -- ^ RTCM observation header-                         -> m ObservationHeader-fromGpsObservationHeader totalMsgs n hdr = do-  t <- toGpsTimeNano hdr-  return ObservationHeader-    { _observationHeader_t     = t-    -- First nibble is the size of the sequence (n), second nibble is the-    -- zero-indexed counter (ith packet of n). See observation header packing-    -- https://github.com/swift-nav/libsbp/blob/master/spec/yaml/swiftnav/sbp/observation.yaml#L63-    , _observationHeader_n_obs = totalMsgs `shiftL` 4 .|. n-    }---- | Construct an L1 GnssSignal----toL1GnssSignal :: Word8 -> GpsL1Observation -> GnssSignal16-toL1GnssSignal sat l1 =-  GnssSignal16-    { _gnssSignal16_sat  = sat-    , _gnssSignal16_code = if l1 ^. gpsL1Observation_code then l1PSidCode else l1CSidCode-    }---- | Construct an L1 SBP PackedObsContent an RTCM satellite vehicle observation----fromL1SatelliteObservation :: MonadStore e m-                           => Word8               -- ^ Satellite PRN-                           -> GpsL1Observation-                           -> GpsL1ExtObservation-                           -> m PackedObsContent-fromL1SatelliteObservation sat l1 l1e = do-  -- Checks GPS L1 code indicator for RTCM message 1002.-  -- See DF010, pg. 3-17 of the RTCM3 spec.-  let sid = toL1GnssSignal sat l1-  return PackedObsContent-    { _packedObsContent_P     = toP_L1 l1 l1e-    , _packedObsContent_L     = toL_L1 l1 l1e-    , _packedObsContent_D     = Doppler 0 0-    , _packedObsContent_cn0   = toCn0_L1 l1e-    , _packedObsContent_lock  = toLock $ l1 ^. gpsL1Observation_lockTime-    , _packedObsContent_sid   = sid-    , _packedObsContent_flags = 0x7 -- Doppler Invalid-                                    -- 1/2 cycle phase ambiguity resolved-                                    -- carrier valid-                                    -- pseudorange valid-    }---- | Construct an L2 GnssSignal----toL2GnssSignal :: Word8 -> GpsL2Observation -> Maybe GnssSignal16-toL2GnssSignal sat l2 = do-  code <- M.lookup (l2 ^. gpsL2Observation_code) l2codeToSBPSignalCode-  return GnssSignal16-    { _gnssSignal16_sat  = fromIntegral sat-    , _gnssSignal16_code = code-    }---- | Construct an L2 SBP PackedObsContent an RTCM satellite vehicle observation----fromL2SatelliteObservation :: MonadStore e m-                           => Word8                  -- ^ Satellite PRN-                           -> GpsL1Observation-                           -> GpsL1ExtObservation-                           -> GpsL2Observation-                           -> GpsL2ExtObservation-                           -> m (Maybe PackedObsContent)-fromL2SatelliteObservation sat l1 l1e l2 l2e =-  -- Checks GPS L2 code indicator.-  -- See DF016, pg. 3-17 of the RTCM3 spec.-  maybe' (toL2GnssSignal sat l2) (return Nothing) $ \sid -> do-    return $ Just PackedObsContent-      { _packedObsContent_P     = toP_L2 l1 l1e l2-      , _packedObsContent_L     = toL_L2 l1 l1e l2 l2e-      , _packedObsContent_D     = Doppler 0 0-      , _packedObsContent_cn0   = toCn0_L2 l2e-      , _packedObsContent_lock  = toLock $ l2 ^. gpsL2Observation_lockTime-      , _packedObsContent_sid   = sid-      , _packedObsContent_flags = 0x7 -- Doppler Invalid-                                      -- 1/2 cycle phase ambiguity resolved-                                      -- carrier valid-                                      -- pseudorange valid-    }- -- | Validate IODE/IODC flags. The IODC and IODE flags (least significant bits) should be -- equal. IODC is Word16 while IODE is Word8, so we shift IODC to drop the most significant bits. -- We return a 1 if valid, 0 if invalid.@@ -461,21 +101,6 @@     , _ephemerisCommonContent_health_bits  = m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth     } --- | Construct SBP GPS observation message (possibly chunked).----chunkToMsgObs :: MonadStore e m-              => GpsObservationHeader -- ^ RTCM observation header-              -> Word8                -- ^ Total messages-              -> Word8                -- ^ Message in sequence-              -> [PackedObsContent]-              -> m MsgObs-chunkToMsgObs hdr totalMsgs n packed = do-  header <- fromGpsObservationHeader totalMsgs n hdr-  return MsgObs-    { _msgObs_header = header-    , _msgObs_obs    = packed-    }- -- | 16 bit SBP Sender Id is 12 bit RTCM Station Id with high nibble or'd in -- toSender :: Word16 -> Word16@@ -484,97 +109,35 @@ -- | Decode SBP 'fitInterval' from RTCM/GPS 'fitIntervalFlag' and IODC. -- Implementation adapted from libswiftnav/ephemeris.c. decodeFitInterval :: Bool -> Word16 -> Word32-decodeFitInterval fitInt iodc =-  if not fitInt then 4 * 60 * 60 else-    if iodc >= 240 && iodc <= 247 then 8 * 60 * 60 else-      if (iodc >= 248 && iodc <= 255) || iodc == 496 then 14 * 60 * 60 else-        if (iodc >= 497 && iodc <= 503) || (iodc >= 1021 && iodc <= 1023) then 26 * 60 * 60 else-          if iodc >= 504 && iodc <= 510 then 50 * 60 * 60 else-            if iodc == 511 || (iodc >= 752 && iodc <= 756) then 74 * 60 * 60 else-              if iodc == 757 then 98 * 60 * 60 else-                6 * 60 * 60+decodeFitInterval fitInt iodc+  | not fitInt = 4 * 60 * 60+  | iodc >= 240 && iodc <= 247 = 8 * 60 * 60+  | (iodc >= 248 && iodc <= 255) || iodc == 496 = 14 * 60 * 60+  | (iodc >= 497 && iodc <= 503) || (iodc >= 1021 && iodc <= 1023) = 26 * 60 * 60+  | iodc >= 504 && iodc <= 510 = 50 * 60 * 60+  | iodc == 511 || (iodc >= 752 && iodc <= 756) = 74 * 60 * 60+  | iodc == 757 = 98 * 60 * 60+  | otherwise = 6 * 60 * 60  -- | Convert between RTCM/GPS URA ("User Range Accuracy") index to a number in meters. -- See section 2.5.3, "User Range Accuracy", in the GPS signal specification. -- Indices 1, 3, and 5 are hard-coded according to spec, and 15 is hard-coded according -- to SBP/Piksi convention. gpsUriToUra :: Double -> Double-gpsUriToUra uri =-  if uri < 0 then -1 else-    if uri == 1 then 2.8 else-      if uri == 3 then 5.7 else-        if uri == 5 then 11.3 else-          if uri == 15 then 6144 else-            if uri <= 6 then 2 ** (1 + (uri / 2)) else-              if uri > 6 && uri < 15 then 2 ** (uri - 2) else-                -1+gpsUriToUra uri+  | uri < 0 = -1+  | uri == 1 = 2.8+  | uri == 3 = 5.7+  | uri == 5 = 11.3+  | uri == 15 = 6144+  | uri <= 6 = 2 ** (1 + (uri / 2))+  | uri > 6 && uri < 15 = 2 ** (uri - 2)+  | otherwise = -1  -------------------------------------------------------------------------------- -- RTCM to SBP conversion utilities: RTCM Msgs. 1002 (L1 RTK), 1004 (L1+L2 RTK), -- 1005 (antenna position), 1006 (antenna position). ---- | Construct an L1 SBP PackedObsContent from an RTCM Msg 1002.----fromObservation1002 :: MonadStore e m => Observation1002 -> m [PackedObsContent]-fromObservation1002 obs = do-  obs1 <- fromL1SatelliteObservation sat l1 l1e-  return $-    -- Only lower set of PRN numbers (1-32) are supported-    if sat > maxSats then mempty else-      if invalid_L1 l1 then mempty else-        [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 <- concatMapM fromObservation1002 $ 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 :: MonadStore e m => Observation1004 -> m [PackedObsContent]-fromObservation1004 obs = do-  obs1 <- fromL1SatelliteObservation sat l1 l1e-  obs2 <- fromL2SatelliteObservation sat l1 l1e l2 l2e-  return $-    -- Only lower set of PRN numbers (1-32) are supported-    if sat > maxSats then mempty else-      if invalid_L1 l1 && invalid_L2 l2 then mempty else-        if invalid_L1 l1 then maybeToList obs2 else-          if invalid_L2 l2 then [obs1] else-            obs1 : maybeToList obs2-  where-    sat = obs ^. observation1004_sat-    l1  = obs ^. observation1004_l1-    l1e = obs ^. observation1004_l1e-    l2  = obs ^. observation1004_l2-    l2e = obs ^. observation1004_l2e----- | Convert an RTCM L1+L2 1004 observation into multiple SBP MsgObs.------ This chunking takes places because the number of observations in a given 1004--- 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 <- concatMapM fromObservation1004 $ m ^. msg1004_observations-  let chunks    = zip [0..] $ chunksOf maxObsPerMessage obs-      totalMsgs = fromIntegral $ length chunks-  forM chunks $ uncurry $ chunkToMsgObs hdr totalMsgs- -- | Convert an RTCM 1005 antenna reference position message into an SBP -- MsgBasePosEcef. fromMsg1005 :: MonadStore e m => Msg1005 -> m MsgBasePosEcef@@ -602,27 +165,27 @@   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 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_toc      = toc     } @@ -630,14 +193,8 @@ -- convert :: MonadStore e m => RTCM3Msg -> m [SBPMsg] convert = \case-  (RTCM3Msg1002 m _rtcm3) -> do-    let sender = m ^. msg1002_header ^. gpsObservationHeader_station-    m' <- fromMsg1002 m-    return $ flip fmap m' $ \x -> SBPMsgObs x $ toSBP x $ toSender sender-  (RTCM3Msg1004 m _rtcm3) -> do-    let sender = m ^. msg1004_header ^. gpsObservationHeader_station-    m' <- fromMsg1004 m-    return $ flip fmap m' $ \x -> SBPMsgObs x $ toSBP x $ toSender sender+  (RTCM3Msg1002 m _rtcm3) -> toSBPMsgObs m+  (RTCM3Msg1004 m _rtcm3) -> toSBPMsgObs m   (RTCM3Msg1005 m _rtcm3) -> do     let sender =  m ^. msg1005_reference ^. antennaReference_station     m' <- fromMsg1005 m@@ -646,10 +203,8 @@     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+  (RTCM3Msg1010 m _rtcm3) -> toSBPMsgObs m+  (RTCM3Msg1012 m _rtcm3) -> toSBPMsgObs m   (RTCM3Msg1019 m _rtcm3) -> do     let sender = 0     m' <- fromMsg1019 m

@@ -0,0 +1,348 @@+{-# LANGUAGE NoImplicitPrelude #-}++-- |+-- Module:      Data.RTCM3.SBP.Observations+-- Copyright:   Copyright (C) 2016 Swift Navigation, Inc.+-- License:     LGPL-3+-- Maintainer:  Swift Navigation <dev@swiftnav.com>+-- Stability:   experimental+-- Portability: portable+--+-- RTCMv3 to SBP Observation Conversions.++module Data.RTCM3.SBP.Observations+  ( toSBPMsgObs+  ) where++import BasicPrelude+import Control.Lens+import Data.Bits+import Data.Int+import Data.IORef+import Data.List.Extra+import Data.RTCM3+import Data.RTCM3.SBP.Time+import Data.RTCM3.SBP.Types+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+  return z++-- | Update and convert stored and incoming GPS times.+--+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)++-- | Default observation doppler.+--+obsDoppler :: Doppler+obsDoppler = Doppler 0 0++-- | Default observation flags.+--+obsFlags :: Word8+obsFlags = 7++-- | Calculate pseudorange.+--+pseudorange :: Double -> Word32 -> Word8 -> Double+pseudorange u p amb = 0.02 * fromIntegral p + u * fromIntegral amb++-- | Calculate pseudorange difference.+--+pseudorangeDifference :: Int16 -> Double+pseudorangeDifference d = 0.02 * fromIntegral d++-- | GPS pseudorange unit.+--+gpsPseudorange :: Double+gpsPseudorange = 299792.458++-- | GLONASS pseudorange unit.+--+glonassPseudorange :: Double+glonassPseudorange = 599584.916++-- | Convert to SBP pseudorange.+--+toP :: Double -> Word32+toP p = round $ 50 * p++-- | Calculate carrier-phase.+--+carrierPhase :: Double -> Double -> Int32 -> Double+carrierPhase u p d = (p + 0.0005 * fromIntegral d) / (299792458.0 / u)++-- | GPS L1 carrier-phase unit.+--+gpsL1CarrierPhase :: Double+gpsL1CarrierPhase = 1.57542e9++-- | GPS L2 carrier-phase unit.+--+gpsL2CarrierPhase :: Double+gpsL2CarrierPhase = 1.22760e9++-- | GLONASS L1 carrier-phase unit.+--+glonassL1CarrierPhase :: Word8 -> Double+glonassL1CarrierPhase fcn = 1.602e9 + fromIntegral fcn * 0.5625e6++-- | GLONASS L2 carrier-phase unit.+--+glonassL2CarrierPhase :: Word8 -> Double+glonassL2CarrierPhase fcn = 1.246e9 + fromIntegral fcn * 0.4375e6++-- | 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++-- | Convert from RTCMv3 lock indicator.+--+lock :: Word8 -> Word32+lock t+  |   t <  24 = 1000 * fromIntegral t+  |   t <  48 = 1000 * fromIntegral t *  2 -   24+  |   t <  72 = 1000 * fromIntegral t *  4 -  120+  |   t <  96 = 1000 * fromIntegral t *  8 -  408+  |   t < 120 = 1000 * fromIntegral t * 16 - 1176+  |   t < 127 = 1000 * fromIntegral t * 32 - 3096+  | otherwise = 1000 * 937++-- | 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++-- | Map GPS L1 signal.+--+gpsL1Signal :: Word8 -> Bool -> GnssSignal16+gpsL1Signal sat code+  | code      = GnssSignal16 sat 5+  | otherwise = GnssSignal16 sat 0++-- | Map GPS L2 signal.+--+gpsL2Signal :: Word8 -> Word8 -> GnssSignal16+gpsL2Signal sat code+  | code == 0 = GnssSignal16 sat 1+  | otherwise = GnssSignal16 sat 6++-- | Map GLONASS L1 signal.+--+glonassL1Signal :: Word8 -> Bool -> Maybe GnssSignal16+glonassL1Signal sat code+  | code      = Nothing+  | otherwise = Just $ GnssSignal16 sat 3++-- | Map GLONASS L2 signal.+--+glonassL2Signal :: Word8 -> Word8 -> Maybe GnssSignal16+glonassL2Signal sat code+  | code == 0 = Just $ GnssSignal16 sat 4+  | otherwise = Nothing++-- | Max GPS satellite number.+--+gpsMaxSat :: Word8+gpsMaxSat = 32++-- | Max GLONASS satellite number.+--+glonassMaxSat :: Word8+glonassMaxSat = 24++-- | Produce SBP packed observation from GPS L1 observation.+--+toGpsL1PackedObsContents :: Word8 -> GpsL1Observation -> GpsL1ExtObservation -> Maybe PackedObsContent+toGpsL1PackedObsContents sat l1o l1eo+  | no        = Nothing+  | otherwise = Just PackedObsContent+      { _packedObsContent_P     = toP p1+      , _packedObsContent_L     = toL l1+      , _packedObsContent_D     = obsDoppler+      , _packedObsContent_cn0   = l1eo ^. gpsL1ExtObservation_cnr+      , _packedObsContent_lock  = toLock $ lock (l1o ^. gpsL1Observation_lockTime)+      , _packedObsContent_sid   = gpsL1Signal sat (l1o ^. gpsL1Observation_code)+      , _packedObsContent_flags = obsFlags+      }+  where+    no = sat > gpsMaxSat || l1o ^. gpsL1Observation_pseudorange == 524288 || l1o ^. gpsL1Observation_carrierMinusCode == -524288+    p1 = pseudorange gpsPseudorange (l1o ^. gpsL1Observation_pseudorange) (l1eo ^. gpsL1ExtObservation_ambiguity)+    l1 = carrierPhase gpsL1CarrierPhase p1 (l1o ^. gpsL1Observation_carrierMinusCode)++-- | Produce SBP packed observation from GPS L1 + L2 observation.+--+toGpsL2PackedObsContents :: Word8 -> GpsL1Observation -> GpsL1ExtObservation -> GpsL2Observation -> GpsL2ExtObservation -> Maybe PackedObsContent+toGpsL2PackedObsContents sat l1o l1eo l2o l2eo+  | no        = Nothing+  | otherwise = Just PackedObsContent+      { _packedObsContent_P     = toP p2+      , _packedObsContent_L     = toL l2+      , _packedObsContent_D     = obsDoppler+      , _packedObsContent_cn0   = l2eo ^. gpsL2ExtObservation_cnr+      , _packedObsContent_lock  = toLock $ lock (l2o ^. gpsL2Observation_lockTime)+      , _packedObsContent_sid   = gpsL2Signal sat (l2o ^. gpsL2Observation_code)+      , _packedObsContent_flags = obsFlags+      }+  where+    no = sat > gpsMaxSat || l2o ^. gpsL2Observation_pseudorangeDifference == -8192 || l2o ^. gpsL2Observation_carrierMinusCode == -524288+    p1 = pseudorange gpsPseudorange (l1o ^. gpsL1Observation_pseudorange) (l1eo ^. gpsL1ExtObservation_ambiguity)+    p2 = p1 + pseudorangeDifference (l2o ^. gpsL2Observation_pseudorangeDifference)+    l2 = carrierPhase gpsL2CarrierPhase p1 (l2o ^. gpsL2Observation_carrierMinusCode)++-- | Produce SBP packed observation from GLONASS L1 observation.+--+toGlonassL1PackedObsContents :: Word8 -> GlonassL1Observation -> GlonassL1ExtObservation -> Maybe PackedObsContent+toGlonassL1PackedObsContents sat l1o l1eo+  | no        = Nothing+  | otherwise = do+      sid <- glonassL1Signal sat (l1o ^. glonassL1Observation_code)+      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_flags = obsFlags+        }+  where+    no = sat > glonassMaxSat || l1o ^. glonassL1Observation_carrierMinusCode == -524288+    p1 = pseudorange glonassPseudorange (l1o ^. glonassL1Observation_pseudorange) (l1eo ^. glonassL1ExtObservation_ambiguity)+    l1 = carrierPhase (glonassL1CarrierPhase (l1o ^. glonassL1Observation_frequency)) p1 (l1o ^. glonassL1Observation_carrierMinusCode)++-- | Produce SBP packed observation from GLONASS L1 + L2 observation.+--+toGlonassL2PackedObsContents :: Word8 -> GlonassL1Observation -> GlonassL1ExtObservation -> GlonassL2Observation -> GlonassL2ExtObservation -> Maybe PackedObsContent+toGlonassL2PackedObsContents sat l1o l1eo l2o l2eo+  | no        = Nothing+  | otherwise = do+      sid <- glonassL2Signal sat (l2o ^. glonassL2Observation_code)+      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_flags = obsFlags+        }+  where+    no = sat > glonassMaxSat || l2o ^. glonassL2Observation_pseudorangeDifference == -8192 || l2o ^. glonassL2Observation_carrierMinusCode == -524288+    p1 = pseudorange glonassPseudorange (l1o ^. glonassL1Observation_pseudorange) (l1eo ^. glonassL1ExtObservation_ambiguity)+    p2 = p1 + pseudorangeDifference (l2o ^. glonassL2Observation_pseudorangeDifference)+    l2 = carrierPhase (glonassL2CarrierPhase (l1o ^. glonassL1Observation_frequency)) p1 (l2o ^. glonassL2Observation_carrierMinusCode)++-- | FromObservation produces L1 and L2 SBP packed observation(s).+--+class FromObservation a where+  l1PackedObsContents :: a -> Maybe PackedObsContent+  l2PackedObsContents :: a -> Maybe PackedObsContent++instance FromObservation Observation1002 where+  l1PackedObsContents o = toGpsL1PackedObsContents (o ^. observation1002_sat) (o ^. observation1002_l1) (o ^. observation1002_l1e)+  l2PackedObsContents _o = Nothing++instance FromObservation Observation1004 where+  l1PackedObsContents o = toGpsL1PackedObsContents (o ^. observation1004_sat) (o ^. observation1004_l1) (o ^. observation1004_l1e)+  l2PackedObsContents o = toGpsL2PackedObsContents (o ^. observation1004_sat) (o ^. observation1004_l1) (o ^. observation1004_l1e) (o ^. observation1004_l2) (o ^. observation1004_l2e)++instance FromObservation Observation1010 where+  l1PackedObsContents o = toGlonassL1PackedObsContents (o ^. observation1010_sat) (o ^. observation1010_l1) (o ^. observation1010_l1e)+  l2PackedObsContents _o = Nothing++instance FromObservation Observation1012 where+  l1PackedObsContents o = toGlonassL1PackedObsContents (o ^. observation1012_sat) (o ^. observation1012_l1) (o ^. observation1012_l1e)+  l2PackedObsContents o = toGlonassL2PackedObsContents (o ^. observation1012_sat) (o ^. observation1012_l1) (o ^. observation1012_l1e) (o ^. observation1012_l2) (o ^. observation1012_l2e)++-- | Convert RTCMv3 observation to SBP packed observation(s).+--+toPackedObsContent :: FromObservation a => [a] -> [PackedObsContent]+toPackedObsContent = concatMap $ (<>) <$> maybeToList . l1PackedObsContents <*> maybeToList . l2PackedObsContents++-- | Derive sender from station.+--+toSender :: Word16 -> Word16+toSender station = station .|. 61440++-- | FromObservations produces GPS time, SBP packed observations, and sender from RTCMv3 observation messages.+--+class FromObservations a where+  gpsTimeNano       :: MonadStore e m => a -> m GpsTimeNano+  packedObsContents :: a -> [PackedObsContent]+  sender            :: a -> Word16++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)++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)++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)++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)++-- | Convert RTCMv3 observations 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'+  where+    maxObs  = (maxSize - hdrSize) `div` obsSize+    maxSize = 255+    hdrSize = 11+    obsSize = 17++-- | 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

@@ -0,0 +1,125 @@+{-# LANGUAGE NoImplicitPrelude #-}++-- |+-- Module:      Data.RTCM3.SBP.Time+-- Copyright:   Copyright (C) 2016 Swift Navigation, Inc.+-- License:     LGPL-3+-- Maintainer:  Swift Navigation <dev@swiftnav.com>+-- Stability:   experimental+-- Portability: portable+--+-- SBP GPS Time helpers.++module Data.RTCM3.SBP.Time+  ( currentGpsTime+  , rolloverTowGpsTime+  , rolloverEpochGpsTime+  ) where++import BasicPrelude+import Control.Lens+import Data.Time+import Data.Time.Calendar.WeekDate+import Data.Word+import SwiftNav.SBP++-- | Beginning of GPS time.+--+gpsEpoch :: Day+gpsEpoch = fromGregorian 1980 1 6++-- | Number of GPS leap seconds.+--+gpsLeapSeconds :: Integer+gpsLeapSeconds = 18++-- | Number of GPS leap milliseconds.+--+gpsLeapMillis :: Integer+gpsLeapMillis = 1000 * gpsLeapSeconds++-- | Hour seconds+--+hourSeconds :: Integer+hourSeconds = 60 * 60++-- | Hour milliseconds+--+hourMillis :: Integer+hourMillis = 1000 * hourSeconds++-- | Day seconds+--+daySeconds :: Integer+daySeconds = 24 * hourSeconds++-- | Day milliseconds+--+dayMillis :: Integer+dayMillis = 1000 * daySeconds++-- | Number of seconds in a week.+--+weekSeconds :: Integer+weekSeconds = 7 * daySeconds++-- | Number of milliseconds in a week.+--+weekMillis :: Integer+weekMillis = 1000 * weekSeconds++-- | Produce GPS week number from a GPS UTC time.+--+toWn :: UTCTime -> Word16+toWn t = fromIntegral weeks+  where+    days  = diffDays (utctDay t) gpsEpoch+    weeks = days `div` 7++-- | 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)++-- | Get current GPS time.+--+currentGpsTime :: MonadIO m => m GpsTimeNano+currentGpsTime = do+  t <- liftIO $ addUTCTime (fromIntegral gpsLeapSeconds) <$> getCurrentTime+  return $ GpsTimeNano (toTow t) 0 (toWn t)++-- | Update GPS time based on GPS time of week, handling week rollover.+--+rolloverTowGpsTime :: Word32 -> GpsTimeNano -> GpsTimeNano+rolloverTowGpsTime tow t = t & gpsTimeNano_tow .~ tow & rollover+  where+    rollover+      | increment = gpsTimeNano_wn +~ 1+      | decrement = gpsTimeNano_wn +~ -1+      | otherwise = gpsTimeNano_wn +~ 0+    diff      = fromIntegral tow - fromIntegral (t ^. gpsTimeNano_tow)+    increment = diff < -weekMillis `div` 2+    decrement = diff > weekMillis `div` 2++-- | Update GPS time based on GLONASS epoch, handling week rollover.+--+rolloverEpochGpsTime :: Word32 -> GpsTimeNano -> GpsTimeNano+rolloverEpochGpsTime epoch t = rolloverTowGpsTime tow t+  where+    epoch' = fromIntegral epoch - 3 * hourMillis + gpsLeapMillis+    epoch''+      | epoch' < 0 = epoch' + dayMillis+      | otherwise  = epoch'+    dow = fromIntegral (t ^. gpsTimeNano_tow) `div` dayMillis+    tod = fromIntegral (t ^. gpsTimeNano_tow) - dow * dayMillis+    dow'+      | increment = dow + 1 `mod` 7+      | decrement = dow - 1 `mod` 7+      | otherwise = dow+    increment = epoch'' - tod > dayMillis `div` 2+    decrement = tod - epoch'' > dayMillis `div` 2+    tow = fromIntegral $ dow' * dayMillis + epoch''

@@ -19,23 +19,6 @@ import Data.RTCM3 import SwiftNav.SBP -fromMsgObs :: MsgObs -> Msg1002-fromMsgObs _obs = Msg1002-  { _msg1002_header = undefined-  , _msg1002_observations = undefined-  }--fromMsgBasePosEcef :: MsgBasePosEcef -> Msg1005-fromMsgBasePosEcef _basePosEcef = Msg1005-  { _msg1005_reference = undefined-  }--convert :: SBPMsg -> Maybe RTCM3Msg+convert :: Monad m => SBPMsg -> m [RTCM3Msg] convert = \case-  (SBPMsgObs m _sbp) -> Just $-    RTCM3Msg1002 m' $ toRTCM3 m' where-      m' = fromMsgObs m-  (SBPMsgBasePosEcef m _sbp) -> Just $-    RTCM3Msg1005 m' $ toRTCM3 m' where-      m' = fromMsgBasePosEcef m-  _sbpMsg -> Nothing+  _sbpMsg -> return mempty

@@ -57,10 +57,10 @@                     )  isL1 :: PackedObsContent -> Bool-isL1 obs = (obs ^. packedObsContent_sid ^. gnssSignal16_code) == l1CSidCode+isL1 obs = (obs ^. packedObsContent_sid ^. gnssSignal16_code) == 0  isL2 :: PackedObsContent -> Bool-isL2 obs = code == l2CMSidCode || code == l2PSidCode+isL2 obs = code == 1 || code == 6   where     code = obs ^. packedObsContent_sid ^. gnssSignal16_code @@ -71,14 +71,14 @@ cn0 obs = 0.25 * fromIntegral (obs ^. packedObsContent_cn0)  pseudorange :: PackedObsContent -> Double-pseudorange obs = fromIntegral (obs ^. packedObsContent_P) / sbpCMinM+pseudorange obs = fromIntegral (obs ^. packedObsContent_P) / 50  carrierPhase :: PackedObsContent -> Double carrierPhase obs = whole + fraction   where     phase    = obs ^. packedObsContent_L     whole    = fromIntegral (phase ^. carrierPhase_i)-    fraction = fromIntegral (phase ^. carrierPhase_f) / q32Width+    fraction = fromIntegral (phase ^. carrierPhase_f) / 256  assertExpectedBase :: SBPMsg -> (Double, Double, Double) -> Assertion assertExpectedBase (SBPMsgBasePosEcef posEcef _) pos =@@ -86,8 +86,9 @@ assertExpectedBase _                             _   = assertFailure "Invalid message type!"  assertObsHeader :: SBPMsg -> ObservationHeader -> Assertion-assertObsHeader (SBPMsgObs obs _) header =-  assertEqual "Observation header is not equal" header $ obs ^. msgObs_header+assertObsHeader (SBPMsgObs obs _) header = do+  assertEqual "Observation header is not equal" (header ^. observationHeader_n_obs) $ obs ^. msgObs_header ^. observationHeader_n_obs+  assertEqual "Observation header is not equal" (header ^. observationHeader_t ^. gpsTimeNano_tow) $ obs ^. msgObs_header ^. observationHeader_t ^. gpsTimeNano_tow assertObsHeader _                 _      = assertFailure "Invalid message type!"  assertObs :: HashMap Word8 (Double, Double, Double)@@ -121,8 +122,8 @@       ctol_L2 = 0.004 -- carrier phase / L2 wavelength = 0.001m/0.250m cycles   forM_ (obs' ^. msgObs_obs) $ \packed ->     case () of-      _ |  isL1 packed -> assertObs testObs_L1 packed ptol ctol_L1 l1CSidCode-        |  isL2 packed -> assertObs testObs_L2 packed ptol ctol_L2 l2PSidCode+      _ |  isL1 packed -> assertObs testObs_L1 packed ptol ctol_L1 0+        |  isL2 packed -> assertObs testObs_L2 packed ptol ctol_L2 6         |  otherwise   -> assertFailure "Not L1 or L2!" assertMsgObs _                  =  assertFailure "Invalid message type!" @@ -334,23 +335,6 @@        toWn 57593 @?= 1907     ] -testUpdateGpsTimeNano :: TestTree-testUpdateGpsTimeNano =-  testGroup "Update GPS Time"-    [ testCase "old TOW < new TOW" $ do-        let old = GpsTimeNano 1 0 10-            new = updateGpsTimeNano 2 old-        new ^. gpsTimeNano_wn @?= 10-    , testCase "old TOW = new TOW" $ do-        let old = GpsTimeNano 1 0 10-            new = updateGpsTimeNano 1 old-        new ^. gpsTimeNano_wn @?= 10-    , testCase "old TOW > new TOW" $ do-        let old = GpsTimeNano 1 0 10-            new = updateGpsTimeNano 0 old-        new ^. gpsTimeNano_wn @?= 11-    ]- testValidateIodcIode :: TestTree testValidateIodcIode =   testGroup "Validate IODC/IODE"@@ -401,7 +385,6 @@     [ testMsg1004     , testMsg1019     , testToWn-    , testUpdateGpsTimeNano     , testValidateIodcIode     , testUriToUra     ]