module Data.RTCM3.SBP
( l1CSidCode
, l2CMSidCode
, l2PSidCode
, sbpCMinM
, q32Width
, 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
twoCM :: Double
twoCM = 0.02
sbpCMinM :: Double
sbpCMinM = 50
lightSpeedMMSEC :: Double
lightSpeedMMSEC = 299792.458
lightSpeedMS :: Double
lightSpeedMS = 299792458.0
l1frequency :: Double
l1frequency = 1.57542e9
l2frequency :: Double
l2frequency = 1.22760e9
maxSats :: Word8
maxSats = 32
q32Width :: Double
q32Width = 256
phaseRangeRes :: Double
phaseRangeRes = 0.0005
l1CSidCode :: Word8
l1CSidCode = 0
l2CMSidCode :: Word8
l2CMSidCode = 1
l1PSidCode :: Word8
l1PSidCode = 5
l2PSidCode :: Word8
l2PSidCode = 6
codeIndicator_L2C :: Word8
codeIndicator_L2C = 0
codeIndicator_L2P :: Word8
codeIndicator_L2P = 1
codeIndicator_L2D :: Word8
codeIndicator_L2D = 2
codeIndicator_L2W :: Word8
codeIndicator_L2W = 3
l2codeToSBPSignalCode :: HashMap Word8 Word8
l2codeToSBPSignalCode = M.fromList
[ (codeIndicator_L2C, l2CMSidCode)
, (codeIndicator_L2P, l2PSidCode)
, (codeIndicator_L2D, l2PSidCode)
, (codeIndicator_L2W, l2PSidCode)
]
maxObsPerMessage :: Int
maxObsPerMessage = (maxPayloadSize headerSize) `div` packedObsSize
where
maxPayloadSize = 255
headerSize = 11
packedObsSize = 17
gpsPi :: Double
gpsPi = 3.1415926535898
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
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
}
updateGpsTimeNano :: Word32 -> GpsTimeNano -> GpsTimeNano
updateGpsTimeNano tow gpsTime =
gpsTime & gpsTimeNano_tow .~ tow &
if tow >= gpsTime ^. gpsTimeNano_tow then id else
gpsTimeNano_wn %~ (+ 1)
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
toGpsTimeSec :: MonadStore e m => Word16 -> Word16 -> m GpsTimeSec
toGpsTimeSec wn tow = do
stateWn <- view storeWn >>= liftIO . readIORef
return $ GpsTimeSec
{ _gpsTimeSec_tow = 16 * fromIntegral tow
, _gpsTimeSec_wn = stateWn `shiftR` 10 `shiftL` 10 + wn
}
mjdEpoch :: Word16
mjdEpoch = 44244
toWn :: Word16 -> Word16
toWn mjd = (mjd mjdEpoch) `div` 7
invalid_L1 :: GpsL1Observation -> Bool
invalid_L1 l1 =
l1 ^. gpsL1Observation_pseudorange == 524288 ||
l1 ^. gpsL1Observation_carrierMinusCode == 524288
invalid_L2 :: GpsL2Observation -> Bool
invalid_L2 l2 =
l2 ^. gpsL2Observation_pseudorangeDifference == 8192 ||
l2 ^. gpsL2Observation_carrierMinusCode == 524288
metricPseudorange :: GpsL1Observation -> GpsL1ExtObservation -> Double
metricPseudorange l1 l1e =
twoCM * fromIntegral (l1 ^. gpsL1Observation_pseudorange) +
lightSpeedMMSEC * fromIntegral (l1e ^. gpsL1ExtObservation_ambiguity)
toP_L1 :: GpsL1Observation -> GpsL1ExtObservation -> Word32
toP_L1 l1 l1e = round $ sbpCMinM * metricPseudorange l1 l1e
toP_L2 :: GpsL1Observation -> GpsL1ExtObservation -> GpsL2Observation -> Word32
toP_L2 l1 l1e l2 = round $ p * sbpCMinM where
p = metricPseudorange l1 l1e +
twoCM * fromIntegral (l2 ^. gpsL2Observation_pseudorangeDifference)
toL_L1 :: GpsL1Observation -> GpsL1ExtObservation -> CarrierPhase
toL_L1 l1 l1e = CarrierPhase
{ _carrierPhase_i = fromIntegral li'
, _carrierPhase_f = fromIntegral lf'
} where
p = metricPseudorange l1 l1e
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
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
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)
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
fromGpsObservationHeader :: MonadStore e m
=> Word8
-> Word8
-> GpsObservationHeader
-> m ObservationHeader
fromGpsObservationHeader totalMsgs n hdr = do
t <- toGpsTimeNano hdr
return ObservationHeader
{ _observationHeader_t = t
, _observationHeader_n_obs = totalMsgs `shiftL` 4 .|. n
}
toL1GnssSignal :: Word8 -> GpsL1Observation -> GnssSignal16
toL1GnssSignal sat l1 =
GnssSignal16
{ _gnssSignal16_sat = sat
, _gnssSignal16_code = if l1 ^. gpsL1Observation_code then l1PSidCode else l1CSidCode
}
fromL1SatelliteObservation :: MonadStore e m
=> Word8
-> GpsL1Observation
-> GpsL1ExtObservation
-> m PackedObsContent
fromL1SatelliteObservation sat l1 l1e = do
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
}
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
}
fromL2SatelliteObservation :: MonadStore e m
=> Word8
-> GpsL1Observation
-> GpsL1ExtObservation
-> GpsL2Observation
-> GpsL2ExtObservation
-> m (Maybe PackedObsContent)
fromL2SatelliteObservation sat l1 l1e l2 l2e =
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
}
validateIodcIode :: Word16 -> Word8 -> Word8
validateIodcIode iodc iode =
if iodc' == iode then 1 else 0
where
iodc' = fromIntegral $ iodc `shiftL` 8 `shiftR` 8
toGpsEphemerisCommonContent :: MonadStore e 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
}
, _ephemerisCommonContent_toe = toe
, _ephemerisCommonContent_ura = gpsUriToUra (fromIntegral $ m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth)
, _ephemerisCommonContent_fit_interval = decodeFitInterval (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
}
chunkToMsgObs :: MonadStore e m
=> GpsObservationHeader
-> Word8
-> Word8
-> [PackedObsContent]
-> m MsgObs
chunkToMsgObs hdr totalMsgs n packed = do
header <- fromGpsObservationHeader totalMsgs n hdr
return MsgObs
{ _msgObs_header = header
, _msgObs_obs = packed
}
toSender :: Word16 -> Word16
toSender = (.|. 0xf000)
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
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
fromObservation1002 :: MonadStore e m => Observation1002 -> m [PackedObsContent]
fromObservation1002 obs = do
obs1 <- fromL1SatelliteObservation sat l1 l1e
return $
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
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
fromObservation1004 :: MonadStore e m => Observation1004 -> m [PackedObsContent]
fromObservation1004 obs = do
obs1 <- fromL1SatelliteObservation sat l1 l1e
obs2 <- fromL2SatelliteObservation sat l1 l1e l2 l2e
return $
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
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
fromMsg1005 :: MonadStore e m => Msg1005 -> m MsgBasePosEcef
fromMsg1005 m =
return MsgBasePosEcef
{ _msgBasePosEcef_x = fromEcefVal $ m ^. msg1005_reference ^. antennaReference_ecef_x
, _msgBasePosEcef_y = fromEcefVal $ m ^. msg1005_reference ^. antennaReference_ecef_y
, _msgBasePosEcef_z = fromEcefVal $ m ^. msg1005_reference ^. antennaReference_ecef_z
}
fromMsg1006 :: MonadStore e m => Msg1006 -> m MsgBasePosEcef
fromMsg1006 m =
return MsgBasePosEcef
{ _msgBasePosEcef_x = fromEcefVal $ m ^. msg1006_reference ^. antennaReference_ecef_x
, _msgBasePosEcef_y = fromEcefVal $ m ^. msg1006_reference ^. antennaReference_ecef_y
, _msgBasePosEcef_z = fromEcefVal $ m ^. msg1006_reference ^. antennaReference_ecef_z
}
fromMsg1019 :: MonadStore e m => Msg1019 -> m MsgEphemerisGps
fromMsg1019 m = do
commonContent <- toGpsEphemerisCommonContent m
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_toc = toc
}
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
(RTCM3Msg1005 m _rtcm3) -> do
let sender = m ^. msg1005_reference ^. antennaReference_station
m' <- fromMsg1005 m
return [SBPMsgBasePosEcef m' $ toSBP m' $ toSender sender]
(RTCM3Msg1006 m _rtcm3) -> do
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
(RTCM3Msg1019 m _rtcm3) -> do
let sender = 0
m' <- fromMsg1019 m
return [SBPMsgEphemerisGps m' $ toSBP m' $ toSender sender]
_rtcm3Msg -> return mempty
newStore :: IO Store
newStore = do
day <- utctDay <$> getCurrentTime
let wn = fromIntegral $ div (diffDays day (fromGregorian 1980 1 6)) 7
Store <$> newIORef wn <*> newIORef mempty