AWR6843: Determine the influence of changes in the chirp configuration on the radar's performance

Hello,

I'm trying to understand your problem. The data is accessed from the UART stream. There is a format to how the data is output. You can find a description of that at the bottom of the People Counting User's Guide. 

For plotting purposes, do you need to create your own GUI? We have developed a python GUI included in the Industrial Toolbox. Instructions on how to use the GUI are included in the People Counting User's Guide. You could edit this python source to print out the information you are looking for. Essentially, doing this you no longer have to parse the UART stream, it's already done for you. 

I'm not sure what you mean by "I would also like to know how i could determine the target's range based on the point cloud's range". The range for each point and target can be calculated by finding the magnitude of the x, y, and z positions. 

hello,

I used the exe file of the GUI for the 3D people counting demo and it worked very well. 

But when I tried to modify the source code and run the gui_main.py program I had errors due to fbs_runtime. 

"if (compileGui):
#from fbs_runtime.application_context import ApplicationContext #user's code
from fbs_runtime.application_context.PyQt5 import ApplicationContext"

So i figured out I could directly use oob_parser.py to have direct access to data that I could plot using matplotlib as it is easier than Qt.

The frame returned by the function ReadandParseData() in the oob_parser.py file contains Point Cloud, Target List,Target Indexes, number of detected points in point cloud, number of detected targets, frame number, Fail value and classifier output info. 

There are 5 arrays for Pointcloud 

Correct me if I am wrong but:  

 "elev"     is   #1 array    ?
 "azimuth"  is  #2 array    ?
 "doppler"  is   #3 array    ?
 "range"    is   #4 array    ?
 "snr"      is   #5 array    ?

Here is an example of what I got after a run. 

number of detected targets or people count 2

xtab = [ 0.20753038 -0.96751738]

ytab = [0.65486616 0.59176791]

ztab = [-0.03903227 0.02514087]

xtab 1st target= 0.20753037929534912
ytab 1st target= 0.6548661589622498
ztab 1st target= -0.03903226554393768

array number 1 of pointcloud= [0.08322238 0.08999831 0.0968776 0.1058307 0.11469871 0.10351357
 0.11318215 0.12283201 0.11013612 0.12066517 0.13095302 0.12813612]

array number 2 of pointcloud= [0.69018774 0.68839037 0.68745598 0.75098835 0.81391689 0.68490705
 0.74888008 0.81272923 0.68246677 0.74771084 0.81146029 0.74646682]

array number 3 of pointcloud= [-0.02086187 -0.04170497 -0.04170497 -0.02275909 -0.00821986 -0.06251053
 -0.04549769 -0.00821986 -0.07635081 -0.04549769 -0.00821986 -0.04549769]

array number 4 of pointcloud= [0.27832001 0.27832001 0.13916 0.27832001 0.34804001 0.13916
 0.13916 0.27832001 0.13916 0.13916 0.27832001 0.13916 ]

array number 5 of pointcloud= [31.3599993 36.51999918 41.99999906 27.11999939 13.99999969 47.35999894
 31.75999929 18.63999958 52.11999884 35.7599992 23.51999947 38.11999915]

I was asking how I could determine the target's range based on the point cloud's range because I wanted to use a corner reflector for measures

I assumed that a  target was represented by a set of points (in the cloud points)  and after multiple code runs I noticed that there were no x,y,z positions when the object  was not a person. 

But I could sometimes have values in Pointcloud.  Therefore I assumed  that I could use the range info in pointcloud (Array number 4 ? ) but it didn't really make sense as for the example above the range array (#4 of pointcloud)  is very different from the actual range (Ytab)

The 3D people demo does detect non human target also or I am completly wrong and some sort of filter is used  ? 

Note: The oob_parser.py, gui_main.py etc were found in C:\ti\mmwave_industrial_toolbox_4_7_0\labs\people_counting\visualizer

The 2 python codes below are the provided oob_parser.py and my UartReader.py in which i call the ReadandParseUart function 

# This file intends to plot data returned by the function readAndParseUart(self) of oob_parser_py
# oob_parser is distributed by Texas instrument along with their 3D people counting DEMO.
# oob_parser.py is in \ti\mmwave_industrial_toolbox_4_7_0\labs\people_counting\visualizer

# readAndParseUart() is always called - first read the UART, then call a function to parse the specific demo output
# This will return 1 frame of data. This must be called for each frame of data that is expected. It will return a dict containing:
#   1. Point Cloud
#   2. Target List
#   3. Target Indexes
#   4. number of detected points in point cloud
#   5. number of detected targets
#   6. frame number
#   7. Fail - if one, data is bad
#   8. classifier output

# ----------------------------IMPORT----------------------------

from oob_parser import uartParserSDK
import time
import matplotlib.pyplot as plt
import datetime
import matplotlib.animation as animation
#import matplotlib;matplotlib.use("TkAgg")
from multiprocessing import Process

from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import pandas as pd

# --------------------------------PARSER OBJECT-------------------------------------

parser = uartParserSDK(type="3D People Counting")
parser.frameTime = 50  # 50ms frame time

# -------------------------------UART CONNECTION------------------------------------
try:
    uart = "COM" + str(8)
    data = "COM" + str(9)
    # TODO: find the serial ports automatically.
    parser.connectComPorts(uart, data)
except Exception as e:
    print(e)
    print('Com port connection failed')
# -------------------------------UART CONNECTION\END------------------------------------

# -------------------------------READING CONFIGURATION FILE ------------------------------------

counter = 0
chirpCount = 0
profile = {}

fname = r"ISK_6m_default.cfg"  # Configuration file
cfg_file = open(fname, 'r')
cfg = cfg_file.readlines()
print(cfg)

for line in cfg:
    args = line.split()
    if (len(args) > 0):
        if (args[0] == 'channelCfg'):
            Antennas = {'Rx1': bin(int(args[1]))[2], 'Rx2': bin(int(args[1]))[3], 'Rx3': bin(int(args[1]))[4],
                        'Rx4': bin(int(args[1]))[5], 'Tx1': bin(int(args[2]))[2], 'Tx2': bin(int(args[2]))[3],
                        'Tx3': bin(int(args[2]))[4]}
            print("\n Enabled antennas = ", Antennas, "\n")
        elif (args[0] == 'SceneryParam' or args[0] == 'boundaryBox'):
            boundaryLine = counter
            profile['leftX'] = float(args[1])
            profile['rightX'] = float(args[2])
            profile['nearY'] = float(args[3])
            profile['farY'] = float(args[4])
            profile['bottomZ'] = float(args[5])
            profile['topZ'] = float(args[6])
            profile['bottomZ'] = float(-3)
            profile['topZ'] = float(3)
        elif (args[0] == 'staticBoundaryBox'):
            staticLine = counter
        elif (args[0] == 'profileCfg'):
            profile['startFreq'] = float(args[2])
            profile['idle'] = float(args[3])
            profile['adcStart'] = float(args[4])
            profile['rampEnd'] = float(args[5])
            profile['slope'] = float(args[8])
            profile['samples'] = float(args[10])
            profile['sampleRate'] = float(args[11])
        elif (args[0] == 'frameCfg'):
            profile['numLoops'] = float(args[3])
            profile['numTx'] = float(args[2]) + 1
        elif (args[0] == 'chirpCfg'):
            chirpCount += 1
        elif (args[0] == 'sensorPosition'):
            profile['sensorHeight'] = float(args[1])
            print('Sensor Height from cfg = ', str(profile['sensorHeight']))
            profile['az_tilt'] = float(args[2])
            profile['elev_tilt'] = float(args[3])
    counter += 1

# FURTHER COMPUTATION
profile['maxRange'] = profile['sampleRate'] * 1e3 * 0.9 * 3e8 / (2 * profile['slope'] * 1e12)

profile['groudLevelZ'] = 3 - profile['sensorHeight']

bw = profile['samples'] / (profile['sampleRate'] * 1e3) * profile['slope'] * 1e12
profile['Range Resolution'] = 3e8 / (2 * bw)

Tc = (profile['idle'] * 1e-6 + profile['rampEnd'] * 1e-6) * chirpCount
lda = 3e8 / (profile['startFreq'] * 1e9)

profile['maxVelocity'] = lda / (4 * Tc)

profile['velocityRes'] = lda / (2 * Tc * profile['numLoops'] * profile['numTx'])
# -------------------------------READING CONFIGURATION FILE \ END------------------------------------


# -------------------------------SENDING CONFIGURATION TO THE BOARD---------------------------------
parser.sendCfg(cfg)
print("configuration sent")
# -------------------------------SENDING CONFIGURATION TO THE BOARD \END----------------------------


# -------------------------------CONFIGURATION RECAP'-----------------------------------------------
print("\n\n  Config profile = ", profile, "\n\n")
# -------------------------------CONFIGURATION RECAP' \END------------------------------------------


# ARRAYS TO PLOT THE EVOLUTION OF DATA (X,Y,Z positions, velocity , acceleration , etc ) of 1 target. target[0]
x = []
y = []
z = []
vx = []
vy = []
vz = []
accx = []
accy = []
accz = []
rangetab = []

# variables for EVOLUTION PLOT
plot_number = -1
plotonce = 1
plotisReady = 0
OKframenumber = 0

# ------------------DICTIONNARIES to save info about detected targets and points-------------------------------

Targets = {"targetId": [], "posX": [], "posY": [], \
           "posZ": [], "velX": [], "velY": [], "velZ": [], \
           "accX": [], "accY": [], "accZ": []}

Point_cloud = {"elev": [], "azimuth": [], "doppler": [], \
               "range": [], "snr": []}


# ------------------DICTIONNARIES to save info about detected targets and points \END-------------------------------

# ------------------ FUNCTION TO UPDATE THE 3D scatter point plot  ---------------------
#----------------------------(Not working yet because it blocks main program)--------------------#
# the scatter point plot , plots a point according to it's x,y and z coordinates
#def runGraph():
    #fig2 = plt.figure()
    # ax = fig2.add_subplot(111, projection='3d')
    # title = ax.set_title('3D position')
    #ax.set_xlabel('X Label')
    # ax.set_ylabel('Y Label')
    # ax.set_zlabel('Z Label')
    #OKframenumber=0
    #def update_graph(num):
        #data = df[df['num'] == num]
        #graph.set_data(data.x, data.y)
        # graph.set_3d_properties(data.z)
        # title.set_text('3D Test, time={}'.format(num))
        #return title, graph,


    #print("\n \n \n OK FRAMENUMBER = ", OKframenumber, "\n\n\n ")
    #df = pd.DataFrame({"num": OKframenumber, "x": Targets['posX'], "y": Targets['posY'], "z": Targets['posZ']})
    # data = df[df['num'] == OKframenumber]


    # graph, = ax.plot(data.x, data.y, data.z, linestyle="", marker="o")
    # OKframenumber += 1
    #plt.ion()
    #ani = matplotlib.animation.FuncAnimation(fig2, update_graph(OKframenumber), 19, interval=100, blit=True)
    #plt.draw()
    #plt.pause(0.001)
    #plt.show()
    #plt.close(fig2)
# ------------------ FUNTION TO UPDATE THE 3D scatter point \END ---------------------


# ------------------ Evolution  PLOT function ---------------------
def plotEvolution():
    #if (plotisReady == 1 and plotonce == 1):  # PLOTS ~2 MINUTES AFTER RUN START TIME
    fig, (pos, vel, acc) = plt.subplots(3)
    fig.suptitle('postion and velocity through time')
    pos2 = pos.twinx()
    vel2 = vel.twinx()
    acc2 = acc.twinx()
    # pos.plot(time_plot, xtab, 'r', label="Posx")
    pos.plot(x, color='r', label="Posx")
    # pos.plot(time_plot, ytab, 'b', label="posY")
    pos2.plot(y, color='b', label="posY")
    # pos.plot(z, color='g', label="posZ")
    # pos.xlabel("time")
    pos.set_ylabel("Position")
    pos.legend(loc="upper left")
    pos2.legend(loc="upper right")
    # pos.legend()
    vel.plot(vx, color='r', label="Velx")
    vel2.plot(vy, color='b', label="VelY")
    # vel.plot(vz, color='g', label="VelZ")
    # vel.xlabel("time")
    vel.set_ylabel("Velocity")
    vel.legend(loc="upper left")
    vel2.legend(loc="upper right")
    # vel.legend()
    acc.plot(accx, color='r', label="AccX")
    acc2.plot(accy, color='b', label="AccY")
    # Zacc.plot(accz, color='g', label="AccZ")
    # vel.xlabel("time")
    acc.set_ylabel("acceleration")
    acc.legend(loc="upper left")
    acc2.legend(loc="upper right")
    filename_plot = "target_plot_" + str((plot_number + 1)) + '.png'
    plt.savefig(filename_plot)
    plt.show()
    plotisReady = 0
    plotonce = 0

# ------------------ EVolution plot function \ END---------------------





start_time = datetime.datetime.now().minute  # START OF THE EVOLUTION PLOT TIMER




# ------------------ CONTINUOUS READ OF THE UART ---------------------
def main_program():
   # plotonce= 1
    while (1):
        sTime = datetime.datetime.now().second

        # ----------------------------------------------------------------------------------------------------
        # ----------------------------------------------------------------------------------------------------
        # ------------------ READING AND PARSE OF DATA  {use of readandParseUART() } -------------------------

        data = parser.readAndParseUart()

        # -------------------------- READING AND PARSE OF DATA -----------------------------------------------
        # ----------------------------------------------------------------------------------------------------
        # ----------------------------------------------------------------------------------------------------

        if ((datetime.datetime.now().minute - start_time) == 2):

            plotisReady = 1  # EVOLUTION PLOT IS READY TO BE PLOTTED

            # ------------------ POSITION, VELOCITY, ACCELERATION EVOLUTION PLOT of 1 target.  ---------------------

            plottedonce= plotEvolution()
            # ------------------ EVOLUTION PLOT \END ---------------------


        # print("point cloud ", data[0])
        # print("Target List", data[1])
        print("Target Indexes", data[2])
        print("number of detected points in point cloud", data[3])
        print("number of detected targets or people count", data[4])
        print("frame number", data[5])
        print("If 1, data is bad", data[6])
        print("classifier output", data[7])

        print("\nxtab =", data[1][1])
        print("\nytab =", data[1][2])
        print("\nztab =", data[1][3])
        # print("1st target ID =", data[1][0][0])

        print("xtab 1st target=", data[1][1][0])
        print("ytab 1st target=", data[1][2][0])
        print("ztab 1st target=", data[1][3][0])
        # pointCloud=data[0]

        # --------------------------- parsed data in DICTIONNARIES ----------------------

        # "targetId", "posX" ,  "posY", "posZ" "velX" "velY" "velZ" "accX" "accY""accZ"

        # Are they in meters (posX, posY, posZ)
        # in m/s for "velX" "velY" "velZ"
        # and in m^2/s for "accX" "accY""accZ"

        i = 0
        for key, value in Targets.items():
            Targets[key] = data[1][i]
            i += 1


            # "elev" "azimuth" "doppler" "range" "snr"
        i = 0
        for key, value in Point_cloud.items():
            Point_cloud[key] = data[0][i]
            i += 1

        # --------------------------- parsed data in DICTIONNARIES \ END  ----------------------



        # ---------------PRINTING POINTCLOUD ARRAYS ----------------------

        # "elev" is     #1 array    ????
        # "azimuth"     #2 array    ????
        # "doppler"     #3 array    ????
        # "range"       #4 array    ????
        # "snr"         #5 array    ????
        for i in range(0, len(data[0])):
            print("array  number ", i, "of pointcloud=", data[0][i])

        # print("len de data[1] or target list = ", len(data[1]))
        # print("len de data[0] or pointcloud = ", len(data[0]))

        # ---------------PRINTING POINTCLOUD ARRAYS  \ END ----------------------



        if(data[6]!=1): # if data is ok (aka a target is detected well) according to ReadandParse() if data[6] =1 data is bad


            # ------------------ POSITION, VELOCITY, ACCELERATION ARRAY UPDATE FOR EVOLUTION PLOT ---------------------
            x.append(Targets["posX"][0])
            y.append(Targets["posY"][0])
            z.append(Targets["posZ"][0])
            vx.append(Targets["velX"][0])
            vy.append(Targets["velY"][0])
            vz.append(Targets["velZ"][0])
            accx.append(Targets["accX"][0])
            accy.append(Targets["accY"][0])
            accz.append(Targets["accZ"][0])
            rangetab.append(Point_cloud["range"])



        eTime = datetime.datetime.now().second

        print("\n SLEEP TIME = ", (eTime - sTime))
        if (abs((eTime - sTime)) == 59):
            sleeptime = 0
        else:
            sleeptime = abs(eTime - sTime)
        #time.sleep(sleeptime)
        #time.sleep(0.03)

if __name__ == '__main__':
   # p = Process(target=runGraph)
   # p.start()
    main_program()
  #  p.join()

import struct
import sys
import serial
import binascii
import time
import numpy as np
import math
import datetime
import matplotlib.pyplot as plt

from graphUtilities import rotX


