def find_mapping(infile, outfile):
stream = BitStream()
with open(infile) as f:
byte = f.read(1)
while byte:
if byte == '\x01':
stream.append('0b1')
else:
stream.append('0b0')
byte = f.read(1)
iteration = 0
for mapping in bitmappings:
stream.pos = 0
tmpstream = BitStream()
for pie in stream.cut(2):
index = pie.read('uint:2')
tmpstream.append( BitArray(uint=mapping[index], length=2) )
# look for the start of the flag in the stream of bits
pos = tmpstream.find(ZULU)
if len(pos) > 0:
# print("Found the bytes we are looking for on iteration {}".format(iteration))
# print("pos = {}".format(pos))
tmpstream <<= pos[0] % 8
data = tmpstream.tobytes()
# print(data)
with open(outfile, 'wb') as fh:
fh.write(data)
break
else:
# print("Did not find the header")
iteration += 1
class PacketDecoder(object):
HEADER = "0x59"
FOOTER = "0x5254464d"
LENGTH = 56 * 8
PATTERN = "2BH9I8B2I"
packet_store = []
def __init__(self, listener_func):
self.listener_func = listener_func
self.stream = BitStream()
def push(self, byte_array):
self.stream.append(byte_array)
def handle(self):
while True:
if len(self.stream) < self.LENGTH:
break
# Look for a header
header_pos = self.stream.find(self.HEADER)
if header_pos:
header_pos = header_pos[0]
else:
self.stream.clear()
break
end_pos = header_pos + self.LENGTH
if len(self.stream) < end_pos:
# Not Enough Data
break
# Extract potential packet
potential_packet = self.stream[header_pos:end_pos].bytes
valid_packet = self.analyze_packet(potential_packet)
if valid_packet:
self.stream = self.stream[end_pos:]
else:
self.stream = self.stream[header_pos + 1 :]
self.save_packets()
self.packet_store = []
def analyze_packet(self, byte_string):
if len(byte_string) != 56:
return
data = struct.unpack(self.PATTERN, byte_string)
header = data[0]
footer = data[21]
if hex(header) == self.HEADER and hex(footer) == self.FOOTER:
if self.validate_checksums(data):
packet = self.create_packet(data)
self.packet_store.append(packet)
return True
return False
def validate_checksums(self, packet_data):
# voltage_checksum = packet_data[12]
# current_checksum = packet_data[13]
# period_checksum = packet_data[14]
# active_checksum = packet_data[15]
# reactive_checksum = packet_data[16]
# apparent_power = packet_data[17]
# phase_angle_checksum = packet_data[18]
# power_factor_checksum = packet_data[19]
# checksum = packet_data[20]
return True
def create_packet(self, packet_data):
flags = packet_data[2]
epoch = packet_data[3]
voltage = packet_data[4] * (2.37748 * 10 ** -4) + -0.14427
current = packet_data[5] * (113240.82786) + 953.97194
period = packet_data[6]
active_power = packet_data[7]
reactive_power = packet_data[8]
apparent_power = packet_data[9]
phase_angle = packet_data[10]
power_factor = packet_data[11]
return Packet(
flags=flags,
epoch=epoch,
voltage=voltage,
current=current,
period=period,
active_power=active_power,
reactive_power=reactive_power,
apparent_power=apparent_power,
phase_angle=phase_angle,
power_factor=power_factor,
)
def save_packets(self):
# submit packets to lib
if self.packet_store:
self.listener_func(self.packet_store)
class PacketDecoder(object):
HEADER = '0x59'
FOOTER = '0x5254464d'
LENGTH = 56 * 8
PATTERN = "2BH9I8B2I"
packet_store = []
def __init__(self, listener_func):
self.listener_func = listener_func
self.stream = BitStream()
def push(self, byte_array):
self.stream.append(byte_array)
def handle(self):
while True:
if len(self.stream) < self.LENGTH:
break
#Look for a header
header_pos = self.stream.find(self.HEADER)
if header_pos:
header_pos = header_pos[0]
else:
self.stream.clear()
break
end_pos = header_pos + self.LENGTH
if len(self.stream) < end_pos:
# Not Enough Data
break
# Extract potential packet
potential_packet = self.stream[header_pos:end_pos].bytes
valid_packet = self.analyze_packet(potential_packet)
if valid_packet:
self.stream = self.stream[end_pos:]
else:
self.stream = self.stream[header_pos + 1:]
self.save_packets()
self.packet_store = []
def analyze_packet(self, byte_string):
if len(byte_string) != 56:
return
data = struct.unpack(self.PATTERN, byte_string)
header = data[0]
footer = data[21]
if hex(header) == self.HEADER and hex(footer) == self.FOOTER:
if self.validate_checksums(data):
packet = self.create_packet(data)
self.packet_store.append(packet)
return True
return False
def validate_checksums(self, packet_data):
#voltage_checksum = packet_data[12]
#current_checksum = packet_data[13]
#period_checksum = packet_data[14]
#active_checksum = packet_data[15]
#reactive_checksum = packet_data[16]
#apparent_power = packet_data[17]
#phase_angle_checksum = packet_data[18]
#power_factor_checksum = packet_data[19]
#checksum = packet_data[20]
return True
def create_packet(self, packet_data):
flags = packet_data[2]
epoch = packet_data[3]
voltage = packet_data[4] * (2.37748 * 10**-4) + -0.14427
current = packet_data[5] * (113240.82786) + 953.97194
period = packet_data[6]
active_power = packet_data[7]
reactive_power = packet_data[8]
apparent_power = packet_data[9]
phase_angle = packet_data[10]
power_factor = packet_data[11]
return Packet(
flags=flags,
epoch=epoch,
voltage=voltage,
current=current,
period=period,
active_power=active_power,
reactive_power=reactive_power,
apparent_power=apparent_power,
phase_angle=phase_angle,
power_factor=power_factor,
)
def save_packets(self):
# submit packets to lib
if self.packet_store:
self.listener_func(self.packet_store)
class NtripStream:
def __init__(self):
self.__CLIENTVERSION = __version__
self.__CLIENTNAME = "Bedrock Solutions NtripClient/" + f"{self.__CLIENTVERSION}"
self.casterUrl = None
self.ntripWriter = None
self.ntripReader = None
self.ntripVersion = 2
self.ntripMountPoint = None
self.ntripAuthString = ""
self.ntripRequestHeader = ""
self.ntripResponseHeader = []
self.ntripResponseStatusCode = None
self.ntripStreamChunked = False
self.nmeaString = ""
self.rtcmFrameBuffer = BitStream()
self.rtcmFramePreample = False
self.rtcmFrameAligned = False
async def openNtripConnection(self, casterUrl: str) -> bool:
"""
Connects to a caster.
Parameters
----------
casterUrl : str
http[s]://caster.hostname.net:port.
Raises
------
TimeoutError
DESCRIPTION.
OSError
DESCRIPTION.
Returns
-------
bool
DESCRIPTION.
"""
self.casterUrl = urlsplit(casterUrl)
try:
if self.casterUrl.scheme == "https":
self.ntripReader, self.ntripWriter = await asyncio.open_connection(
self.casterUrl.hostname, self.casterUrl.port, ssl=True
)
else:
self.ntripReader, self.ntripWriter = await asyncio.open_connection(
self.casterUrl.hostname, self.casterUrl.port
)
except TimeoutError as error:
raise TimeoutError(
f"Connection to {casterUrl} timed out: {error}"
) from None
logging.error(f"Connection to {casterUrl} timed out: {error}")
return False
except OSError as error:
raise OSError(f"Connection to {casterUrl} failed with: {error}") from None
logging.error(f"Connection to {casterUrl} failed with: {error}")
return False
logging.info(
f"{self.ntripMountPoint}: Connection to {casterUrl} open. "
"Ready to write."
)
return True
def setRequestSourceTableHeader(self, casterUrl: str) -> None:
"""
Parameters
----------
casterUrl : str
DESCRIPTION.
Returns
-------
None
DESCRIPTION.
"""
self.casterUrl = urlsplit(casterUrl)
timestamp = strftime("%a, %d %b %Y %H:%M:%S GMT", gmtime())
self.ntripRequestHeader = (
f"GET / HTTP/1.1\r\n"
f"Host: {self.casterUrl.netloc}\r\n"
f"Ntrip-Version: Ntrip/"
f"{self.ntripVersion}.0\r\n"
f"User-Agent: NTRIP {self.__CLIENTNAME}\r\n"
f"Date: {timestamp}\r\n"
f"Connection: close\r\n"
f"\r\n"
).encode("ISO-8859-1")
def setRequestStreamHeader(
self,
casterUrl: str,
ntripMountPoint: str,
ntripUser: str = None,
ntripPassword: str = None,
nmeaString: str = None,
) -> None:
"""
Parameters
----------
casterUrl : str
DESCRIPTION.
ntripMountPoint : str
DESCRIPTION.
ntripUser : str, optional
DESCRIPTION. The default is None.
ntripPassword : str, optional
DESCRIPTION. The default is None.
nmeaString : str, optional
DESCRIPTION. The default is None.
: TYPE
DESCRIPTION.
Returns
-------
None
DESCRIPTION.
"""
self.casterUrl = urlsplit(casterUrl)
self.ntripMountPoint = ntripMountPoint
timestamp = strftime("%a, %d %b %Y %H:%M:%S GMT", gmtime())
if nmeaString:
self.nmeaString = nmeaString.encode("ISO-8859-1")
if ntripUser and ntripPassword:
ntripAuth = b64encode(
(ntripUser + ":" + ntripPassword).encode("ISO-8859-1")
).decode()
self.ntripAuthString = f"Authorization: Basic {ntripAuth}\r\n"
self.ntripRequestHeader = (
f"GET /{ntripMountPoint} HTTP/1.1\r\n"
f"Host: {self.casterUrl.netloc}\r\n"
"Ntrip-Version: Ntrip/"
f"{self.ntripVersion}.0\r\n"
f"User-Agent: NTRIP {self.__CLIENTNAME}\r\n"
+ self.ntripAuthString
+ self.nmeaString
+ f"Date: {timestamp}\r\n"
"Connection: close\r\n"
"\r\n"
).encode("ISO-8859-1")
def setRequestServerHeader(
self,
casterUrl: str,
ntripMountPoint: str,
ntripUser: str = None,
ntripPassword: str = None,
ntripVersion: int = 2,
) -> None:
"""
Parameters
----------
casterUrl : str
DESCRIPTION.
ntripMountPoint : str
DESCRIPTION.
ntripUser : str, optional
DESCRIPTION. The default is None.
ntripPassword : str, optional
DESCRIPTION. The default is None.
ntripVersion : int, optional
DESCRIPTION. The default is 2.
: TYPE
DESCRIPTION.
Returns
-------
None
DESCRIPTION.
"""
self.casterUrl = urlsplit(casterUrl)
if ntripVersion == 1:
self.ntripVersion = 1
timestamp = strftime("%a, %d %b %Y %H:%M:%S GMT", gmtime())
if self.ntripVersion == 2:
if ntripUser and ntripPassword:
ntripAuth = b64encode(
(ntripUser + ":" + ntripPassword).encode("ISO-8859-1")
).decode()
self.ntripAuthString = f"Authorization: Basic {ntripAuth}\r\n"
self.ntripRequestHeader = (
f"POST /{ntripMountPoint} HTTP/1.1\r\n"
f"Host: {self.casterUrl.netloc}\r\n"
"Ntrip-Version: Ntrip/"
f"{self.ntripVersion}.0\r\n"
+ self.ntripAuthString
+ "User-Agent: NTRIP "
f"{self.__CLIENTNAME}\r\n"
f"Date: {timestamp}\r\n"
"Connection: close\r\n"
"\r\n"
).encode("ISO-8859-1")
elif self.ntripVersion == 1:
if ntripPassword:
ntripAuth = b64encode(ntripPassword.encode("ISO-8859-1")).decode()
self.ntripRequestHeader = (
f"SOURCE {ntripAuth} "
f"/{ntripMountPoint} HTTP/1.1\r\n"
"Source-Agent: NTRIP "
f"{self.__CLIENTNAME}\r\n"
"\r\n"
).encode("ISO-8859-1")
def getHeaderStrings(self, rawHeader: Union[bytes, list]) -> str:
"""
Parameters
----------
rawHeader : [bytes, list]
DESCRIPTION.
Returns
-------
str
DESCRIPTION.
"""
if isinstance(rawHeader, bytes):
headerStrings = rawHeader.decode("ISO-8859-1").split("\r\n")
if isinstance(rawHeader, list):
headerStrings = []
for rawLine in rawHeader:
headerStrings.append(rawLine.decode("ISO-8859-1").rstrip())
return headerStrings
async def getNtripResponseHeader(self) -> None:
"""
Raises
------
ConnectionError
DESCRIPTION.
Returns
-------
None
DESCRIPTION.
"""
self.ntripResponseHeader = []
ntripResponseHeaderTimestamp = []
rawHeader = []
while True:
try:
line = await self.ntripReader.readline()
except (asyncio.IncompleteReadError, asyncio.LimitOverrunError) as error:
raise ConnectionError(
f"Connection to {self.casterUrl} failed with: {error}"
) from None
logging.error(f"Connection to {self.casterUrl} failed with: {error}")
ntripResponseHeaderTimestamp.append(time())
if not line:
break
rawHeader.append(line)
if line.decode("ISO-8859-1").rstrip() == "":
break
self.ntripResponseHeader = self.getHeaderStrings(rawHeader)
if "Transfer-Encoding: chunked".lower() in [
line.lower() for line in self.ntripResponseHeader
]:
self.ntripStreamChunked = True
logging.info(f"{self.ntripMountPoint}: Stream is chunked")
statusResponse = self.ntripResponseHeader[0].split(" ")
if len(statusResponse) > 1:
self.ntripResponseStatusCode = statusResponse[1]
else:
self.ntripResponseStatusCode = 0
def ntripResponseStatusOk(self) -> bool:
"""
Raises
------
ConnectionError
DESCRIPTION.
Returns
-------
bool
DESCRIPTION.
"""
if self.ntripResponseStatusCode == "200":
self.rtcmFramePreample = False
self.rtcmFrameAligned = False
return True
else:
logging.error(
f"{self.ntripMountPoint}: Response error "
f"{self.ntripResponseStatusCode}!"
)
for line in self.ntripResponseHeader:
logging.error(f"{self.ntripMountPoint}: TCP response: {line}")
raise ConnectionError(
f"{self.ntripMountPoint}: {self.ntripResponseHeader[0]}"
)
self.ntripWriter.close()
return False
async def sendRequestHeader(self) -> None:
"""
Returns
-------
None
DESCRIPTION.
"""
self.ntripWriter.write(self.ntripRequestHeader)
await self.ntripWriter.drain()
for line in self.getHeaderStrings(self.ntripRequestHeader):
logging.debug(f"TCP request: {line}")
logging.info(f"{self.ntripMountPoint}: Request sent.")
await self.getNtripResponseHeader()
if self.ntripResponseStatusOk():
for line in self.ntripResponseHeader:
logging.debug(f"TCP response: {line}")
async def requestSourcetable(self, casterUrl: str) -> list:
"""
Parameters
----------
casterUrl : str
DESCRIPTION.
Raises
------
ConnectionError
DESCRIPTION.
Returns
-------
list
DESCRIPTION.
"""
await self.openNtripConnection(casterUrl)
self.setRequestSourceTableHeader(casterUrl)
await self.sendRequestHeader()
ntripSourcetable = []
while True:
try:
line = await self.ntripReader.readline()
except (asyncio.IncompleteReadError, asyncio.LimitOverrunError) as error:
raise ConnectionError(
f"Connection to {self.casterUrl} failed with: {error}"
) from None
logging.error(f"Connection to {self.casterUrl} failed with: {error}")
return []
if not line:
break
line = line.decode("ISO-8859-1").rstrip()
if line == "ENDSOURCETABLE":
ntripSourcetable.append(line)
self.ntripWriter.close()
logging.info("Sourcetabel received.")
break
else:
ntripSourcetable.append(line)
return ntripSourcetable
async def requestNtripServer(
self,
casterUrl: str,
mountPoint: str,
user: str = None,
passwd: str = None,
ntripVersion: int = 2,
) -> None:
"""
Parameters
----------
casterUrl : str
DESCRIPTION.
mountPoint : str
DESCRIPTION.
user : str, optional
DESCRIPTION. The default is None.
passwd : str, optional
DESCRIPTION. The default is None.
ntripVersion : int, optional
DESCRIPTION. The default is 2.
: TYPE
DESCRIPTION.
Returns
-------
None
DESCRIPTION.
"""
self.ntripVersion = ntripVersion
self.ntripMountPoint = mountPoint
await self.openNtripConnection(casterUrl)
self.setRequestServerHeader(
self.casterUrl.geturl(), self.ntripMountPoint, user, passwd
)
await self.sendRequestHeader()
async def requestNtripStream(
self, casterUrl: str, mountPoint: str, user: str = None, passwd: str = None
) -> None:
"""
Parameters
----------
casterUrl : str
DESCRIPTION.
mountPoint : str
DESCRIPTION.
user : str, optional
DESCRIPTION. The default is None.
passwd : str, optional
DESCRIPTION. The default is None.
Returns
-------
None
DESCRIPTION.
"""
self.ntripMountPoint = mountPoint
await self.openNtripConnection(casterUrl)
self.setRequestStreamHeader(
self.casterUrl.geturl(), self.ntripMountPoint, user, passwd
)
await self.sendRequestHeader()
async def sendRtcmFrame(self, rtcmFrame: BitStream) -> None:
self.ntripWriter.write(rtcmFrame)
await self.ntripWriter.drain()
async def getRtcmFrame(self):
rtcm3FramePreample = Bits(bin="0b11010011")
rtcm3FrameHeaderFormat = "bin:8, pad:6, uint:10"
rtcmFrameComplete = False
while not rtcmFrameComplete:
if self.ntripStreamChunked:
try:
rawLine = await self.ntripReader.readuntil(b"\r\n")
length = int(rawLine[:-2].decode("ISO-8859-1"), 16)
rawLine = await self.ntripReader.readexactly(length + 2)
except (
asyncio.IncompleteReadError,
asyncio.LimitOverrunError,
) as error:
raise ConnectionError(
f"Connection to {self.casterUrl} failed with: {error}"
"during data reception."
) from None
logging.error(
f"Connection to {self.casterUrl} failed with: {error}"
"during data reception."
)
if rawLine[-2:] != b"\r\n":
logging.error(
f"{self.ntripMountPoint}:Chunk malformed. "
"Expected \r\n as ending. Closing connection!"
)
raise IOError("Chunk malformed ")
receivedBytes = BitStream(rawLine[:-2])
logging.debug(f"Chunk {receivedBytes.length}:{length * 8}. ")
else:
rawLine = await self.ntripReader.read(2048)
receivedBytes = BitStream(rawLine)
timeStamp = time()
if self.ntripStreamChunked and receivedBytes.length != length * 8:
logging.error(
f"{self.ntripMountPoint}:Chunk incomplete "
f"{receivedBytes.length}:{length * 8}. "
"Closing connection! "
)
raise IOError("Chunk incomplete ")
self.rtcmFrameBuffer += receivedBytes
if not self.rtcmFrameAligned:
rtcmFramePos = self.rtcmFrameBuffer.find(
rtcm3FramePreample, bytealigned=True
)
if rtcmFramePos:
firstFrame = rtcmFramePos[0]
self.rtcmFrameBuffer = self.rtcmFrameBuffer[firstFrame:]
self.rtcmFramePreample = True
else:
self.rtcmFrameBuffer = BitStream()
if self.rtcmFramePreample and self.rtcmFrameBuffer.length >= 48:
(rtcmPreAmple, rtcmPayloadLength) = self.rtcmFrameBuffer.peeklist(
rtcm3FrameHeaderFormat
)
rtcmFrameLength = (rtcmPayloadLength + 6) * 8
if self.rtcmFrameBuffer.length >= rtcmFrameLength:
rtcmFrame = self.rtcmFrameBuffer[:rtcmFrameLength]
calcCrc = crc24q(rtcmFrame[:-24])
frameCrc = rtcmFrame[-24:].unpack("uint:24")
if calcCrc == frameCrc[0]:
self.rtcmFrameAligned = True
self.rtcmFrameBuffer = self.rtcmFrameBuffer[rtcmFrameLength:]
rtcmFrameComplete = True
else:
self.rtcmFrameAligned = False
self.rtcmFrameBuffer = self.rtcmFrameBuffer[8:]
logging.warning(
f"{self.ntripMountPoint}:CRC mismatch "
f"{hex(calcCrc)} != {rtcmFrame[-24:]}."
f" Realigning!"
)
return rtcmFrame, timeStamp