diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -1,112 +1,104 @@ -name: gnss-converters -version: 0.3.25 -cabal-version: >=1.22 -build-type: Simple -license: BSD3 -copyright: Copyright (C) 2016, 2017 Swift Navigation, Inc. -maintainer: Swift Navigation -homepage: http://github.com/swift-nav/gnss-converters -synopsis: GNSS Converters. -description: - Haskell bindings for GNSS converters. -category: Network -author: Mark Fine , Joshua Gross +name: gnss-converters +version: 0.3.28 +synopsis: GNSS Converters. +description: Haskell bindings for GNSS converters. +homepage: http://github.com/swift-nav/gnss-converters +license: BSD3 +author: Mark Fine , Joshua Gross +maintainer: Swift Navigation +copyright: Copyright (C) 2016, 2017 Swift Navigation, Inc. +category: Network +build-type: Simple +cabal-version: >= 1.22 source-repository head - type: git - location: https://github.com/swift-nav/gnss-converters + type: git + location: https://github.com/swift-nav/gnss-converters library - exposed-modules: - Data.RTCM3.Replay - Data.RTCM3.SBP - Data.RTCM3.SBP.Biases - Data.RTCM3.SBP.Ephemerides - Data.RTCM3.SBP.Observations - Data.RTCM3.SBP.Positions - Data.RTCM3.SBP.Time - Data.RTCM3.SBP.Types - SwiftNav.SBP.RTCM3 - 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, - monad-control >=1.0.1.0, - mtl >=2.2.1, - resourcet >=1.1.9, - rtcm >=0.2.11, - sbp >=2.3.2, - time >=1.5.0.1, - transformers-base >=0.4.4, - vector >=0.11.0.0 - default-language: Haskell2010 - hs-source-dirs: src - ghc-options: -Wall + hs-source-dirs: src + exposed-modules: Data.RTCM3.Replay + , Data.RTCM3.SBP + , Data.RTCM3.SBP.Biases + , Data.RTCM3.SBP.Ephemerides + , Data.RTCM3.SBP.Observations + , Data.RTCM3.SBP.Positions + , Data.RTCM3.SBP.Time + , Data.RTCM3.SBP.Types + , SwiftNav.SBP.RTCM3 + ghc-options: -Wall + build-depends: base >= 4.8 && < 5 + , basic-prelude + , conduit + , exceptions + , extra + , lens + , monad-control + , mtl + , resourcet + , rtcm + , sbp + , time + , transformers-base + , vector + default-language: Haskell2010 executable sbp2rtcm3 - main-is: SBP2RTCM3.hs - build-depends: - base >=4.8.2.0, - basic-prelude >=0.5.2, - binary-conduit >=1.2.4.1, - conduit >=1.2.10, - conduit-extra >=1.1.16, - gnss-converters - default-language: Haskell2010 - hs-source-dirs: main - ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + hs-source-dirs: main + main-is: SBP2RTCM3.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , binary-conduit + , conduit + , conduit-extra + , gnss-converters + default-language: Haskell2010 executable rtcm32sbp - main-is: RTCM32SBP.hs - build-depends: - base >=4.8.2.0, - basic-prelude >=0.5.2, - binary-conduit >=1.2.4.1, - conduit >=1.2.10, - conduit-extra >=1.1.16, - gnss-converters - default-language: Haskell2010 - hs-source-dirs: main - ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + hs-source-dirs: main + main-is: RTCM32SBP.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , binary-conduit + , conduit + , conduit-extra + , gnss-converters + default-language: Haskell2010 executable rtcm32rtcm3 - main-is: RTCM32RTCM3.hs - build-depends: - base >=4.8.2.0, - basic-prelude >=0.5.2, - binary-conduit >=1.2.4.1, - conduit >=1.2.10, - conduit-extra >=1.1.16, - gnss-converters - default-language: Haskell2010 - hs-source-dirs: main - ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + hs-source-dirs: main + main-is: RTCM32RTCM3.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , binary-conduit + , conduit + , conduit-extra + , gnss-converters + default-language: Haskell2010 test-suite test - type: exitcode-stdio-1.0 - main-is: Test.hs - build-depends: - aeson >=0.11.3.0, - aeson-pretty >=0.7.2, - base >=4.8.2.0, - basic-prelude >=0.5.2, - binary-conduit >=1.2.4.1, - bytestring >=0.10.6.0, - conduit >=1.2.10, - conduit-extra >=1.1.16, - gnss-converters, - lens >=4.13, - sbp >=2.3.2, - tasty >=0.11.2.1, - tasty-golden >=2.3.1.1, - tasty-hunit >=0.9.2, - time >=1.5.0.1 - default-language: Haskell2010 - hs-source-dirs: test - other-modules: - Test.Data.RTCM3.SBP - Test.Data.RTCM3.SBP.Time - ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + type: exitcode-stdio-1.0 + hs-source-dirs: test + main-is: Test.hs + other-modules: Test.Data.RTCM3.SBP + , Test.Data.RTCM3.SBP.Time + build-depends: aeson + , aeson-pretty + , base + , basic-prelude + , binary-conduit + , bytestring + , conduit + , conduit-extra + , gnss-converters + , lens + , sbp + , tasty + , tasty-golden + , tasty-hunit + , time + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + default-language: Haskell2010 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 @@ -41,7 +41,8 @@ (RTCM3Msg1012 m _rtcm3) -> Observations.converter m (RTCM3Msg1005 m _rtcm3) -> Positions.converter m (RTCM3Msg1006 m _rtcm3) -> Positions.converter m - (RTCM3Msg1019 m _rtcm3) -> Ephemerides.converter m + (RTCM3Msg1019 m _rtcm3) -> Ephemerides.gpsConverter m + (RTCM3Msg1020 m _rtcm3) -> Ephemerides.glonassConverter m (RTCM3Msg1230 m _rtcm3) -> Biases.converter m _rtcm3Msg -> mempty diff --git a/src/Data/RTCM3/SBP/Ephemerides.hs b/src/Data/RTCM3/SBP/Ephemerides.hs --- a/src/Data/RTCM3/SBP/Ephemerides.hs +++ b/src/Data/RTCM3/SBP/Ephemerides.hs @@ -11,7 +11,8 @@ -- RTCMv3 to SBP Ephemerides Conversions. module Data.RTCM3.SBP.Ephemerides - ( converter + ( gpsConverter + , glonassConverter ) where import BasicPrelude @@ -19,6 +20,7 @@ import Data.Bits import Data.Conduit import Data.RTCM3 +import Data.RTCM3.SBP.Time import Data.RTCM3.SBP.Types import Data.Word import SwiftNav.SBP @@ -38,6 +40,22 @@ , _gpsTimeSec_wn = wn' `shiftR` 10 `shiftL` 10 + wn } +-- | Produce GPS time from GLONASS RTCM time. +-- +toGlonassTimeSec :: MonadStore e m => Word8 -> m GpsTimeSec +toGlonassTimeSec epoch = do + time <- view storeCurrentGpsTime + t <- liftIO time + let epoch' = fromIntegral epoch * 15 * minuteMillis - 3 * hourMillis + gpsLeapMillis + epoch'' + | epoch' < 0 = epoch' + dayMillis + | otherwise = epoch' + dow = fromIntegral (t ^. gpsTime_tow) `div` dayMillis + pure GpsTimeSec + { _gpsTimeSec_tow = fromIntegral $ (dow * dayMillis + epoch'') `div` 1000 + , _gpsTimeSec_wn = t ^. gpsTime_wn + } + -- | 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. @@ -51,21 +69,41 @@ -- 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. -uriToUra :: Double -> Double +uriToUra :: Word8 -> Double uriToUra 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) + | uri <= 6 = 2 ** (1 + (fromIntegral uri / 2)) + | uri > 6 && uri < 15 = 2 ** (fromIntegral uri - 2) | otherwise = -1 +-- | Lookup Table 4.4 from GLONASS ICD: convert F_T word to accuracy in meters +-- +ftToUra :: Word8 -> Double +ftToUra ft + | ft == 0 = 1 + | ft == 1 = 2 + | ft == 2 = 2.5 + | ft == 3 = 4 + | ft == 4 = 5 + | ft == 5 = 7 + | ft == 6 = 10 + | ft == 7 = 12 + | ft == 8 = 14 + | ft == 9 = 16 + | ft == 10 = 32 + | ft == 11 = 64 + | ft == 12 = 128 + | ft == 13 = 256 + | ft == 14 = 512 + | otherwise = -1 + -- | Decode SBP 'fitInterval' from RTCM/GPS 'fitIntervalFlag' and IODC. -- Implementation adapted from libswiftnav/ephemeris.c. -fitInterval :: Bool -> Word16 -> Word32 -fitInterval fitInt iodc +gpsFitInterval :: Bool -> Word16 -> Word32 +gpsFitInterval fitInt iodc | not fitInt = 4 * 60 * 60 | iodc >= 240 && iodc <= 247 = 8 * 60 * 60 | (iodc >= 248 && iodc <= 255) || iodc == 496 = 14 * 60 * 60 @@ -75,7 +113,18 @@ | iodc == 757 = 98 * 60 * 60 | otherwise = 6 * 60 * 60 +-- | Convert GLONASS fit interval - Table 4.3 from GLONASS ICD. +-- +glonassFitInterval :: Word8 -> Word32 +glonassFitInterval fi + | fi == 0 = (60 + 10) * 60 + | fi == 1 = (30 + 10) * 60 + | fi == 2 = (45 + 10) * 60 + | fi == 3 = (60 + 10) * 60 + | otherwise = (60 + 10) * 60 + -- | Construct an EphemerisCommonContent from an RTCM 1019 message. +-- toGpsEphemerisCommonContent :: MonadStore e m => Msg1019 -> m EphemerisCommonContent toGpsEphemerisCommonContent m = do toe <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toe) @@ -85,46 +134,96 @@ , _gnssSignal_code = 0 -- there is an L2P status flag in msg 1019, but I don't think that applies } , _ephemerisCommonContent_toe = toe - , _ephemerisCommonContent_ura = uriToUra (fromIntegral $ m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth) - , _ephemerisCommonContent_fit_interval = fitInterval (m ^. msg1019_ephemeris ^. gpsEphemeris_fitInterval) (m ^. msg1019_ephemeris ^. gpsEphemeris_iodc) + , _ephemerisCommonContent_ura = uriToUra (m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth) + , _ephemerisCommonContent_fit_interval = gpsFitInterval (m ^. msg1019_ephemeris ^. gpsEphemeris_fitInterval) (m ^. msg1019_ephemeris ^. gpsEphemeris_iodc) , _ephemerisCommonContent_valid = validateIodcIode (m ^. msg1019_ephemeris ^. gpsEphemeris_iodc) (m ^. msg1019_ephemeris ^. gpsEphemeris_iode) , _ephemerisCommonContent_health_bits = m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth } +-- | Construct an EphemerisCommonContent from an RTCM 1020 message. +-- +toGlonassEphemerisCommonContent :: MonadStore e m => Msg1020 -> m EphemerisCommonContent +toGlonassEphemerisCommonContent m = do + toe <- toGlonassTimeSec (m ^. msg1020_ephemeris ^. glonassEphemeris_tb) + pure EphemerisCommonContent + { _ephemerisCommonContent_sid = GnssSignal + { _gnssSignal_sat = m ^. msg1020_header ^. glonassEphemerisHeader_sat + , _gnssSignal_code = 3 + } + , _ephemerisCommonContent_toe = toe + , _ephemerisCommonContent_ura = ftToUra (m ^. msg1020_ephemeris ^. glonassEphemeris_mft) + , _ephemerisCommonContent_fit_interval = glonassFitInterval (m ^. msg1020_ephemeris ^. glonassEphemeris_p1) + , _ephemerisCommonContent_valid = 1 + , _ephemerisCommonContent_health_bits = bool 0 1 $ (m ^. msg1020_ephemeris ^. glonassEphemeris_mln5) || (m ^. msg1020_ephemeris ^. glonassEphemeris_mi3) + } + +(##) :: (Floating a1, Integral a2) => a1 -> a2 -> a1 +(##) p x = (2 ** p) * fromIntegral x + -------------------------------------------------------------------------------- -- RTCM to SBP conversion utilities: RTCM Msgs. 1002 (L1 RTK), 1004 (L1+L2 RTK), -- 1005 (antenna position), 1006 (antenna position). -- | Convert an RTCM 1019 GPS ephemeris message into an SBP MsgEphemerisGps. -converter :: MonadStore e m => Msg1019 -> Conduit i m [SBPMsg] -converter m = do +-- +gpsConverter :: MonadStore e m => Msg1019 -> Conduit i m [SBPMsg] +gpsConverter m = do common <- toGpsEphemerisCommonContent m toc <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toc) - let pi' = 3.1415926535898 - p ## x = (2 ** p) * fromIntegral x - m' = MsgEphemerisGps - { _msgEphemerisGps_common = common - , _msgEphemerisGps_tgd = (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_tgd) - , _msgEphemerisGps_c_rs = (-5) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_rs) - , _msgEphemerisGps_c_rc = (-5) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_rc) - , _msgEphemerisGps_c_uc = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_uc) - , _msgEphemerisGps_c_us = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_us) - , _msgEphemerisGps_c_ic = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_ic) - , _msgEphemerisGps_c_is = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_is) - , _msgEphemerisGps_dn = pi' * (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_dn) - , _msgEphemerisGps_m0 = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_m0) - , _msgEphemerisGps_ecc = (-33) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_ecc) - , _msgEphemerisGps_sqrta = (-19) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_sqrta) - , _msgEphemerisGps_omega0 = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_omega0) - , _msgEphemerisGps_omegadot = pi' * (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_omegadot) - , _msgEphemerisGps_w = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_w) - , _msgEphemerisGps_inc = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_i0) - , _msgEphemerisGps_inc_dot = pi' * (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_idot) - , _msgEphemerisGps_af0 = (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_af0) - , _msgEphemerisGps_af1 = (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_af1) - , _msgEphemerisGps_af2 = (-55) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_af2) - , _msgEphemerisGps_iodc = m ^. msg1019_ephemeris ^. gpsEphemeris_iodc - , _msgEphemerisGps_iode = m ^. msg1019_ephemeris ^. gpsEphemeris_iode - , _msgEphemerisGps_toc = toc - } + let pi' = 3.1415926535898 + m' = MsgEphemerisGps + { _msgEphemerisGps_common = common + , _msgEphemerisGps_tgd = (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_tgd) + , _msgEphemerisGps_c_rs = (-5) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_rs) + , _msgEphemerisGps_c_rc = (-5) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_rc) + , _msgEphemerisGps_c_uc = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_uc) + , _msgEphemerisGps_c_us = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_us) + , _msgEphemerisGps_c_ic = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_ic) + , _msgEphemerisGps_c_is = (-29) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_c_is) + , _msgEphemerisGps_dn = pi' * (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_dn) + , _msgEphemerisGps_m0 = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_m0) + , _msgEphemerisGps_ecc = (-33) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_ecc) + , _msgEphemerisGps_sqrta = (-19) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_sqrta) + , _msgEphemerisGps_omega0 = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_omega0) + , _msgEphemerisGps_omegadot = pi' * (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_omegadot) + , _msgEphemerisGps_w = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_w) + , _msgEphemerisGps_inc = pi' * (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_i0) + , _msgEphemerisGps_inc_dot = pi' * (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_idot) + , _msgEphemerisGps_af0 = (-31) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_af0) + , _msgEphemerisGps_af1 = (-43) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_af1) + , _msgEphemerisGps_af2 = (-55) ## (m ^. msg1019_ephemeris ^. gpsEphemeris_af2) + , _msgEphemerisGps_iodc = m ^. msg1019_ephemeris ^. gpsEphemeris_iodc + , _msgEphemerisGps_iode = m ^. msg1019_ephemeris ^. gpsEphemeris_iode + , _msgEphemerisGps_toc = toc + } yield [SBPMsgEphemerisGps m' $ toSBP m' 61440] + +-- | Convert an RTCM 1020 GLONASS ephemeris message into an SBP MsgEphemerisGlo. +-- +glonassConverter :: MonadStore e m => Msg1020 -> Conduit i m [SBPMsg] +glonassConverter m = do + common <- toGlonassEphemerisCommonContent m + let m' = MsgEphemerisGlo + { _msgEphemerisGlo_common = common + , _msgEphemerisGlo_gamma = (-40) ## (m ^. msg1020_ephemeris ^. glonassEphemeris_gammaN) + , _msgEphemerisGlo_tau = (-30) ## (m ^. msg1020_ephemeris ^. glonassEphemeris_tauN) + , _msgEphemerisGlo_d_tau = (-30) ## (m ^. msg1020_ephemeris ^. glonassEphemeris_mdeltatau) + , _msgEphemerisGlo_pos = (* 1000) . ((-11) ##) <$> + [ m ^. msg1020_ephemeris ^. glonassEphemeris_xn + , m ^. msg1020_ephemeris ^. glonassEphemeris_yn + , m ^. msg1020_ephemeris ^. glonassEphemeris_zn + ] + , _msgEphemerisGlo_vel = (* 1000) . ((-20) ##) <$> + [ m ^. msg1020_ephemeris ^. glonassEphemeris_xndot + , m ^. msg1020_ephemeris ^. glonassEphemeris_yndot + , m ^. msg1020_ephemeris ^. glonassEphemeris_zndot + ] + , _msgEphemerisGlo_acc = (* 1000) . ((-30) ##) <$> + [ m ^. msg1020_ephemeris ^. glonassEphemeris_xndotdot + , m ^. msg1020_ephemeris ^. glonassEphemeris_yndotdot + , m ^. msg1020_ephemeris ^. glonassEphemeris_zndotdot + ] + , _msgEphemerisGlo_fcn = (m ^. msg1020_header ^. glonassEphemerisHeader_channel) + 1 + , _msgEphemerisGlo_iod = ((m ^. msg1020_ephemeris ^. glonassEphemeris_tb) * 15 * 60) .&. 127 + } + yield [SBPMsgEphemerisGlo m' $ toSBP m' 61440] 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 @@ -13,6 +13,7 @@ module Data.RTCM3.SBP.Time ( gpsLeapMillis + , minuteMillis , hourMillis , dayMillis , weekMillis @@ -45,6 +46,16 @@ -- gpsLeapMillis :: Integer gpsLeapMillis = 1000 * gpsLeapSeconds + +-- | Minute seconds +-- +minuteSeconds :: Integer +minuteSeconds = 60 + +-- | Minute milliseconds +-- +minuteMillis :: Integer +minuteMillis = 1000 * minuteSeconds -- | Hour seconds --