from ctypes import Structure, c_ubyte, c_uint16, c_uint32, c_int16
|
from bitarray import bitarray, util
|
import math
|
|
"""
|
UDP packets 0 --> ... --> UDP packets n
|
Handshake --> HeaderBlock --> DetectionBlock --> TrackerBlock --> FooterBlock
|
|
"""
|
|
|
class Handshake(Structure):
|
"""
|
Handshake structure, 24 bytes
|
"""
|
_pack_ = 1
|
_fields_ = [
|
('Magic', c_ubyte*8),
|
('FrameDataLength', c_uint32),
|
('Reserved', c_ubyte * 12)
|
]
|
|
|
class HeaderBlock(Structure):
|
"""
|
Header structure, 48 bytes
|
"""
|
_pack_ = 1
|
_fields_ = [
|
('Magic', c_ubyte * 8), # [2 1 4 3 6 5 8 7]
|
('FrameNumber', c_uint32), # frame number
|
('VersionNumber', c_uint32), # Version number, uint32_t format: MMddhhmm
|
('NumberOfDetection', c_uint16), # Number of Detection in the frame
|
('NumberOfTrack', c_uint16), # Number of Detection in the frame
|
('HostSpeed', c_uint16), # divide by 100 to get ego speed in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s)
|
('HostAngle', c_uint16), # divide by 100 to get host angle in degree. Clockwise rotation corresponds to positive rotation.(e.g. 1400 = 14 deg)
|
('Reserved', c_ubyte * 8),
|
('RangeAccuracyIdx', c_uint16), # divide by 10000 to get value in m
|
('DopplerAccuracyIdx', c_uint16), # divide by 10000 to get value in m/s
|
('AzimuthAccuracyIdx', c_uint16), # divide by 10000 to get value in degree
|
('ElevationAccuracyIdx', c_uint16), # divide by 10000 to get value in degree
|
('DspWorkload', c_ubyte),
|
('ARMWorkload', c_ubyte),
|
('Reserved2', c_ubyte * 6),
|
]
|
def __new__(cls, buf):
|
# new instance from buf
|
return cls.from_buffer_copy(buf)
|
|
def __init__(self, data):
|
# claculate parameters
|
self.RangeAccuracy = self.RangeAccuracyIdx / 10000.0
|
self.DopplerAccuracy = self.DopplerAccuracyIdx / 10000.0
|
self.AzimuthAccuracy = self.AzimuthAccuracyIdx / 10000.0
|
self.ElevationAccuracy = self.ElevationAccuracyIdx / 10000.0
|
|
|
|
class FooterBlock(Structure):
|
"""
|
Footer structure, 32 bytes
|
"""
|
_pack_ = 1
|
_fields_ = [
|
('Reserved1', c_ubyte * 8),
|
('RangeAccuracyIdx', c_uint16), # divide by 10000 to get value in m
|
('DopplerAccuracyIdx', c_uint16), # divide by 10000 to get value in m
|
('AzimuthAccuracyIdx', c_uint16), # divide by 10000 to get value in degree
|
('ElevationAccuracyIdx', c_uint16), # divide by 10000 to get value in degree
|
('Reserved2', c_ubyte * 16),
|
]
|
def __new__(cls, buf):
|
# new instance from buf
|
return cls.from_buffer_copy(buf)
|
|
def __init__(self, data):
|
# claculate parameters
|
self.RangeAccuracy = self.RangeAccuracyIdx / 10000.0
|
self.DopplerAccuracy = self.DopplerAccuracyIdx / 10000.0
|
self.AzimuthAccuracy = self.AzimuthAccuracyIdx / 10000.0
|
self.ElevationAccuracy = self.ElevationAccuracyIdx / 10000.0
|
|
|
class DetectionBlock:
|
"""
|
Detection structure
|
using bitarray parse structure because of ctypes can't parse compact bitfields
|
"""
|
|
def __init__(self, header: HeaderBlock, footer: FooterBlock, data):
|
buf = bytearray(data)
|
buf.reverse()
|
bits = bitarray()
|
bits.frombytes(bytes(buf))
|
# print(bits)
|
# parse data structure
|
self.Flag = bits[1]
|
self.RangeIndex = util.ba2int(bits[54:64])
|
self.DropplerIndex = util.ba2int(bits[45:54])
|
if bits[44]:
|
self.DropplerIndex -= 512
|
self.AzimuthIndex = util.ba2int(bits[35:44])
|
if bits[34]:
|
self.AzimuthIndex -= 512
|
self.BetaIndex = util.ba2int(bits[25:34])
|
if bits[24]:
|
self.BetaIndex -= 512
|
self.PowerValue = util.ba2int(bits[8:24])
|
|
if not self.Flag:
|
# using header's parameters
|
RangeAccuracy = header.RangeAccuracy
|
DopplerAccuracy = header.DopplerAccuracy
|
AzimuthAccuracy = header.AzimuthAccuracy
|
ElevationAccuracy = header.ElevationAccuracy
|
else:
|
# using footer's parameters
|
RangeAccuracy = footer.RangeAccuracy
|
DopplerAccuracy = footer.DopplerAccuracy
|
AzimuthAccuracy = footer.AzimuthAccuracy
|
ElevationAccuracy = footer.ElevationAccuracy
|
|
# claculation
|
self.Power = self.PowerValue / 100.0
|
self.Range = self.RangeIndex * RangeAccuracy
|
self.Doppler = self.DropplerIndex * DopplerAccuracy
|
self.Beta = self.BetaIndex * ElevationAccuracy
|
self.Azimuth = self.AzimuthIndex * AzimuthAccuracy
|
# degrees to radians
|
alpha = math.radians(self.Azimuth)
|
beta = math.radians(self.Beta)
|
# XYZ: The XYZ coordinates can be retrieved using the following equation:
|
# X = Range * Sin(Alpha) * Cos(Beta)
|
# Y = Range * Sin(Beta)
|
# Z = Range * Cos(Alpha) * Cos(Beta)
|
# or
|
# Z = sqrt(Range^2 - X^2 - Y^2)
|
self.X = self.Range * math.sin(alpha) * math.cos(beta)
|
self.Y = self.Range * math.sin(beta)
|
self.Z = math.sqrt(self.Range * self.Range -
|
self.X * self.X - self.Y * self.Y)
|
|
class TrackerBlock(Structure):
|
"""
|
Tracker structure
|
"""
|
_pack_ = 1
|
_fields_ = [
|
('TrackID', c_uint32), # 0 to 4294967296 Track ID
|
('XPos', c_int16), # -32768 to 32767, divide by 100 to get x in m (e.g. 6453 = 64.53 m, e.g.: -2456 = -24.56 m)
|
('YPos', c_int16), # -32768 to 32767, divide by 100 to get y in m (e.g. 6453 = 64.53 m, e.g. -2456 = -24.56 m)
|
('ZPos', c_uint16), # 0 to 65535, divide by 100 to get z in m (e.g. 12654 = 126.54 m)
|
('XDot', c_int16), # -32768 to 32767, divide by 100 to get speed in the x direction in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s)
|
('YDot', c_int16), # -32768 to 32767, divide by 100 to get speed in the y direction in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s)
|
('ZDot', c_int16), # -32768 to 32767, divide by 100 to get speed in the z direction in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s)
|
('Res1', c_uint16),
|
('Res2', c_uint16),
|
('Res3', c_uint16),
|
('Flag', c_uint16), # 0 to 65535. 16 1-bit flags, the flag definitions are internal to Oculii.
|
# Bit 3 – Bit 15 : Reserved
|
# Bit 0, Bit 1, Bit 2 : Track Quality. Currently only values 1 and 2 are used. Filter to use only value ‘2’.
|
('Class', c_uint16), # 0 to 5. Reserved for future.
|
# 0: Unknown Class
|
# 1: Pedestrian
|
# 2: Motorcycle/Bike
|
# 3: Vehicle and SUV
|
# 4: Bus and Truck
|
# 5: Background
|
('Conf', c_uint16),
|
('Res4', c_uint16),
|
('Res5', c_uint16),
|
]
|
