diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,5 @@ +# Revision history for odid + +## 0.1.0.0 -- YYYY-mm-dd + +* First version. Released on an unsuspecting world. diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2026 David Cox + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 --- /dev/null +++ b/README.md @@ -0,0 +1,19 @@ +# Open Drone ID + +ASTM F3411-22a + +Usage + +``` +odid --help +odid w basic foo | odid r +``` + +Development + +``` +cabal build +cabal test +cabal haddock +cabal install +``` diff --git a/exe/Main.hs b/exe/Main.hs new file mode 100644 --- /dev/null +++ b/exe/Main.hs @@ -0,0 +1,294 @@ +module Main (main) where + +import Data.Binary +import Data.Binary.Get +import Data.Binary.Put +import Data.Bits +import Data.ByteString.Lazy (ByteString) +import qualified Data.ByteString.Lazy as BS +import qualified Data.ByteString.Lazy.Char8 as BSC +import Data.Char +import Data.Int +import Data.ODID +import Data.Version +import Options.Applicative +import Paths_odid +import Prettyprinter + +main :: IO () +main = do + cli <- customExecParser prefs' $ pinfo $ showVersion version + case cli of + ReadODID fM -> print . pretty . runGet (get :: Get Msg) =<< maybe BS.getContents BS.readFile fM + WriteODID msg fM -> maybe BS.putStr BS.writeFile fM $ runPut $ put msg + PackODID fs fM -> do + msgs <- mapM decodeFile fs + let msg = Msg (MsgHdr 2 Pack) $ PackBdy 0x19 (fromIntegral $ length msgs) msgs + maybe BS.putStr BS.writeFile fM $ runPut $ put msg + +prefs' :: ParserPrefs +prefs' = prefs $ showHelpOnError <> showHelpOnEmpty + +pinfo :: String -> ParserInfo CLI +pinfo v = info (parser <**> simpleVersioner v <**> helper) $ progDesc "Open Drone ID" + +data CLI = ReadODID (Maybe String) | WriteODID Msg (Maybe String) + | PackODID [String] (Maybe String) + +parser :: Parser CLI +parser = hsubparser $ mconcat + [ command "r" $ info (ReadODID <$> optional fileArg) $ progDesc "Read ODID data" + , command "w" $ info (WriteODID <$> msgParser <*> optional fileArg) $ progDesc "Write ODID data" + , command "p" $ info (PackODID <$> some fileOpt <*> optional fileArg) $ progDesc "Pack ODID data" + ] + +msgParser :: Parser Msg +msgParser = hsubparser $ mconcat + [ command "basic" $ info basicIDParser $ progDesc "Basic ID" + , command "loc" $ info locationParser $ progDesc "Location/Vector" + , command "auth" $ info authParser $ progDesc "Authentication" + , command "self" $ info selfIDParser $ progDesc "Self ID" + , command "sys" $ info systemParser $ progDesc "System" + , command "op" $ info opIDParser $ progDesc "Operator ID" + ] + +basicIDParser :: Parser Msg +basicIDParser = fmap (Msg $ MsgHdr 2 BasicIDTy) $ + BasicIDBdy <$> parseIDType <*> parseUAType <*> parseUASID <*> parseRsvdBytes + +parseRsvdBytes :: Parser ByteString +parseRsvdBytes = fmap readRsvd $ option auto $ long "rsvd" <> value 0 + <> showDefault <> help "Reserved 3 bytes. Example 0xaabbcc." + where + readRsvd :: Word32 -> ByteString + readRsvd r = BS.pack $ fromIntegral <$> [b0, b1, 0xFF .&. r] + where + b0 = 0xFF .&. r `shiftR` 16 + b1 = 0xFF .&. r `shiftR` 8 + +parseIDType :: Parser IDType +parseIDType = asum + [ flag IDTypeNone IDTypeNone $ long "no-id" <> help "No ID type. Default ID type." + , flag' SerialNum $ long "serial-num" <> help (show $ pretty SerialNum) + , flag' CAARegID $ long "caa-reg-id" <> help (show $ pretty CAARegID) + , flag' UTMUUID $ long "utm-uuid" <> help (show $ pretty UTMUUID) + , flag' SpecificSessionID $ long "session" <> help (show $ pretty SpecificSessionID) + ] + +parseUAType :: Parser UAType +parseUAType = asum $ map mkFlag [None ..] + where + mkFlag None = flag None None $ long "none" + mkFlag GroundObstacle = flag' GroundObstacle $ long "ground-obstacle" + mkFlag t = flag' t $ long $ map toLower $ show t + +parseUASID :: Parser ByteString +parseUASID = pad 20 <$> strArgument (metavar "UASID") + +selfIDParser :: Parser Msg +selfIDParser = fmap (Msg $ MsgHdr 2 SelfIDTy) $ + SelfIDBdy <$> option auto (short 't' <> value 0 <> showDefault <> help "Description type") + <*> pad 23 `fmap` strArgument (metavar "DESC" <> help "Description") + +opIDParser :: Parser Msg +opIDParser = fmap (Msg $ MsgHdr 2 OperatorID) $ + OpIDBdy <$> parseOpIDTy <*> parseOpID <*> parseRsvdBytes + where + parseOpIDTy = option auto $ short 't' <> help "Operator ID type" + parseOpID = fmap (pad 20) $ strArgument $ metavar "ID" <> help "ASCII text" + +pad :: Int64 -> String -> ByteString +pad n s = BS.take n $ BSC.pack s <> BS.replicate n 0x00 + +fileArg :: Parser String +fileArg = strArgument $ metavar "FILE" <> completer (bashCompleter "file") + <> help "Optional binary input file otherwise stream STDIN." + +fileOpt :: Parser String +fileOpt = strOption $ short 'm' <> long "msg" <> metavar "FILE" + <> help "Message file(s) to pack" + +locationParser :: Parser Msg +locationParser = fmap (Msg (MsgHdr 2 Location) . LocBdy) $ + LocMsg <$> opStatusParser <*> switch (long "flag-rsvd" <> help "Reserved flag") + <*> heightTypeParser + <*> switch (long "180" <> help "E/W direction segment switch. >=180 active otherwise <180") + <*> speedMultParser <*> option auto (long "track-dir" <> help "Track direction 0-359 deg.") + <*> option auto (long "speed" <> help "Ground speed m/s") + <*> option auto (long "vert-speed" <> help "Vertical speed m/s") + <*> parseLat <*> parseLon + <*> option auto (long "pres-alt" <> help "Pressure altitude") + <*> option auto (long "geo-alt" <> help "Geodetic altitude") + <*> option auto (long "height") <*> vertAccParser "vert-acc" + <*> horizAccParser <*> vertAccParser "baro-acc" <*> speedAccParser + <*> option auto (long "timestamp") <*> option auto (long "tstamp-acc-rsvd") + <*> option auto (long "tstamp-acc") <*> option auto (long "loc-rsvd") + +parseLat :: Parser Double +parseLat = option auto $ long "lat" <> help "Latitude" + +parseLon :: Parser Double +parseLon = option auto $ long "lon" <> help "Longitude" + +opStatusParser :: Parser OpStatus +opStatusParser = asum + [ flag Undeclared Undeclared $ long "undeclared" <> help "Default operational status" + , flag' Ground $ long "ground" + , flag' Airborne $ long "airborne" + , flag' Emergency $ long "emergency" + , flag' RemoteIDSystemFailure $ long "failure" <> help "Remote ID system failure" + , option auto $ long "op-rsvd" <> help "Reserved" + ] + +heightTypeParser :: Parser HeightType +heightTypeParser = flag AboveTakeoff AboveTakeoff + (long "above-takeoff" <> help "Default height type") <|> + flag' AGL (long "agl" <> help "Above ground level height type") + +speedMultParser :: Parser Bool +speedMultParser = flag False False (long "25" <> help "Default speed multiplier x0.25") + <|> flag' True (long "75" <> help "Speed multiplier x0.75") + +vertAccParser :: String -> Parser VertAcc +vertAccParser s = asum + [ fmap readAcc $ option auto $ long s <> help "Vertical accuracy m" + , fmap VertAccRsvd $ option auto $ long (s ++ "-rsvd") <> help "Reserved low nibble" + ] + where + readAcc :: Double -> VertAcc + readAcc m + | m < 1 = VertAccLT1M + | m < 3 = VertAccLT3M + | m < 10 = VertAccLT10M + | m < 25 = VertAccLT25M + | m < 45 = VertAccLT45M + | m < 150 = VertAccLT150M + | otherwise = VertAccGTE150M + +horizAccParser :: Parser HorizAcc +horizAccParser = asum + [ fmap readAcc $ option auto $ long "horiz-acc" <> help "Horizontal accuracy m" + , fmap HorizAccRsvd $ option auto $ long "horiz-acc-rsvd" + <> help "Horizontal accuracy reserved value" + ] + where + readAcc :: Double -> HorizAcc + readAcc d + | d < 1 = LT1M + | d < 3 = LT3M + | d < 10 = LT10M + | d < 30 = LT30M + | d < 92.6 = LT005NM + | d < 185.2 = LT01NM + | d < 555.6 = LT03NM + | d < 926 = LT05NM + | d < 1852 = LT1NM + | d < 3704 = LT2NM + | d < 7408 = LT4NM + | d < 18520 = LT10NM + | otherwise = GT10NM + +speedAccParser :: Parser SpeedAcc +speedAccParser = asum + [ fmap readAcc $ option auto $ long "speed-acc" <> help "Speed accuracy m/s" + , fmap SpeedAccRsvd $ option auto $ long "speed-acc-rsvd" + <> help "Reserved speed accuracy low nibble" + ] + where + readAcc :: Double -> SpeedAcc + readAcc s + | s < 0.3 = LT03MS + | s < 1 = LT1MS + | s < 3 = LT3MS + | s < 10 = LT10MS + | otherwise = GTE10MS + +systemParser :: Parser Msg +systemParser = fmap (Msg (MsgHdr 2 System) . SysBdy) $ + SysMsg <$> parseClassType <*> parseOpLocSrc <*> parseLat + <*> parseLon <*> parseAreaCnt <*> parseAreaRad <*> parseAreaCeil + <*> parseAreaFlor <*> parseClassCat <*> parseClassClass + <*> parseOpAlt <*> parseSysTimestamp <*> parseSysRsvd + +parseClassType :: Parser ClassType +parseClassType = asum + [ flag ClassTypeUndeclared ClassTypeUndeclared $ long "undeclared" + <> help "Undeclared class type, default" + , flag' EuroUnion $ long "eu" <> help "European Union class type" + , option auto $ long "ct-rsvd" <> help "Class type reserved 2-7" + ] + +parseOpLocSrc :: Parser OpLocSrc +parseOpLocSrc = asum + [ flag Takeoff Takeoff $ long "takeoff" <> help "Default operator location" + , flag' Dynamic $ long "dynamic" <> help "Dynamic operator location" + , flag' Fixed $ long "fixed" <> help "Fixed operator location" + ] + +parseAreaCnt :: Parser Word16 +parseAreaCnt = option auto $ long "area-cnt" <> value 1 <> showDefault + <> help "Number of aircraft in area, group or formation" + +parseAreaRad :: Parser Integer +parseAreaRad = option auto $ long "area-rad" <> value 0 <> showDefault + <> help "Radius in meters of cylindrical area of group or formation" + +parseAreaCeil :: Parser Double +parseAreaCeil = option auto $ long "area-ceil" <> value 0 <> showDefault + <> help "Group operations ceiling in meters" + +parseAreaFlor :: Parser Double +parseAreaFlor = option auto $ long "area-floor" <> value 0 <> showDefault + <> help "Group operations floor in meters" + +parseClassCat :: Parser ClassCat +parseClassCat = asum + [ flag Undefined Undefined $ long "undefined" <> help "Class category undefined. Default." + , flag' Open $ long "open" <> help "Class category open" + , flag' Specific $ long "specific" <> help "Class category specific" + , flag' Certified $ long "certified" <> help "Class category certified" + , option auto $ long "cat-rsvd" <> help "Class category reserved" + ] + +parseClassClass :: Parser Word8 +parseClassClass = option auto $ long "class" <> value 0 <> showDefault + <> help "UA Classification class low nibble 0-15" + +parseOpAlt :: Parser Double +parseOpAlt = option auto $ long "op-alt" <> value 0 <> showDefault + <> help "Operator altitude meters" + +parseSysTimestamp :: Parser Word32 +parseSysTimestamp = option auto $ long "timestamp" <> value 0 <> showDefault + <> help "32 bit timestamp in seconds since 00:00:00 01/01/2019" + +parseSysRsvd :: Parser Word8 +parseSysRsvd = option auto $ long "sys-rsvd" <> value 0 <> showDefault + <> help "Reserved" + +authParser :: Parser Msg +authParser = fmap (Msg (MsgHdr 2 Auth) . AuthBdy) $ + AuthMsg <$> parseAuthType <*> parsePageNum <*> optional parsePage0 <*> parseSignature + where + parseSignature = fmap BS.pack $ some $ argument auto $ metavar "BYTE" <> help "Signature bytes" + +parseAuthType :: Parser AuthType +parseAuthType = asum + [ flag AuthNone AuthNone $ long "none" <> help "No auth, default." + , flag' UASIDSig $ long "uasid" <> help "UASID signature" + , flag' OpIDSig $ long "opid" <> help "Operator ID signature" + , flag' MsgSetSig $ long "msg-set" <> help "Message set signature" + , flag' AuthNRID $ long "nrid" <> help "Network Remote ID signature" + , flag' SpecificAuth $ long "specific" <> help "Specific signature" + , option auto $ long "auth-rsvd" <> help "Reserved" + , option auto $ long "auth-priv" <> help "Private" + ] + +parsePageNum :: Parser Word8 +parsePageNum = option auto $ long "page-num" <> help "Page number" + +parsePage0 :: Parser (Word8, Word8, Word32) +parsePage0 = (,,) + <$> option auto (long "last-page-idx" <> metavar "WORD8" <> help "Last page index") + <*> option auto (long "length" <> metavar "WORD8" <> help "page length") + <*> option auto (long "timestamp" <> metavar "WORD32" <> help "Timestamp") diff --git a/lib/Data/ODID.hs b/lib/Data/ODID.hs new file mode 100644 --- /dev/null +++ b/lib/Data/ODID.hs @@ -0,0 +1,615 @@ +{-# LANGUAGE OverloadedStrings #-} + +-- | Open Drone ID +module Data.ODID + ( Msg(..), MsgHdr(..), MsgType(..), MsgBdy(..) + , IDType(..), UASID, UAType(..) + , SysMsg(..), ClassType(..), ClassCat(..), OpLocSrc(..) + , AuthMsg(..), AuthType(..) + , LocMsg(..), OpStatus(..), HorizAcc(..), VertAcc(..), SpeedAcc(..) + , HeightType(..) + ) where + +import Control.Monad +import Data.Binary +import Data.Binary.Get +import Data.Binary.Put +import Data.Bits +import Data.ByteString.Lazy (ByteString) +import qualified Data.ByteString.Lazy as BS +import qualified Data.ByteString.Lazy.Char8 as BSC +import Data.Function +import Data.Int +import Foreign +import Numeric +import Prettyprinter + +data Msg = Msg{msgHdr :: MsgHdr, msgBdy :: MsgBdy} + deriving (Eq, Read, Show) + +instance Binary Msg where + get = get >>= \hdr -> Msg hdr <$> case msgType hdr of + BasicIDTy -> get >>= \b -> BasicIDBdy (getIDType $ b `shiftR` 4) + (toEnum $ fromIntegral $ b .&. 0xF) <$> getLazyByteString 20 <*> getLazyByteString 3 + Location -> LocBdy <$> get + Auth -> AuthBdy <$> get + SelfIDTy -> SelfIDBdy <$> get <*> getLazyByteString 23 + System -> SysBdy <$> get + OperatorID -> OpIDBdy <$> get <*> getLazyByteString 20 <*> getLazyByteString 3 + Pack -> get >>= \sz -> get >>= \nm -> + PackBdy sz nm <$> replicateM (fromIntegral nm) get + RsvdTy _ -> RsvdBdy <$> getLazyByteString 24 + where + getIDType t = case t of + 0 -> IDTypeNone + 1 -> SerialNum + 2 -> CAARegID + 3 -> UTMUUID + 4 -> SpecificSessionID + n -> IDTypeRsvd n + + put (Msg hdr bdy) = put hdr <> case bdy of + BasicIDBdy t ua uasid rsvd -> do + let ty = case t of + IDTypeNone -> 0 + SerialNum -> 1 + CAARegID -> 2 + UTMUUID -> 3 + SpecificSessionID -> 4 + IDTypeRsvd r -> r + putWord8 $ ty `shiftL` 4 .|. fromIntegral (fromEnum ua) + putLazyByteString $ uasid <> rsvd + LocBdy l -> put l + AuthBdy a -> put a + SelfIDBdy ty desc -> put ty <> putLazyByteString desc + SysBdy s -> put s + OpIDBdy t i r -> put t <> putLazyByteString (i <> r) + PackBdy sz nm ms -> put sz <> put nm <> foldMap put ms + RsvdBdy bs -> putLazyByteString bs + +instance Pretty Msg where + pretty (Msg hdr bdy) = vsep [pretty hdr, pretty bdy] + +-- | Unmanned aircraft +data UAType = None | Aeroplane | Heli | Gyro | Hybrid | Ornith | Glider | Kite + | FreeBalloon | CaptiveBalloon | Airship | Parachute | Rocket + | TetheredPwrAircraft | GroundObstacle | Other + deriving (Bounded, Eq, Enum, Read, Show) + +instance Pretty UAType where + pretty ua = case ua of + None -> "None" + Aeroplane -> "Aeroplane" + Heli -> "Helicopter" + Gyro -> "Gyroplane" + Hybrid -> "Hybrid Lift" + Ornith -> "Ornithopter" + Glider -> "Glider" + Kite -> "Kite" + FreeBalloon -> "Free Balloon" + CaptiveBalloon -> "Captive Balloon" + Airship -> "Airship" + Parachute -> "Parachute" + Rocket -> "Rocket" + TetheredPwrAircraft -> "Tethered Powered Aircraft" + GroundObstacle -> "Ground Obstacle" + Other -> "Other" + +-- | Operational status +data OpStatus = Undeclared | Ground | Airborne | Emergency + | RemoteIDSystemFailure | OpStatusRsvd Word8 + deriving (Eq, Read, Show) + +instance Pretty OpStatus where + pretty s = case s of + RemoteIDSystemFailure -> "Remote ID System Failure" + OpStatusRsvd r -> "Reserved" <+> pretty r + _ -> viaShow s + +data MsgType = BasicIDTy | Location | Auth | SelfIDTy | System | OperatorID + | Pack | RsvdTy Word8 + deriving (Eq, Read, Show) + +instance Pretty MsgType where + pretty t = case t of + BasicIDTy -> "Basic ID" + SelfIDTy -> "Self ID" + OperatorID -> "Operator ID" + RsvdTy r -> "Reserved" <+> pretty r + _ -> viaShow t + +data MsgHdr = MsgHdr{msgVer :: Word8, msgType :: MsgType} + deriving (Eq, Read, Show) + +instance Binary MsgHdr where + get = readHdr <$> get + where + readHdr b = MsgHdr (b .&. 0xF) $ case b `shiftR` 4 of + 0x0 -> BasicIDTy + 0x1 -> Location + 0x2 -> Auth + 0x3 -> SelfIDTy + 0x4 -> System + 0x5 -> OperatorID + 0xF -> Pack + n -> RsvdTy n + + put (MsgHdr v t) = putWord8 $ tNyb `shiftL` 4 .|. v + where + tNyb = case t of + BasicIDTy -> 0x0 + Location -> 0x1 + Auth -> 0x2 + SelfIDTy -> 0x3 + System -> 0x4 + OperatorID -> 0x5 + Pack -> 0xF + RsvdTy r -> r + +instance Pretty MsgHdr where + pretty (MsgHdr v t) = "v" <> pretty v <+> pretty t + +data MsgBdy = BasicIDBdy IDType UAType UASID ByteString | LocBdy LocMsg | AuthBdy AuthMsg + | SelfIDBdy Word8 ByteString | SysBdy SysMsg | OpIDBdy Word8 ByteString ByteString + | PackBdy Word8 Word8 [Msg] | RsvdBdy ByteString + deriving (Eq, Read, Show) + +instance Pretty MsgBdy where + pretty m = case m of + BasicIDBdy idTy uaTy uasid rsvd -> vsep ["ID:" <+> pretty idTy + , "UA:" <+> pretty uaTy, "UASID:" <+> pretty (BSC.unpack uasid) + , "RSVD:" <+> prettyBytes rsvd] + LocBdy l -> pretty l + AuthBdy a -> pretty a + SelfIDBdy ty desc -> vsep ["Type:" <+> pretty ty, "Desc:" <+> pretty (BSC.unpack desc)] + SysBdy s -> pretty s + OpIDBdy t i r -> vsep ["Type:" <+> pretty t, "ID:" <+> pretty (BSC.unpack i) + , "Rsvd:" <+> pretty (BSC.unpack r)] + PackBdy sz nm ms -> vsep ["Size=" <> pretty sz <+> "Cnt=" <> pretty nm + , indent 2 $ vsep $ pretty <$> ms] + RsvdBdy bs -> "Reserved:" <+> prettyBytes bs + +type UASID = ByteString + +data IDType = IDTypeNone | SerialNum | CAARegID | UTMUUID | SpecificSessionID + | IDTypeRsvd Word8 + deriving (Eq, Read, Show) + +instance Pretty IDType where + pretty t = case t of + IDTypeNone -> "None" + SerialNum -> "Serial Number (ANSI/CTA-2063-A)" + CAARegID -> "CAA Assigned Registration ID" + UTMUUID -> "UTM Assigned UUID" + SpecificSessionID -> "Specific Session ID" + IDTypeRsvd r -> "Reserved" <+> pretty r + +data HorizAcc + = GT10NM -- ^ >=18.52 km (10 NM) or Unknown + | LT10NM -- ^ <18.52 km (10 NM) + | LT4NM -- ^ <7.408 km (4 NM) + | LT2NM -- ^ <3.704 km (2 NM) + | LT1NM -- ^ <1852 m (1 NM) + | LT05NM -- ^ <926 m (0.5 NM) + | LT03NM -- ^ <555.6 m (0.3 NM) + | LT01NM -- ^ <185.2 m (0.1 NM) + | LT005NM -- ^ <92.6 m (0.05 NM) + | LT30M -- ^ <30 m + | LT10M -- ^ <10 m + | LT3M -- ^ <3 m + | LT1M -- ^ <1 m + | HorizAccRsvd Word8 -- ^ Reserved + deriving (Eq, Read, Show) + +instance Pretty HorizAcc where + pretty a = case a of + GT10NM -> ">=18.52 km (10 NM) or Unknown" + LT10NM -> "<18.52 km (10 NM)" + LT4NM -> "<7.408 km (4 NM)" + LT2NM -> "<3.704 km (2 NM)" + LT1NM -> "<1852 m (1 NM)" + LT05NM -> "<926 m (0.5 NM)" + LT03NM -> "<555.6 m (0.3 NM)" + LT01NM -> "<185.2 m (0.1 NM)" + LT005NM -> "<92.6 m (0.05 NM)" + LT30M -> "<30 m" + LT10M -> "<10 m" + LT3M -> "<3 m" + LT1M -> "<1 m" + HorizAccRsvd n -> "Reserved" <+> pretty n + +data ClassCat = Undefined | Open | Specific | Certified | ClassCatRsvd Word8 + deriving (Eq, Read, Show) + +instance Pretty ClassCat where + pretty (ClassCatRsvd n) = "Reserved" <+> pretty n + pretty c = viaShow c + +data VertAcc + = VertAccGTE150M -- ^ >=150 m or Unknown + | VertAccLT150M-- ^ <150 m + | VertAccLT45M -- ^ <45 m + | VertAccLT25M -- ^ <25 m + | VertAccLT10M -- ^ <10 m + | VertAccLT3M -- ^ <3 m + | VertAccLT1M -- ^ <1 m + | VertAccRsvd Word8 + deriving (Eq, Read, Show) + +instance Pretty VertAcc where + pretty a = case a of + VertAccGTE150M -> ">=150 m or Unknown" + VertAccLT150M -> "<150 m" + VertAccLT45M -> "<45 m" + VertAccLT25M -> "<25 m" + VertAccLT10M -> "<10 m" + VertAccLT3M -> "<3 m" + VertAccLT1M -> "<1 m" + VertAccRsvd n -> "Reserved" <+> pretty n + +readVertAcc :: Word8 -> VertAcc +readVertAcc n = case n of + 0 -> VertAccGTE150M + 1 -> VertAccLT150M + 2 -> VertAccLT45M + 3 -> VertAccLT25M + 4 -> VertAccLT10M + 5 -> VertAccLT3M + 6 -> VertAccLT1M + _ -> VertAccRsvd n + +writeVertAcc :: VertAcc -> Word8 +writeVertAcc vacc = case vacc of + VertAccGTE150M -> 0 + VertAccLT150M -> 1 + VertAccLT45M -> 2 + VertAccLT25M -> 3 + VertAccLT10M -> 4 + VertAccLT3M -> 5 + VertAccLT1M -> 6 + VertAccRsvd r -> r + +data SpeedAcc = GTE10MS | LT10MS | LT3MS | LT1MS | LT03MS | SpeedAccRsvd Word8 + deriving (Eq, Read, Show) + +instance Pretty SpeedAcc where + pretty a = case a of + GTE10MS -> ">=10 m/s or Unknown" + LT10MS -> "<10 m/s" + LT3MS -> "<3 m/s" + LT1MS -> "<1 m/s" + LT03MS -> "<0.3 m/s" + SpeedAccRsvd n -> "Reserved" <+> pretty n + +data HeightType = AboveTakeoff | AGL + deriving (Bounded, Enum, Eq, Read, Show) + +instance Pretty HeightType where + pretty AboveTakeoff = "Above Takeoff" + pretty AGL = "AGL" + +-- | Location message +data LocMsg = LocMsg + { locOpStatus :: OpStatus, locFlagsRsvd :: Bool, locHeightType :: HeightType + , locFlagsDir :: Bool, locFlagsMult :: Bool, locTrackDir :: Integer, locSpeed :: Double + , locVertSpeed :: Double, locLat :: Double, locLon :: Double + , locPresAlt :: Double, locGeoAlt :: Double, locHeight :: Double + , locVertAcc :: VertAcc, locHorzAcc :: HorizAcc + , locBaroAltAcc :: VertAcc, locSpeedAcc :: SpeedAcc + , locTimestamp :: Double -- ^ seconds after the hour + , locTStampAccRsvd :: Word8, locTStampAcc :: Double -- ^ timestamp accuracy seconds, 0.1 s res + , locRsvd :: Word8 + } + deriving (Eq, Read, Show) + +instance Binary LocMsg where + get = getWord8 >>= \b -> do + let opStatus = case b `shiftR` 4 of + 0 -> Undeclared + 1 -> Ground + 2 -> Airborne + 3 -> Emergency + 4 -> RemoteIDSystemFailure + n -> OpStatusRsvd n + flgsRsvd = testBit b 3 + ht = if testBit b 2 then AGL else AboveTakeoff + dir = testBit b 1 + mul = testBit b 0 + trackDir <- applyWhen dir (+ 180) . fromIntegral <$> getWord8 + speed <- decSpeed mul . fromIntegral <$> getWord8 + vertSpeed <- (* 0.5) . fromIntegral <$> getInt8 + lat <- decLatLon <$> getInt32le + lon <- decLatLon <$> getInt32le + palt <- decodeAlt <$> getWord16le + galt <- decodeAlt <$> getWord16le + hgt <- decodeAlt <$> getWord16le + vhacc <- getWord8 + let vacc = readVertAcc $ vhacc `shiftR` 4 + hacc = case vhacc .&. 0xF of + 0 -> GT10NM + 1 -> LT10NM + 2 -> LT4NM + 3 -> LT2NM + 4 -> LT1NM + 5 -> LT05NM + 6 -> LT03NM + 7 -> LT01NM + 8 -> LT005NM + 9 -> LT30M + 10 -> LT10M + 11 -> LT3M + 12 -> LT1M + n -> HorizAccRsvd n + bacc <- getWord8 + let baroAcc = readVertAcc $ bacc `shiftR` 4 + speedAcc = case bacc .&. 0xF of + 0 -> GTE10MS + 1 -> LT10MS + 2 -> LT3MS + 3 -> LT1MS + 4 -> LT03MS + n -> SpeedAccRsvd n + tstmp <- (* 0.1) . fromIntegral <$> getWord16le + tstmprsvdacc <- getWord8 + let tstmpacc = fromIntegral (tstmprsvdacc .&. 0xF) * 0.1 + LocMsg opStatus flgsRsvd ht dir mul trackDir speed vertSpeed lat + lon palt galt hgt vacc hacc baroAcc speedAcc tstmp + (tstmprsvdacc `shiftR` 4) tstmpacc <$> getWord8 + where + decSpeed mul s = if mul then s * 0.75 + 255 * 0.25 else s * 0.25 + + put l = do + putWord8 $ opStatus `shiftL` 4 .|. rsvdFlag `shiftL` 3 .|. htTy `shiftL` 2 .|. + ewDir `shiftL` 1 .|. spMult + putWord8 $ fromIntegral $ applyWhen (locTrackDir l >= 180) (subtract 180) $ locTrackDir l + putWord8 $ round $ encSpeed $ locSpeed l + putInt8 $ round $ locVertSpeed l * 2 + putInt32le $ encLatLon $ locLat l + putInt32le $ encLatLon $ locLon l + putWord16le $ encodeAlt $ locPresAlt l + putWord16le $ encodeAlt $ locGeoAlt l + putWord16le $ encodeAlt $ locHeight l + putWord8 $ writeVertAcc (locVertAcc l) `shiftL` 4 .|. hAcc + putWord8 $ writeVertAcc (locBaroAltAcc l) `shiftL` 4 .|. spAcc + putWord16le $ round $ locTimestamp l / 0.1 + putWord8 $ locTStampAccRsvd l `shiftL` 4 .|. round (locTStampAcc l / 0.1) + putWord8 $ locRsvd l + where + opStatus = case locOpStatus l of + Undeclared -> 0 + Ground -> 1 + Airborne -> 2 + Emergency -> 3 + RemoteIDSystemFailure -> 4 + OpStatusRsvd n -> n + rsvdFlag = fromBool $ locFlagsRsvd l + htTy = fromIntegral $ fromEnum $ locHeightType l + ewDir = fromBool $ locFlagsDir l + spMult = fromBool $ locFlagsMult l + hAcc = case locHorzAcc l of + GT10NM -> 0 + LT10NM -> 1 + LT4NM -> 2 + LT2NM -> 3 + LT1NM -> 4 + LT05NM -> 5 + LT03NM -> 6 + LT01NM -> 7 + LT005NM -> 8 + LT30M -> 9 + LT10M -> 10 + LT3M -> 11 + LT1M -> 12 + HorizAccRsvd r -> r + spAcc = case locSpeedAcc l of + GTE10MS -> 0 + LT10MS -> 1 + LT3MS -> 2 + LT1MS -> 3 + LT03MS -> 4 + SpeedAccRsvd r -> r + +instance Pretty LocMsg where + pretty l = vsep + ["Operational Status:" <+> pretty (locOpStatus l) + , "Flags", indent 2 $ vsep + [ "Reserved:" <+> if locFlagsRsvd l then "1" else "0" + , "Height type:" <+> pretty (locHeightType l) + , "E/W Dir Seg:" <+> if locFlagsDir l then ">=180" else "<180" + , "Speed mult:" <+> if locFlagsMult l then "x0.75" else "x0.25" + ] + , "Track dir:" <+> pretty (locTrackDir l) <+> "deg" + , "Speed:" <+> pretty (locSpeed l) <+> "m/s" + , "Vert Speed:" <+> pretty (locVertSpeed l) <+> "m/s" + , "Latitude:" <+> pretty (locLat l) <+> "deg" + , "Longitude:" <+> pretty (locLon l) <+> "deg" + , "Pressure Alt:" <+> pretty (locPresAlt l) <+> "m" + , "Geodetic Alt:" <+> pretty (locGeoAlt l) <+> "m" + , "Height:" <+> pretty (locHeight l) <+> "m" + , "Vert Acc:" <+> pretty (locVertAcc l) + , "Horz Acc:" <+> pretty (locHorzAcc l) + , "Baro Alt Acc:" <+> pretty (locBaroAltAcc l) + , "Speed Acc:" <+> pretty (locSpeedAcc l) + , "Timestamp:" <+> pretty (locTimestamp l) + , "Reserved:" <+> pretty (locTStampAccRsvd l) + , "TStamp Acc:" <+> pretty (locTStampAcc l) <+> "s" + , "Reserved:" <+> pretty (locRsvd l) + ] + +data ClassType = ClassTypeUndeclared | EuroUnion | ClassTypeRsvd Word8 + deriving (Eq, Read, Show) + +instance Pretty ClassType where + pretty ClassTypeUndeclared = "Undeclared" + pretty EuroUnion = "European Union" + pretty (ClassTypeRsvd n) = "Reserved" <+> pretty n + +-- | Operator location source type +data OpLocSrc = Takeoff | Dynamic | Fixed + deriving (Bounded, Enum, Eq, Read, Show) + +instance Pretty OpLocSrc where + pretty = viaShow + +data SysMsg = SysMsg + { sysClassType :: ClassType, sysOpSrcType :: OpLocSrc, sysOpLat :: Double + , sysOpLon :: Double, sysArCnt :: Word16, sysArRad :: Integer, sysArCeil :: Double + , sysArFloor :: Double, sysClassCat :: ClassCat, sysClassClass :: Word8 + , sysOpAlt :: Double, sysTimestamp :: Word32, sysRsvd :: Word8 + } + deriving (Eq, Read, Show) + +instance Binary SysMsg where + get = getWord8 >>= \flags -> do + let classType = case 0x7 .&. flags `shiftR` 2 of + 0 -> ClassTypeUndeclared + 1 -> EuroUnion + n -> ClassTypeRsvd n + srcType = if testBit flags 1 then Fixed else toEnum $ fromBool $ testBit flags 0 + opLat <- decLatLon <$> getInt32le + opLon <- decLatLon <$> getInt32le + arCnt <- getWord16le + arRad <- (* 10) . fromIntegral <$> getWord8 + arCeil <- decodeAlt <$> getWord16le + arFlor <- decodeAlt <$> getWord16le + uaClass <- getWord8 + let classCat = case uaClass `shiftR` 4 of + 0 -> Undefined + 1 -> Open + 2 -> Specific + 3 -> Certified + r -> ClassCatRsvd r + SysMsg classType srcType opLat opLon arCnt arRad arCeil arFlor classCat + (uaClass .&. 0xF) <$> fmap decodeAlt getWord16le <*> getWord32le <*> getWord8 + + put m = do + putWord8 $ classTy `shiftL` 2 .|. fromIntegral (fromEnum $ sysOpSrcType m) + putInt32le $ encLatLon $ sysOpLat m + putInt32le $ encLatLon $ sysOpLon m + putWord16le $ sysArCnt m + putWord8 $ fromIntegral $ sysArRad m `div` 10 + putWord16le $ encodeAlt $ sysArCeil m + putWord16le $ encodeAlt $ sysArFloor m + putWord8 $ classCat `shiftL` 4 .|. sysClassClass m .&. 0xF + putWord16le $ encodeAlt $ sysOpAlt m + putWord32le $ sysTimestamp m + putWord8 $ sysRsvd m + where + classTy = case sysClassType m of + ClassTypeUndeclared -> 0 + EuroUnion -> 1 + ClassTypeRsvd r -> r + classCat = case sysClassCat m of + Undefined -> 0 + Open -> 1 + Specific -> 2 + Certified -> 3 + ClassCatRsvd r -> r + +instance Pretty SysMsg where + pretty s = vsep + [ "Class:" <+> pretty (sysClassType s) + , "Op Src:" <+> pretty (sysOpSrcType s) + , "Op Lat:" <+> pretty (sysOpLat s) <+> "deg" + , "Op Lon:" <+> pretty (sysOpLon s) <+> "deg" + , "Area Cnt:" <+> pretty (sysArCnt s) + , "Area Rad:" <+> pretty (sysArRad s) <+> "m" + , "Area Ceil:" <+> pretty (sysArCeil s) <+> "m" + , "Area Floor:" <+> pretty (sysArFloor s) <+> "m" + , "UA Category:" <+> pretty (sysClassCat s) + , "UA Class:" <+> pretty (sysClassClass s) + , "Op Alt:" <+> pretty (sysOpAlt s) <+> "m" + , "Timestamp:" <+> pretty (sysTimestamp s) + , "Reserved:" <+> pretty (sysRsvd s) + ] + +data AuthMsg = AuthMsg AuthType + Word8 -- ^ Page number + (Maybe (Word8, Word8, Word32)) -- ^ PageN last page index, length, timestamp + ByteString -- ^ signature + deriving (Eq, Read, Show) + +instance Binary AuthMsg where + get = getWord8 >>= \b -> do + let ty = case b `shiftR` 4 of + 0 -> AuthNone + 1 -> UASIDSig + 2 -> OpIDSig + 3 -> MsgSetSig + 4 -> AuthNRID + 5 -> SpecificAuth + n | n >= 6 && n <= 9 -> AuthRsvd n + | otherwise -> AuthPriv n + pge = b .&. 0xF + if pge == 0 + then do + z <- (,,) <$> getWord8 <*> getWord8 <*> getWord32le + AuthMsg ty pge (Just z) <$> getLazyByteString 17 + else AuthMsg ty pge Nothing <$> getLazyByteString 23 + + put (AuthMsg ty pge (Just (lpi, l, ts)) sig) = do + putWord8 $ writeAuthType ty `shiftL` 4 .|. pge .&. 0xF + putWord8 lpi <> putWord8 l <> putWord32le ts <> putLazyByteString sig + put (AuthMsg ty pge Nothing sig) = do + putWord8 $ writeAuthType ty `shiftL` 4 .|. pge .&. 0xF + putLazyByteString sig + +instance Pretty AuthMsg where + pretty (AuthMsg ty pge (Just (lpi, l, ts)) sig) = vsep + ["Type:" <+> pretty ty, "Page:" <+> pretty pge, "Last Page Index:" <+> pretty lpi + , "Length:" <+> pretty l, "Timestamp:" <+> pretty ts, "Signature:" <+> prettyBytes sig + ] + pretty (AuthMsg ty pge Nothing sig) = vsep + ["Type:" <+> pretty ty, "Page:" <+> pretty pge, "Signature:" <+> prettyBytes sig] + +data AuthType + = AuthNone | UASIDSig | OpIDSig | MsgSetSig | AuthNRID | SpecificAuth + | AuthRsvd Word8 | AuthPriv Word8 + deriving (Eq, Read, Show) + +instance Pretty AuthType where + pretty t = case t of + AuthNone -> "None" + UASIDSig -> "UAS ID Signature" + OpIDSig -> "Operator ID Signature" + MsgSetSig -> "Message Set Signature" + AuthNRID -> "Auth by Network Remote ID" + SpecificAuth -> "Specific Auth" + AuthRsvd r -> "Reserved" <+> pretty r + AuthPriv p -> "Private" <+> pretty p + +writeAuthType :: AuthType -> Word8 +writeAuthType ty = case ty of + AuthNone -> 0 + UASIDSig -> 1 + OpIDSig -> 2 + MsgSetSig -> 3 + AuthNRID -> 4 + SpecificAuth -> 5 + AuthRsvd r -> r + AuthPriv p -> p + +encodeAlt :: Double -> Word16 +encodeAlt x = round $ (x + 1000) * 2 + +decodeAlt :: Word16 -> Double +decodeAlt x = fromIntegral x * 0.5 - 1000 + +encLatLon :: Double -> Int32 +encLatLon = round . (* latLonMult) + +decLatLon :: Int32 -> Double +decLatLon = (/ latLonMult) . fromIntegral + +latLonMult :: Double +latLonMult = 10 ^ (7 :: Int) + +encSpeed :: Double -> Double +encSpeed s | s <= 255 * 0.25 = s / 0.25 + | s > 255 * 0.25 && s < 254.25 = (s - (255 * 0.25)) / 0.75 + | otherwise = 254 + +prettyBytes :: ByteString -> Doc ann +prettyBytes bs = "0x" <> foldMap prettyByte (BS.unpack bs) + where + prettyByte b = pretty $ showHex (b `shiftR` 4) "" <> showHex (b .&. 0xF) "" diff --git a/odid.cabal b/odid.cabal new file mode 100644 --- /dev/null +++ b/odid.cabal @@ -0,0 +1,57 @@ +cabal-version: 3.0 +name: odid +version: 0.1.0.0 +synopsis: Open Drone ID +description: ASTM F3411-22a +homepage: https://github.com/dopamane/odid +license: MIT +license-file: LICENSE +author: dopamane +maintainer: dwc1295@gmail.com +copyright: David Cox +category: Data +build-type: Simple +extra-doc-files: CHANGELOG.md + README.md + +library + default-language: Haskell2010 + ghc-options: -Wall -O2 + hs-source-dirs: lib + exposed-modules: Data.ODID + build-depends: + base < 5, + binary, + bytestring, + prettyprinter + +executable odid + default-language: Haskell2010 + ghc-options: -Wall -O2 -threaded + hs-source-dirs: exe + main-is: Main.hs + other-modules: Paths_odid + autogen-modules: Paths_odid + build-depends: + base < 5, + binary, + bytestring, + odid, + optparse-applicative, + prettyprinter + +test-suite test + default-language: Haskell2010 + ghc-options: -Wall -O2 -threaded + hs-source-dirs: test + type: exitcode-stdio-1.0 + main-is: Main.hs + build-depends: + base < 5, + binary, + bytestring, + hedgehog, + odid, + tasty, + tasty-hedgehog, + tasty-hunit diff --git a/test/Main.hs b/test/Main.hs new file mode 100644 --- /dev/null +++ b/test/Main.hs @@ -0,0 +1,64 @@ +{-# LANGUAGE OverloadedStrings #-} + +module Main (main) where + +import Data.Binary +import qualified Data.ByteString.Lazy as BS +import Data.ODID +import Hedgehog +import qualified Hedgehog.Gen as Gen +import qualified Hedgehog.Range as Range +import Test.Tasty +import Test.Tasty.Hedgehog +import Test.Tasty.HUnit + +main :: IO () +main = defaultMain $ testGroup "Test.ODID" [testBasicID, testLocation] + +testBasicID :: TestTree +testBasicID = testCase "BasicID" $ do + let input = Msg (MsgHdr 2 BasicIDTy) $ + BasicIDBdy CAARegID Heli "01234567890123456789" $ BS.replicate 3 0x00 + decode (encode input) @?= input + +testLocation :: TestTree +testLocation = testCase "Location" $ do + let input = Msg (MsgHdr 2 Location) $ LocBdy + LocMsg{locOpStatus=Ground, locFlagsRsvd=False, locHeightType=AGL + , locFlagsDir=False, locFlagsMult=False, locTrackDir=135 + , locSpeed=5, locVertSpeed=7.5, locLat=34.0522, locLon=118.2437 + , locPresAlt=10.5, locGeoAlt=8.5, locHeight=2 + , locVertAcc=VertAccLT3M, locHorzAcc=LT1M + , locBaroAltAcc=VertAccLT3M, locSpeedAcc=LT1MS, locTimestamp=0 + , locTStampAccRsvd=0, locTStampAcc=0.1, locRsvd=0} + decode (encode input) @?= input + +{- +testMsgBinaryTrip :: TestTree +testMsgBinaryTrip = testProperty "Msg" $ property $ binTrip =<< forAll genMsg + +genMsg :: MonadGen m => m Msg +genMsg = do + ver <- Gen.word8 $ Range.linear 0 15 + typ <- Gen.element msgTypes + Msg (MsgHdr ver typ) <$> case typ of + BasicIDTy -> BasicIDBdy <$> Gen.enumBounded <*> Gen.enumBounded + <*> BS.fromStrict `fmap` Gen.bytes (Range.singleton 20) + <*> BS.fromStrict `fmap` Gen.bytes (Range.singleton 3) + Location -> LocBdy <$> Gen.discard + Auth -> Gen.discard + SelfIDTy -> SelfIDBdy <$> Gen.enumBounded + <*> BS.fromStrict `fmap` Gen.bytes (Range.singleton 23) + System -> Gen.discard + OperatorID -> OpIDBdy <$> Gen.enumBounded + <*> BS.fromStrict `fmap` Gen.bytes (Range.singleton 20) + <*> BS.fromStrict `fmap` Gen.bytes (Range.singleton 3) + Pack -> Gen.discard + +genOpStatus :: MonadGen m => m OpStatus +genOpStatus = Gen.choice $ OpStatusRsvd `fmap` Gen.word8 (Range.linear 0 15) : map pure + [Undeclared, Ground, Airborne, Emergency, RemoteIDSystemFailure] + +binTrip :: (MonadTest m, Show a, Eq a, Binary a) => a -> m () +binTrip d = tripping d encode $ fmap (\(_, _, a) -> a) . decodeOrFail +-}