# Initialize this Class to create a UART Parser. Initialization takes one argument:
# 1: String Lab_Type - These can be:
#   a. 3D People Counting
#   b. SDK Out of Box Demo
#   c. Long Range People Detection²
#   d. Indoor False Detection Mitigation
#   e. (Legacy): Overhead People Counting
#   f. (Legacy) 2D People Counting
# Default is (f). Once initialize, call connectComPorts(self, UartComPort, DataComPort) to connect to device com ports.
# Then call readAndParseUart() to read one frame of data from the device. The gui this is packaged with calls this every frame period.
# readAndParseUart() will return all radar detection and tracking information.
class uartParserSDK():
    def __init__(self, type='(Legacy) 2D People Counting'):
        self.headerLength = 52
        self.magicWord = 0x708050603040102
        self.threeD = 0
        self.ifdm = 0
        self.replay = 0
        self.SDK3xPointCloud = 0
        self.SDK3xPC = 0
        self.capon3D = 0
        self.aop = 0
        self.maxPoints = 1150
        # USER DEFINED
        self.data_to_plot = {"targetId": [],"posX": [], "posY": [], \
                             "velX": [], "velY": [], "accX": [], "accY": [], \
                             "numTargets": [], "time": []}
        self.magic_found = 0
        self.Startplot_time=datetime.datetime.now().minute
        self.plotisReady=0
        self.plotonce=1
        self.plot_number=-1
        # USER DEFINED
        if (type == '(Legacy): Overhead People Counting'):
            self.threeD = 1
        elif (type == 'Sense and Detect HVAC Control'):
            self.ifdm = 1
        elif (type == 'Replay'):  # unused
            self.replay = 1
        elif (type == "SDK Out of Box Demo"):
            self.SDK3xPointCloud = 1
        elif (type == "Long Range People Detection"):
            self.SDK3xPC = 1
        elif (type == '3D People Counting'):
            self.capon3D = 1
        elif (type == 'Capon3DAOP'):  # unused
            self.capon3D = 1
            self.aop = 1
        # data storage
        self.pcPolar = np.zeros((5, self.maxPoints))
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.numDetectedObj = 0
        self.targetBufPing = np.ones((10, 20)) * -1
        self.indexBufPing = np.zeros((1, self.maxPoints))
        self.classifierOutput = []
        self.frameNum = 0
        self.missedFrames = 0
        self.byteData = bytes(1)
        self.oldData = []
        self.indexes = []
        self.numDetectedTarget = 0
        self.fail = 0
        self.unique = []
        self.savedData = []
        self.saveNum = 0
        self.saveNumTxt = 0
        self.replayData = []
        self.startTimeLast = 0
        self.saveReplay = 0
        self.savefHist = 0
        self.saveBinary = 0
        self.saveTextFile = 1
        self.fHistRT = np.empty((100, 1), dtype=np.object)
        self.plotDimension = 0
        self.getUnique = 0
        self.CaponEC = 0

        self.printVerbosity = 0  # set 0 for limited logFile printing, 1 for more logging

        if (self.capon3D):
            # 3D people counting format
            # [frame #][header,pt cloud data,target info]
            # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
            # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
            # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]
            self.textStructCapon3D = np.zeros(1000 * 3 * self.maxPoints * 10).reshape(
                (1000, 3, self.maxPoints, 10))  # [frame #][header,pt cloud data,target info]

        if (self.ifdm):
            # Sense and direct format
            # [frame #][header,pt cloud data,target info]
            # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
            # [][pt cloud][pt index][#range, azim, doppler, snr]
            # [][target][Target #][TID,x,y,vx,vy,ax,ay]
            self.textStruct2D = np.zeros(1000 * 3 * self.maxPoints * 7).reshape(
                (1000, 3, self.maxPoints, 7))  # [frame #][header,pt cloud data,target info]

    # below funtions are used for converting output of labs that do not match SDK 3.x DPIF output
    # convert 2D polar People Counting to 3D Cartesian
    def polar2Cart(self):
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[1, n])  # y
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[1, n])  # x
        self.pcBufPing[3, :] = self.pcPolar[2, 0:self.numDetectedObj]  # doppler
        self.pcBufPing[4, :] = self.pcPolar[3, 0:self.numDetectedObj]  # snr
        self.pcBufPing[2, :self.numDetectedObj] = 0  # Z is zero in 2D case

    # convert 3D people counting polar to 3D cartesian
    def polar2Cart3D(self):
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[2, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[2, n])  # z
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.sin(self.pcPolar[1, n])  # x
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.cos(self.pcPolar[1, n])  # y
        self.pcBufPing[3, :] = self.pcPolar[3, 0:self.numDetectedObj]  # doppler
        self.pcBufPing[4, :] = self.pcPolar[4, 0:self.numDetectedObj]  # snr
        # print(self.pcBufPing[:,:10])

    # decode People Counting TLV Header
    def tlvHeaderDecode(self, data):
        # print(len(data))
        tlvType, tlvLength = struct.unpack('2I', data)
        return tlvType, tlvLength

    # decode People Counting Point Cloud TLV
    def parseDetectedObjects(self, data, tlvLength):
        objStruct = '4f'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength) / 16)
        for i in range(self.numDetectedObj):
            try:
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i] = struct.unpack(
                    objStruct, data[:objSize])
                data = data[16:]
            except:
                self.numDectedObj = i
                break
        self.polar2Cart()

    # decode IFDM point Cloud TLV
    def parseDetectedObjectsIFDM(self, data, tlvLength):
        pUnitStruct = '4f'
        pUnitSize = struct.calcsize(pUnitStruct)
        pUnit = struct.unpack(pUnitStruct, data[:pUnitSize])
        data = data[pUnitSize:]
        objStruct = '2B2h'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength - 16) / objSize)
        # print('Parsed Points: ', self.numDetectedObj)
        for i in range(self.numDetectedObj):
            try:
                az, doppler, ran, snr = struct.unpack(objStruct, data[:objSize])
                data = data[objSize:]
                # get range, azimuth, doppler, snr
                self.pcPolar[0, i] = ran * pUnit[2]  # range
                if (az >= 128):
                    az -= 256
                self.pcPolar[1, i] = math.radians(az * pUnit[0])  # azimuth
                self.pcPolar[2, i] = doppler * pUnit[1]  # doppler
                self.pcPolar[3, i] = snr * pUnit[3]  # snr

                # Sense and direct format
                # [frame #][header,pt cloud data,target info]
                # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                # [][pt cloud][pt index][#range, azim, doppler, snr]
                # [][target][Target #][TID,x,y,vx,vy,ax,ay]
                self.textStruct2D[self.frameNum % 1000, 1, i, 0] = self.pcPolar[0, i]  # range
                self.textStruct2D[self.frameNum % 1000, 1, i, 1] = self.pcPolar[1, i]  # az
                self.textStruct2D[self.frameNum % 1000, 1, i, 2] = self.pcPolar[2, i]  # doppler
                self.textStruct2D[self.frameNum % 1000, 1, i, 3] = self.pcPolar[3, i]  # snr

            except:
                self.numDetectedObj = i
                break
        self.polar2Cart()

    # decode 3D People Counting Point Cloud TLV
    def parseDetectedObjects3D(self, data, tlvLength):
        objStruct = '5f'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int(tlvLength / 20)
        for i in range(self.numDetectedObj):
            try:
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i], self.pcPolar[
                    4, i] = struct.unpack(objStruct, data[:objSize])
                data = data[20:]
            except:
                self.numDectedObj = i
                print('failed to get point cloud')
                break
        self.polar2Cart3D()

    # support for Capoin 3D point cloud
    # decode Capon 3D point Cloud TLV
    def parseCapon3DPolar(self, data, tlvLength):
        pUnitStruct = '5f'  # elev, azim, doppler, range, snr
        pUnitSize = struct.calcsize(pUnitStruct)
        pUnit = struct.unpack(pUnitStruct, data[:pUnitSize])
        data = data[pUnitSize:]
        objStruct = '2bh2H'  # 2 int8, 1 int16, 2 uint16
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength - pUnitSize) / objSize)
        # if (self.printVerbosity == 1):
        # print('Parsed Points: ', self.numDetectedObj)
        for i in range(self.numDetectedObj):
            try:
                elev, az, doppler, ran, snr = struct.unpack(objStruct, data[:objSize])
                # print(elev, az, doppler, ran, snr)
                data = data[objSize:]
                # get range, azimuth, doppler, snr
                self.pcPolar[0, i] = ran * pUnit[3]  # range
                if (az >= 128):
                    print('Az greater than 127')
                    az -= 256
                if (elev >= 128):
                    print('Elev greater than 127')
                    elev -= 256
                if (doppler >= 32768):
                    print('Doppler greater than 32768')
                    doppler -= 65536
                self.pcPolar[1, i] = az * pUnit[1]  # azimuth
                self.pcPolar[2, i] = elev * pUnit[0]  # elevation
                self.pcPolar[3, i] = doppler * pUnit[2]  # doppler
                self.pcPolar[4, i] = snr * pUnit[4]  # snr

                # add pt cloud data to textStructCapon3DCapon3D for text file printing
                # self.textStructCapon3DCapon3D[,,,] = [frame #][header,pt cloud data,target info]
                # [][pt cloud = 0][pt index][#elev, azim, doppler, range, snr]
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 0] = self.pcPolar[2, i]  # elev
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 1] = self.pcPolar[1, i]  # az
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 2] = self.pcPolar[3, i]  # doppler
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 3] = self.pcPolar[0, i]  # range
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 4] = self.pcPolar[4, i]  # snr
            except:
                self.numDetectedObj = i
                print('Point Cloud TLV Parser Failed')
                break
        self.polar2Cart3D()

    # decode 2D People Counting Target List TLV
    def parseDetectedTracks(self, data, tlvLength):
        if (self.plotDimension):
            targetStruct = 'I8f9ff'
        else:
            targetStruct = 'I6f9ff'
        targetSize = struct.calcsize(targetStruct)
        self.numDetectedTarget = int(tlvLength / targetSize)
        targets = np.empty((13, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            targetData = struct.unpack(targetStruct, data[:targetSize])
            targets[0, i] = int(targetData[0])  # TID
            targets[1:3, i] = targetData[1:3]  # X,Y
            targets[3, i] = 0  # Z=0
            targets[4:6, i] = targetData[3:5]  # vX,Vy
            targets[6, i] = 0  # vZ=0
            targets[7:9, i] = targetData[5:7]  # aX,aY
            targets[9, i] = 0  # az=0
            if (self.plotDimension):
                targets[10:12, i] = targetData[7:9]
                targets[12, i] = 1
            else:
                targets[10:12, i] = [0.75, 0.75]
                targets[12, i] = 1
            data = data[targetSize:]

            if (self.saveTextFile):
                self.textStruct2D[self.frameNum % 1000, 2, i, 0] = targets[0, i]  # TID
                self.textStruct2D[self.frameNum % 1000, 2, i, 1] = targets[1, i]  # x
                self.textStruct2D[self.frameNum % 1000, 2, i, 2] = targets[2, i]  # y

                self.textStruct2D[self.frameNum % 1000, 2, i, 3] = targets[4, i]  # vx
                self.textStruct2D[self.frameNum % 1000, 2, i, 4] = targets[5, i]  # vy

                self.textStruct2D[self.frameNum % 1000, 2, i, 5] = targets[7, i]  # ax
                self.textStruct2D[self.frameNum % 1000, 2, i, 6] = targets[8, i]  # ay

                if (self.printVerbosity == 1):
                    print('target added to textStructCapon3D')
        self.targetBufPing = targets

        # decode 3D People Counting Target List TLV

    def parseDetectedTracks3D(self, data, tlvLength):
        targetStruct = 'I9f'
        targetSize = struct.calcsize(targetStruct)
        self.numDetectedTarget = int(tlvLength / targetSize)
        targets = np.empty((13, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            targetData = struct.unpack(targetStruct, data[:targetSize])
            targets[0:7, i] = targetData[0:7]
            targets[7:10, i] = [0, 0, 0]
            targets[10:13, i] = targetData[7:10]
            data = data[targetSize:]
        self.targetBufPing = targets

    # decode Target Index TLV
    def parseTargetAssociations(self, data):
        targetStruct = 'B'
        targetSize = struct.calcsize(targetStruct)
        numIndexes = int(len(data) / targetSize)
        self.indexes = []
        self.unique = []
        try:
            for i in range(numIndexes):
                ind = struct.unpack(targetStruct, data[:targetSize])
                self.indexes.append(ind[0])
                data = data[targetSize:]
            if (self.getUnique):
                uTemp = self.indexes[math.ceil(numIndexes / 2):]
                self.indexes = self.indexes[:math.ceil(numIndexes / 2)]
                for i in range(math.ceil(numIndexes / 8)):
                    for j in range(8):
                        self.unique.append(getBit(uTemp[i], j))
        except:
            print('TLV Index Parse Fail')

    # decode Classifier output
    def parseClassifierOutput(self, data):
        classifierDataStruct = 'Ii'
        clOutSize = struct.calcsize(classifierDataStruct)
        self.classifierOutput = np.zeros((2, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            self.classifierOutput[0, i], self.classifierOutput[1, i] = struct.unpack(classifierDataStruct,
                                                                                     data[:clOutSize])
            data = data[clOutSize:]

    # below is for labs that are compliant with SDK 3.x  This code can parse the point cloud TLV and point cloud side info TLV from the OOB demo.
    # It can parse the SDK3.x Compliant People Counting demo "tracker_dpc"
    # get SDK3.x Cartesian Point Cloud
    def parseSDK3xPoints(self, dataIn, numObj):
        pointStruct = '4f'
        pointLength = struct.calcsize(pointStruct)
        try:
            for i in range(numObj):
                self.pcBufPing[0, i], self.pcBufPing[1, i], self.pcBufPing[2, i], self.pcBufPing[3, i] = struct.unpack(
                    pointStruct, dataIn[:pointLength])
                dataIn = dataIn[pointLength:]
            self.pcBufPing = self.pcBufPing[:, :numObj]
        except Exception as e:
            print(e)
            self.fail = 1

    # get Side Info SDK 3.x
    def parseSDK3xSideInfo(self, dataIn, numObj):
        sideInfoStruct = '2h'
        sideInfoLength = struct.calcsize(sideInfoStruct)
        try:
            for i in range(numObj):
                self.pcBufPing[4, i], unused = struct.unpack(sideInfoStruct, dataIn[:sideInfoLength])
                dataIn = dataIn[sideInfoLength:]
        except Exception as e:
            print(e)
            self.fail = 1

    # convert SDK compliant Polar Point Cloud to Cartesian
    def polar2CartSDK3(self):
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[2, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[2, n])  # z
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.sin(self.pcPolar[1, n])  # x
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.cos(self.pcPolar[1, n])  # y
        self.pcBufPing[3, :] = self.pcPolar[3, 0:self.numDetectedObj]  # doppler

    # decode SDK3.x Format Point Cloud in Polar Coordinates
    def parseSDK3xPolar(self, dataIn, tlvLength):
        pointStruct = '4f'
        pointLength = struct.calcsize(pointStruct)
        self.numDetectedObj = int(tlvLength / pointLength)
        try:
            for i in range(self.numDetectedObj):
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i] = struct.unpack(
                    pointStruct, dataIn[:pointLength])
                dataIn = dataIn[pointLength:]
        except:
            self.fail = 1
            return
        self.polar2CartSDK3()

    # decode 3D People Counting Target List TLV

    # 3D Struct format

    # uint32_t     tid;     /*! @brief   tracking ID */
    # float        posX;    /*! @brief   Detected target X coordinate, in m */
    # float        posY;    /*! @brief   Detected target Y coordinate, in m */
    # float        posZ;    /*! @brief   Detected target Z coordinate, in m */
    # float        velX;    /*! @brief   Detected target X velocity, in m/s */
    ##float       velY;    /*! @brief   Detected target Y velocity, in m/s */
    # float        velZ;    /*! @brief   Detected target Z velocity, in m/s */
    # float        accX;    /*! @brief   Detected target X acceleration, in m/s2 */
    # float        accY;    /*! @brief   Detected target Y acceleration, in m/s2 */
    # float        accZ;    /*! @brief   Detected target Z acceleration, in m/s2 */
    # float        ec[16];  /*! @brief   Target Error covarience matrix, [4x4 float], in row major order, range, azimuth, elev, doppler */
    # float        g;
    # float        confidenceLevel;    /*! @brief   Tracker confidence metric*/

    def parseDetectedTracksSDK3x(self, data, tlvLength):
        if (self.printVerbosity == 1):
            print(tlvLength)
        if (self.CaponEC):
            targetStruct = 'I27f'
        else:
            # targetStruct = 'I15f'
            targetStruct = 'I27f'
        targetSize = struct.calcsize(targetStruct)
        if (self.printVerbosity == 1):
            print('TargetSize=', targetSize)
        self.numDetectedTarget = int(tlvLength / targetSize)
        if (self.printVerbosity == 1):
            print('Num Detected Targets = ', self.numDetectedTarget)
        targets = np.empty((16, self.numDetectedTarget))
        rotTarget = [0, 0, 0]
        # theta = self.profile['elev_tilt']
        # print('theta = ',theta)
        # Rx = np.matrix([[ 1, 0           , 0           ],
        #           [ 0, math.cos(theta),-math.sin(theta)],
        #           [ 0, math.sin(theta), math.cos(theta)]])
        try:
            for i in range(self.numDetectedTarget):
                targetData = struct.unpack(targetStruct, data[:targetSize])
                if (self.printVerbosity == 1):
                    print(targetData)
                # tid, x, y
                if (self.CaponEC):
                    targets[0:13, i] = targetData[0:13]
                else:
                    # tid, pos x, pos y
                    targets[0:3, i] = targetData[0:3]
                    if (self.printVerbosity == 1):
                        print('Target Data TID,X,Y = ', targets[0:3, i])
                        print('i = ', i)
                    # pos z
                    targets[3, i] = targetData[3]

                    # rotTargetDataX,rotTargetDataY,rotTargetDataZ = rotX (targetData[1],targetData[2],targetData[3],self.profile['elev_tilt'])

                    # print('Target Data TID,X,Y = ',rotTargetDataX,', ',rotTargetDataY,', ',rotTargetDataZ)
                    # vel x, vel y
                    targets[4:6, i] = targetData[4:6]
                    # vel z
                    targets[6, i] = targetData[6]
                    # acc x, acc y
                    targets[7:9, i] = targetData[7:9]
                    # acc z
                    targets[9, i] = targetData[9]
                    # ec[16]
                    # targets[10:14,i]=targetData[10:14]
                    targets[10:13, i] = targetData[10:13]  # Chris 2020-12-18
                    if (self.printVerbosity == 1):
                        print('ec = ', targets[10:13, i])
                    # g
                    # targets[14,i]=targetData[14]
                    targets[14, i] = targetData[26]
                    if (self.printVerbosity == 1):
                        print('g= ', targets[14, i])
                    # confidenceLevel
                    # targets[15,i]=targetData[15]
                    targets[15, i] = targetData[27]
                    if (self.printVerbosity == 1):
                        print('Confidence Level = ', targets[15, i])

                    # self.textStructCapon3D[[frame #],[header,pt cloud data,target info],index,data]
                    # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
                    # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]
                    if (self.saveTextFile):
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 0] = targets[0, i]  # TID
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 1] = targets[1, i]  # x
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 2] = targets[2, i]  # y
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 3] = targets[3, i]  # z
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 4] = targets[4, i]  # vx
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 5] = targets[5, i]  # vy
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 6] = targets[6, i]  # vz
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 7] = targets[7, i]  # ax
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 8] = targets[8, i]  # ay
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 9] = targets[9, i]  # az
                        if (self.printVerbosity == 1):
                            print('target added to textStructCapon3D')
                data = data[targetSize:]
        except:
            print('Target TLV parse failed')
        self.targetBufPing = targets
        if (self.printVerbosity == 1):
            print(targets)

    # all TLV header decoding functions are below. Each lab with a Unique header or unique TLV set has its own header parsing function
    # decode Header and rest of TLVs for Legacy Labs and Indoor False detection mitigation
    def tlvHeader(self, data):
        # search for magic word
        self.targetBufPing = np.zeros((12, 1))
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.indexes = []
        frameNum = -1
        self.numDetectedTarget = 0
        self.numDetectedObj = 0
        # search until we find magic word
        while (1):
            try:
                magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q10I2H', data[:self.headerLength])
            except:
                # bad data, return
                self.fail = 1
                return data
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                data = data[1:]
                self.magic_found = 1
            else:
                # we have correct magic word, proceed to parse rest of data
                print("\nMagic word is ok\n")
                self.magic_found= 1
                break

        # Sense and direct format
        # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
        if (self.saveTextFile):
            self.textStruct2D[self.frameNum % 1000, 0, 0, 0] = magic
            self.textStruct2D[self.frameNum % 1000, 0, 1, 0] = version
            self.textStruct2D[self.frameNum % 1000, 0, 2, 0] = platform
            self.textStruct2D[self.frameNum % 1000, 0, 3, 0] = timestamp
            self.textStruct2D[self.frameNum % 1000, 0, 4, 0] = packetLength
            self.textStruct2D[self.frameNum % 1000, 0, 5, 0] = frameNum
            self.textStruct2D[self.frameNum % 1000, 0, 6, 0] = subFrameNum
            self.textStruct2D[self.frameNum % 1000, 0, 7, 0] = chirpMargin
            self.textStruct2D[self.frameNum % 1000, 0, 8, 0] = frameMargin
            self.textStruct2D[self.frameNum % 1000, 0, 9, 0] = uartSentTime
            self.textStruct2D[self.frameNum % 1000, 0, 10, 0] = trackProcessTime
            self.textStruct2D[self.frameNum % 1000, 0, 11, 0] = numTLVs
            self.textStruct2D[self.frameNum % 1000, 0, 12, 0] = checksum
            if (self.printVerbosity == 1):
                print('FrameNumber = ', self.textStruct2D[self.frameNum % 1000, 0, 5, 0])

        if (self.frameNum != frameNum):
            self.missedFrames += 1
            self.frameNum = frameNum
        self.frameNum += 1
        if (len(data) < packetLength):
            ndata = self.dataCom.read(packetLength - len(data))
            if (self.saveBinary):
                self.oldData += ndata
            data += ndata
        data = data[self.headerLength:]
        for i in range(numTLVs):
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(data[:8])
            except:
                print('read fail: not enough data')
                self.missedFrames += 1
                self.fail = 1
                break
            try:
                data = data[8:]
                if (tlvType == 6):
                    if (self.threeD):
                        self.parseDetectedObjects3D(data[:tlvLength], tlvLength - 8)
                    elif (self.ifdm):
                        self.parseDetectedObjectsIFDM(data[:tlvLength], tlvLength - 8)
                    else:
                        self.parseDetectedObjects(data[:tlvLength], tlvLength - 8)
                elif (tlvType == 7):
                    if (self.threeD):
                        self.parseDetectedTracks3D(data[:tlvLength], tlvLength - 8)
                    else:
                        self.parseDetectedTracks(data[:tlvLength], tlvLength - 8)
                elif (tlvType == 8):
                    self.parseTargetAssociations(data[:tlvLength - 8])
                elif (tlvType == 9):
                    self.parseClassifierOutput(data[:tlvLength - 8])
                data = data[tlvLength - 8:]
            except:
                print('Not enough data')
                print('Data length: ', len(data))
                print('Reported Packet Length: ', packetLength)
                self.fail = 1
                return data
        return data

    # parsing for SDK 3.x Point Cloud
    def sdk3xTLVHeader(self, dataIn):
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        headerStruct = 'Q8I'
        headerLength = struct.calcsize(headerStruct)
        tlvHeaderLength = 8
        # search until we find magic word
        while (1):
            try:
                magic, version, totalPacketLen, platform, self.frameNum, timeCPUCycles, self.numDetectedObj, numTLVs, subFrameNum = struct.unpack(
                    headerStruct, dataIn[:headerLength])
            except:
                # bad data, return
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # we have correct magic word, proceed to parse rest of data
                break
        dataIn = dataIn[headerLength:]
        remainingData = totalPacketLen - len(dataIn)
        count = 0
        # check to ensure we have all of the data
        while (remainingData > 0 and count < 3):
            newData = self.dataCom.read(remainingData)
            remainingData = totalPacketLen - len(dataIn) - len(newData)
            dataIn += newData
            count += 1
            if (self.saveBinary):
                self.oldData += newData
        # now check TLVs
        # print ('got tlvs sdk3x')
        for i in range(numTLVs):
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
            except Exception as e:
                print(e)
                print('failed to read OOB SDK3.x TLV')
            dataIn = dataIn[tlvHeaderLength:]
            if (tlvType == 1):
                self.parseSDK3xPoints(dataIn[:tlvLength], self.numDetectedObj)
                dataIn = dataIn[tlvLength:]
            elif (tlvType == 7):
                self.parseSDK3xSideInfo(dataIn[:tlvLength], self.numDetectedObj)
                dataIn = dataIn[tlvLength:]
        return dataIn

    # parsing for SDK 3.x DPIF compliant People Counting
    def sdk3xPCHeader(self, dataIn):
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.targetBufPing = np.zeros((13, 20))
        self.indexes = []
        tlvHeaderLength = 8
        # search until we find magic word
        while (1):
            try:
                magic, version, platform, timestamp, packetLength, self.frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q10I2H', dataIn[:self.headerLength])
            except:
                # bad data, return
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # we have correct magic word, proceed to parse rest of data
                break
        dataIn = dataIn[self.headerLength:]
        remainingData = packetLength - len(dataIn)
        if (self.printVerbosity == 1):
            print('pl: ', packetLength)
            print('remainingData ', remainingData)
        # check to ensure we have all of the data
        # check to ensure we have all of the data
        count = 0
        while (remainingData > 0 and count < 3):
            if (self.printVerbosity == 1):
                print('RD Loop')
            newData = self.dataCom.read(remainingData)
            remainingData = packetLength - len(dataIn) - len(newData)
            dataIn += newData
            count += 1
            if (remainingData == 0):
                if (self.saveBinary):
                    self.oldData += newData
        # now check TLVs
        if (self.printVerbosity == 1):
            print('Frame: ', self.frameNum)
            print(len(dataIn))
            print(numTLVs)
        for i in range(numTLVs):
            try:
                # print("DataIn Type", type(dataIn))
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
                if (self.printVerbosity == 1):
                    print('TLV length = ', tlvLength)
            except Exception as e:
                if (self.printVerbosity == 1):
                    print(e)
                    print('failed to read OOB SDK3.x TLV')
                    print('TLV num: ', i)
            dataIn = dataIn[tlvHeaderLength:]
            dataLength = tlvLength
            print("\n \n TLV TYPE = ",tlvType,"\n \n")
            if (tlvType == 6):
                # DPIF Polar Coordinates
                # print('pointcloud lrpd')
                self.parseSDK3xPolar(dataIn[:dataLength], dataLength)
            elif (tlvType == 7):
                # target 3D
                self.parseDetectedTracksSDK3x(dataIn[:dataLength], dataLength)
            elif (tlvType == 8):
                # target index
                self.parseTargetAssociations(dataIn[:dataLength])
            elif (tlvType == 9):
                # side info
                self.parseSDK3xSideInfo(dataIn[:dataLength], self.numDetectedObj)
            dataIn = dataIn[dataLength:]
        return dataIn

    # parsing for 3D People Counting lab
    def Capon3DHeader(self, dataIn):
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.pcPolar = np.zeros((5, self.maxPoints))
        self.targetBufPing = np.zeros((13, 20))
        self.numDetectedTarget = 0
        self.numDetectedObj = 0
        self.indexes = []
        tlvHeaderLength = 8
        headerLength = 48
        # stay in this loop until we find the magic word or run out of data to parse
        while (1):
            try:
                magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q9I2H', dataIn[:headerLength])
            except Exception as e:
                # bad data, return
                # print("Cannot Read Frame Header")
                # print(e)
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # got magic word, proceed to parse
                break

        dataIn = dataIn[headerLength:]
        remainingData = packetLength - len(dataIn) - headerLength
        # check to ensure we have all of the data
        # print('remaining data = ',remainingData)
        if (remainingData > 0):
            newData = self.dataCom.read(remainingData)
            remainingData = packetLength - len(dataIn) - headerLength - len(newData)
            dataIn += newData
            if (self.saveBinary):
                self.oldData += newData
        if (self.saveTextFile):
            self.textStructCapon3D[self.frameNum % 1000, 0, 0, 0] = magic
            self.textStructCapon3D[self.frameNum % 1000, 0, 1, 0] = version
            self.textStructCapon3D[self.frameNum % 1000, 0, 2, 0] = packetLength
            self.textStructCapon3D[self.frameNum % 1000, 0, 3, 0] = platform
            self.textStructCapon3D[self.frameNum % 1000, 0, 4, 0] = frameNum
            self.textStructCapon3D[self.frameNum % 1000, 0, 5, 0] = subFrameNum
            self.textStructCapon3D[self.frameNum % 1000, 0, 6, 0] = chirpMargin
            self.textStructCapon3D[self.frameNum % 1000, 0, 7, 0] = frameMargin
            self.textStructCapon3D[self.frameNum % 1000, 0, 8, 0] = uartSentTime
            self.textStructCapon3D[self.frameNum % 1000, 0, 9, 0] = trackProcessTime
            self.textStructCapon3D[self.frameNum % 1000, 0, 10, 0] = numTLVs
            self.textStructCapon3D[self.frameNum % 1000, 0, 11, 0] = checksum
            if (self.printVerbosity == 1):
                print('FrameNumber = ', self.textStructCapon3D[self.frameNum % 1000, 0, 4, 0])

        # now check TLVs
        for i in range(numTLVs):
            # try:
            # print("DataIn Type", type(dataIn))
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
                dataIn = dataIn[tlvHeaderLength:]
                dataLength = tlvLength - tlvHeaderLength
            except:
                print('TLV Header Parsing Failure')
                self.fail = 1
                return dataIn
            print("\n\n \n TLV TYPE IS ",tlvType, "\n\n\n")
            if (tlvType == 6):
                # DPIF Polar Coordinates
                self.parseCapon3DPolar(dataIn[:dataLength], dataLength)
            elif (tlvType == 7):
                # target 3D
                self.parseDetectedTracksSDK3x(dataIn[:dataLength], dataLength)
            elif (tlvType == 8):
                # target index
                self.parseTargetAssociations(dataIn[:dataLength])
            elif (tlvType == 9):
                if (self.printVerbosity == 1):
                    print('type9')
                # side info
                # self.parseSDK3xSideInfo(dataIn[:dataLength], self.numDetectedObj)
            dataIn = dataIn[dataLength:]
            # except Exception as e:
            #    print(e)
            #    print ('failed to read OOB SDK3.x TLV')
        if (self.frameNum + 1 != frameNum):
            self.missedFrames += frameNum - (self.frameNum + 1)
        self.frameNum = frameNum
        return dataIn

    # This function is always called - first read the UART, then call a function to parse the specific demo output
    # This will return 1 frame of data. This must be called for each frame of data that is expected. It will return a dict containing:
    #   1. Point Cloud
    #   2. Target List
    #   3. Target Indexes
    #   4. number of detected points in point cloud
    #   5. number of detected targets
    #   6. frame number
    #   7. Fail - if one, data is bad
    #   8. classifier output
    # Point Cloud and Target structure are liable to change based on the lab. Output is always cartesian.
    def readAndParseUart(self):
        self.fail = 0
        if (self.replay):
            return self.replayHist()
        numBytes = 4666
        data = self.dataCom.read(numBytes)
        if (self.byteData is None):
            self.byteData = data
        else:
            self.byteData += data
        if (self.saveBinary):
            self.oldData += data
        # try:
        print("self.capon3D  =",self.capon3D ,"self.ifdm=",self.ifdm)
        if (self.SDK3xPointCloud == 1):
            self.byteData = self.sdk3xTLVHeader(self.byteData)
        elif (self.SDK3xPC == 1):
            self.byteData = self.sdk3xPCHeader(self.byteData)
        elif (self.capon3D == 1):
            self.byteData = self.Capon3DHeader(self.byteData)
        else:
            self.byteData = self.tlvHeader(self.byteData)
        # except Exception as e:
        #    print(e)
        #    self.fail = 1
        # return data after parsing and save to replay file
        if (self.fail):
            return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput
        if (self.saveBinary):
            if (self.frameNum % 1000 == 0):
                toSave = bytes(self.oldData)
                fileName = 'binData/pHistBytes_' + str(self.saveNum) + '.bin'
                self.saveNum += 1
                bfile = open(fileName, 'wb')
                bfile.write(toSave)
                self.oldData = []
                print('Missed Frames ' + str(self.missedFrames) + '/1000')
                self.missedFrames = 0
                bfile.close
        if (self.saveTextFile):
            if (self.frameNum % 1000 == 0):
                if (self.capon3D):
                    toSave = self.textStructCapon3D
                    print("\n\n to save for CAPON3D \n\n")
                elif (self.ifdm):
                    toSave = self.textStruct2D
                    print("\n\n to save for ifdm \n\n")
                print('Saved data file ', self.saveNumTxt)
                #fileName = 'binData/pHistText_' + str(self.saveNumTxt) + '.csv'
                fileName = 'pHistText_' + str(self.saveNumTxt) + '.csv'
                if (self.saveNumTxt < 75):
                    self.saveNumTxt += 1
                else:
                    self.saveNumTxt = 0
                tfile = open(fileName, 'w')
                tfile.write('This file contains parsed UART data in sensor centric coordinates\n')
                tfile.write('file format version 1.0 \n ')
                # tfile.write(str(toSave))

                if (self.capon3D):
                    # [frame #][header,pt cloud data,target info]
                    # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
                    # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]

                    for i in range(1000):



                        tfile.write(' for capon 3D \n ')
                        tfile.write(
                            'magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum\n')
                        self.data_to_plot["posX"].append(self.textStructCapon3D[i, 2, 0, 1])
                        self.data_to_plot["posY"].append(self.textStructCapon3D[i, 2, 0, 2])
                        self.data_to_plot["accX"].append(self.textStructCapon3D[i, 2, 0, 3])
                        self.data_to_plot["accY"].append(self.textStructCapon3D[i, 2, 0, 4])
                        timenow = str(datetime.datetime.now().hour), ':', str(datetime.datetime.now().minute)
                        timenow = "".join(timenow)
                        self.data_to_plot["time"].append(timenow)
                        #fileName = 'data_to_plot_' + str(self.saveNumTxt) + '.txt'
                        #xfile = open(fileName, 'w')
                        #xfile.write("Xposition tab")
                        #xfile.write( str(self.data_to_plot["posX"]))
                        #xfile.write("Yposition tab")
                        #xfile.write(str(self.data_to_plot["posY"]))
                        #xfile.close()
            #if (self.magic_found == 1):
                            #self.data_to_plot["posX"].append(self.textStructCapon3D[i, 2, 0, 1])
                          #  self.data_to_plot["poxY"].append(self.textStructCapon3D[i, 2, 0, 2])
                           # self.data_to_plot["accX"].append(self.textStructCapon3D[i, 2, 0, 3])
                            #self.data_to_plot["accY"].append(self.textStructCapon3D[i, 2, 0, 4])
                           # tfile.write('TID,x,y,z,vx,vy,vz,ax,ay,az\n')

                           #for j in range(np.count_nonzero(self.textStructCapon3D[i, 2, :, 1])):
                            # self.data_to_plot["accY"].append(self.textStructCapon3D[i, 2, j, 4])

                        #USER CODE: print position of 1 target (the fist one)

                        for j in range(0, 12):
                            tfile.write(str(self.textStructCapon3D[i, 0, j, 0]))
                            tfile.write(',')
                            # print(str(self.textStructCapon3D[i,0,j,0]))
                        tfile.write('\n')
                        tfile.write('elev, azim, doppler, range, snr\n')
                        for j in range(np.count_nonzero(self.textStructCapon3D[i, 1, :,
                                                        0])):  # self.numDetectedObj):#len(self.textStructCapon3D[i,1,:,0]!=0)):
                            for k in range(5):
                                tfile.write(str(self.textStructCapon3D[i, 1, j, k]))
                                tfile.write(',')
                            tfile.write('\n')

                        tfile.write('TID,x,y,z,vx,vy,vz,ax,ay,az\n')
                        for j in range(np.count_nonzero(self.textStructCapon3D[i, 2, :, 1])):
                            for k in range(10):
                                tfile.write(str(self.textStructCapon3D[i, 2, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                    self.textStructCapon3D = np.zeros(1000 * 3 * 12 * self.maxPoints).reshape(
                        (1000, 3, 12, self.maxPoints))  # [frame #][header,pt cloud data,target info]
                    tfile.close

                if (self.ifdm):
                    # Sense and direct format
                    # [frame #][header,pt cloud data,target info]
                    # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#range, azim, doppler, snr]
                    # [][target][Target #][TID,x,y,vx,vy,ax,ay]
                    for i in range(1000):
                        tfile.write(
                            'magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum\n')
                        for j in range(13):
                            tfile.write(str(self.textStruct2D[i, 0, j, 0]))
                            tfile.write(',')
                        tfile.write('\n')
                        tfile.write('range, azim, doppler, snr\n')
                        for j in range(np.count_nonzero(self.textStruct2D[i, 1, :, 0])):
                            for k in range(4):
                                tfile.write(str(self.textStruct2D[i, 1, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                        tfile.write('TID,x,y,vx,vy,ax,ay\n')
                        for j in range(np.count_nonzero(self.textStruct2D[i, 2, :, 1])):
                            for k in range(7):
                                tfile.write(str(self.textStruct2D[i, 2, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                    self.textStruct2D = np.zeros(1000 * 3 * self.maxPoints * 7).reshape(
                        (1000, 3, self.maxPoints, 7))  # [frame #][header,pt cloud data,target info]
                    tfile.close

        parseEnd = int(round(time.time() * 1000))
        # print (self.pcBufPing)
        return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput

    # find various utility functions here for connecting to COM Ports, send data, etc...
    # connect to com ports
    # Call this function to connect to the comport. This takes arguments self (intrinsic), uartCom, and dataCom. No return, but sets internal variables in the parser object.
    def connectComPorts(self, uartCom, dataCom):
        self.uartCom = serial.Serial(uartCom, 115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                     timeout=0.3)
        if (self.capon3D == 1 and self.aop == 0):
            self.dataCom = serial.Serial(dataCom, 921600 * 1, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                         timeout=0.025)
        else:
            self.dataCom = serial.Serial(dataCom, 921600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                         timeout=0.025)
        self.dataCom.reset_output_buffer()
        print('Connected')

    # send cfg over uart
    def sendCfg(self, cfg):
        for line in cfg:
            time.sleep(.1)
            self.uartCom.write(line.encode())
            ack = self.uartCom.readline()
            print(ack)
            ack = self.uartCom.readline()
            print(ack)
        time.sleep(3)
        self.uartCom.reset_input_buffer()
        self.uartCom.close()

    # send single command to device over UART Com.
    def sendLine(self, line):
        self.uartCom.write(line.encode())
        ack = self.uartCom.readline()
        print(ack)
        ack = self.uartCom.readline()
        print(ack)

    def replayHist(self):
        if (self.replayData):
            # print('reading data')
            # print('fail: ',self.fail)
            # print(len(self.replayData))
            # print(self.replayData[0:8])
            self.replayData = self.Capon3DHeader(self.replayData)
            # print('fail: ',self.fail)
            return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput
            # frameData = self.replayData[0]
            # self.replayData = self.replayData[1:]
            # return frameData['PointCloud'], frameData['Targets'], frameData['Indexes'], frameData['Number Points'], frameData['NumberTracks'],frameData['frame'],0, frameData['ClassifierOutput'], frameData['Uniqueness']
        else:
            filename = 'overheadDebug/binData/pHistBytes_' + str(self.saveNum) + '.bin'
            # filename = 'Replay1Person10mShort/pHistRT'+str(self.saveNum)+'.pkl'
            self.saveNum += 1
            try:
                dfile = open(filename, 'rb', 0)
            except:
                print('cant open ', filename)
                return -1
            self.replayData = bytes(list(dfile.read()))
            if (self.replayData):
                print('entering replay')
                return self.replayHist()
            else:
                return -1


def getBit(byte, bitNum):
    mask = 1 << bitNum
    if (byte & mask):
        return 1
    else:
        return 0

import struct
import sys
import serial
import binascii
import time
import numpy as np
import math
import datetime
import matplotlib.pyplot as plt

from graphUtilities import rotX


# Initialize this Class to create a UART Parser. Initialization takes one argument:
# 1: String Lab_Type - These can be:
#   a. 3D People Counting
#   b. SDK Out of Box Demo
#   c. Long Range People Detection²
#   d. Indoor False Detection Mitigation
#   e. (Legacy): Overhead People Counting
#   f. (Legacy) 2D People Counting
# Default is (f). Once initialize, call connectComPorts(self, UartComPort, DataComPort) to connect to device com ports.
# Then call readAndParseUart() to read one frame of data from the device. The gui this is packaged with calls this every frame period.
# readAndParseUart() will return all radar detection and tracking information.
class uartParserSDK():
    def __init__(self, type='(Legacy) 2D People Counting'):
        self.headerLength = 52
        self.magicWord = 0x708050603040102
        self.threeD = 0
        self.ifdm = 0
        self.replay = 0
        self.SDK3xPointCloud = 0
        self.SDK3xPC = 0
        self.capon3D = 0
        self.aop = 0
        self.maxPoints = 1150
        # USER DEFINED
        self.data_to_plot = {"targetId": [],"posX": [], "posY": [], \
                             "velX": [], "velY": [], "accX": [], "accY": [], \
                             "numTargets": [], "time": []}
        self.magic_found = 0
        self.Startplot_time=datetime.datetime.now().minute
        self.plotisReady=0
        self.plotonce=1
        self.plot_number=-1
        # USER DEFINED
        if (type == '(Legacy): Overhead People Counting'):
            self.threeD = 1
        elif (type == 'Sense and Detect HVAC Control'):
            self.ifdm = 1
        elif (type == 'Replay'):  # unused
            self.replay = 1
        elif (type == "SDK Out of Box Demo"):
            self.SDK3xPointCloud = 1
        elif (type == "Long Range People Detection"):
            self.SDK3xPC = 1
        elif (type == '3D People Counting'):
            self.capon3D = 1
        elif (type == 'Capon3DAOP'):  # unused
            self.capon3D = 1
            self.aop = 1
        # data storage
        self.pcPolar = np.zeros((5, self.maxPoints))
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.numDetectedObj = 0
        self.targetBufPing = np.ones((10, 20)) * -1
        self.indexBufPing = np.zeros((1, self.maxPoints))
        self.classifierOutput = []
        self.frameNum = 0
        self.missedFrames = 0
        self.byteData = bytes(1)
        self.oldData = []
        self.indexes = []
        self.numDetectedTarget = 0
        self.fail = 0
        self.unique = []
        self.savedData = []
        self.saveNum = 0
        self.saveNumTxt = 0
        self.replayData = []
        self.startTimeLast = 0
        self.saveReplay = 0
        self.savefHist = 0
        self.saveBinary = 0
        self.saveTextFile = 1
        self.fHistRT = np.empty((100, 1), dtype=np.object)
        self.plotDimension = 0
        self.getUnique = 0
        self.CaponEC = 0

        self.printVerbosity = 0  # set 0 for limited logFile printing, 1 for more logging

        if (self.capon3D):
            # 3D people counting format
            # [frame #][header,pt cloud data,target info]
            # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
            # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
            # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]
            self.textStructCapon3D = np.zeros(1000 * 3 * self.maxPoints * 10).reshape(
                (1000, 3, self.maxPoints, 10))  # [frame #][header,pt cloud data,target info]

        if (self.ifdm):
            # Sense and direct format
            # [frame #][header,pt cloud data,target info]
            # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
            # [][pt cloud][pt index][#range, azim, doppler, snr]
            # [][target][Target #][TID,x,y,vx,vy,ax,ay]
            self.textStruct2D = np.zeros(1000 * 3 * self.maxPoints * 7).reshape(
                (1000, 3, self.maxPoints, 7))  # [frame #][header,pt cloud data,target info]

    # below funtions are used for converting output of labs that do not match SDK 3.x DPIF output
    # convert 2D polar People Counting to 3D Cartesian
    def polar2Cart(self):
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[1, n])  # y
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[1, n])  # x
        self.pcBufPing[3, :] = self.pcPolar[2, 0:self.numDetectedObj]  # doppler
        self.pcBufPing[4, :] = self.pcPolar[3, 0:self.numDetectedObj]  # snr
        self.pcBufPing[2, :self.numDetectedObj] = 0  # Z is zero in 2D case

    # convert 3D people counting polar to 3D cartesian
    def polar2Cart3D(self):
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[2, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[2, n])  # z
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.sin(self.pcPolar[1, n])  # x
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.cos(self.pcPolar[1, n])  # y
        self.pcBufPing[3, :] = self.pcPolar[3, 0:self.numDetectedObj]  # doppler
        self.pcBufPing[4, :] = self.pcPolar[4, 0:self.numDetectedObj]  # snr
        # print(self.pcBufPing[:,:10])

    # decode People Counting TLV Header
    def tlvHeaderDecode(self, data):
        # print(len(data))
        tlvType, tlvLength = struct.unpack('2I', data)
        return tlvType, tlvLength

    # decode People Counting Point Cloud TLV
    def parseDetectedObjects(self, data, tlvLength):
        objStruct = '4f'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength) / 16)
        for i in range(self.numDetectedObj):
            try:
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i] = struct.unpack(
                    objStruct, data[:objSize])
                data = data[16:]
            except:
                self.numDectedObj = i
                break
        self.polar2Cart()

    # decode IFDM point Cloud TLV
    def parseDetectedObjectsIFDM(self, data, tlvLength):
        pUnitStruct = '4f'
        pUnitSize = struct.calcsize(pUnitStruct)
        pUnit = struct.unpack(pUnitStruct, data[:pUnitSize])
        data = data[pUnitSize:]
        objStruct = '2B2h'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength - 16) / objSize)
        # print('Parsed Points: ', self.numDetectedObj)
        for i in range(self.numDetectedObj):
            try:
                az, doppler, ran, snr = struct.unpack(objStruct, data[:objSize])
                data = data[objSize:]
                # get range, azimuth, doppler, snr
                self.pcPolar[0, i] = ran * pUnit[2]  # range
                if (az >= 128):
                    az -= 256
                self.pcPolar[1, i] = math.radians(az * pUnit[0])  # azimuth
                self.pcPolar[2, i] = doppler * pUnit[1]  # doppler
                self.pcPolar[3, i] = snr * pUnit[3]  # snr

                # Sense and direct format
                # [frame #][header,pt cloud data,target info]
                # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                # [][pt cloud][pt index][#range, azim, doppler, snr]
                # [][target][Target #][TID,x,y,vx,vy,ax,ay]
                self.textStruct2D[self.frameNum % 1000, 1, i, 0] = self.pcPolar[0, i]  # range
                self.textStruct2D[self.frameNum % 1000, 1, i, 1] = self.pcPolar[1, i]  # az
                self.textStruct2D[self.frameNum % 1000, 1, i, 2] = self.pcPolar[2, i]  # doppler
                self.textStruct2D[self.frameNum % 1000, 1, i, 3] = self.pcPolar[3, i]  # snr

            except:
                self.numDetectedObj = i
                break
        self.polar2Cart()

    # decode 3D People Counting Point Cloud TLV
    def parseDetectedObjects3D(self, data, tlvLength):
        objStruct = '5f'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int(tlvLength / 20)
        for i in range(self.numDetectedObj):
            try:
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i], self.pcPolar[
                    4, i] = struct.unpack(objStruct, data[:objSize])
                data = data[20:]
            except:
                self.numDectedObj = i
                print('failed to get point cloud')
                break
        self.polar2Cart3D()

    # support for Capoin 3D point cloud
    # decode Capon 3D point Cloud TLV
    def parseCapon3DPolar(self, data, tlvLength):
        pUnitStruct = '5f'  # elev, azim, doppler, range, snr
        pUnitSize = struct.calcsize(pUnitStruct)
        pUnit = struct.unpack(pUnitStruct, data[:pUnitSize])
        data = data[pUnitSize:]
        objStruct = '2bh2H'  # 2 int8, 1 int16, 2 uint16
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength - pUnitSize) / objSize)
        # if (self.printVerbosity == 1):
        # print('Parsed Points: ', self.numDetectedObj)
        for i in range(self.numDetectedObj):
            try:
                elev, az, doppler, ran, snr = struct.unpack(objStruct, data[:objSize])
                # print(elev, az, doppler, ran, snr)
                data = data[objSize:]
                # get range, azimuth, doppler, snr
                self.pcPolar[0, i] = ran * pUnit[3]  # range
                if (az >= 128):
                    print('Az greater than 127')
                    az -= 256
                if (elev >= 128):
                    print('Elev greater than 127')
                    elev -= 256
                if (doppler >= 32768):
                    print('Doppler greater than 32768')
                    doppler -= 65536
                self.pcPolar[1, i] = az * pUnit[1]  # azimuth
                self.pcPolar[2, i] = elev * pUnit[0]  # elevation
                self.pcPolar[3, i] = doppler * pUnit[2]  # doppler
                self.pcPolar[4, i] = snr * pUnit[4]  # snr

                # add pt cloud data to textStructCapon3DCapon3D for text file printing
                # self.textStructCapon3DCapon3D[,,,] = [frame #][header,pt cloud data,target info]
                # [][pt cloud = 0][pt index][#elev, azim, doppler, range, snr]
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 0] = self.pcPolar[2, i]  # elev
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 1] = self.pcPolar[1, i]  # az
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 2] = self.pcPolar[3, i]  # doppler
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 3] = self.pcPolar[0, i]  # range
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 4] = self.pcPolar[4, i]  # snr
            except:
                self.numDetectedObj = i
                print('Point Cloud TLV Parser Failed')
                break
        self.polar2Cart3D()

    # decode 2D People Counting Target List TLV
    def parseDetectedTracks(self, data, tlvLength):
        if (self.plotDimension):
            targetStruct = 'I8f9ff'
        else:
            targetStruct = 'I6f9ff'
        targetSize = struct.calcsize(targetStruct)
        self.numDetectedTarget = int(tlvLength / targetSize)
        targets = np.empty((13, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            targetData = struct.unpack(targetStruct, data[:targetSize])
            targets[0, i] = int(targetData[0])  # TID
            targets[1:3, i] = targetData[1:3]  # X,Y
            targets[3, i] = 0  # Z=0
            targets[4:6, i] = targetData[3:5]  # vX,Vy
            targets[6, i] = 0  # vZ=0
            targets[7:9, i] = targetData[5:7]  # aX,aY
            targets[9, i] = 0  # az=0
            if (self.plotDimension):
                targets[10:12, i] = targetData[7:9]
                targets[12, i] = 1
            else:
                targets[10:12, i] = [0.75, 0.75]
                targets[12, i] = 1
            data = data[targetSize:]

            if (self.saveTextFile):
                self.textStruct2D[self.frameNum % 1000, 2, i, 0] = targets[0, i]  # TID
                self.textStruct2D[self.frameNum % 1000, 2, i, 1] = targets[1, i]  # x
                self.textStruct2D[self.frameNum % 1000, 2, i, 2] = targets[2, i]  # y

                self.textStruct2D[self.frameNum % 1000, 2, i, 3] = targets[4, i]  # vx
                self.textStruct2D[self.frameNum % 1000, 2, i, 4] = targets[5, i]  # vy

                self.textStruct2D[self.frameNum % 1000, 2, i, 5] = targets[7, i]  # ax
                self.textStruct2D[self.frameNum % 1000, 2, i, 6] = targets[8, i]  # ay

                if (self.printVerbosity == 1):
                    print('target added to textStructCapon3D')
        self.targetBufPing = targets

        # decode 3D People Counting Target List TLV

    def parseDetectedTracks3D(self, data, tlvLength):
        targetStruct = 'I9f'
        targetSize = struct.calcsize(targetStruct)
        self.numDetectedTarget = int(tlvLength / targetSize)
        targets = np.empty((13, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            targetData = struct.unpack(targetStruct, data[:targetSize])
            targets[0:7, i] = targetData[0:7]
            targets[7:10, i] = [0, 0, 0]
            targets[10:13, i] = targetData[7:10]
            data = data[targetSize:]
        self.targetBufPing = targets

    # decode Target Index TLV
    def parseTargetAssociations(self, data):
        targetStruct = 'B'
        targetSize = struct.calcsize(targetStruct)
        numIndexes = int(len(data) / targetSize)
        self.indexes = []
        self.unique = []
        try:
            for i in range(numIndexes):
                ind = struct.unpack(targetStruct, data[:targetSize])
                self.indexes.append(ind[0])
                data = data[targetSize:]
            if (self.getUnique):
                uTemp = self.indexes[math.ceil(numIndexes / 2):]
                self.indexes = self.indexes[:math.ceil(numIndexes / 2)]
                for i in range(math.ceil(numIndexes / 8)):
                    for j in range(8):
                        self.unique.append(getBit(uTemp[i], j))
        except:
            print('TLV Index Parse Fail')

    # decode Classifier output
    def parseClassifierOutput(self, data):
        classifierDataStruct = 'Ii'
        clOutSize = struct.calcsize(classifierDataStruct)
        self.classifierOutput = np.zeros((2, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            self.classifierOutput[0, i], self.classifierOutput[1, i] = struct.unpack(classifierDataStruct,
                                                                                     data[:clOutSize])
            data = data[clOutSize:]

    # below is for labs that are compliant with SDK 3.x  This code can parse the point cloud TLV and point cloud side info TLV from the OOB demo.
    # It can parse the SDK3.x Compliant People Counting demo "tracker_dpc"
    # get SDK3.x Cartesian Point Cloud
    def parseSDK3xPoints(self, dataIn, numObj):
        pointStruct = '4f'
        pointLength = struct.calcsize(pointStruct)
        try:
            for i in range(numObj):
                self.pcBufPing[0, i], self.pcBufPing[1, i], self.pcBufPing[2, i], self.pcBufPing[3, i] = struct.unpack(
                    pointStruct, dataIn[:pointLength])
                dataIn = dataIn[pointLength:]
            self.pcBufPing = self.pcBufPing[:, :numObj]
        except Exception as e:
            print(e)
            self.fail = 1

    # get Side Info SDK 3.x
    def parseSDK3xSideInfo(self, dataIn, numObj):
        sideInfoStruct = '2h'
        sideInfoLength = struct.calcsize(sideInfoStruct)
        try:
            for i in range(numObj):
                self.pcBufPing[4, i], unused = struct.unpack(sideInfoStruct, dataIn[:sideInfoLength])
                dataIn = dataIn[sideInfoLength:]
        except Exception as e:
            print(e)
            self.fail = 1

    # convert SDK compliant Polar Point Cloud to Cartesian
    def polar2CartSDK3(self):
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[2, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[2, n])  # z
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.sin(self.pcPolar[1, n])  # x
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.cos(self.pcPolar[1, n])  # y
        self.pcBufPing[3, :] = self.pcPolar[3, 0:self.numDetectedObj]  # doppler

    # decode SDK3.x Format Point Cloud in Polar Coordinates
    def parseSDK3xPolar(self, dataIn, tlvLength):
        pointStruct = '4f'
        pointLength = struct.calcsize(pointStruct)
        self.numDetectedObj = int(tlvLength / pointLength)
        try:
            for i in range(self.numDetectedObj):
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i] = struct.unpack(
                    pointStruct, dataIn[:pointLength])
                dataIn = dataIn[pointLength:]
        except:
            self.fail = 1
            return
        self.polar2CartSDK3()

    # decode 3D People Counting Target List TLV

    # 3D Struct format

    # uint32_t     tid;     /*! @brief   tracking ID */
    # float        posX;    /*! @brief   Detected target X coordinate, in m */
    # float        posY;    /*! @brief   Detected target Y coordinate, in m */
    # float        posZ;    /*! @brief   Detected target Z coordinate, in m */
    # float        velX;    /*! @brief   Detected target X velocity, in m/s */
    ##float       velY;    /*! @brief   Detected target Y velocity, in m/s */
    # float        velZ;    /*! @brief   Detected target Z velocity, in m/s */
    # float        accX;    /*! @brief   Detected target X acceleration, in m/s2 */
    # float        accY;    /*! @brief   Detected target Y acceleration, in m/s2 */
    # float        accZ;    /*! @brief   Detected target Z acceleration, in m/s2 */
    # float        ec[16];  /*! @brief   Target Error covarience matrix, [4x4 float], in row major order, range, azimuth, elev, doppler */
    # float        g;
    # float        confidenceLevel;    /*! @brief   Tracker confidence metric*/

    def parseDetectedTracksSDK3x(self, data, tlvLength):
        if (self.printVerbosity == 1):
            print(tlvLength)
        if (self.CaponEC):
            targetStruct = 'I27f'
        else:
            # targetStruct = 'I15f'
            targetStruct = 'I27f'
        targetSize = struct.calcsize(targetStruct)
        if (self.printVerbosity == 1):
            print('TargetSize=', targetSize)
        self.numDetectedTarget = int(tlvLength / targetSize)
        if (self.printVerbosity == 1):
            print('Num Detected Targets = ', self.numDetectedTarget)
        targets = np.empty((16, self.numDetectedTarget))
        rotTarget = [0, 0, 0]
        # theta = self.profile['elev_tilt']
        # print('theta = ',theta)
        # Rx = np.matrix([[ 1, 0           , 0           ],
        #           [ 0, math.cos(theta),-math.sin(theta)],
        #           [ 0, math.sin(theta), math.cos(theta)]])
        try:
            for i in range(self.numDetectedTarget):
                targetData = struct.unpack(targetStruct, data[:targetSize])
                if (self.printVerbosity == 1):
                    print(targetData)
                # tid, x, y
                if (self.CaponEC):
                    targets[0:13, i] = targetData[0:13]
                else:
                    # tid, pos x, pos y
                    targets[0:3, i] = targetData[0:3]
                    if (self.printVerbosity == 1):
                        print('Target Data TID,X,Y = ', targets[0:3, i])
                        print('i = ', i)
                    # pos z
                    targets[3, i] = targetData[3]

                    # rotTargetDataX,rotTargetDataY,rotTargetDataZ = rotX (targetData[1],targetData[2],targetData[3],self.profile['elev_tilt'])

                    # print('Target Data TID,X,Y = ',rotTargetDataX,', ',rotTargetDataY,', ',rotTargetDataZ)
                    # vel x, vel y
                    targets[4:6, i] = targetData[4:6]
                    # vel z
                    targets[6, i] = targetData[6]
                    # acc x, acc y
                    targets[7:9, i] = targetData[7:9]
                    # acc z
                    targets[9, i] = targetData[9]
                    # ec[16]
                    # targets[10:14,i]=targetData[10:14]
                    targets[10:13, i] = targetData[10:13]  # Chris 2020-12-18
                    if (self.printVerbosity == 1):
                        print('ec = ', targets[10:13, i])
                    # g
                    # targets[14,i]=targetData[14]
                    targets[14, i] = targetData[26]
                    if (self.printVerbosity == 1):
                        print('g= ', targets[14, i])
                    # confidenceLevel
                    # targets[15,i]=targetData[15]
                    targets[15, i] = targetData[27]
                    if (self.printVerbosity == 1):
                        print('Confidence Level = ', targets[15, i])

                    # self.textStructCapon3D[[frame #],[header,pt cloud data,target info],index,data]
                    # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
                    # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]
                    if (self.saveTextFile):
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 0] = targets[0, i]  # TID
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 1] = targets[1, i]  # x
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 2] = targets[2, i]  # y
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 3] = targets[3, i]  # z
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 4] = targets[4, i]  # vx
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 5] = targets[5, i]  # vy
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 6] = targets[6, i]  # vz
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 7] = targets[7, i]  # ax
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 8] = targets[8, i]  # ay
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 9] = targets[9, i]  # az
                        if (self.printVerbosity == 1):
                            print('target added to textStructCapon3D')
                data = data[targetSize:]
        except:
            print('Target TLV parse failed')
        self.targetBufPing = targets
        if (self.printVerbosity == 1):
            print(targets)

    # all TLV header decoding functions are below. Each lab with a Unique header or unique TLV set has its own header parsing function
    # decode Header and rest of TLVs for Legacy Labs and Indoor False detection mitigation
    def tlvHeader(self, data):
        # search for magic word
        self.targetBufPing = np.zeros((12, 1))
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.indexes = []
        frameNum = -1
        self.numDetectedTarget = 0
        self.numDetectedObj = 0
        # search until we find magic word
        while (1):
            try:
                magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q10I2H', data[:self.headerLength])
            except:
                # bad data, return
                self.fail = 1
                return data
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                data = data[1:]
                self.magic_found = 1
            else:
                # we have correct magic word, proceed to parse rest of data
                print("\nMagic word is ok\n")
                self.magic_found= 1
                break

        # Sense and direct format
        # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
        if (self.saveTextFile):
            self.textStruct2D[self.frameNum % 1000, 0, 0, 0] = magic
            self.textStruct2D[self.frameNum % 1000, 0, 1, 0] = version
            self.textStruct2D[self.frameNum % 1000, 0, 2, 0] = platform
            self.textStruct2D[self.frameNum % 1000, 0, 3, 0] = timestamp
            self.textStruct2D[self.frameNum % 1000, 0, 4, 0] = packetLength
            self.textStruct2D[self.frameNum % 1000, 0, 5, 0] = frameNum
            self.textStruct2D[self.frameNum % 1000, 0, 6, 0] = subFrameNum
            self.textStruct2D[self.frameNum % 1000, 0, 7, 0] = chirpMargin
            self.textStruct2D[self.frameNum % 1000, 0, 8, 0] = frameMargin
            self.textStruct2D[self.frameNum % 1000, 0, 9, 0] = uartSentTime
            self.textStruct2D[self.frameNum % 1000, 0, 10, 0] = trackProcessTime
            self.textStruct2D[self.frameNum % 1000, 0, 11, 0] = numTLVs
            self.textStruct2D[self.frameNum % 1000, 0, 12, 0] = checksum
            if (self.printVerbosity == 1):
                print('FrameNumber = ', self.textStruct2D[self.frameNum % 1000, 0, 5, 0])

        if (self.frameNum != frameNum):
            self.missedFrames += 1
            self.frameNum = frameNum
        self.frameNum += 1
        if (len(data) < packetLength):
            ndata = self.dataCom.read(packetLength - len(data))
            if (self.saveBinary):
                self.oldData += ndata
            data += ndata
        data = data[self.headerLength:]
        for i in range(numTLVs):
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(data[:8])
            except:
                print('read fail: not enough data')
                self.missedFrames += 1
                self.fail = 1
                break
            try:
                data = data[8:]
                if (tlvType == 6):
                    if (self.threeD):
                        self.parseDetectedObjects3D(data[:tlvLength], tlvLength - 8)
                    elif (self.ifdm):
                        self.parseDetectedObjectsIFDM(data[:tlvLength], tlvLength - 8)
                    else:
                        self.parseDetectedObjects(data[:tlvLength], tlvLength - 8)
                elif (tlvType == 7):
                    if (self.threeD):
                        self.parseDetectedTracks3D(data[:tlvLength], tlvLength - 8)
                    else:
                        self.parseDetectedTracks(data[:tlvLength], tlvLength - 8)
                elif (tlvType == 8):
                    self.parseTargetAssociations(data[:tlvLength - 8])
                elif (tlvType == 9):
                    self.parseClassifierOutput(data[:tlvLength - 8])
                data = data[tlvLength - 8:]
            except:
                print('Not enough data')
                print('Data length: ', len(data))
                print('Reported Packet Length: ', packetLength)
                self.fail = 1
                return data
        return data

    # parsing for SDK 3.x Point Cloud
    def sdk3xTLVHeader(self, dataIn):
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        headerStruct = 'Q8I'
        headerLength = struct.calcsize(headerStruct)
        tlvHeaderLength = 8
        # search until we find magic word
        while (1):
            try:
                magic, version, totalPacketLen, platform, self.frameNum, timeCPUCycles, self.numDetectedObj, numTLVs, subFrameNum = struct.unpack(
                    headerStruct, dataIn[:headerLength])
            except:
                # bad data, return
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # we have correct magic word, proceed to parse rest of data
                break
        dataIn = dataIn[headerLength:]
        remainingData = totalPacketLen - len(dataIn)
        count = 0
        # check to ensure we have all of the data
        while (remainingData > 0 and count < 3):
            newData = self.dataCom.read(remainingData)
            remainingData = totalPacketLen - len(dataIn) - len(newData)
            dataIn += newData
            count += 1
            if (self.saveBinary):
                self.oldData += newData
        # now check TLVs
        # print ('got tlvs sdk3x')
        for i in range(numTLVs):
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
            except Exception as e:
                print(e)
                print('failed to read OOB SDK3.x TLV')
            dataIn = dataIn[tlvHeaderLength:]
            if (tlvType == 1):
                self.parseSDK3xPoints(dataIn[:tlvLength], self.numDetectedObj)
                dataIn = dataIn[tlvLength:]
            elif (tlvType == 7):
                self.parseSDK3xSideInfo(dataIn[:tlvLength], self.numDetectedObj)
                dataIn = dataIn[tlvLength:]
        return dataIn

    # parsing for SDK 3.x DPIF compliant People Counting
    def sdk3xPCHeader(self, dataIn):
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.targetBufPing = np.zeros((13, 20))
        self.indexes = []
        tlvHeaderLength = 8
        # search until we find magic word
        while (1):
            try:
                magic, version, platform, timestamp, packetLength, self.frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q10I2H', dataIn[:self.headerLength])
            except:
                # bad data, return
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # we have correct magic word, proceed to parse rest of data
                break
        dataIn = dataIn[self.headerLength:]
        remainingData = packetLength - len(dataIn)
        if (self.printVerbosity == 1):
            print('pl: ', packetLength)
            print('remainingData ', remainingData)
        # check to ensure we have all of the data
        # check to ensure we have all of the data
        count = 0
        while (remainingData > 0 and count < 3):
            if (self.printVerbosity == 1):
                print('RD Loop')
            newData = self.dataCom.read(remainingData)
            remainingData = packetLength - len(dataIn) - len(newData)
            dataIn += newData
            count += 1
            if (remainingData == 0):
                if (self.saveBinary):
                    self.oldData += newData
        # now check TLVs
        if (self.printVerbosity == 1):
            print('Frame: ', self.frameNum)
            print(len(dataIn))
            print(numTLVs)
        for i in range(numTLVs):
            try:
                # print("DataIn Type", type(dataIn))
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
                if (self.printVerbosity == 1):
                    print('TLV length = ', tlvLength)
            except Exception as e:
                if (self.printVerbosity == 1):
                    print(e)
                    print('failed to read OOB SDK3.x TLV')
                    print('TLV num: ', i)
            dataIn = dataIn[tlvHeaderLength:]
            dataLength = tlvLength
            print("\n \n TLV TYPE = ",tlvType,"\n \n")
            if (tlvType == 6):
                # DPIF Polar Coordinates
                # print('pointcloud lrpd')
                self.parseSDK3xPolar(dataIn[:dataLength], dataLength)
            elif (tlvType == 7):
                # target 3D
                self.parseDetectedTracksSDK3x(dataIn[:dataLength], dataLength)
            elif (tlvType == 8):
                # target index
                self.parseTargetAssociations(dataIn[:dataLength])
            elif (tlvType == 9):
                # side info
                self.parseSDK3xSideInfo(dataIn[:dataLength], self.numDetectedObj)
            dataIn = dataIn[dataLength:]
        return dataIn

    # parsing for 3D People Counting lab
    def Capon3DHeader(self, dataIn):
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.pcPolar = np.zeros((5, self.maxPoints))
        self.targetBufPing = np.zeros((13, 20))
        self.numDetectedTarget = 0
        self.numDetectedObj = 0
        self.indexes = []
        tlvHeaderLength = 8
        headerLength = 48
        # stay in this loop until we find the magic word or run out of data to parse
        while (1):
            try:
                magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q9I2H', dataIn[:headerLength])
            except Exception as e:
                # bad data, return
                # print("Cannot Read Frame Header")
                # print(e)
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # got magic word, proceed to parse
                break

        dataIn = dataIn[headerLength:]
        remainingData = packetLength - len(dataIn) - headerLength
        # check to ensure we have all of the data
        # print('remaining data = ',remainingData)
        if (remainingData > 0):
            newData = self.dataCom.read(remainingData)
            remainingData = packetLength - len(dataIn) - headerLength - len(newData)
            dataIn += newData
            if (self.saveBinary):
                self.oldData += newData
        if (self.saveTextFile):
            self.textStructCapon3D[self.frameNum % 1000, 0, 0, 0] = magic
            self.textStructCapon3D[self.frameNum % 1000, 0, 1, 0] = version
            self.textStructCapon3D[self.frameNum % 1000, 0, 2, 0] = packetLength
            self.textStructCapon3D[self.frameNum % 1000, 0, 3, 0] = platform
            self.textStructCapon3D[self.frameNum % 1000, 0, 4, 0] = frameNum
            self.textStructCapon3D[self.frameNum % 1000, 0, 5, 0] = subFrameNum
            self.textStructCapon3D[self.frameNum % 1000, 0, 6, 0] = chirpMargin
            self.textStructCapon3D[self.frameNum % 1000, 0, 7, 0] = frameMargin
            self.textStructCapon3D[self.frameNum % 1000, 0, 8, 0] = uartSentTime
            self.textStructCapon3D[self.frameNum % 1000, 0, 9, 0] = trackProcessTime
            self.textStructCapon3D[self.frameNum % 1000, 0, 10, 0] = numTLVs
            self.textStructCapon3D[self.frameNum % 1000, 0, 11, 0] = checksum
            if (self.printVerbosity == 1):
                print('FrameNumber = ', self.textStructCapon3D[self.frameNum % 1000, 0, 4, 0])

        # now check TLVs
        for i in range(numTLVs):
            # try:
            # print("DataIn Type", type(dataIn))
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
                dataIn = dataIn[tlvHeaderLength:]
                dataLength = tlvLength - tlvHeaderLength
            except:
                print('TLV Header Parsing Failure')
                self.fail = 1
                return dataIn
            print("\n\n \n TLV TYPE IS ",tlvType, "\n\n\n")
            if (tlvType == 6):
                # DPIF Polar Coordinates
                self.parseCapon3DPolar(dataIn[:dataLength], dataLength)
            elif (tlvType == 7):
                # target 3D
                self.parseDetectedTracksSDK3x(dataIn[:dataLength], dataLength)
            elif (tlvType == 8):
                # target index
                self.parseTargetAssociations(dataIn[:dataLength])
            elif (tlvType == 9):
                if (self.printVerbosity == 1):
                    print('type9')
                # side info
                # self.parseSDK3xSideInfo(dataIn[:dataLength], self.numDetectedObj)
            dataIn = dataIn[dataLength:]
            # except Exception as e:
            #    print(e)
            #    print ('failed to read OOB SDK3.x TLV')
        if (self.frameNum + 1 != frameNum):
            self.missedFrames += frameNum - (self.frameNum + 1)
        self.frameNum = frameNum
        return dataIn

    # This function is always called - first read the UART, then call a function to parse the specific demo output
    # This will return 1 frame of data. This must be called for each frame of data that is expected. It will return a dict containing:
    #   1. Point Cloud
    #   2. Target List
    #   3. Target Indexes
    #   4. number of detected points in point cloud
    #   5. number of detected targets
    #   6. frame number
    #   7. Fail - if one, data is bad
    #   8. classifier output
    # Point Cloud and Target structure are liable to change based on the lab. Output is always cartesian.
    def readAndParseUart(self):
        self.fail = 0
        if (self.replay):
            return self.replayHist()
        numBytes = 4666
        data = self.dataCom.read(numBytes)
        if (self.byteData is None):
            self.byteData = data
        else:
            self.byteData += data
        if (self.saveBinary):
            self.oldData += data
        # try:
        print("self.capon3D  =",self.capon3D ,"self.ifdm=",self.ifdm)
        if (self.SDK3xPointCloud == 1):
            self.byteData = self.sdk3xTLVHeader(self.byteData)
        elif (self.SDK3xPC == 1):
            self.byteData = self.sdk3xPCHeader(self.byteData)
        elif (self.capon3D == 1):
            self.byteData = self.Capon3DHeader(self.byteData)
        else:
            self.byteData = self.tlvHeader(self.byteData)
        # except Exception as e:
        #    print(e)
        #    self.fail = 1
        # return data after parsing and save to replay file
        if (self.fail):
            return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput
        if (self.saveBinary):
            if (self.frameNum % 1000 == 0):
                toSave = bytes(self.oldData)
                fileName = 'binData/pHistBytes_' + str(self.saveNum) + '.bin'
                self.saveNum += 1
                bfile = open(fileName, 'wb')
                bfile.write(toSave)
                self.oldData = []
                print('Missed Frames ' + str(self.missedFrames) + '/1000')
                self.missedFrames = 0
                bfile.close
        if (self.saveTextFile):
            if (self.frameNum % 1000 == 0):
                if (self.capon3D):
                    toSave = self.textStructCapon3D
                    print("\n\n to save for CAPON3D \n\n")
                elif (self.ifdm):
                    toSave = self.textStruct2D
                    print("\n\n to save for ifdm \n\n")
                print('Saved data file ', self.saveNumTxt)
                #fileName = 'binData/pHistText_' + str(self.saveNumTxt) + '.csv'
                fileName = 'pHistText_' + str(self.saveNumTxt) + '.csv'
                if (self.saveNumTxt < 75):
                    self.saveNumTxt += 1
                else:
                    self.saveNumTxt = 0
                tfile = open(fileName, 'w')
                tfile.write('This file contains parsed UART data in sensor centric coordinates\n')
                tfile.write('file format version 1.0 \n ')
                # tfile.write(str(toSave))

                if (self.capon3D):
                    # [frame #][header,pt cloud data,target info]
                    # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
                    # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]

                    for i in range(1000):



                        tfile.write(' for capon 3D \n ')
                        tfile.write(
                            'magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum\n')
                        self.data_to_plot["posX"].append(self.textStructCapon3D[i, 2, 0, 1])
                        self.data_to_plot["posY"].append(self.textStructCapon3D[i, 2, 0, 2])
                        self.data_to_plot["accX"].append(self.textStructCapon3D[i, 2, 0, 3])
                        self.data_to_plot["accY"].append(self.textStructCapon3D[i, 2, 0, 4])
                        timenow = str(datetime.datetime.now().hour), ':', str(datetime.datetime.now().minute)
                        timenow = "".join(timenow)
                        self.data_to_plot["time"].append(timenow)
                        #fileName = 'data_to_plot_' + str(self.saveNumTxt) + '.txt'
                        #xfile = open(fileName, 'w')
                        #xfile.write("Xposition tab")
                        #xfile.write( str(self.data_to_plot["posX"]))
                        #xfile.write("Yposition tab")
                        #xfile.write(str(self.data_to_plot["posY"]))
                        #xfile.close()
            #if (self.magic_found == 1):
                            #self.data_to_plot["posX"].append(self.textStructCapon3D[i, 2, 0, 1])
                          #  self.data_to_plot["poxY"].append(self.textStructCapon3D[i, 2, 0, 2])
                           # self.data_to_plot["accX"].append(self.textStructCapon3D[i, 2, 0, 3])
                            #self.data_to_plot["accY"].append(self.textStructCapon3D[i, 2, 0, 4])
                           # tfile.write('TID,x,y,z,vx,vy,vz,ax,ay,az\n')

                           #for j in range(np.count_nonzero(self.textStructCapon3D[i, 2, :, 1])):
                            # self.data_to_plot["accY"].append(self.textStructCapon3D[i, 2, j, 4])

                        #USER CODE: print position of 1 target (the fist one)

                        for j in range(0, 12):
                            tfile.write(str(self.textStructCapon3D[i, 0, j, 0]))
                            tfile.write(',')
                            # print(str(self.textStructCapon3D[i,0,j,0]))
                        tfile.write('\n')
                        tfile.write('elev, azim, doppler, range, snr\n')
                        for j in range(np.count_nonzero(self.textStructCapon3D[i, 1, :,
                                                        0])):  # self.numDetectedObj):#len(self.textStructCapon3D[i,1,:,0]!=0)):
                            for k in range(5):
                                tfile.write(str(self.textStructCapon3D[i, 1, j, k]))
                                tfile.write(',')
                            tfile.write('\n')

                        tfile.write('TID,x,y,z,vx,vy,vz,ax,ay,az\n')
                        for j in range(np.count_nonzero(self.textStructCapon3D[i, 2, :, 1])):
                            for k in range(10):
                                tfile.write(str(self.textStructCapon3D[i, 2, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                    self.textStructCapon3D = np.zeros(1000 * 3 * 12 * self.maxPoints).reshape(
                        (1000, 3, 12, self.maxPoints))  # [frame #][header,pt cloud data,target info]
                    tfile.close

                if (self.ifdm):
                    # Sense and direct format
                    # [frame #][header,pt cloud data,target info]
                    # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#range, azim, doppler, snr]
                    # [][target][Target #][TID,x,y,vx,vy,ax,ay]
                    for i in range(1000):
                        tfile.write(
                            'magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum\n')
                        for j in range(13):
                            tfile.write(str(self.textStruct2D[i, 0, j, 0]))
                            tfile.write(',')
                        tfile.write('\n')
                        tfile.write('range, azim, doppler, snr\n')
                        for j in range(np.count_nonzero(self.textStruct2D[i, 1, :, 0])):
                            for k in range(4):
                                tfile.write(str(self.textStruct2D[i, 1, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                        tfile.write('TID,x,y,vx,vy,ax,ay\n')
                        for j in range(np.count_nonzero(self.textStruct2D[i, 2, :, 1])):
                            for k in range(7):
                                tfile.write(str(self.textStruct2D[i, 2, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                    self.textStruct2D = np.zeros(1000 * 3 * self.maxPoints * 7).reshape(
                        (1000, 3, self.maxPoints, 7))  # [frame #][header,pt cloud data,target info]
                    tfile.close

        parseEnd = int(round(time.time() * 1000))
        # print (self.pcBufPing)
        return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput

    # find various utility functions here for connecting to COM Ports, send data, etc...
    # connect to com ports
    # Call this function to connect to the comport. This takes arguments self (intrinsic), uartCom, and dataCom. No return, but sets internal variables in the parser object.
    def connectComPorts(self, uartCom, dataCom):
        self.uartCom = serial.Serial(uartCom, 115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                     timeout=0.3)
        if (self.capon3D == 1 and self.aop == 0):
            self.dataCom = serial.Serial(dataCom, 921600 * 1, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                         timeout=0.025)
        else:
            self.dataCom = serial.Serial(dataCom, 921600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                         timeout=0.025)
        self.dataCom.reset_output_buffer()
        print('Connected')

    # send cfg over uart
    def sendCfg(self, cfg):
        for line in cfg:
            time.sleep(.1)
            self.uartCom.write(line.encode())
            ack = self.uartCom.readline()
            print(ack)
            ack = self.uartCom.readline()
            print(ack)
        time.sleep(3)
        self.uartCom.reset_input_buffer()
        self.uartCom.close()

    # send single command to device over UART Com.
    def sendLine(self, line):
        self.uartCom.write(line.encode())
        ack = self.uartCom.readline()
        print(ack)
        ack = self.uartCom.readline()
        print(ack)

    def replayHist(self):
        if (self.replayData):
            # print('reading data')
            # print('fail: ',self.fail)
            # print(len(self.replayData))
            # print(self.replayData[0:8])
            self.replayData = self.Capon3DHeader(self.replayData)
            # print('fail: ',self.fail)
            return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput
            # frameData = self.replayData[0]
            # self.replayData = self.replayData[1:]
            # return frameData['PointCloud'], frameData['Targets'], frameData['Indexes'], frameData['Number Points'], frameData['NumberTracks'],frameData['frame'],0, frameData['ClassifierOutput'], frameData['Uniqueness']
        else:
            filename = 'overheadDebug/binData/pHistBytes_' + str(self.saveNum) + '.bin'
            # filename = 'Replay1Person10mShort/pHistRT'+str(self.saveNum)+'.pkl'
            self.saveNum += 1
            try:
                dfile = open(filename, 'rb', 0)
            except:
                print('cant open ', filename)
                return -1
            self.replayData = bytes(list(dfile.read()))
            if (self.replayData):
                print('entering replay')
                return self.replayHist()
            else:
                return -1


def getBit(byte, bitNum):
    mask = 1 << bitNum
    if (byte & mask):
        return 1
    else:
        return 0

Any moving target is detected in the People Counting demo. 

so if the people counting demo only sends data when it detects moving objects, to detect inanimate targets I have to use another demo like the OOB one ? 

Aicha:

I will be taking over support on this thread for Sabeeh. To reestablish a base line for support, I read above that you are trying to get a range estimate of target from a set of points that given in the point cloud. Is this correct?

Best regards,

Connor Desmond

hello, so i use the oob_parser.py file and try to make my own plots. 

as you can see from the code i uploaded, 

I use the ISK_6m_default.cfg config file 

parser = uartParserSDK(type="3D People Counting")

parser.frameTime = 50  # 50ms frame time

data = parser.readAndParseUart() 



All i do is take the data that the readAndParseUart function from oob_parser is sending and plot it. 
so readAndParseUart() returns

"point cloud "  which is  data[0]
"Target List"             data[1]
"Target Indexes"          data[2]
"number of detected points in point cloud"          data[3]
"number of detected targets or people count"        data[4]
"frame number"                                      data[5]
"integer indicating wheter or not data is bad ( If 1, data is bad)" data[6])
"classifier output"       data[7])



What i want to do is plot just like the out of the box demo visualizer the 3D scatter plot, the range profile for zero doppler and the doppler range plot 

>>>>>>>I couldn't find a parser for the out of the box demo , that's why I use the oob_parser.py for the 3D people counting demo. 
 

*** 
From "Target List"/  data[1] I was able to get the "targetId", "posX" , "posY", "posZ" "velX" "velY" "velZ" "accX" "accY""accZ" of  targets
with: 

x positions for all targets in array form being data[1][1]
y array  data[1][2]
z array  data[1][3]
Velocity with regard to x axis is data[1][4] and so on. 

to get the x position from 1 target Xpos_1st_target= data[1][1][0]

# -------------My codes results for a particular frame as the radar is directed at me and i move my hands #  ---------------#: 

Target Indexes [1, 1, 1, 1, 254, 1, 1, 254, 1, 1, 1, 254, 1, 1, 1, 254, 1, 1, 1, 254, 254, 1, 1, 1, 254, 254, 1, 1, 254, 254, 1, 1, 254, 1, 1, 254, 1, 1, 254, 1, 254, 1, 1]
number of detected points in point cloud 12
number of detected targets or people count 1
frame number 1143
If 1, data is bad 0
classifier output []

xtab = [0.12638721]

ytab = [0.59357595]

ztab = [0.06745166]
xtab 1st target= 0.12638720870018005
ytab 1st target= 0.5935759544372559
ztab 1st target= 0.06745165586471558
Velocity X tab for targets -0.05277254432439804
Velocity Y tab for targets -0.11206091940402985
Velocity Z tab for targets 0.023658424615859985
Acceleration X tab for targets -0.09787748008966446
Acceleration Y tab for targets -0.2583736181259155
Acceleration Z tab for targets 0.0359615795314312

According to the user's guide, the Target Index TLV consists of an array of target IDs. 
A targetID at index i is the target to which point i of the previous frame's point cloud was associated.
Valid IDs range from 0-249. (with 254 meaning the point is not associated because it's located outside boundary of interest) 

>>>>>>>>> the y position in ytab 1st target= 0.5935759544372559 is the data I use to measure the distance between a target (me) and the radar in meters.
It seems to be pretty correct way of getting the distance. 

>>>>>>>>> Why is there more valid target IDs (value 1 in Target indexes) than targets ? 


***
So data[0] returned by readandparseUart() is point cloud. So depending on the number of points detected, the number of columns of data[0] changes. 
but data[0] always has 5 rows but i am not sure in which order are "elev","azimuth","doppler", "range" and "snr" information 

# -------------My codes results ---------------#: 

array[0] (elevation ?) of Point cloud [0.12123474 0.14058207 0.1449916 0.16585598 0.15543976 0.1716154
 0.18878373 0.15961489 0.17735767 0.16476674 0.18308219 0.16990211]

 array[1] (azimuth ?) of Point cloud [0.47479371 0.52846125 0.52389284 0.57678138 0.52088843 0.57509401
 0.63262616 0.51599155 0.57334913 0.51436963 0.57154692 0.51269627]

array[2] (doppler ?) of Point cloud [0.12512456 0.15724637 0.168151 0.19888471 0.168151 0.19888471
 0.21878105 0.17898838 0.19888471 0.17898838 0.19888471 0.17898838]

array[3] (range ? ) of Point cloud [ 0.06972 0.06972 0.06972 0.06972 0.06972 0.06972 -0.06972 0.06972
 0.06972 0.06972 -0.06972 -0.06972]

array[4] (snr ? ) of Point cloud [ 6.51999985 11.23999975 11.51999974 10.75999976 11.11999975 10.99999975
 6.99999984 9.99999978 10.63999976 8.63999981 9.75999978 7.11999984]

the #3 array of point cloud (if it is indeed the range in meters ) seems a little bit odd. I seem to always get 0.06972 

Now correct me if I am wrong but points in the point cloud represents targets right. Like there can be many points that represent 1 target. 

If we consider there is only 1 target than all detected points in the point cloud represent that particular target ?

Shouldn't I be able to have a range estimate  from a set of points that are given in the point cloud by (for example) computing the mean value of not too far away (from each other)  points 

hello, thank you for the precision on Point cloud.  It's all very clear now. 

But then why is does the oob_parser readandparseUart function return "point cloud "  (data[0] ) and "Target List" ( data[1] ). What is a considered a target here ???? 

Thank you for taking time to answer me since the beginning. 

Hello, 

Thank you for your answer.

I was able to get the data I wanted (Target's position, velocity , acceleration) as well as the point cloud for the 3D people counting demo.

There is just one problem left with it, all the fields in the parsed/ returned data are null when the radar is not directed towards people with a minimum of motion.  

Can I send you my code (UartReader.py)  so that you can try it? 

I am curious as to why, when there is no motion , no data is sent.

I modified the returned data from readandparseUart() [oob_parser.py] so that i could get polar information for Pointcloud.

So here is what readandparseUart() [oob_parser.py] returns: 

return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput,self.polar_returned

>>> self.pcBufPing = Parseddata[0]= ["x","y","z","doppler","snr"] of Pointcloud

>>> self.polar_returned = "range", "azimuth", "elev" of Point cloud


I would suggest (for tests) to direct the radar towards objects for a first run and check the csv files. (

self.saveTextFile = 1 in oob_parser so the frames are saved) 

then try it again by directing it at you and moving you hands for example. 

I use C:\ti\mmwave_industrial_toolbox_4_7_0\labs\people_counting\68xx_3D_people_counting\chirp_configs\ISK_6m_default.cfg 
and flashed 3D_people_count_68xx_demo.bin.

Below is oob_parser.py: 

import struct
import sys
import serial
import binascii
import time
import numpy as np
import math
import os
from PyQt5.QtCore import QDateTime, Qt, QTimer, QThread, pyqtSignal
from PyQt5.QtWidgets import (QApplication, QCheckBox, QComboBox, QDateTimeEdit,
        QDial, QDialog, QGridLayout, QGroupBox, QHBoxLayout, QLabel, QLineEdit,
        QProgressBar, QPushButton, QRadioButton, QScrollBar, QSizePolicy,
        QSlider, QSpinBox, QStyleFactory, QTableWidget, QTableWidgetItem, QTabWidget, QTextEdit,
        QVBoxLayout, QWidget, QFileDialog, QButtonGroup)
from PyQt5.QtGui import QPixmap
from PyQt5.QtCore import QDateTime, Qt, QTimer, QThread, pyqtSignal
import pyqtgraph as pg
import pyqtgraph.opengl as gl

#User
import copy
from copy import copy as cp
from openpyxl import Workbook
from openpyxl import load_workbook
import csv
import re
#User

# Initialize this Class to create a UART Parser. Initialization takes one argument:
# 1: String Lab_Type - These can be:
#   a. 3D People Counting
#   b. SDK Out of Box Demo
#   c. Long Range People Detection²
#   d. Indoor False Detection Mitigation
#   e. (Legacy): Overhead People Counting
#   f. (Legacy) 2D People Counting
# Default is (f). Once initialize, call connectComPorts(self, UartComPort, DataComPort) to connect to device com ports.
# Then call readAndParseUart() to read one frame of data from the device. The gui this is packaged with calls this every frame period.
# readAndParseUart() will return all radar detection and tracking information.
class uartParserSDK():
    def __init__(self, type='(Legacy) 2D People Counting'):
        #user defined
        self.demoType=type
        self.stop_xlsx=0
        #user defined
        self.headerLength = 52
        self.magicWord = 0x708050603040102
        self.threeD = 0
        self.ifdm = 0
        self.replay = 0
        self.SDK3xPointCloud = 0
        self.SDK3xPC = 0
        self.capon3D = 0
        self.aop = 0
        self.maxPoints = 1150
        if (type == '(Legacy): Overhead People Counting'):
            self.threeD = 1
        elif (type == 'Sense and Detect HVAC Control'):
            self.ifdm = 1
        elif (type == 'Replay'):  # unused
            self.replay = 1
        elif (type == "SDK Out of Box Demo"):
            self.SDK3xPointCloud = 1
        elif (type == "Long Range People Detection"):
            self.SDK3xPC = 1
        elif (type == '3D People Counting'):
            self.capon3D = 1
        elif (type == 'Capon3DAOP'):  # unused
            self.capon3D = 1
            self.aop = 1
        # data storage
        self.pcPolar = np.zeros((5, self.maxPoints))
        self.polar_returned = np.zeros((5, self.maxPoints))
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.numDetectedObj = 0
        self.targetBufPing = np.ones((10, 20)) * -1
        self.indexBufPing = np.zeros((1, self.maxPoints))
        self.classifierOutput = []
        self.frameNum = 0
        self.missedFrames = 0
        self.byteData = bytes(1)
        self.oldData = []
        self.indexes = []
        self.numDetectedTarget = 0
        self.fail = 0
        self.unique = []
        self.savedData = []
        self.saveNum = 0
        self.saveNumTxt = 0
        self.replayData = []
        self.startTimeLast = 0
        self.saveReplay = 0
        self.savefHist = 1
        self.saveBinary = 0
        self.saveTextFile = 1
        self.fHistRT = np.empty((100, 1), dtype=np.object)
        self.plotDimension = 0
        self.getUnique = 0
        self.CaponEC = 0

        self.printVerbosity = 0  # set 0 for limited logFile printing, 1 for more logging
        self.filename_xlsx="" #User defined
        if (self.capon3D):
            # 3D people counting format
            # [frame #][header,pt cloud data,target info]
            # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
            # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
            # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]
            self.textStructCapon3D = np.zeros(1000 * 3 * self.maxPoints * 10).reshape(
                (1000, 3, self.maxPoints, 10))  # [frame #][header,pt cloud data,target info]

        if (self.ifdm):
            # Sense and direct format
            # [frame #][header,pt cloud data,target info]
            # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
            # [][pt cloud][pt index][#range, azim, doppler, snr]
            # [][target][Target #][TID,x,y,vx,vy,ax,ay]
            self.textStruct2D = np.zeros(1000 * 3 * self.maxPoints * 7).reshape(
                (1000, 3, self.maxPoints, 7))  # [frame #][header,pt cloud data,target info]

    # below funtions are used for converting output of labs that do not match SDK 3.x DPIF output
    # convert 2D polar People Counting to 3D Cartesian
    def polar2Cart(self):
        print("\n function polar2Cart used \n")
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[1, n])  # y
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[1, n])  # x
        self.pcBufPing[3, :] = self.pcPolar[2, 0:self.numDetectedObj]  # doppler
        self.pcBufPing[4, :] = self.pcPolar[3, 0:self.numDetectedObj]  # snr
        self.pcBufPing[2, :self.numDetectedObj] = 0  # Z is zero in 2D case
        self.polar_returned = cp(self.pcPolar[:5, :self.numDetectedObj])

    # convert 3D people counting polar to 3D cartesian
    def polar2Cart3D(self):
        print("\n function polar2Cart3D used \n")
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[2, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[2, n])  # z
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.sin(self.pcPolar[1, n])  # x
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.cos(self.pcPolar[1, n])  # y
        self.pcBufPing[3, :] = self.pcPolar[3, 0:self.numDetectedObj]  # doppler
        self.pcBufPing[4, :] = self.pcPolar[4, 0:self.numDetectedObj]  # snr
        # print(self.pcBufPing[:,:10])
        self.polar_returned = cp(self.pcPolar[:5,:self.numDetectedObj])

    # decode People Counting TLV Header
    def tlvHeaderDecode(self, data):
        print("\n function tlvHeaderDecode used \n")
        # print(len(data))
        tlvType, tlvLength = struct.unpack('2I', data)
        return tlvType, tlvLength

    # decode People Counting Point Cloud TLV
    def parseDetectedObjects(self, data, tlvLength):
        print("\n function parseDetectedObjects used \n")
        objStruct = '4f'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength) / 16)
        for i in range(self.numDetectedObj):
            try:
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i] = struct.unpack(
                    objStruct, data[:objSize])
                data = data[16:]
            except:
                self.numDectedObj = i
                break
        self.polar2Cart()

    # decode IFDM point Cloud TLV
    def parseDetectedObjectsIFDM(self, data, tlvLength):
        print("\n function parseDetectedObjectsIFDM used \n")
        pUnitStruct = '4f'
        pUnitSize = struct.calcsize(pUnitStruct)
        pUnit = struct.unpack(pUnitStruct, data[:pUnitSize])
        data = data[pUnitSize:]
        objStruct = '2B2h'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength - 16) / objSize)
        # print('Parsed Points: ', self.numDetectedObj)
        for i in range(self.numDetectedObj):
            try:
                az, doppler, ran, snr = struct.unpack(objStruct, data[:objSize])
                data = data[objSize:]
                # get range, azimuth, doppler, snr
                self.pcPolar[0, i] = ran * pUnit[2]  # range
                if (az >= 128):
                    az -= 256
                self.pcPolar[1, i] = math.radians(az * pUnit[0])  # azimuth
                self.pcPolar[2, i] = doppler * pUnit[1]  # doppler
                self.pcPolar[3, i] = snr * pUnit[3]  # snr

                # Sense and direct format
                # [frame #][header,pt cloud data,target info]
                # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                # [][pt cloud][pt index][#range, azim, doppler, snr]
                # [][target][Target #][TID,x,y,vx,vy,ax,ay]
                self.textStruct2D[self.frameNum % 1000, 1, i, 0] = self.pcPolar[0, i]  # range
                self.textStruct2D[self.frameNum % 1000, 1, i, 1] = self.pcPolar[1, i]  # az
                self.textStruct2D[self.frameNum % 1000, 1, i, 2] = self.pcPolar[2, i]  # doppler
                self.textStruct2D[self.frameNum % 1000, 1, i, 3] = self.pcPolar[3, i]  # snr

            except:
                self.numDetectedObj = i
                break
        self.polar2Cart()

    # decode 3D People Counting Point Cloud TLV
    def parseDetectedObjects3D(self, data, tlvLength):
        print("\n function parseDetectedObjects3D used \n")
        objStruct = '5f'
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int(tlvLength / 20)
        for i in range(self.numDetectedObj):
            try:
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i], self.pcPolar[
                    4, i] = struct.unpack(objStruct, data[:objSize])
                data = data[20:]
            except:
                self.numDectedObj = i
                print('failed to get point cloud')
                break
        self.polar2Cart3D()

    # support for Capoin 3D point cloud
    # decode Capon 3D point Cloud TLV
    def parseCapon3DPolar(self, data, tlvLength):
        print("\n function parseCapon3DPolar used \n")
        pUnitStruct = '5f'  # elev, azim, doppler, range, snr
        pUnitSize = struct.calcsize(pUnitStruct)
        pUnit = struct.unpack(pUnitStruct, data[:pUnitSize])
        data = data[pUnitSize:]
        objStruct = '2bh2H'  # 2 int8, 1 int16, 2 uint16
        objSize = struct.calcsize(objStruct)
        self.numDetectedObj = int((tlvLength - pUnitSize) / objSize)
        # if (self.printVerbosity == 1):
        # print('Parsed Points: ', self.numDetectedObj)
        for i in range(self.numDetectedObj):
            try:
                elev, az, doppler, ran, snr = struct.unpack(objStruct, data[:objSize])
                # print(elev, az, doppler, ran, snr)
                data = data[objSize:]
                # get range, azimuth, doppler, snr
                self.pcPolar[0, i] = ran * pUnit[3]  # range
                if (az >= 128):
                    print('Az greater than 127')
                    az -= 256
                if (elev >= 128):
                    print('Elev greater than 127')
                    elev -= 256
                if (doppler >= 32768):
                    print('Doppler greater than 32768')
                    doppler -= 65536
                self.pcPolar[1, i] = az * pUnit[1]  # azimuth
                self.pcPolar[2, i] = elev * pUnit[0]  # elevation
                self.pcPolar[3, i] = doppler * pUnit[2]  # doppler
                self.pcPolar[4, i] = snr * pUnit[4]  # snr

                # add pt cloud data to textStructCapon3DCapon3D for text file printing
                # self.textStructCapon3DCapon3D[,,,] = [frame #][header,pt cloud data,target info]
                # [][pt cloud = 0][pt index][#elev, azim, doppler, range, snr]
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 0] = self.pcPolar[2, i]  # elev
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 1] = self.pcPolar[1, i]  # az
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 2] = self.pcPolar[3, i]  # doppler
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 3] = self.pcPolar[0, i]  # range
                self.textStructCapon3D[self.frameNum % 1000, 1, i, 4] = self.pcPolar[4, i]  # snr
            except:
                self.numDetectedObj = i
                print('Point Cloud TLV Parser Failed')
                break
        self.polar2Cart3D()

    # decode 2D People Counting Target List TLV
    def parseDetectedTracks(self, data, tlvLength):
        print("\n function parseDetectedTracks used \n")
        if (self.plotDimension):
            targetStruct = 'I8f9ff'
        else:
            targetStruct = 'I6f9ff'
        targetSize = struct.calcsize(targetStruct)
        self.numDetectedTarget = int(tlvLength / targetSize)
        targets = np.empty((13, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            targetData = struct.unpack(targetStruct, data[:targetSize])
            targets[0, i] = int(targetData[0])  # TID
            targets[1:3, i] = targetData[1:3]  # X,Y
            targets[3, i] = 0  # Z=0
            targets[4:6, i] = targetData[3:5]  # vX,Vy
            targets[6, i] = 0  # vZ=0
            targets[7:9, i] = targetData[5:7]  # aX,aY
            targets[9, i] = 0  # az=0
            if (self.plotDimension):
                targets[10:12, i] = targetData[7:9]
                targets[12, i] = 1
            else:
                targets[10:12, i] = [0.75, 0.75]
                targets[12, i] = 1
            data = data[targetSize:]

            if (self.saveTextFile):
                self.textStruct2D[self.frameNum % 1000, 2, i, 0] = targets[0, i]  # TID
                self.textStruct2D[self.frameNum % 1000, 2, i, 1] = targets[1, i]  # x
                self.textStruct2D[self.frameNum % 1000, 2, i, 2] = targets[2, i]  # y

                self.textStruct2D[self.frameNum % 1000, 2, i, 3] = targets[4, i]  # vx
                self.textStruct2D[self.frameNum % 1000, 2, i, 4] = targets[5, i]  # vy

                self.textStruct2D[self.frameNum % 1000, 2, i, 5] = targets[7, i]  # ax
                self.textStruct2D[self.frameNum % 1000, 2, i, 6] = targets[8, i]  # ay

                if (self.printVerbosity == 1):
                    print('target added to textStructCapon3D')
        self.targetBufPing = targets

        # decode 3D People Counting Target List TLV

    def parseDetectedTracks3D(self, data, tlvLength):
        print("\n function parseDetectedTracks3D used \n")
        targetStruct = 'I9f'
        targetSize = struct.calcsize(targetStruct)
        self.numDetectedTarget = int(tlvLength / targetSize)
        targets = np.empty((13, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            targetData = struct.unpack(targetStruct, data[:targetSize])
            targets[0:7, i] = targetData[0:7]
            targets[7:10, i] = [0, 0, 0]
            targets[10:13, i] = targetData[7:10]
            data = data[targetSize:]
        self.targetBufPing = targets

    # decode Target Index TLV
    def parseTargetAssociations(self, data):
        print("\n function parseTargetAssociations used \n")
        targetStruct = 'B'
        targetSize = struct.calcsize(targetStruct)
        numIndexes = int(len(data) / targetSize)
        self.indexes = []
        self.unique = []
        try:
            for i in range(numIndexes):
                ind = struct.unpack(targetStruct, data[:targetSize])
                self.indexes.append(ind[0])
                data = data[targetSize:]
            if (self.getUnique):
                uTemp = self.indexes[math.ceil(numIndexes / 2):]
                self.indexes = self.indexes[:math.ceil(numIndexes / 2)]
                for i in range(math.ceil(numIndexes / 8)):
                    for j in range(8):
                        self.unique.append(getBit(uTemp[i], j))
        except:
            print('TLV Index Parse Fail')

    # decode Classifier output
    def parseClassifierOutput(self, data):
        print("\n function parseClassifierOutput used \n")
        classifierDataStruct = 'Ii'
        clOutSize = struct.calcsize(classifierDataStruct)
        self.classifierOutput = np.zeros((2, self.numDetectedTarget))
        for i in range(self.numDetectedTarget):
            self.classifierOutput[0, i], self.classifierOutput[1, i] = struct.unpack(classifierDataStruct,
                                                                                     data[:clOutSize])
            data = data[clOutSize:]

    # below is for labs that are compliant with SDK 3.x  This code can parse the point cloud TLV and point cloud side info TLV from the OOB demo.
    # It can parse the SDK3.x Compliant People Counting demo "tracker_dpc"
    # get SDK3.x Cartesian Point Cloud
    def parseSDK3xPoints(self, dataIn, numObj):
        print("\n function parseSDK3xPoints used \n")
        pointStruct = '4f'
        pointLength = struct.calcsize(pointStruct)
        try:
            for i in range(numObj):
                self.pcBufPing[0, i], self.pcBufPing[1, i], self.pcBufPing[2, i], self.pcBufPing[3, i] = struct.unpack(
                    pointStruct, dataIn[:pointLength])
                dataIn = dataIn[pointLength:]
            self.pcBufPing = self.pcBufPing[:, :numObj]
            print("in parseSD3xPOints pcbuffing= ",self.pcBufPing)
        except Exception as e:
            print(e)
            self.fail = 1

    # get Side Info SDK 3.x
    def parseSDK3xSideInfo(self, dataIn, numObj):
        print("\n function parseSDK3xSideInfo used \n")
        sideInfoStruct = '2h'
        sideInfoLength = struct.calcsize(sideInfoStruct)
        try:
            for i in range(numObj):
                self.pcBufPing[4, i], unused = struct.unpack(sideInfoStruct, dataIn[:sideInfoLength])
                dataIn = dataIn[sideInfoLength:]
                print("in parseSD3xsidepoints pcbuffing= [4,: ] ", self.pcBufPing[4])
        except Exception as e:
            print(e)
            self.fail = 1

    # convert SDK compliant Polar Point Cloud to Cartesian
    def polar2CartSDK3(self):
        print("\n function polar2CartSDK3 used \n")
        self.pcBufPing = np.empty((5, self.numDetectedObj))
        for n in range(0, self.numDetectedObj):
            self.pcBufPing[2, n] = self.pcPolar[0, n] * math.sin(self.pcPolar[2, n])  # z
            self.pcBufPing[0, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.sin(self.pcPolar[1, n])  # x
            self.pcBufPing[1, n] = self.pcPolar[0, n] * math.cos(self.pcPolar[2, n]) * math.cos(self.pcPolar[1, n])  # y
        self.pcBufPing[3, :] = self.pcPolar[3, 0:self.numDetectedObj]  # doppler

        self.polar_returned = cp(self.pcPolar[:5,:self.numDetectedObj])


    # decode SDK3.x Format Point Cloud in Polar Coordinates
    def parseSDK3xPolar(self, dataIn, tlvLength):
        print("\n function parseSDK3xPolar used \n")
        pointStruct = '4f'
        pointLength = struct.calcsize(pointStruct)
        self.numDetectedObj = int(tlvLength / pointLength)
        try:
            for i in range(self.numDetectedObj):
                self.pcPolar[0, i], self.pcPolar[1, i], self.pcPolar[2, i], self.pcPolar[3, i] = struct.unpack(
                    pointStruct, dataIn[:pointLength])
                dataIn = dataIn[pointLength:]
        except:
            self.fail = 1
            return
        self.polar2CartSDK3()

    # decode 3D People Counting Target List TLV

    # 3D Struct format

    # uint32_t     tid;     /*! @brief   tracking ID */
    # float        posX;    /*! @brief   Detected target X coordinate, in m */
    # float        posY;    /*! @brief   Detected target Y coordinate, in m */
    # float        posZ;    /*! @brief   Detected target Z coordinate, in m */
    # float        velX;    /*! @brief   Detected target X velocity, in m/s */
    ##float       velY;    /*! @brief   Detected target Y velocity, in m/s */
    # float        velZ;    /*! @brief   Detected target Z velocity, in m/s */
    # float        accX;    /*! @brief   Detected target X acceleration, in m/s2 */
    # float        accY;    /*! @brief   Detected target Y acceleration, in m/s2 */
    # float        accZ;    /*! @brief   Detected target Z acceleration, in m/s2 */
    # float        ec[16];  /*! @brief   Target Error covarience matrix, [4x4 float], in row major order, range, azimuth, elev, doppler */
    # float        g;
    # float        confidenceLevel;    /*! @brief   Tracker confidence metric*/

    def parseDetectedTracksSDK3x(self, data, tlvLength):
        print("\n function parseDetectedTracksSDK3x used \n")
        if (self.printVerbosity == 1):
            print(tlvLength)
        if (self.CaponEC):
            targetStruct = 'I27f'
        else:
            # targetStruct = 'I15f'
            targetStruct = 'I27f'
        targetSize = struct.calcsize(targetStruct)
        if (self.printVerbosity == 1):
            print('TargetSize=', targetSize)
        self.numDetectedTarget = int(tlvLength / targetSize)
        if (self.printVerbosity == 1):
            print('Num Detected Targets = ', self.numDetectedTarget)
        targets = np.empty((16, self.numDetectedTarget))
        rotTarget = [0, 0, 0]
        # theta = self.profile['elev_tilt']
        # print('theta = ',theta)
        # Rx = np.matrix([[ 1, 0           , 0           ],
        #           [ 0, math.cos(theta),-math.sin(theta)],
        #           [ 0, math.sin(theta), math.cos(theta)]])
        try:
            for i in range(self.numDetectedTarget):
                targetData = struct.unpack(targetStruct, data[:targetSize])
                if (self.printVerbosity == 1):
                    print(targetData)
                # tid, x, y
                if (self.CaponEC):
                    targets[0:13, i] = targetData[0:13]
                else:
                    # tid, pos x, pos y
                    targets[0:3, i] = targetData[0:3]
                    if (self.printVerbosity == 1):
                        print('Target Data TID,X,Y = ', targets[0:3, i])
                        print('i = ', i)
                    # pos z
                    targets[3, i] = targetData[3]

                    # rotTargetDataX,rotTargetDataY,rotTargetDataZ = rotX (targetData[1],targetData[2],targetData[3],self.profile['elev_tilt'])

                    # print('Target Data TID,X,Y = ',rotTargetDataX,', ',rotTargetDataY,', ',rotTargetDataZ)
                    # vel x, vel y
                    targets[4:6, i] = targetData[4:6]
                    # vel z
                    targets[6, i] = targetData[6]
                    # acc x, acc y
                    targets[7:9, i] = targetData[7:9]
                    # acc z
                    targets[9, i] = targetData[9]
                    # ec[16]
                    # targets[10:14,i]=targetData[10:14]
                    targets[10:13, i] = targetData[10:13]  # Chris 2020-12-18
                    if (self.printVerbosity == 1):
                        print('ec = ', targets[10:13, i])
                    # g
                    # targets[14,i]=targetData[14]
                    targets[14, i] = targetData[26]
                    if (self.printVerbosity == 1):
                        print('g= ', targets[14, i])
                    # confidenceLevel
                    # targets[15,i]=targetData[15]
                    targets[15, i] = targetData[27]
                    if (self.printVerbosity == 1):
                        print('Confidence Level = ', targets[15, i])

                    # self.textStructCapon3D[[frame #],[header,pt cloud data,target info],index,data]
                    # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
                    # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]
                    if (self.saveTextFile):
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 0] = targets[0, i]  # TID
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 1] = targets[1, i]  # x
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 2] = targets[2, i]  # y
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 3] = targets[3, i]  # z
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 4] = targets[4, i]  # vx
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 5] = targets[5, i]  # vy
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 6] = targets[6, i]  # vz
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 7] = targets[7, i]  # ax
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 8] = targets[8, i]  # ay
                        self.textStructCapon3D[self.frameNum % 1000, 2, i, 9] = targets[9, i]  # az
                        if (self.printVerbosity == 1):
                            print('target added to textStructCapon3D')
                data = data[targetSize:]
        except:
            print('Target TLV parse failed')
        self.targetBufPing = targets
        if (self.printVerbosity == 1):
            print(targets)

    # all TLV header decoding functions are below. Each lab with a Unique header or unique TLV set has its own header parsing function
    # decode Header and rest of TLVs for Legacy Labs and Indoor False detection mitigation
    def tlvHeader(self, data):
        print("\n function tlvHeader used \n")
        # search for magic word
        self.targetBufPing = np.zeros((12, 1))
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.indexes = []
        frameNum = -1
        self.numDetectedTarget = 0
        self.numDetectedObj = 0
        # search until we find magic word
        while (1):
            try:
                magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q10I2H', data[:self.headerLength])
            except:
                # bad data, return
                self.fail = 1
                return data
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                data = data[1:]
            else:
                # we have correct magic word, proceed to parse rest of data
                break

        # Sense and direct format
        # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
        if (self.saveTextFile):
            self.textStruct2D[self.frameNum % 1000, 0, 0, 0] = magic
            self.textStruct2D[self.frameNum % 1000, 0, 1, 0] = version
            self.textStruct2D[self.frameNum % 1000, 0, 2, 0] = platform
            self.textStruct2D[self.frameNum % 1000, 0, 3, 0] = timestamp
            self.textStruct2D[self.frameNum % 1000, 0, 4, 0] = packetLength
            self.textStruct2D[self.frameNum % 1000, 0, 5, 0] = frameNum
            self.textStruct2D[self.frameNum % 1000, 0, 6, 0] = subFrameNum
            self.textStruct2D[self.frameNum % 1000, 0, 7, 0] = chirpMargin
            self.textStruct2D[self.frameNum % 1000, 0, 8, 0] = frameMargin
            self.textStruct2D[self.frameNum % 1000, 0, 9, 0] = uartSentTime
            self.textStruct2D[self.frameNum % 1000, 0, 10, 0] = trackProcessTime
            self.textStruct2D[self.frameNum % 1000, 0, 11, 0] = numTLVs
            self.textStruct2D[self.frameNum % 1000, 0, 12, 0] = checksum
            if (self.printVerbosity == 1):
                print('FrameNumber = ', self.textStruct2D[self.frameNum % 1000, 0, 5, 0])

        if (self.frameNum != frameNum):
            self.missedFrames += 1
            self.frameNum = frameNum
        self.frameNum += 1
        if (len(data) < packetLength):
            ndata = self.dataCom.read(packetLength - len(data))
            if (self.saveBinary):
                self.oldData += ndata
            data += ndata
        data = data[self.headerLength:]
        for i in range(numTLVs):
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(data[:8])
            except:
                print('read fail: not enough data')
                self.missedFrames += 1
                self.fail = 1
                break
            try:
                data = data[8:]
                print("TLV type = ",tlvType)
                if (tlvType == 6):
                    if (self.threeD):
                        self.parseDetectedObjects3D(data[:tlvLength], tlvLength - 8)
                    elif (self.ifdm):
                        self.parseDetectedObjectsIFDM(data[:tlvLength], tlvLength - 8)
                    else:
                        self.parseDetectedObjects(data[:tlvLength], tlvLength - 8)
                elif (tlvType == 7):
                    if (self.threeD):
                        self.parseDetectedTracks3D(data[:tlvLength], tlvLength - 8)
                    else:
                        self.parseDetectedTracks(data[:tlvLength], tlvLength - 8)
                elif (tlvType == 8):
                    self.parseTargetAssociations(data[:tlvLength - 8])
                elif (tlvType == 9):
                    self.parseClassifierOutput(data[:tlvLength - 8])
                data = data[tlvLength - 8:]
            except:
                print('Not enough data')
                print('Data length: ', len(data))
                print('Reported Packet Length: ', packetLength)
                self.fail = 1
                return data
        return data

    # parsing for SDK 3.x Point Cloud
    def sdk3xTLVHeader(self, dataIn):
        print("\n function sdk3xTLVHeader used \n")

        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        headerStruct = 'Q8I'
        headerLength = struct.calcsize(headerStruct)
        tlvHeaderLength = 8
        # search until we find magic word
        while (1):
            try:
                magic, version, totalPacketLen, platform, self.frameNum, timeCPUCycles, self.numDetectedObj, numTLVs, subFrameNum = struct.unpack(
                    headerStruct, dataIn[:headerLength])
            except:
                # bad data, return
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # we have correct magic word, proceed to parse rest of data
                break
        dataIn = dataIn[headerLength:]
        remainingData = totalPacketLen - len(dataIn)
        count = 0
        # check to ensure we have all of the data
        while (remainingData > 0 and count < 3):
            newData = self.dataCom.read(remainingData)
            remainingData = totalPacketLen - len(dataIn) - len(newData)
            dataIn += newData
            count += 1
            if (self.saveBinary):
                self.oldData += newData
        # now check TLVs
        # print ('got tlvs sdk3x')
        print("numTLVs = ", numTLVs)
        for i in range(numTLVs):
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
            except Exception as e:
                print(e)
                print('failed to read OOB SDK3.x TLV')
            dataIn = dataIn[tlvHeaderLength:]
            print("TLV type = ", tlvType)
            if (tlvType == 1):
                self.parseSDK3xPoints(dataIn[:tlvLength], self.numDetectedObj)
                dataIn = dataIn[tlvLength:]
            elif (tlvType == 7):
                self.parseSDK3xSideInfo(dataIn[:tlvLength], self.numDetectedObj)
                dataIn = dataIn[tlvLength:]
        return dataIn

    # parsing for SDK 3.x DPIF compliant People Counting
    def sdk3xPCHeader(self, dataIn):
        print("\n function sdk3xPCHeader used \n")
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        #self.polar_returned = np.zeros((5, self.maxPoints))
        self.targetBufPing = np.zeros((13, 20))
        self.indexes = []
        tlvHeaderLength = 8
        # search until we find magic word
        while (1):
            try:
                magic, version, platform, timestamp, packetLength, self.frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q10I2H', dataIn[:self.headerLength])
            except:
                # bad data, return
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # we have correct magic word, proceed to parse rest of data
                break
        dataIn = dataIn[self.headerLength:]
        remainingData = packetLength - len(dataIn)
        if (self.printVerbosity == 1):
            print('pl: ', packetLength)
            print('remainingData ', remainingData)
        # check to ensure we have all of the data
        # check to ensure we have all of the data
        count = 0
        while (remainingData > 0 and count < 3):
            if (self.printVerbosity == 1):
                print('RD Loop')
            newData = self.dataCom.read(remainingData)
            remainingData = packetLength - len(dataIn) - len(newData)
            dataIn += newData
            count += 1
            if (remainingData == 0):
                if (self.saveBinary):
                    self.oldData += newData
        # now check TLVs
        if (self.printVerbosity == 1):
            print('Frame: ', self.frameNum)
            print(len(dataIn))
            print(numTLVs)
        for i in range(numTLVs):
            try:
                # print("DataIn Type", type(dataIn))
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
                if (self.printVerbosity == 1):
                    print('TLV length = ', tlvLength)
            except Exception as e:
                if (self.printVerbosity == 1):
                    print(e)
                    print('failed to read OOB SDK3.x TLV')
                    print('TLV num: ', i)
            dataIn = dataIn[tlvHeaderLength:]
            dataLength = tlvLength
            print("TLV type = ",tlvType)
            if (tlvType == 6):
                # DPIF Polar Coordinates
                # print('pointcloud lrpd')
                self.parseSDK3xPolar(dataIn[:dataLength], dataLength)
            elif (tlvType == 7):
                # target 3D
                self.parseDetectedTracksSDK3x(dataIn[:dataLength], dataLength)
            elif (tlvType == 8):
                # target index
                self.parseTargetAssociations(dataIn[:dataLength])
            elif (tlvType == 9):
                # side info
                self.parseSDK3xSideInfo(dataIn[:dataLength], self.numDetectedObj)
            dataIn = dataIn[dataLength:]
        return dataIn

    # parsing for 3D People Counting lab
    def Capon3DHeader(self, dataIn):
        print("\n function Capon3DHeader used \n")
        # reset point buffers
        self.pcBufPing = np.zeros((5, self.maxPoints))
        self.pcPolar = np.zeros((5, self.maxPoints))
        self.polar_returned = np.zeros((5, self.maxPoints))
        self.targetBufPing = np.zeros((13, 20))
        self.numDetectedTarget = 0
        self.numDetectedObj = 0
        self.indexes = []
        tlvHeaderLength = 8
        headerLength = 48
        # stay in this loop until we find the magic word or run out of data to parse
        while (1):
            try:
                magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum = struct.unpack(
                    'Q9I2H', dataIn[:headerLength])
            except Exception as e:
                # bad data, return
                # print("Cannot Read Frame Header")
                # print(e)
                self.fail = 1
                return dataIn
            if (magic != self.magicWord):
                # wrong magic word, increment pointer by 1 and try again
                dataIn = dataIn[1:]
            else:
                # got magic word, proceed to parse
                break

        dataIn = dataIn[headerLength:]
        remainingData = packetLength - len(dataIn) - headerLength
        # check to ensure we have all of the data
        # print('remaining data = ',remainingData)
        if (remainingData > 0):
            newData = self.dataCom.read(remainingData)
            remainingData = packetLength - len(dataIn) - headerLength - len(newData)
            dataIn += newData
            if (self.saveBinary):
                self.oldData += newData
        if (self.saveTextFile):
            self.textStructCapon3D[self.frameNum % 1000, 0, 0, 0] = magic
            self.textStructCapon3D[self.frameNum % 1000, 0, 1, 0] = version
            self.textStructCapon3D[self.frameNum % 1000, 0, 2, 0] = packetLength
            self.textStructCapon3D[self.frameNum % 1000, 0, 3, 0] = platform
            self.textStructCapon3D[self.frameNum % 1000, 0, 4, 0] = frameNum
            self.textStructCapon3D[self.frameNum % 1000, 0, 5, 0] = subFrameNum
            self.textStructCapon3D[self.frameNum % 1000, 0, 6, 0] = chirpMargin
            self.textStructCapon3D[self.frameNum % 1000, 0, 7, 0] = frameMargin
            self.textStructCapon3D[self.frameNum % 1000, 0, 8, 0] = uartSentTime
            self.textStructCapon3D[self.frameNum % 1000, 0, 9, 0] = trackProcessTime
            self.textStructCapon3D[self.frameNum % 1000, 0, 10, 0] = numTLVs
            self.textStructCapon3D[self.frameNum % 1000, 0, 11, 0] = checksum
            if (self.printVerbosity == 1):
                print('FrameNumber = ', self.textStructCapon3D[self.frameNum % 1000, 0, 4, 0])

        # now check TLVs
        for i in range(numTLVs):
            # try:
            # print("DataIn Type", type(dataIn))
            try:
                tlvType, tlvLength = self.tlvHeaderDecode(dataIn[:tlvHeaderLength])
                dataIn = dataIn[tlvHeaderLength:]
                dataLength = tlvLength - tlvHeaderLength
            except:
                print('TLV Header Parsing Failure')
                self.fail = 1
                return dataIn
            print("TLV type = ", tlvType)
            if (tlvType == 6):
                # DPIF Polar Coordinates
                self.parseCapon3DPolar(dataIn[:dataLength], dataLength)
            elif (tlvType == 7):
                # target 3D
                self.parseDetectedTracksSDK3x(dataIn[:dataLength], dataLength)
            elif (tlvType == 8):
                # target index
                self.parseTargetAssociations(dataIn[:dataLength])
            elif (tlvType == 9):
                if (self.printVerbosity == 1):
                    print('type9')
                # side info
                # self.parseSDK3xSideInfo(dataIn[:dataLength], self.numDetectedObj)
            dataIn = dataIn[dataLength:]
            # except Exception as e:
            #    print(e)
            #    print ('failed to read OOB SDK3.x TLV')
        if (self.frameNum + 1 != frameNum):
            self.missedFrames += frameNum - (self.frameNum + 1)
        self.frameNum = frameNum
        return dataIn

    # This function is always called - first read the UART, then call a function to parse the specific demo output
    # This will return 1 frame of data. This must be called for each frame of data that is expected. It will return a dict containing:
    #   1. Point Cloud
    #   2. Target List
    #   3. Target Indexes
    #   4. number of detected points in point cloud
    #   5. number of detected targets
    #   6. frame number
    #   7. Fail - if one, data is bad
    #   8. classifier output
    # Point Cloud and Target structure are liable to change based on the lab. Output is always cartesian.
    def readAndParseUart(self):
        self.fail = 0
        if (self.replay):
            return self.replayHist()
        numBytes = 4666
        data = self.dataCom.read(numBytes)
        if (self.byteData is None):
            self.byteData = data
        else:
            self.byteData += data
        if (self.saveBinary):
            self.oldData += data
        # try:
        if (self.SDK3xPointCloud == 1):
            self.byteData = self.sdk3xTLVHeader(self.byteData)
        elif (self.SDK3xPC == 1):
            self.byteData = self.sdk3xPCHeader(self.byteData)
        elif (self.capon3D == 1):
            self.byteData = self.Capon3DHeader(self.byteData)
        else:
            self.byteData = self.tlvHeader(self.byteData)
        # except Exception as e:
        #    print(e)
        #    self.fail = 1
        # return data after parsing and save to replay file
        if (self.fail):
            return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput,self.polar_returned
        if (self.saveBinary):
            if (self.frameNum % 1000 == 0):
                toSave = bytes(self.oldData)
                fileName = 'binData/pHistBytes_' + str(self.saveNum) + '.bin'
                self.saveNum += 1
                bfile = open(fileName, 'wb')
                bfile.write(toSave)
                self.oldData = []
                print('Missed Frames ' + str(self.missedFrames) + '/1000')
                self.missedFrames = 0
                bfile.close
        if (self.saveTextFile):
            if (self.frameNum % 1000 == 0):
                if (self.capon3D):
                    toSave = self.textStructCapon3D
                elif (self.ifdm):
                    toSave = self.textStruct2D
                print('Saved data file ', self.saveNumTxt)
                #fileName = 'binData/pHistText_' + str(self.saveNumTxt) + '.csv'
                fileName = 'Frames_' + str(self.saveNumTxt) + '.csv'
                if (self.saveNumTxt < 75):
                    self.saveNumTxt += 1
                else:
                    self.saveNumTxt = 0
                    self.stop_xlsx= 1
                tfile = open(fileName, 'w')
                tfile.write('This file contains parsed UART data in sensor centric coordinates\n')
                tfile.write('file format version 1.0\n')
                # tfile.write(str(toSave))

                if (self.capon3D):
                    # [frame #][header,pt cloud data,target info]
                    # [][header][magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#elev, azim, doppler, range, snr]
                    # [][target][Target #][TID,x,y,z,vx,vy,vz,ax,ay,az]

                    for i in range(1000):
                        tfile.write(
                            'magic, version, packetLength, platform, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum\n')
                        for j in range(0, 12):
                            tfile.write(str(self.textStructCapon3D[i, 0, j, 0]))
                            tfile.write(',')
                            # print(str(self.textStructCapon3D[i,0,j,0]))
                        tfile.write('\n')
                        tfile.write('elev, azim, doppler, range, snr\n')
                        for j in range(np.count_nonzero(self.textStructCapon3D[i, 1, :,
                                                        0])):  # self.numDetectedObj):#len(self.textStructCapon3D[i,1,:,0]!=0)):
                            for k in range(5):
                                tfile.write(str(self.textStructCapon3D[i, 1, j, k]))
                                tfile.write(',')
                            tfile.write('\n')

                        tfile.write('TID,x,y,z,vx,vy,vz,ax,ay,az\n')
                        for j in range(np.count_nonzero(self.textStructCapon3D[i, 2, :, 1])):
                            for k in range(10):
                                tfile.write(str(self.textStructCapon3D[i, 2, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                    self.textStructCapon3D = np.zeros(1000 * 3 * 12 * self.maxPoints).reshape(
                        (1000, 3, 12, self.maxPoints))  # [frame #][header,pt cloud data,target info]
                    tfile.close

                    pattern = r'[0-9]'
                    self.filename_xlsx=re.sub(pattern, '', fileName)
                    self.filename_xlsx= self.filename_xlsx.replace("csv", "xlsx").replace("_","")

                    if self.saveNumTxt == 1:
                        wb = Workbook()
                        ws = wb.active
                        with open(fileName) as f:
                            for row in csv.reader(f):
                                ws.append(row)
                        wb.save(self.filename_xlsx)
                        print("self.stop_xlsx =",self.stop_xlsx,"self.saveNumTxt",self.saveNumTxt)
                    elif ( (self.saveNumTxt % 3) == 1 and self.stop_xlsx !=1 ): #TODO evolving integer for stop_xlsx and save in new xlsx when (self.saveNumTxt % 8) == 1 because excel files have a max of possible rows ~1Million
                        print("we go there")
                        wb = load_workbook(self.filename_xlsx)
                        ws = wb.active
                        with open(fileName) as f:
                            for row in csv.reader(f):
                                ws.append(row)
                        wb.save(self.filename_xlsx)
                        for i in range(3):
                            os.remove("Frames_" + str(i) + ".csv")


                if (self.ifdm):
                    # Sense and direct format
                    # [frame #][header,pt cloud data,target info]
                    # [][header][magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum]
                    # [][pt cloud][pt index][#range, azim, doppler, snr]
                    # [][target][Target #][TID,x,y,vx,vy,ax,ay]
                    for i in range(1000):
                        tfile.write(
                            'magic, version, platform, timestamp, packetLength, frameNum, subFrameNum, chirpMargin, frameMargin, uartSentTime, trackProcessTime, numTLVs, checksum\n')
                        for j in range(13):
                            tfile.write(str(self.textStruct2D[i, 0, j, 0]))
                            tfile.write(',')
                        tfile.write('\n')
                        tfile.write('range, azim, doppler, snr\n')
                        for j in range(np.count_nonzero(self.textStruct2D[i, 1, :, 0])):
                            for k in range(4):
                                tfile.write(str(self.textStruct2D[i, 1, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                        if np.count_nonzero(self.textStruct2D[i, 2, :, 1]) != 0:
                            tfile.write('TID,x,y,vx,vy,ax,ay\n')
                        for j in range(np.count_nonzero(self.textStruct2D[i, 2, :, 1])):
                            for k in range(7):
                                tfile.write(str(self.textStruct2D[i, 2, j, k]))
                                tfile.write(',')
                            tfile.write('\n')
                    self.textStruct2D = np.zeros(1000 * 3 * self.maxPoints * 7).reshape(
                        (1000, 3, self.maxPoints, 7))  # [frame #][header,pt cloud data,target info]
                    tfile.close

        parseEnd = int(round(time.time() * 1000))
        # print (self.pcBufPing)
        return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput,self.polar_returned

    # find various utility functions here for connecting to COM Ports, send data, etc...
    # connect to com ports
    # Call this function to connect to the comport. This takes arguments self (intrinsic), uartCom, and dataCom. No return, but sets internal variables in the parser object.
    def connectComPorts(self, uartCom, dataCom):
        self.uartCom = serial.Serial(uartCom, 115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                     timeout=0.3)
        if (self.capon3D == 1 and self.aop == 0):
            self.dataCom = serial.Serial(dataCom, 921600 * 1, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                         timeout=0.025)
        else:
            self.dataCom = serial.Serial(dataCom, 921600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE,
                                         timeout=0.025)
        self.dataCom.reset_output_buffer()
        print('Uart Ports Connected')

    # send cfg over uart
    def sendCfg(self, cfg):
        for line in cfg:
            time.sleep(.1)
            self.uartCom.write(line.encode())
            ack = self.uartCom.readline()
            print(ack)
            ack = self.uartCom.readline()
            print(ack)
        time.sleep(3)
        self.uartCom.reset_input_buffer()
        self.uartCom.close()
        print("Configuration sent")

    # send single command to device over UART Com.
    def sendLine(self, line):
        self.uartCom.write(line.encode())
        ack = self.uartCom.readline()
        print(ack)
        ack = self.uartCom.readline()
        print(ack)

    def replayHist(self):
        if (self.replayData):
            # print('reading data')
            # print('fail: ',self.fail)
            # print(len(self.replayData))
            # print(self.replayData[0:8])
            self.replayData = self.Capon3DHeader(self.replayData)
            # print('fail: ',self.fail)
            return self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput
            # frameData = self.replayData[0]
            # self.replayData = self.replayData[1:]
            # return frameData['PointCloud'], frameData['Targets'], frameData['Indexes'], frameData['Number Points'], frameData['NumberTracks'],frameData['frame'],0, frameData['ClassifierOutput'], frameData['Uniqueness']
        else:
            filename = 'overheadDebug/binData/pHistBytes_' + str(self.saveNum) + '.bin'
            # filename = 'Replay1Person10mShort/pHistRT'+str(self.saveNum)+'.pkl'
            self.saveNum += 1
            try:
                dfile = open(filename, 'rb', 0)
            except:
                print('cant open ', filename)
                return -1
            self.replayData = bytes(list(dfile.read()))
            if (self.replayData):
                print('entering replay')
                return self.replayHist()
            else:
                return -1


def getBit(byte, bitNum):
    mask = 1 << bitNum
    if (byte & mask):
        return 1
    else:
        return 0




and below is my code:

 
import math
from oob_parser_ori import uartParserSDK
import serial
#import time
#import matplotlib.pyplot as plt
#import datetime
import tkinter as tk
from tkinter import *
import tkinter.filedialog
import tkinter.ttk as ttk  # combobox
from tkinter.ttk import *
from tkinter import messagebox
import serial.tools.list_ports
import numpy as np
import sys
import xlsxwriter

#TODO  test for sdk out of box demo, make another one_by_one for plot testing
class setup():
    def __init__(self):
        self.userPort = "COM -1"
        self.dataPort = "COM -1"
        self.profile = {}
        self.default=-1
        self.root=Tk()
        self.demo_name=""
        self.config_file=""
        self.profile_filename = 'Profile_.xlsx'
       # self.greetings_demo()
        #self.parser = uartParserSDK(type=demo_name)
        #self.parser.frametime

    def main_program(self):

        #self.mainProgram_start_time = time.time()
        while (1):

             # ------------------ READING AND PARSE OF DATA  {use of readandParseUART() from oob_parser_ori.py } -------------------------

            self.parsedData=self.parser.readAndParseUart()

            print("\n length of data is ", len(self.parsedData))
            print("Parsed data = ",self.parsedData)
            for i in range(len(self.parsedData)):
                print(f"parsedData[{i}] = ", self.parsedData[i])

            # if ((time.time() - self.mainProgram_start_time) > 60): #TODO plot evolution plot

            #("pcbufping (x,y,z,doppler,snr) ", self.parsedData[0])
            print("length de parsed data [0] aka pcbufping = ", len (self.parsedData[0]))
            for i in range(len(self.parsedData[0])):
                print(f"parsedData[0][{i}] = ", self.parsedData[0][i])
            print("\nx PC= ",self.parsedData[0][0])
            print("\ny PC = ", self.parsedData[0][1])
            print("\nz PC = ", self.parsedData[0][2])
            print("\ndoppler = ",self.parsedData[0][3])
            print("\nsnr = ",self.parsedData[0][4])

            #print("polar_returned ( range,azimuth,elevation,doppler,snr) ", self.parsedData[8])
            print("\nrange = ", self.parsedData[8][0])
            print("\nazimuth = ", self.parsedData[8][1])
            print("\nelevation = ", self.parsedData[8][2])
            #print("\ndoppler = ", self.parsedData[8][3])
            #print("\nsnr = ", self.parsedData[8][4])

            #print("Target List", self.parsedData[1])
            print("Target Indexes", self.parsedData[2])
            print("number of detected points in point cloud", self.parsedData[3])
            print("number of detected targets or people count", self.parsedData[4])
            print("frame number", self.parsedData[5])
            print("If 1, data is bad", self.parsedData[6])
            print("classifier output", self.parsedData[7])

            if self.demo_name == "3D People Counting":
                i = 0
                for key, value in self.Targets.items(): #keys may  not ordered?  > for key in ["targetId, "posX"], "posY", "posZ", "velX", "velY", "velZ","accX", "accY", "accZ"]
                #for key in sorted(self.Targets.keys()):

                    self.Targets[key] = self.parsedData[1][i]
                    i += 1

                print("Position X tab for targets", self.Targets["posX"][0])
                print("Position Y tab for targets", self.Targets["posY"][0])
                print("Position Z tab for targets", self.Targets["posZ"][0])
                print("Velocity X tab for targets", self.Targets["velX"][0])
                print("Velocity Y tab for targets", self.Targets["velY"][0])
                print("Velocity Z tab for targets", self.Targets["velZ"][0])
                print("Acceleration X tab for targets", self.Targets["accX"][0])
                print("Acceleration Y tab for targets", self.Targets["accY"][0])
                print("Acceleration Z tab for targets", self.Targets["accZ"][0])
                print("Targets =", self.Targets)


            i = 0
            #for key in self.Point_cloud.keys() - self.ignore_keys_1:
            for key in ["x","y","z","doppler","snr"]:
                self.Point_cloud[key] = self.parsedData[0][i]

                i += 1
            i = 0
            for key in ["range","azimuth","elev"]:
                self.Point_cloud[key] = self.parsedData[8][i]

                i += 1

            print("Point cloud= ",self.Point_cloud)



    def program(self):
        self.greetings_demo()
        self.parser=uartParserSDK(type=self.demo_name)
        self.init_data_structure()
        self.greetings_ComPorts()
        self.Config_profile()
        self.main_program()
        #self.parser.frametime=50 # 50 ms



    def init_data_structure(self):
        if self.demo_name == "SDK Out of Box Demo":

            self.Point_cloud = {"x": [], "y": [], "z": [], "doppler": [], "snr": [], "range": [], "azimuth": [],
                                "elev": []}

        elif self.demo_name == "3D People Counting":

            self.Targets = {"targetId": [], "posX": [], "posY": [], "posZ": [],
                            "velX": [], "velY": [], "velZ": [],
                            "accX": [], "accY": [], "accZ": []}
            # ec[16] Target Error covarience matrix ; g ; confidenceLevel( Tracker confidence metric) also available

            self.Point_cloud = {"x": [], "y": [], "z": [], "doppler": [], "snr": [], "range": [], "azimuth": [],
                                "elev": []}
            #self.ignore_keys_1 =["range","azimuth","elev"]
            #self.ignore_keys_2=["x","y","z","doppler","snr"]


        #elif self.demo_name == "Sense and Detect HVAC Control":
           # self.Targets = {"targetId": [], "posX": [], "posY": [],
                       #     "velX": [], "velY": [],
                        #    "accX": [], "accY": []}

            #self.Point_cloud = {"x": [], "y": [], "doppler": [], "snr": [], "range": [], "azimuth": []}

    def retrieve(self):
        if (self.Combo.get() == "Pick a Demo"):
            indication = tk.Label(text="Please choose a Demo")
            indication.pack()
            self.root.update_idletasks()
            self.root.update()
        else:
            self.demo_name = str(self.Combo.get())
            print("Choosen demo:", self.demo_name)
            self.root.destroy()

    def onWindow_closing(self):
        if messagebox.askokcancel("Quit", "Do you want to quit?"):
            self.root.destroy()
            sys.exit("Program stopped")

    def greetings_demo(self):
        self.root.title("GUI ")
        self.root.iconbitmap('icon.ico')
        self.root.geometry("200x150")
        frame = Frame(self.root)
        frame.pack()
        #TODO (only SDK oob and 3D People counting supported for now)
        #list = ["SDK Out of Box Demo", "Sense and Detect HVAC Control", "3D People Counting",
                 #"Long Range People Detection"]

        list = ["SDK Out of Box Demo", "3D People Counting"]

        self.Combo = ttk.Combobox(frame, values=list)
        self.Combo.set("Pick a Demo")
        self.Combo.pack(padx=4, pady=4)
        Button = tk.Button(frame, text="Submit", command=lambda: [f() for f in [self.retrieve]])
        Button.pack(padx=4, pady=4)


        self.root.protocol("WM_DELETE_WINDOW", self.onWindow_closing)
        self.root.mainloop()

    def greetings_ComPorts(self):
        if self.demo_name == "":
            self.root = Tk()
            self.greetings_demo()

        self.root = Tk()
        self.root.title("GUI ")
        self.root.iconbitmap('icon.ico')
        frame = tk.Frame(self.root)
        frame.pack()
        self.ComPorts_autodetection()

        #label1=tk.Label(text=f"default Setup for AWR6843: \n\nSDK Out of Box Demo \n\n user com port: {text} \n\ndata com port {dataPort_temp} \n ",anchor='w')\
        label = tk.Label(frame,text=f"Com ports are automatically detected for AWR6843. \n User port = {self.userPort} \n Data port = {self.dataPort}\n if default setup is exact, press submit, else change manually \n" )
        label.pack()

        default_com = tk.Button(frame,
                                text="submit",
                                fg="blue",
                                command=lambda: self.Com_Ports_connection(1))
        default_com.pack()

        change = tk.Button(frame,
                           text="change setup",
                           fg='#ff1944',
                           command=lambda: [f() for f in [lambda: self.Com_Ports_connection(0)]])
        change.pack()
        self.root.protocol("WM_DELETE_WINDOW", self.onWindow_closing)
        self.root.mainloop()

    def com_connection(self,userPort, dataPort):
        try:
            self.parser.connectComPorts(userPort, dataPort)
        except Exception as e:
            print(e)
            print('Com port connection failed')

    def ComPorts_autodetection(self):
        ports = serial.tools.list_ports.comports(include_links=False)
        userPort = -1
        dataPort = -1
        for p in ports:
            if 'Silicon Labs Dual CP2105 USB to UART Bridge: Enhanced COM Port' in p.description:
                self.userPort= p.device
                print("user com port number", self.userPort)
            if 'Silicon Labs Dual CP2105 USB to UART Bridge: Standard COM Port' in p.description:
                self.dataPort = p.device
                print("data com port number", self.dataPort)
        #return userPort, dataPort

    def UserDefined_comPorts(self,user_entry,data_entry):
        self.userPort = 'COM' + user_entry.get()

        self.dataPort = 'COM' + data_entry.get()
        if (user_entry.get() == "" or data_entry.get() == ""):
            label = tk.Label(text="Please provide both com ports")
            label.pack()
            # self.root.mainloop()
            self.root.update_idletasks()
            self.root.update()
        print("user com=", self.userPort, "data com = ", self.dataPort)
        self.root.destroy()
        self.com_connection(self.userPort, self.dataPort)
        self.cfg_file_load()


    def Com_Ports_connection(self,default):

        if (default) :
           self.com_connection(self.userPort,self.dataPort)
           self.root.destroy()
           self.cfg_file_load()
        else:
            self.root.destroy()
            self.root = Tk()
            self.root.title("GUI ")
            self.root.iconbitmap('icon.ico')
            frame = tk.Frame(self.root)
            frame.pack()
            tk.Label(frame,text="Press ok button when done").pack()
            tk.Label(frame,text="Please provide user com port #").pack()
            user_entry=tk.Entry(frame)
            user_entry.pack()
            tk.Label(frame,text="Please provide user com port # ").pack()
            data_entry=tk.Entry(frame)
            data_entry.pack()

            ok = tk.Button(frame,
                           text="ok",
                           fg="blue",
                           command=lambda: self.UserDefined_comPorts(user_entry,data_entry))

            ok.pack(side=tk.LEFT)
            self.root.mainloop()

    def cfg_file_load(self):
        self.root = Tk()
        self.root.title("GUI ")
        self.root.iconbitmap('icon.ico')
        # root.overrideredirect(1)
        self.root.withdraw()
        print("Please provide the adequate config file (.cfg)")
        cfgfile_path = tk.filedialog.askopenfilename(title="Please provide the adequate config file (.cfg)")
        print("Selected config file = ", cfgfile_path)
        self.config_file = r"{}".format(cfgfile_path)
        print("config file=",self.config_file)
        self.root.destroy()

    def Config_profile(self):
        counter = 0
        chirpCount = 0
        cfg_file = open(self.config_file, 'r')
        cfg = cfg_file.readlines()
        print(cfg)

        for line in cfg:
            args = line.split()
            if (len(args) > 0):
                if (args[0] == 'channelCfg'):
                    self.antenna_setup = {'Rx1': bin(int(args[1]))[2], 'Rx2': bin(int(args[1]))[3], 'Rx3': bin(int(args[1]))[4],
                                'Rx4': bin(int(args[1]))[5], 'Tx1': bin(int(args[2]))[2], 'Tx2': bin(int(args[2]))[3],
                                'Tx3': bin(int(args[2]))[4]}
                    print("\n Enabled antennas = ", self.antenna_setup, "\n")
                elif (args[0] == 'SceneryParam' or args[0] == 'boundaryBox'):
                    boundaryLine = counter
                    self.profile['leftX'] = float(args[1])
                    self.profile['rightX'] = float(args[2])
                    self.profile['nearY'] = float(args[3])
                    self.profile['farY'] = float(args[4])
                    self.profile['bottomZ'] = float(args[5])
                    self.profile['topZ'] = float(args[6])
                    self.profile['bottomZ'] = float(-3)
                    self.profile['topZ'] = float(3)
                elif (args[0] == 'staticBoundaryBox'):
                    staticLine = counter
                elif (args[0] == 'profileCfg'):
                    self.profile['startFreq'] = float(args[2])
                    self.profile['idle'] = float(args[3])
                    self.profile['adcStart'] = float(args[4])
                    self.profile['rampEnd'] = float(args[5])
                    self.profile['slope'] = float(args[8])
                    self.profile['samples'] = float(args[10])
                    self.profile['sampleRate'] = float(args[11])
                elif (args[0] == 'frameCfg'):
                    self.profile['numLoops'] = float(args[3])
                    self.profile['numTx'] = float(args[2]) + 1
                elif (args[0] == 'chirpCfg'):
                    chirpCount += 1
                elif (args[0] == 'sensorPosition'):
                    self.profile['sensorHeight'] = float(args[1])
                    self.profile['az_tilt'] = float(args[2])
                    self.profile['elev_tilt'] = float(args[3])
                    self.profile['groudLevelZ'] = 3 - self.profile['sensorHeight']

            counter += 1

        # FURTHER COMPUTATION
        self.profile['maxRange'] = self.profile['sampleRate'] * 1e3 * 0.9 * 3e8 / (2 * self.profile['slope'] * 1e12)


        bw = self.profile['samples'] / (self.profile['sampleRate'] * 1e3) * self.profile['slope'] * 1e12
        self.profile['Range Resolution'] = 3e8 / (2 * bw)

        Tc = (self.profile['idle'] * 1e-6 + self.profile['rampEnd'] * 1e-6) * chirpCount
        lda = 3e8 / (self.profile['startFreq'] * 1e9)

        self.profile['maxVelocity'] = lda / (4 * Tc)

        self.profile['velocityRes'] = lda / (2 * Tc * self.profile['numLoops'] * self.profile['numTx'])
        workbook = xlsxwriter.Workbook(self.profile_filename)
        worksheet = workbook.add_worksheet()
        row = 0
        col = 0
        for key, value in self.profile.items():
            worksheet.write(row, col, key)
            worksheet.write(row, col + 1, value)
            row += 1
        workbook.close()

        self.parser.sendCfg(cfg)




# Program--------
if __name__ == '__main__':
    setup=setup()
    setup.program()



 


 

Please read till the end as It took me forever to write  :) 

I also tried the Out of box demo as you suggested. I undestand there is no tracking algorithm used so there is no "Target". 

You can also try and select SDK out of box in my code (UartReader.py) to try it out. 

For this demo, you can see in my  main_program(self) function, that I  try to to get  all the point cloud infos: "x","y","z","doppler","snr"  "range", "azimuth", "elev"   of Point cloud as I want to make a path finder code using detected objects. 

Great news is that with this demo, I am able to get info in Point clouds when the radar is facing still objects: 

Here is my result: 

parsedData[0] = [[-8.17614719e-02 -8.58495414e-01 -8.17614734e-01 -2.72538245e-01
-1.54801714e+00 2.40787530e+00]
[ 3.03485692e-02 3.18660110e-01 7.21068442e-01 2.16320539e+00
2.68124437e+00 3.26315093e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00
0.00000000e+00 0.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00
0.00000000e+00 0.00000000e+00]
[ 4.93000000e+02 1.88000000e+02 2.67000000e+02 1.84000000e+02
1.76000000e+02 1.92000000e+02]]

parsedData[1] = Target [don't care >>>> no target for this demo]
parsedData[2] = []   don't care(Target Indexes)

parsedData[3] = 6 = number of detected points in point cloud

parsedData[4] = 0 = number of detected targets or people count (don't care)

parsedData[5] = 251  frame number
parsedData[6] = 0 data is ok 
parsedData[7] = []  classifier output (don't care)
parsedData[8] = [[0. 0. 0. ... 0. 0. 0.]     pc polar for 3D people counting (don't care) 
[0. 0. 0. ... 0. 0. 0.]
[0. 0. 0. ... 0. 0. 0.]
[0. 0. 0. ... 0. 0. 0.]
[0. 0. 0. ... 0. 0. 0.]]

length de parsed data [0] aka pcbufping in OOB_parser.py  = 5

parsedData[0][0] = [-0.08176147 -0.85849541 -0.81761473 -0.27253824 -1.54801714 2.4078753 ] 
parsedData[0][1] = [0.03034857 0.31866011 0.72106844 2.16320539 2.68124437 3.26315093]
parsedData[0][2] = [0. 0. 0. 0. 0. 0.]
parsedData[0][3] = [0. 0. 0. 0. 0. 0.]
parsedData[0][4] = [493. 188. 267. 184. 176. 192.]

x PC= [-0.08176147 -0.85849541 -0.81761473 -0.27253824 -1.54801714 2.4078753 ]

y PC = [0.03034857 0.31866011 0.72106844 2.16320539 2.68124437 3.26315093]

z PC = [0. 0. 0. 0. 0. 0.]    No Z ? 

doppler = [0. 0. 0. 0. 0. 0.]  No doppler ? (I was moving )

snr = [493. 188. 267. 184. 176. 192.]   is this right ???

range = [0. 0. 0. ... 0. 0. 0.]   no range ?

azimuth = [0. 0. 0. ... 0. 0. 0.] no azimuth ?

BUT I am just unable to have range and doppler info with this demo using the oob_parser.py 

Yet I tried the demo with the mmwave demo viisualizer 3.5 version and I do have all the plots 

I also read the .dat after recording the test using mmwave visualizer with  the mmw_demo_example_script.py and parser_mmw_demo.py files from C:\ti\mmwave_sdk_03_05_00_04\packages\ti\demo\Parser_scripts 

and got this:  (I USED THE SAME CONFIG FILE for both the mmwave visualizer and the oob_parser )

Flashed this:

C:\ti\mmwave_industrial_toolbox_4_7_0\labs\out_of_box_demo\68xx_mmwave_sdk_dsp\prebuilt_binaries\xwr68xx_mmw_demo.bin

and used this .cfg file

% ***************************************************************
% Created for SDK ver:03.05
% Created using Visualizer ver:3.5.0.0
% Frequency:60
% Platform:xWR68xx
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):15
% Range Resolution(m):0.044
% Maximum unambiguous Range(m):9.02
% Maximum Radial Velocity(m/s):1
% Radial velocity resolution(m/s):0.13
% Frame Duration(msec):100
% RF calibration data:None
% Range Detection Threshold (dB):15
% Doppler Detection Threshold (dB):15
% Range Peak Grouping:enabled
% Doppler Peak Grouping:enabled
% Static clutter removal:disabled
% Angle of Arrival FoV: Full FoV
% Range FoV: Full FoV
% Doppler FoV: Full FoV
% ***************************************************************
sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 5 0
adcCfg 2 1
adcbufCfg -1 0 1 1 1
profileCfg 0 60 567 7 57.14 0 0 70 1 256 5209 0 0 158
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 4
frameCfg 0 1 16 0 100 1 0
lowPower 0 0
guiMonitor -1 1 1 0 0 0 1
cfarCfg -1 0 2 8 4 3 0 15 1
cfarCfg -1 1 0 4 2 3 1 15 1
multiObjBeamForming -1 1 0.5
clutterRemoval -1 0
calibDcRangeSig -1 0 -5 8 256
extendedMaxVelocity -1 0
bpmCfg -1 0 0 1
lvdsStreamCfg -1 0 0 0
compRangeBiasAndRxChanPhase 0.0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
measureRangeBiasAndRxChanPhase 0 1.5 0.2
CQRxSatMonitor 0 3 5 121 0
CQSigImgMonitor 0 127 4
analogMonitor 0 0
aoaFovCfg -1 -90 90 -90 90
cfarFovCfg -1 0 0 8.92
cfarFovCfg -1 1 -1 1.00
calibData 0 0 0
sensorStart

Parser result: 0
totalBytesParsed: 76032
headerStartIndex = 0
totalPacketNumBytes = 928
platform = b'000a6843'
frameNumber = 180
timeCpuCycles = 2808824831
numDetObj = 14
numTlv = 5
subFrameNumber = 0
The 1st TLV
type 1
len 224 bytes
The 2nd TLV
type 7
len 56 bytes
x(m) y(m) z(m) v(m/s) Com0range(m) azimuth(deg) elevAngle(deg) snr(0.1dB) noise(0.1dB)
obj 0: -0.081761 0.030349 0.000000 0.000000 0.087212 -69.635868 0 180 948
obj 1: 0.062684 0.060636 0.000000 0.000000 0.087212 45.951372 0 180 948
obj 2: -0.637739 0.457572 0.000000 0.000000 0.784910 -54.340909 0 290 833
obj 3: 0.558703 1.698311 0.000000 0.000000 1.787851 18.209959 0 163 742
obj 4: -0.111741 1.784356 0.000000 0.000000 1.787851 -3.583322 0 163 742
obj 5: 0.034067 0.215353 0.000000 0.120726 0.218031 8.989300 0 172 877
obj 6: -0.040881 0.214164 0.000000 0.120726 0.218031 -10.806923 0 172 877
obj 7: 0.040881 0.258423 0.000000 0.241451 0.261637 8.989299 0 221 824
obj 8: -0.065409 0.253329 0.000000 0.241451 0.261637 -14.477512 0 221 824
obj 9: 0.000000 3.139640 0.000000 0.724354 3.139640 0.000000 0 201 543
obj 10: 0.320232 2.024315 0.000000 -0.362177 2.049488 8.989300 0 189 575
obj 11: 1.152837 1.694511 0.000000 -0.362177 2.049488 34.228866 0 189 575
obj 12: 0.040881 0.214164 0.000000 -0.241451 0.218031 10.806923 0 269 827
obj 13: -0.040881 0.214164 0.000000 -0.241451 0.218031 -10.806923 0 269 827

so with the SDK out of box demo i should be able to have x,y,z, v,range,azimuth,snr and noise. But how can I have access to these info  with the oob_parser.py ??

PS: 

I used a lot of print function in main_program(self). It was just to see what was exactly sent when using the oout of box demo with the oob_parser. 

""" print("\n length of data is ", len(self.parsedData))
print("Parsed data = ",self.parsedData)
for i in range(len(self.parsedData)):
    print(f"parsedData[{i}] = ", self.parsedData[i])    """

You can comment these lines when using the code for 3D people counting to have a sleeker ( :) ) run info window.





*** Also came across this thread that i found somehow usefull with Justin Curewitz  (maybe he was involved in the writing of oob_parser.py)


e2e.ti.com/.../3267011 

I would like to add that for the out of box demo, these are the oob_parser.py function used ( readandparseUard)

 sdk3xTLVHeader 
 tlvHeaderDecode

parseSDK3xPoints

parseSDK3xSideInfo 

AND ,self.pcBufPing is filled (Reminder readandparseUart sends  self.pcBufPing, self.targetBufPing, self.indexes, self.numDetectedObj, self.numDetectedTarget, self.frameNum, self.fail, self.classifierOutput,self.polar_returned) 

In parseSDK3xPoints :

self.pcBufPing[0, i], self.pcBufPing[1, i], self.pcBufPing[2, i], self.pcBufPing[3, i] = struct.unpack(
    pointStruct, dataIn[:pointLength])

In parseSDK3xSideInfo : 

self.pcBufPing[4, i], unused = struct.unpack(sideInfoStruct, dataIn[:sideInfoLength])
dataIn = dataIn[sideInfoLength:]


The code for SDK out of box in oob_parser doesn't have comments regarding the what each field of

self.pcBufPing is for. I just assumed that it was just like 3D people counting ["x","y","z","doppler","snr"].







Thank you for taking the time to check this out. 

Best regards, 

Hello,

• To make something very clear I am not going to read an entire files worth of code. That is beyond the scope of this forum to support. At most I will look at a snippet of code which has a very specific question attached to it.

• For most cases I won't run source code that is not officially released in the SDK or industrial toolbox. It is your responsibility to know how your modifications to working code change said code's behavior.

• The title of the thread hints that you want to characterize radar performance. You are using the People Counting lab, so this has the entire tracking software layer that is fed dynamic point cloud data from the radar device. What performance metric are you trying to characterize? If it is point cloud I would suggest using the OOB, but if you want to pull in tracking then you will need to understand the performance of the OOB, all the parameters associated with the tracker algorithm, how point cloud effects the tracker algorithm etc.

• We provide labs which demonstrate an example of the a radar application e.g. People Counting. These examples come user guides which allow to see the capabilities of the radar for that application. The resources that come with these labs e.g. source code, GUI source code, configuration files etc. are tied to these examples. You can't just blindly modify these resources and expect it to work as expected the projects are too complicated. Modifying these demos requires you to understand that lab through and through.

• Also if you want me to address the questions then they need to be framed in your response more clearly. They shouldn't be in the source code comments, but instead should be clear and concise in a thread response. I shouldn't have to hunt for your questions.

With those points highlighted lets have a reset on supporting this.

1. What performance metric are you trying to characterize?

2. What pieces of collateral are you planning to leverage to achieve this characterization?

3. What aspects of these collateral in their current state differ from what you need them to be?

Answer these questions and we will start from there.

Best regards,

Connor Desmond