diff --git a/iwr6843_utils/parse_tlv.py b/iwr6843_utils/parse_tlv.py index 45a1313..7b66124 100644 --- a/iwr6843_utils/parse_tlv.py +++ b/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__": diff --git a/iwr6843_utils/serial_iwr6843.py b/iwr6843_utils/serial_iwr6843.py index cb77972..4684f52 100644 --- a/iwr6843_utils/serial_iwr6843.py +++ b/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 \ No newline at end of file diff --git a/main_iwr6843.py b/main_iwr6843.py index 6b1eb4b..8e16e47 100644 --- a/main_iwr6843.py +++ b/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, diff --git a/utils/data_utils.py b/utils/data_utils.py index 8838f92..84b440a 100644 --- a/utils/data_utils.py +++ b/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