gnss-converters-0.3.40: src/Data/RTCM3/SBP/MSM.hs
{-# LANGUAGE NoImplicitPrelude #-}
-- |
-- Module: Data.RTCM3.SBP.MSM
-- Copyright: Copyright (C) 2017 Swift Navigation, Inc.
-- License: LGPL-3
-- Maintainer: Swift Navigation <dev@swiftnav.com>
-- Stability: experimental
-- Portability: portable
--
-- RTCMv3 MSM to SBP Observations.
module Data.RTCM3.SBP.MSM
( converter
) where
import BasicPrelude hiding (null)
import Control.Lens
import Data.Bits
import Data.Conduit
import Data.IORef
import Data.List.Extra hiding (filter, map, null, zip)
import Data.RTCM3
import Data.RTCM3.SBP.Time
import Data.RTCM3.SBP.Types
import Data.Vector hiding (filter, length, map, zip)
import Data.Word
import SwiftNav.SBP
-- | Derive sender from station.
--
toSender :: Word16 -> Word16
toSender station = station .|. 61568
masks :: Bits a => Int -> a -> [b] -> [b]
masks n a bs = fst <$> filter snd (zip bs $ testBit a <$> [n-1,n-2..0])
mask :: FiniteBits a => a -> [Word8]
mask n = masks (finiteBitSize n) n [1..]
toCells :: MsmHeader -> [((Int, Word8), (Int, Word8))]
toCells hdr =
map (\(i, (sat, sig)) -> (sat, (i, sig))) $
zip [0..] $ masks (popCount (hdr ^. msmHeader_satelliteMask) * popCount (hdr ^. msmHeader_signalMask)) (hdr ^. msmHeader_cellMask) $ do
sat <- zip [0..] $ mask (hdr ^. msmHeader_satelliteMask)
sig <- mask (hdr ^. msmHeader_signalMask)
pure (sat, sig)
-- | Max GPS satellite number.
--
gpsMaxSat :: Word8
gpsMaxSat = 32
toGpsSat :: Word8 -> Maybe Word8
toGpsSat sat
| sat < 1 = Nothing
| sat > gpsMaxSat = Nothing
| otherwise = Just sat
toGpsCode :: Word8 -> Maybe Word8
toGpsCode sig
| sig == 2 = Just 0
| sig == 3 = Just 5
| sig == 9 = Just 6
| sig == 15 = Just 1
| sig == 16 = Just 7
-- | sig == 17 = Just 8
-- | sig == 22 = Just 9
-- | sig == 23 = Just 10
-- | sig == 24 = Just 11
| otherwise = Nothing
toGpsSignal :: Word8 -> Word8 -> Maybe GnssSignal
toGpsSignal sat sig = do
sat' <- toGpsSat sat
code <- toGpsCode sig
Just $ GnssSignal sat' code
-- | Convert to SBP pseudorange.
--
toP :: Double -> Word32
toP p = round $ 50 * p
-- | 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
-- | GPS pseudorange unit.
--
gpsPseudorange :: Double
gpsPseudorange = 299792.458
toD :: Doppler
toD = Doppler 0 0
toGpsFrequency :: Word8 -> Maybe Double
toGpsFrequency sig
| sig == 2 = Just 1.57542e9
| sig == 3 = Just 1.57542e9
| sig == 9 = Just 1.22760e9
| sig == 15 = Just 1.22760e9
| sig == 16 = Just 1.22760e9
| sig == 17 = Just 1.22760e9
| sig == 22 = Just (115 * 10.23e6)
| sig == 23 = Just (115 * 10.23e6)
| sig == 24 = Just (115 * 10.23e6)
| otherwise = Nothing
-- | Convert from RTCMv3 lock indicator.
--
lock :: Word16 -> Word32
lock t
| t < 64 = 1000 * fromIntegral t
| t < 96 = 1000 * fromIntegral t * 2 - 64
| t < 128 = 1000 * fromIntegral t * 4 - 256
| t < 160 = 1000 * fromIntegral t * 8 - 768
| t < 192 = 1000 * fromIntegral t * 16 - 2048
| t < 224 = 1000 * fromIntegral t * 32 - 5120
| t < 256 = 1000 * fromIntegral t * 64 - 12288
| t < 288 = 1000 * fromIntegral t * 128 - 28672
| t < 320 = 1000 * fromIntegral t * 256 - 65536
| t < 352 = 1000 * fromIntegral t * 512 - 147456
| t < 384 = 1000 * fromIntegral t * 1024 - 327680
| t < 416 = 1000 * fromIntegral t * 2048 - 720896
| t < 448 = 1000 * fromIntegral t * 4096 - 1572864
| t < 480 = 1000 * fromIntegral t * 8192 - 3407872
| t < 512 = 1000 * fromIntegral t * 16384 - 7340032
| t < 544 = 1000 * fromIntegral t * 32768 - 15728640
| t < 576 = 1000 * fromIntegral t * 65536 - 33554432
| t < 608 = 1000 * fromIntegral t * 131072 - 71303168
| t < 640 = 1000 * fromIntegral t * 262144 - 150994944
| t < 672 = 1000 * fromIntegral t * 524288 - 318767104
| t < 704 = 1000 * fromIntegral t * 1048576 - 671088640
| otherwise = 1000 * 67108864
-- | 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
toPackedObsContent1074 :: Msm46SatelliteData -> Msm4SignalData -> (Int, Word8) -> (Int, Word8) -> Maybe PackedObsContent
toPackedObsContent1074 satData sigData (satIndex, sat) (sigIndex, sig)
| no = Nothing
| otherwise = do
sid <- toGpsSignal sat sig
freq <- toGpsFrequency sig
Just PackedObsContent
{ _packedObsContent_P = toP (roughPseudorange + finePseudorange)
, _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0))
, _packedObsContent_D = toD
, _packedObsContent_cn0 = ((sigData ^. msm4SignalData_cnrs) !! sigIndex) * 4
, _packedObsContent_lock = (sigData ^. msm4SignalData_lockTimes) !! sigIndex
, _packedObsContent_sid = sid
, _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved
}
where
no = ((satData ^. msm46SatelliteData_ranges) !! satIndex) == 255 ||
((sigData ^. msm4SignalData_pseudoranges) !! sigIndex) == 16384 ||
((sigData ^. msm4SignalData_phaseranges) !! sigIndex) == 2097152
pseudorangeValid = 1
phaseValid = 2
halfCycleResolved = bool 4 0 $ (sigData ^. msm4SignalData_halfCycles) !! sigIndex
roughPseudorange = fromIntegral ((satData ^. msm46SatelliteData_ranges) !! satIndex) * gpsPseudorange +
fromIntegral ((satData ^. msm46SatelliteData_rangesModulo) !! satIndex) * gpsPseudorange / 1024
finePseudorange = fromIntegral ((sigData ^. msm4SignalData_pseudoranges) !! sigIndex) * gpsPseudorange * (2 ** (-24))
finePhaserange = fromIntegral ((sigData ^. msm4SignalData_phaseranges) !! sigIndex) * gpsPseudorange * (2 ** (-29))
toPackedObsContent1077 :: Msm57SatelliteData -> Msm7SignalData -> (Int, Word8) -> (Int, Word8) -> Maybe PackedObsContent
toPackedObsContent1077 satData sigData (satIndex, sat) (sigIndex, sig)
| no = Nothing
| otherwise = do
sid <- toGpsSignal sat sig
freq <- toGpsFrequency sig
Just PackedObsContent
{ _packedObsContent_P = toP (roughPseudorange + finePseudorange)
, _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0))
, _packedObsContent_D = toD
, _packedObsContent_cn0 = round (fromIntegral ((sigData ^. msm7SignalData_cnrs) !! sigIndex) * ((2 :: Double) ** (-4)) * 4)
, _packedObsContent_lock = toLock $ lock ((sigData ^. msm7SignalData_lockTimes) !! sigIndex)
, _packedObsContent_sid = sid
, _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved
}
where
no = ((satData ^. msm57SatelliteData_ranges) !! satIndex) == 255 ||
((sigData ^. msm7SignalData_pseudoranges) !! sigIndex) == 524288 ||
((sigData ^. msm7SignalData_phaseranges) !! sigIndex) == 8388608
pseudorangeValid = 1
phaseValid = 2
halfCycleResolved = bool 4 0 $ (sigData ^. msm7SignalData_halfCycles) !! sigIndex
roughPseudorange = fromIntegral ((satData ^. msm57SatelliteData_ranges) !! satIndex) * gpsPseudorange +
fromIntegral ((satData ^. msm57SatelliteData_rangesModulo) !! satIndex) * gpsPseudorange / 1024
finePseudorange = fromIntegral ((sigData ^. msm7SignalData_pseudoranges) !! sigIndex) * gpsPseudorange * (2 ** (-29))
finePhaserange = fromIntegral ((sigData ^. msm7SignalData_phaseranges) !! sigIndex) * gpsPseudorange * (2 ** (-31))
class FromObservations a where
gpsTime :: MonadStore e m => a -> m (GpsTime, GpsTime)
packedObsContents :: a -> [PackedObsContent]
sender :: a -> Word16
multiple :: a -> Bool
instance FromObservations Msg1074 where
gpsTime m = toGpsTime (m ^. msg1074_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1074_header . msmHeader_epoch)
packedObsContents m = catMaybes $ uncurry (toPackedObsContent1074 (m ^. msg1074_satelliteData) (m ^. msg1074_signalData)) <$> toCells (m ^. msg1074_header)
sender = toSender . view (msg1074_header . msmHeader_station)
multiple = view (msg1074_header . msmHeader_multiple)
instance FromObservations Msg1075 where
gpsTime m = toGpsTime (m ^. msg1075_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1075_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1075_header . msmHeader_station)
multiple = view (msg1075_header . msmHeader_multiple)
instance FromObservations Msg1076 where
gpsTime m = toGpsTime (m ^. msg1076_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1076_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1076_header . msmHeader_station)
multiple = view (msg1076_header . msmHeader_multiple)
instance FromObservations Msg1077 where
gpsTime m = toGpsTime (m ^. msg1077_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1077_header . msmHeader_epoch)
packedObsContents m = catMaybes $ uncurry (toPackedObsContent1077 (m ^. msg1077_satelliteData) (m ^. msg1077_signalData)) <$> toCells (m ^. msg1077_header)
sender = toSender . view (msg1077_header . msmHeader_station)
multiple = view (msg1077_header . msmHeader_multiple)
instance FromObservations Msg1084 where
gpsTime m = toGpsTime (m ^. msg1084_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1084_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1084_header . msmHeader_station)
multiple = view (msg1084_header . msmHeader_multiple)
instance FromObservations Msg1085 where
gpsTime m = toGpsTime (m ^. msg1085_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1085_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1085_header . msmHeader_station)
multiple = view (msg1085_header . msmHeader_multiple)
instance FromObservations Msg1086 where
gpsTime m = toGpsTime (m ^. msg1086_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1086_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1086_header . msmHeader_station)
multiple = view (msg1086_header . msmHeader_multiple)
instance FromObservations Msg1087 where
gpsTime m = toGpsTime (m ^. msg1087_header . msmHeader_station) $ glonassRolloverGpsTime' (m ^. msg1087_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1087_header . msmHeader_station)
multiple = view (msg1087_header . msmHeader_multiple)
instance FromObservations Msg1094 where
gpsTime m = toGpsTime (m ^. msg1094_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1094_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1094_header . msmHeader_station)
multiple = view (msg1094_header . msmHeader_multiple)
instance FromObservations Msg1095 where
gpsTime m = toGpsTime (m ^. msg1095_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1095_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1095_header . msmHeader_station)
multiple = view (msg1095_header . msmHeader_multiple)
instance FromObservations Msg1096 where
gpsTime m = toGpsTime (m ^. msg1096_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1096_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1096_header . msmHeader_station)
multiple = view (msg1096_header . msmHeader_multiple)
instance FromObservations Msg1097 where
gpsTime m = toGpsTime (m ^. msg1097_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1097_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1097_header . msmHeader_station)
multiple = view (msg1097_header . msmHeader_multiple)
instance FromObservations Msg1104 where
gpsTime m = toGpsTime (m ^. msg1104_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1104_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1104_header . msmHeader_station)
multiple = view (msg1104_header . msmHeader_multiple)
instance FromObservations Msg1105 where
gpsTime m = toGpsTime (m ^. msg1105_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1105_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1105_header . msmHeader_station)
multiple = view (msg1105_header . msmHeader_multiple)
instance FromObservations Msg1106 where
gpsTime m = toGpsTime (m ^. msg1106_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1106_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1106_header . msmHeader_station)
multiple = view (msg1106_header . msmHeader_multiple)
instance FromObservations Msg1107 where
gpsTime m = toGpsTime (m ^. msg1107_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1107_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1107_header . msmHeader_station)
multiple = view (msg1107_header . msmHeader_multiple)
instance FromObservations Msg1114 where
gpsTime m = toGpsTime (m ^. msg1114_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1114_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1114_header . msmHeader_station)
multiple = view (msg1114_header . msmHeader_multiple)
instance FromObservations Msg1115 where
gpsTime m = toGpsTime (m ^. msg1115_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1115_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1115_header . msmHeader_station)
multiple = view (msg1115_header . msmHeader_multiple)
instance FromObservations Msg1116 where
gpsTime m = toGpsTime (m ^. msg1116_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1116_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1116_header . msmHeader_station)
multiple = view (msg1116_header . msmHeader_multiple)
instance FromObservations Msg1117 where
gpsTime m = toGpsTime (m ^. msg1117_header . msmHeader_station) $ gpsRolloverGpsTime (m ^. msg1117_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1117_header . msmHeader_station)
multiple = view (msg1117_header . msmHeader_multiple)
instance FromObservations Msg1124 where
gpsTime m = toGpsTime (m ^. msg1124_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1124_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1124_header . msmHeader_station)
multiple = view (msg1124_header . msmHeader_multiple)
instance FromObservations Msg1125 where
gpsTime m = toGpsTime (m ^. msg1125_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1125_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1125_header . msmHeader_station)
multiple = view (msg1125_header . msmHeader_multiple)
instance FromObservations Msg1126 where
gpsTime m = toGpsTime (m ^. msg1126_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1126_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1126_header . msmHeader_station)
multiple = view (msg1126_header . msmHeader_multiple)
instance FromObservations Msg1127 where
gpsTime m = toGpsTime (m ^. msg1127_header . msmHeader_station) $ beidouRolloverGpsTime (m ^. msg1127_header . msmHeader_epoch)
packedObsContents = const mempty
sender = toSender . view (msg1127_header . msmHeader_station)
multiple = view (msg1127_header . msmHeader_multiple)
-- | Convert RTCMv3 observation(s) to SBP observations in chunks.
--
toMsgObs :: Applicative f => GpsTime -> [PackedObsContent] -> Word16 -> f [SBPMsg]
toMsgObs t obs s = do
let chunks = chunksOf maxObs obs
ifor chunks $ \i obs' -> do
let n = length chunks `shiftL` 4 .|. i
m = MsgObs (ObservationHeader t (fromIntegral n)) obs'
pure $ SBPMsgObs m $ toSBP m s
where
maxObs = (maxSize - hdrSize) `div` obsSize
maxSize = 255
hdrSize = 11
obsSize = 17
-- | Convert RTCMv3 observation message to SBP observations message(s).
--
converter :: (MonadStore e m, FromObservations a) => a -> Conduit i m [SBPMsg]
converter m = do
(t, t') <- gpsTime m
observations <- view storeObservations
obs <- liftIO $ readIORef observations
when (t' /= t) $
unless (null obs) $ do
liftIO $ writeIORef observations mempty
ms <- toMsgObs t (toList obs) $ sender m
yield ms
let obs' = fromList $ packedObsContents m
if multiple m then liftIO $ modifyIORef' observations (obs' <>) else do
obs'' <- liftIO $ readIORef observations
unless (null obs'') $
liftIO $ writeIORef observations mempty
let obs''' = obs' <> obs''
unless (null obs''') $ do
ms <- toMsgObs t' (toList obs''') $ sender m
yield ms