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), ]