module Data.RTCM3.SBP
( toWn
, mjdEpoch
, convert
, newStore
, validateIodcIode
, gpsUriToUra
) where
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
gpsPi :: Double
gpsPi = 3.1415926535898
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
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
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
}
toSender :: Word16 -> Word16
toSender = (.|. 0xf000)
decodeFitInterval :: Bool -> Word16 -> Word32
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
gpsUriToUra :: Double -> Double
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
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) -> toSBPMsgObs m
(RTCM3Msg1004 m _rtcm3) -> toSBPMsgObs m
(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]
(RTCM3Msg1010 m _rtcm3) -> toSBPMsgObs m
(RTCM3Msg1012 m _rtcm3) -> toSBPMsgObs m
(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