diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -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 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 @@ -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 diff --git a/src/Data/RTCM3/SBP/Observations.hs b/src/Data/RTCM3/SBP/Observations.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/SBP/Observations.hs @@ -0,0 +1,348 @@ +{-# LANGUAGE NoImplicitPrelude #-} + +-- | +-- Module: Data.RTCM3.SBP.Observations +-- Copyright: Copyright (C) 2016 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Swift Navigation +-- 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 diff --git a/src/Data/RTCM3/SBP/Time.hs b/src/Data/RTCM3/SBP/Time.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/SBP/Time.hs @@ -0,0 +1,125 @@ +{-# LANGUAGE NoImplicitPrelude #-} + +-- | +-- Module: Data.RTCM3.SBP.Time +-- Copyright: Copyright (C) 2016 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Swift Navigation +-- 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'' diff --git a/src/SwiftNav/SBP/RTCM3.hs b/src/SwiftNav/SBP/RTCM3.hs --- a/src/SwiftNav/SBP/RTCM3.hs +++ b/src/SwiftNav/SBP/RTCM3.hs @@ -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 diff --git a/test/Test/Data/RTCM3/SBP.hs b/test/Test/Data/RTCM3/SBP.hs --- a/test/Test/Data/RTCM3/SBP.hs +++ b/test/Test/Data/RTCM3/SBP.hs @@ -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 ]