Tlv debug

iwr6843_utils/parse_tlv.py

@@ -16,7 +16,7 @@ def tlvHeaderDecode(data):
     return tlvType, tlvLength
 
 
-def parseDetectedObjects(data, numObj, tlvLength):
+def parseDetectedObjects(data, numObj, tlvLength): #TODO debug this part
     detectedPoints = struct.unpack(str(numObj * 4) + 'f', data[:tlvLength])
     detectedPoints = np.asarray(detectedPoints).reshape(numObj, 4)
     # print(detectedPoints)
@@ -43,7 +43,7 @@ def parseStats(data, tlvLength):
 
 
 
-def tlvHeader(in_data):
+def tlvHeader(in_data): #TODO get rid of the buffer overflow problem
     """
 
     :param in_data:
@@ -54,21 +54,27 @@ def tlvHeader(in_data):
 
     # print('Current data len is: ' + str(len(in_data)))
     offset = in_data.find(magic)
+    if offset == -1:
+        print('no magic found')
+        return False, None, None, True
     data = in_data[offset:]
     if len(data) < headerLength:
-        return False, None, None
+        return False, None, None, False
     try:
         magic, version, length, platform, frameNum, cpuCycles, numObj, numTLVs = struct.unpack('Q7I',
-                                                                                               data[:headerLength])
+                                                                                               data[:headerLength]) #would error if length is too low < 36
     except struct.error:
         # print ("Improper TLV structure found: ", (data,))
-        return False, None, None
+        return False, None, None, False
     # print("Packet ID:\t%d "%(frameNum))
     # print("Version:\t%x "%(version))
     # print("Data Len:\t\t%d", length)
     # print("TLV:\t\t%d "%(numTLVs))
     # print("Detect Obj:\t%d "%(numObj))
     # print("Platform:\t%X "%(platform))
+    if length > 3200-36: #To reset the buffer and drop the frame when the TLV is too large
+        print('length over 3200')
+        return False, None, None, True
     if version > 0x01000005 and len(data) >= length:
         try:
             subFrameNum = struct.unpack('I', data[36:40])[0]
@@ -79,6 +85,8 @@ def tlvHeader(in_data):
 
             for i in range(numTLVs):
                 tlvType, tlvLength = tlvHeaderDecode(data[:8])
+                if tlvLength > 3024 - 8:  # to get rid of the frame that would cause overflow
+                    return False, None, None, True
                 data = data[8:]
                 if tlvType == 1:
                     # print('Outputting Points')
@@ -94,12 +102,12 @@ def tlvHeader(in_data):
                 data = data[tlvLength:]
                 pendingBytes -= (8 + tlvLength)
             data = data[pendingBytes:]  # data that are left
-            return True, data, detected_points
+            return True, data, detected_points, False
         except struct.error:
             # print('Packet is not complete yet')
             pass
 
-    return False, None, None
+    return False, None, None, False
 
 
 if __name__ == "__main__":

iwr6843_utils/serial_iwr6843.py

@@ -74,18 +74,22 @@ def parse_stream(data_port):
 
     try:
         data_buffer += data_port.read(data_chunk_size)
-
         if len(data_buffer) > data_buffer_max_size:
             print('Buffer Overflows')
             raise KeyboardInterrupt
+        #Might have to add data_port.read = data_temp; and if data_temp is not b''
+        is_packet_complete, leftover_data, detected_points, signal_reset = tlvHeader(data_buffer)
 
-        is_packet_complete, leftover_data, detected_points = tlvHeader(data_buffer)
-
-        if is_packet_complete:
+        if is_packet_complete & ~signal_reset:
             data_buffer = b'' + leftover_data
             return detected_points
+        elif signal_reset: #added reset bit
+            data_buffer = b''  #test if this works or not
+            print("Resetted")
+            return None
         else:
             return None
+
     except serial.serialutil.SerialException as ssse:
         print('Data port not open.')
         return

main_iwr6843.py

@@ -214,8 +214,8 @@ def main():
     # input_thread.start()
 
     configFileName = 'profiles/profile_further_tuned.cfg'
-    dataPortName = 'COM9'
-    userPortName = 'COM8'
+    dataPortName = 'COM3'
+    userPortName = 'COM5'
 
     # open the serial port to the radar
     user_port, data_port = serial_iwr6843.serialConfig(configFileName, dataPortName=dataPortName,

utils/data_utils.py

@@ -83,7 +83,7 @@ def produce_voxel(points, isCluster=True, isClipping=False):
     if len(points) == 0:  # if there's no detected points
         return np.zeros(tuple([1] + volume_shape))
 
-    points_new = np.asarray([x for x in points if 1.0 > x[3] > -1.0])
+    points_new = np.asarray([x for x in points if 1.0 > x[3] > -1.0]) # TODO Velcity outta bound handled here
     if not np.all(points_new == points):
         print('Warning: point VELOCITY out of bound')
     points = points_new