############################################# ############################################# ### ### ### Drone-Radars Capstone Team ### ### Point-Cloud Mapping with AWR1642 ### ### Updated: 18/02/2018, 1200H ### ### ### ############################################# ############################################# import os import numpy as np import matplotlib.pyplot as plt from matplotlib import cm from mpl_toolkits.mplot3d import Axes3D ''' Accessing the directory of the code and the experimental data ''' # Ensure that no space in the file name full_path = os.getcwd() filename = "21point7seconds_PEOPLE.dat" filename = full_path + "\\experiment_data\\4exp_samjeremyanu\\" + filename ''' Indexing the MMWave Demo Output Information Sent to Host messages ''' # Index will be useful later when reading through binary data in the sequence below MMWDEMO_OUTPUT_MSG_DETECTED_POINTS = 1; MMWDEMO_OUTPUT_MSG_RANGE_PROFILE = 2; MMWDEMO_OUTPUT_MSG_NOISE_PROFILE = 3; MMWDEMO_OUTPUT_MSG_AZIMUT_STATIC_HEAT_MAP = 4; MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP = 5; MMWDEMO_OUTPUT_MSG_STATS = 6; MMWDEMO_OUTPUT_MSG_MAX = 7; ''' Initialising all plotting settings and variables here for testing ''' # You may change any of the variables below to change details about the scatter plot print_frequency = 1 # Plot the graph for every x frames plot_mode = 1 # 1 Represents plotting x,y info. 0 represents plotting doppler,range info algorithm_button = 0; # For future use Threshold = 0 # Set a Threshold Factor between 0 < Threshold < 10000 for detected object amplitudes to cut off FPS = 10.0 # Frames per second of the radar update ScatterX = [] # X axis coordinates for the scatter plot ScatterY = [] # Y axis coordinates for the scatter plot ScatterZ = [] # Z axis coordinates for the scatter plot ScatterAmp = [] # Brightness values for each point FrameCount = 0 # Counter for number of frames plotted Position = np.array([0,0,0]) # Starting Position of Drone PosArray = np.array([]) # Nested Array of all the positions of the Drone Velocity = np.array([-0.018433,0,0]) # XYZ Velocity of Radar in m/s, assume some constant average velocity fin = open(filename, "rb") # Reading the binary file counter = 1 ''' Initialising a dictionary for the data structure ''' # Data is saved as a dictionary containing all frames. The structure is as follows: data = {} # Data (Dict) -> Frame # Frame -> MagicWord + Version + PktLen + Platform + Frame No + TimeCPUCycles + NumDetObj + numTLVs + TLV # TLV -> Contains all TLVs of all the information ''' Define some function that we can use to get the position of our drone ''' # This function will eventually depend on the output from our Drone's Gyro units and INUs def Pos(S,V,T): # Some arbitrary function we assume can give us our drone's position after dt assuming constant velocity return (S+(V*T)) # Returns an XYZ array of the final position of the velocity after timestep T ''' Reading the binary data through the DAT file ''' try: # This currently relies on a saved DAT file, eventually we will have to read the data in real time while True: # Get start position print(Position) magicword = [] nbyte = 0 a = np.fromfile(fin,'uint16',4) for i in a: magicword.append(hex(i)) try: if magicword[0]!='0x102' or magicword[1]!='0x304' or magicword[2]!='0x506' or magicword[3]!='0x708': print ("the magic word now is ", magicword) print ("Data not correct!") print (counter) break except BaseException: print ("end of file") break version = np.int((np.fromfile(fin,'uint32',1) ) ) totalPacketLen = np.int((np.fromfile(fin,'uint32',1) ) ) platform = np.int((np.fromfile(fin,'uint32',1) ) ) frameNumber = np.int((np.fromfile(fin,'uint32',1) ) ) timeCpuCycles = np.int((np.fromfile(fin,'uint32',1) ) ) numDetectedObj = np.int((np.fromfile(fin,'uint32',1) ) ) numTLVs = np.int((np.fromfile(fin,'uint32',1) ) ) subFrameNumber = np.int((np.fromfile(fin,'uint32',1) ) ) nbyte += 40 tlv = {} data[counter] = {"magicword":magicword,"version":version,"totalPacketLen":totalPacketLen,"platform":platform,\ "frameNumber":frameNumber,"timeCpuCycles":timeCpuCycles,\ "numDetectedObj":numDetectedObj,"numTLVs":numTLVs,\ "tlv":tlv} #print ("version: ",version) #print ("totalPacketLen: ",totalPacketLen) #print ("platform: ",platform) #print ("framenumber: ",frameNumber) #print ("timeCpuCycles: ",timeCpuCycles) #print ("numTLVs: ", numTLVs) #print ("numDetectedObj: ", numDetectedObj) #print ("subFrameNumber: ",subFrameNumber) for i in range(0,numTLVs): tlv_type = np.int(np.fromfile(fin,'uint32',1)) tlv_length = np.int((np.fromfile(fin,'uint32',1))) nbyte += 8 max_x = 0 max_y = 0 max_range = 0 max_doppler = 0 if tlv_type == MMWDEMO_OUTPUT_MSG_DETECTED_POINTS: # case 1 tlv_numDetectedObj = np.int(np.fromfile(fin,'uint16',1)) tlv_xyzQformat = np.int(np.fromfile(fin,'uint16',1)) nbyte+=4 """ For x y z data structure, please refer to: html/index.html#dataXYZ It can be clearly seen that the x,y coordinate is not a twisted cordinate but normal cartesian coordinate. """ for numObject in range(0,tlv_numDetectedObj): the_obj_data = {} the_obj_data["rangeidx"] = np.int(np.fromfile(fin,'uint16',1)) the_obj_data["doppleridx"] = np.int(np.fromfile(fin,'int16',1)) the_obj_data["peakVal"] = np.int(np.fromfile(fin,'uint16',1))**0.1 the_obj_data["x"] = np.float((np.fromfile(fin,'int16',1))*(2**(-1*tlv_xyzQformat))) - Position[0] the_obj_data["y"] = np.float((np.fromfile(fin,'int16',1))*(2**(-1*tlv_xyzQformat))) - Position[1] the_obj_data["z"] = np.float((np.fromfile(fin,'int16',1))*(2**(-1*tlv_xyzQformat))) - Position[2] nbyte+=12 tlv[numObject]=the_obj_data if (counter % print_frequency == 0) and (the_obj_data["peakVal"] > Threshold): ScatterX.append(the_obj_data["x"]) ScatterY.append(the_obj_data["y"]) ScatterZ.append(the_obj_data["z"]) # Z axis data depends on the drone's altitude output ScatterAmp.append(the_obj_data["peakVal"]) # Get a position update of the drone Position = Pos(Position,Velocity,1/FPS) print(Position) # Read the other MMDEMO output messages elif tlv_type == MMWDEMO_OUTPUT_MSG_RANGE_PROFILE: np.fromfile(fin,'int8',tlv_length) nbyte += tlv_length #second case elif tlv_type == MMWDEMO_OUTPUT_MSG_NOISE_PROFILE: #third case np.fromfile(fin,'int8',tlv_length) nbyte += tlv_length elif tlv_type == MMWDEMO_OUTPUT_MSG_AZIMUT_STATIC_HEAT_MAP: #fourth case np.fromfile(fin,'int8',tlv_length) nbyte += tlv_length elif tlv_type == MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP: #fifth case np.fromfile(fin,'int8',tlv_length) nbyte += tlv_length elif tlv_type == MMWDEMO_OUTPUT_MSG_STATS: #sixth case np.fromfile(fin,'int8',tlv_length) nbyte += tlv_length elif tlv_type == MMWDEMO_OUTPUT_MSG_MAX: #seventh case np.fromfile(fin,'int8',tlv_length) nbyte += tlv_length ###### last case ##### else: ''' print "tlv data not correct in round %i"%i ''' np.fromfile(fin,'int8',tlv_length) nbyte += tlv_length counter += 1 np.fromfile(fin,'int8',totalPacketLen-nbyte) except BaseException: print ("current round is:",counter) plt.show() fin.close() ####### PLEASE WRITE THE SIGNAL PROCESSING PART BELOW THIS ############ fig = plt.figure() ax = Axes3D(fig) ax.scatter(ScatterX,ScatterY,ScatterZ,c=ScatterAmp,cmap=cm.rainbow) ax.set_xlabel('X Axis') ax.set_ylabel('Y Axis') ax.set_zlabel('Z Axis') plt.show()
Run
Reset
Share
Import
Link
Embed
Language▼
English
中文
Python Fiddle
Python Cloud IDE
Follow @python_fiddle
Browser Version Not Supported
Due to Python Fiddle's reliance on advanced JavaScript techniques, older browsers might have problems running it correctly. Please download the latest version of your favourite browser.
Chrome 10+
Firefox 4+
Safari 5+
IE 10+
Let me try anyway!
url:
Go
Python Snippet
Stackoverflow Question