gnss-converters 0.3.9 → 0.3.10
raw patch · 2 files changed
+131/−1 lines, 2 files
Files
- gnss-converters.cabal +2/−1
- src/Data/RTCM3/SBP/Ephemerides.hs +129/−0
gnss-converters.cabal view
@@ -1,5 +1,5 @@ name: gnss-converters-version: 0.3.9+version: 0.3.10 cabal-version: >=1.22 build-type: Simple license: BSD3@@ -19,6 +19,7 @@ library exposed-modules: Data.RTCM3.SBP+ Data.RTCM3.SBP.Ephemerides Data.RTCM3.SBP.Observations Data.RTCM3.SBP.Positions Data.RTCM3.SBP.Time
+ src/Data/RTCM3/SBP/Ephemerides.hs view
@@ -0,0 +1,129 @@+{-# LANGUAGE NoImplicitPrelude #-}++-- |+-- Module: Data.RTCM3.SBP.Ephemerides+-- Copyright: Copyright (C) 2016 Swift Navigation, Inc.+-- License: LGPL-3+-- Maintainer: Swift Navigation <dev@swiftnav.com>+-- Stability: experimental+-- Portability: portable+--+-- RTCMv3 to SBP Ephemerides Conversions.++module Data.RTCM3.SBP.Ephemerides+ ( converter+ ) where++import BasicPrelude+import Control.Lens+import Data.Bits+import Data.Conduit+import Data.RTCM3+import Data.RTCM3.SBP.Time+import Data.Word+import SwiftNav.SBP++--------------------------------------------------------------------------------+-- GNSS RTCM observation reconstruction utilities++-- | 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 :: MonadIO m => Word16 -> Word16 -> m GpsTimeSec+toGpsTimeSec wn tow = do+ wn' <- view gpsTimeNano_wn <$> currentGpsTime+ return GpsTimeSec+ { _gpsTimeSec_tow = 16 * fromIntegral tow+ , _gpsTimeSec_wn = wn' `shiftR` 10 `shiftL` 10 + 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.+validateIodcIode :: Word16 -> Word8 -> Word8+validateIodcIode iodc iode =+ bool 0 1 $ iodc' == iode+ where+ iodc' = fromIntegral $ iodc `shiftL` 8 `shiftR` 8++-- | 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.+uriToUra :: Double -> 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)+ | otherwise = -1++-- | Decode SBP 'fitInterval' from RTCM/GPS 'fitIntervalFlag' and IODC.+-- Implementation adapted from libswiftnav/ephemeris.c.+fitInterval :: Bool -> Word16 -> Word32+fitInterval 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++-- | Construct an EphemerisCommonContent from an RTCM 1019 message.+toGpsEphemerisCommonContent :: MonadIO m => Msg1019 -> m EphemerisCommonContent+toGpsEphemerisCommonContent m = do+ toe <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toe)+ return EphemerisCommonContent+ { _ephemerisCommonContent_sid = GnssSignal16+ { _gnssSignal16_sat = m ^. msg1019_header ^. gpsEphemerisHeader_sat+ , _gnssSignal16_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_valid = validateIodcIode (m ^. msg1019_ephemeris ^. gpsEphemeris_iodc) (m ^. msg1019_ephemeris ^. gpsEphemeris_iode)+ , _ephemerisCommonContent_health_bits = m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth+ }++--------------------------------------------------------------------------------+-- 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 :: MonadIO m => Msg1019 -> Conduit i m [SBPMsg]+converter 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+ }+ yield [SBPMsgEphemerisGps m' $ toSBP m' 61440]