Share/TI Radar
TI mmwaveISK 6843 with raspberry PI source code
Jimmy.B
2021. 10. 11. 14:41
It's for the connection between TI mmwave ISK 6843 interfaces and Raspberrypi4 (raspbian)
This code is based on github, https://github.com/ibaiGorordo/AWR1642-Read-Data-Python-MMWAVE-SDK-2
GitHub - ibaiGorordo/AWR1642-Read-Data-Python-MMWAVE-SDK-2: Python program to read and plot the data in real time from the AWR16
Python program to read and plot the data in real time from the AWR1642 and IWR1642 mmWave radar boards (Texas Instruments). - GitHub - ibaiGorordo/AWR1642-Read-Data-Python-MMWAVE-SDK-2: Python pro...
github.com
However, the obove is for only ISK 1682 interface message frame which is a little bit differ from the ISK6843.
So I changed the source code for 6843
Just let me share the source code first, and I would share the specific contents soon for understanding.
#try to connect to DB
#PointClound Completed 21.05.27
#Simple Logic then DB
import mysql.connector;
import struct
from datetime import datetime
import serial
import time
import numpy as np
import pyqtgraph as pg
from pyqtgraph.Qt import QtGui
import math
import threading
Maria = mysql.connector.connect(host="10.20.102.202", user="root", passwd="raspberry", database="sensor1");
Cursor = Maria.cursor(prepared=True)
# Change the configuration file name
# configFileName = 'mmw_pplcount_demo_default.cfg'
configFileName = 'ppCntCfg.cfg'
# configFileName = '1443config.cfg'
# configFileName = 'selfConfig1.cfg'
CLIport = {}
Dataport = {}
byteBuffer = np.zeros(2**15,dtype = 'uint8')
byteBufferLength = 0;
# ------------------------------------------------------------------
p_cnt = 0 ;
def printTest():
# now_time = '({})'.format(datetime.today().strftime('%Y-%m-%d %H:%M:%S'))
# print(now_time)
sql = "INSERT INTO data Values(%s,%s,%s)"
Cursor.execute(sql,(
'Check Time Saved ', time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()), 'chk')
)
Maria.commit();
# threading.Timer(10,printTest).start()
# Function to configure the serial ports and send the data from
# the configuration file to the radar
def serialConfig(configFileName):
global CLIport
global Dataport
# Open the serial ports for the configuration and the data ports
# Raspberry pi
CLIport = serial.Serial('/dev/ttyUSB0', 115200)
Dataport = serial.Serial('/dev/ttyUSB1', 921600)
# Windows
#CLIport = serial.Serial('COM3', 115200)
#Dataport = serial.Serial('COM4', 921600)
# Read the configuration file and send it to the board
config = [line.rstrip('\r\n') for line in open(configFileName)]
for i in config:
CLIport.write((i+'\n').encode())
print(i)
time.sleep(0.01)
return CLIport, Dataport
# ------------------------------------------------------------------
# Function to parse the data inside the configuration file
def parseConfigFile(configFileName):
configParameters = {} # Initialize an empty dictionary to store the configuration parameters
# Read the configuration file and send it to the board
config = [line.rstrip('\r\n') for line in open(configFileName)]
for i in config:
# Split the line
splitWords = i.split(" ")
# Hard code the number of antennas, change if other configuration is used
numRxAnt = 4
numTxAnt = 3 #jimmy revised
# Get the information about the profile configuration
if "profileCfg" in splitWords[0]:
startFreq = int(float(splitWords[2]))
idleTime = int(float(splitWords[3])) #j
rampEndTime = float(splitWords[5])
freqSlopeConst = float(splitWords[8])
numAdcSamples = int(splitWords[10])
digOutSampleRate = int(float(splitWords[11]))
numAdcSamplesRoundTo2 = 1;
while numAdcSamples > numAdcSamplesRoundTo2:
numAdcSamplesRoundTo2 = numAdcSamplesRoundTo2 * 2;
digOutSampleRate = int(float(splitWords[11]));
# Get the information about the frame configuration
elif "frameCfg" in splitWords[0]:
chirpStartIdx = int(float(splitWords[1]));#j
chirpEndIdx = int(splitWords[2]);
numLoops = int(float(splitWords[3])); #j
numFrames = int(splitWords[4]);
framePeriodicity = int(float(splitWords[5]));
# Combine the read data to obtain the configuration parameters
numChirpsPerFrame = (chirpEndIdx - chirpStartIdx + 1) * numLoops
configParameters["numDopplerBins"] = numChirpsPerFrame / numTxAnt
configParameters["numRangeBins"] = numAdcSamplesRoundTo2
configParameters["rangeResolutionMeters"] = (3e8 * digOutSampleRate * 1e3) / (2 * freqSlopeConst * 1e12 * numAdcSamples)
configParameters["rangeIdxToMeters"] = (3e8 * digOutSampleRate * 1e3) / (2 * freqSlopeConst * 1e12 * configParameters["numRangeBins"])
configParameters["dopplerResolutionMps"] = 3e8 / (2 * startFreq * 1e9 * (idleTime + rampEndTime) * 1e-6 * configParameters["numDopplerBins"] * numTxAnt)
configParameters["maxRange"] = (300 * 0.9 * digOutSampleRate)/(2 * freqSlopeConst * 1e3)
configParameters["maxVelocity"] = 3e8 / (4 * startFreq * 1e9 * (idleTime + rampEndTime) * 1e-6 * numTxAnt)
return configParameters
# ------------------------------------------------------------------
# Funtion to read and parse the incoming data
def readAndParseData16xx(Dataport, configParameters):
global byteBuffer, byteBufferLength
# Constants
OBJ_STRUCT_SIZE_BYTES = 12;
BYTE_VEC_ACC_MAX_SIZE = 2**15;
MMWDEMO_UART_MSG_POINT_CLOUD_3D = 6; # renamed 2D -> 3D
MMWDEMO_UART_MSG_TARGET_LIST_2D = 7;
MMWDEMO_UART_MSG_TARGET_INDEX_2D = 8;
maxBufferSize = 2**15;
tlvHeaderLengthInBytes = 8;
pointLengthInBytes = 8; # modified
targetLengthInBytes = 68;
magicWord = [2, 1, 4, 3, 6, 5, 8, 7]
# Initialize variables
magicOK = 0 # Checks if magic number has been read
dataOK = 0 # Checks if the data has been read correctly
targetDetected = 0 # Checks if a person has been detected
frameNumber = 0
targetObj = {}
pointObj = {}
readBuffer = Dataport.read(Dataport.in_waiting)
# print('just Read : {0}'.format(readBuffer))
byteVec = np.frombuffer(readBuffer, dtype = 'uint8')
byteCount = len(byteVec)
print('inital byteVec:{0}'.format(byteVec))
# Check that the buffer is not full, and then add the data to the buffer
if (byteBufferLength + byteCount) < maxBufferSize:
# print('check buffer, add data -> buffer ')
byteBuffer[byteBufferLength:byteBufferLength + byteCount] = byteVec[:byteCount] #understand?
# print(byteBufferLength)
# print(byteBufferLength + byteCount)
# print(byteBuffer[40])#104
# print('maybe vec and buff be samed{0}'.format(byteBuffer[0:byteCount]))
# print(len(byteBuffer))
# print(len(byteVec))
byteBufferLength = byteBufferLength + byteCount
# Check that the buffer has some data
if byteBufferLength > 16:
# print('already some data ')
# Check for all possible locations of the magic word
possibleLocs = np.where(byteBuffer == magicWord[0])[0]
# Confirm that is the beginning of the magic word and store the index in startIdx
startIdx = []
for loc in possibleLocs:
check = byteBuffer[loc:loc + 8]
if np.all(check == magicWord):
startIdx.append(loc)
# Check that startIdx is not empty
if startIdx:
# Remove the data before the first start index
if startIdx[0] > 0 and startIdx[0] < byteBufferLength:
byteBuffer[:byteBufferLength - startIdx[0]] = byteBuffer[startIdx[0]:byteBufferLength]
byteBuffer[byteBufferLength-startIdx[0]:] = np.zeros(len(byteBuffer[byteBufferLength-startIdx[0]:]),dtype = 'uint8')
byteBufferLength = byteBufferLength - startIdx[0]
# Check that there have no errors with the byte buffer length
if byteBufferLength < 0:
byteBufferLength = 0
# word array to convert 4 bytes to a 32 bit number
word = [1, 2 ** 8, 2 ** 16, 2 ** 24]
# Read the total packet length
# totalPacketLen = np.matmul(byteBuffer[20:20 + 4], word)
totalPacketLen = np.matmul(byteBuffer[12:12 + 4], word)
# Check that all the packet has been read
if (byteBufferLength >= totalPacketLen) and (byteBufferLength != 0):
# print("now magicOk")
magicOK = 1
# If magicOK is equal to 1 then process the message
if magicOK:
# word array to convert 4 bytes to a 32 bit number
word = [1, 2 ** 8, 2 ** 16, 2 ** 24]
# print("magic okay")
# Initialize the pointer index
idX = 0
# Read the header
# Read the header #why do this ??
print('current bytebuffer now will cook: {0}'.format(byteBuffer[startIdx[0]:byteBufferLength]))
magicNumber = byteBuffer[idX:idX + 8]
idX += 8
# print('m_n:{0}'.format(magicNumber))
# print('now idx:{0}'.format(idX))
# print('magicNumber:{0}'.format(magicNumber))
# print('np.matmul(byteBuffer[idX:idX + 4], word):{0}'.format(np.matmul(byteBuffer[idX:idX + 4], word)))
# print('b_b:{0}'.format(byteBuffer))
# print('what? header :{0}'.format(byteBuffer[0:13]))
version = format(np.matmul(byteBuffer[idX:idX + 4], word), 'x')
idX += 4
# print('pure_buf_version :{0}'.format(byteBuffer[idX:idX+4])) #4053
# print('version_re:{0}'.format(format(np.matmul(byteBuffer[8:12], word), 'x')))
# print('just_version : {0}'.format(np.matmul(bytebuffer[idX:idX+4], word)))
# print('version:{0}'.format(version)) #30 50 00 4
totalPacketLen = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('totalPacketLen:{0}'.format(totalPacketLen))
platform = format(np.matmul(byteBuffer[idX:idX + 4], word), 'x')
idX += 4
# print('platform:{0}'.format(platform))
frameNumber = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('frameNumber:{0}'.format(frameNumber))
subFrameNumber = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('subFrameNumber:{0}'.format(subFrameNumber))
chirpMargin = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('chirpMargin:{0}'.format(chirpMargin))
frameMargin = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('frameMargin:{0}'.format(frameMargin))
trackProcessTime = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('trackProcessTime:{0}'.format(trackProcessTime))
uartSentTime = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('uartSentTime:{0}'.format(uartSentTime))
word = [1, 2 ** 8] #maybe cus 2bytes matmul with numTLv below
numTLVs = np.matmul(byteBuffer[idX:idX + 2], word)
idX += 2
# print('numTLVs:{0}'.format(numTLVs))
checksum = np.matmul(byteBuffer[idX:idX + 2], word)
idX += 2
# print('checksum:{0}'.format(checksum))
# print('done read headData') #actually no! any! correlation with byteBuffer ....-_-;;;
# print('start byte buffer:{0}'.format(byteBuffer))
# Read the TLV messages
# print('for start in range {0}'.format(numTLVs))
for tlvIdx in range(numTLVs):
# word array to convert 4 bytes to a 32 bit number
word = [1, 2 ** 8, 2 ** 16, 2 ** 24]
# Initialize the tlv type
tlv_type = 0
# print("idx:{0}".format(idx))
try:
# Check the header of the TLV message
# print("must come here ")
# print('tlv_type_idx start:{0}'.format(idX))
tlv_type = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('13_tlv_type:{0}'.format(tlv_type))
tlv_length = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
# print('{0}'.format(tlv_length))
except:
pass
#only here
# Read the data depending on the TLV message
if tlv_type == MMWDEMO_UART_MSG_POINT_CLOUD_3D: # HERE HERE
# word array to convert 4 bytes to a 16 bit number
# print("TLV message : pointclound2D IN")
# print("pointCloudStart")
word = [1, 2 ** 8, 2 ** 16, 2 ** 24]
# Calculate the number of detected points
numInputPoints = (tlv_length - tlvHeaderLengthInBytes) // pointLengthInBytes #sth wrong from here
print('numOfInputPoint :{0}'.format(numInputPoints))
# rangeVal = np.zeros(numInputPoints, dtype=np.float32)
# azimuth = np.zeros(numInputPoints, dtype=np.float32)
# dopplerVal = np.zeros(numInputPoints, dtype=np.float32)
# snr = np.zeros(numInputPoints, dtype=np.float32)
elevation = np.zeros(numInputPoints, dtype=np.float32)
azimuth = np.zeros(numInputPoints, dtype=np.float32)
dopplerVal = np.zeros(numInputPoints, dtype=np.float32)
rangeVal = np.zeros(numInputPoints, dtype=np.float32)
snr = np.zeros(numInputPoints, dtype=np.float32)
# nonlocal idx
# print('should be initialized elevationUnit{0}azimuthUnit{1}dopplerUnit{2}dopplerUnit{3}snrUnit{4}'.format(elevationUnit,azimuthUnit,dopplerUnit,rangeUnit,snrUnit))
# print('before byteBuffer{0}'.format(byteBuffer))
# pUnitStruct = '5f'
# pUnitSize = struct.calcsize(pUnitStruct)
# pUnit = struct.unpack(pUnitStruct, byteVec[:pUnitSize]) #byteVec ??
#add by jimmy
el_val = np.zeros(numInputPoints, dtype=np.uint8)
az_val = np.zeros(numInputPoints, dtype=np.uint8)
dop_val = np.zeros(numInputPoints, dtype=np.uint16)
range_val = np.zeros(numInputPoints, dtype=np.uint16)
snr_val = np.zeros(numInputPoints, dtype=np.uint16)
#add by Q
ele_unit = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# print('ele_unit_buffer:{0}'.format(byteBuffer[idX:idX + 4]))
# print('ele_unit:{0}'.format(ele_unit))
idX += 4
azi_unit = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# print('azi_unit_buffer:{0}'.format(byteBuffer[idX:idX + 4]))
# print('azi_unit:{0}'.format(azi_unit))
idX += 4
dop_unit = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# print('dop_unit_buffer:{0}'.format(byteBuffer[idX:idX + 4]))
# print('dop_unit:{0}'.format(dop_unit))
idX += 4
rng_unit = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# print('rng_unit_buffer:{0}'.format(byteBuffer[idX:idX + 4]))
# print('rng_unit:{0}'.format(rng_unit))
idX += 4
snr_unit = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# print('snr_unit_buffer:{0}'.format(byteBuffer[idX:idX + 4]))
# print('snr_unit:{0}'.format(snr_unit))
idX += 4
for objectNum in range(numInputPoints):
elevation[objectNum] = byteBuffer[idX].view(dtype=np.int8)*ele_unit
# print('elevation[{0}] :{1}'.format(objectNum, elevation[objectNum]))
idX += 1
azimuth[objectNum] = byteBuffer[idX].view(dtype=np.int8)*azi_unit
# print('azimuth[{0}] :{1}'.format(objectNum, azimuth[objectNum]))
idX += 1
dopplerVal[objectNum] = byteBuffer[idX:idX+2].view(dtype=np.int16)*dop_unit
# print('dopplerVal[{0}] :{1}'.format(objectNum, dopplerVal[objectNum]))
idX += 2
rangeVal[objectNum] = byteBuffer[idX:idX+2].view(dtype=np.uint16)*rng_unit
# print('rangeVal[{0}] :{1}'.format(objectNum, rangeVal[objectNum]))
idX += 2
snr[objectNum] = byteBuffer[idX:idX+2].view(dtype=np.uint16)*snr_unit
# print('snr[{0}] :{1}'.format(objectNum, snr[objectNum]))
idX += 2
# #only Logic # Read the data for each object #is it right idx position ?
# # print("current idx : {0}".format(idx))
# # print("idX before elevation:{0}".format(idX))
# # print('zero_ele:{0}'.format(elevation))
# print('objNum:{0}'.format(objectNum))
# elevation[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# # elevation = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# # what = np.array([10,215,35,60]).view(dtype=np.float32)
# # test_ele = elevation
# print('ele_buffer :{0}'.format(byteBuffer[idX:idX+4]))
# # print('test_ele:{0}'.format(test_ele))
# # print('wtf:{0}'.format(what))
# # print('shape:{0}'.format(byteBuffer.shape))
# # print('type:{0}'.format(type(byteBuffer)))
# # if(np.array([10,215,35,60]) == byteBuffer[idX:idX+4]):
# # print('SAME!!!!')
# idX += 4
# # print(pUnit[0])
# print('elevation[objectNum]:{0}'.format(elevation[objectNum]))
# # print('elevation[objectNum]:{0}'.format(elevation))
# rangeVal[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# print('range_buffer :{0}'.format(byteBuffer[idX:idX+4]))
# idX += 4
# # print(pUnit[3])
# print('rangeVal[objectNum]:{0}'.format(rangeVal[objectNum] ))
# azimuth[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# idX += 4
# dopplerVal[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# idX += 4
# snr[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
# print('last_snr_buffer:{0}'.format(byteBuffer[idX:idX+4]))
# idX += 4
# #add by jimmy
# word=[1]
# el_val[objectNum] = np.matmul(byteBuffer[idX:idX + 1], word)
# print('new_el_buffer:{0}'.format(byteBuffer[idX:idX + 1]))
# print('new_el_val[objNum]:{0}'.format(el_val[objectNum]))
# idX += 1
# az_val[objectNum] = np.matmul(byteBuffer[idX:idX + 1], word)
# print('new_az_buffer:{0}'.format(byteBuffer[idX:idX + 1]))
# print('new_az_val[objNum]:{0}'.format(az_val[objectNum]))
# idX += 1
# word=[1, 2**8]
# dop_val[objectNum] = np.matmul(byteBuffer[idX:idX+2],word)
# print('new_dop_buffer:{0}'.format(byteBuffer[idX:idX + 2]))
# print('new_dop_val[objNum]:{0}'.format(dop_val[objectNum]))
# idX +=2
# range_val[objectNum] = np.matmul(byteBuffer[idX:idX+2],word)
# print('new_range_buffer:{0}'.format(byteBuffer[idX:idX + 2]))
# print('new_range_val[objNum]:{0}'.format(range_val[objectNum]))
# idX +=2
# snr_val[objectNum] = np.matmul(byteBuffer[idX:idX+2],word)
# print('new_snr_buffer:{0}'.format(byteBuffer[idX:idX + 2]))
# print('new_snr_val[objNum]:{0}'.format(snr_val[objectNum]))
# idX +=2
pointObj = {"numObj": numInputPoints, "range": rangeVal, "azimuth": azimuth,\
"doppler": dopplerVal,"elevation":elevation, "snr": snr}
# print('POINT ALL:{0}'.format(pointObj))
print('POINT_range:{0}'.format(pointObj["range"]))
print('POINT_azi:{0}'.format(pointObj["azimuth"]))
print('POINT_ele:{0}'.format(pointObj["elevation"]))
dataOK = 1
elif tlv_type == MMWDEMO_UART_MSG_TARGET_LIST_2D:
word = [1, 2 ** 8, 2 ** 16, 2 ** 24]
# Calculate the number of target points
numTargetPoints = (tlv_length - tlvHeaderLengthInBytes) // targetLengthInBytes
# Initialize the arrays
print('numTargetPoints:{0}'.format(numTargetPoints)) # fine
targetId = np.zeros(numTargetPoints, dtype=np.uint32)
posX = np.zeros(numTargetPoints, dtype=np.float32)
posY = np.zeros(numTargetPoints, dtype=np.float32)
velX = np.zeros(numTargetPoints, dtype=np.float32)
velY = np.zeros(numTargetPoints, dtype=np.float32)
accX = np.zeros(numTargetPoints, dtype=np.float32)
accY = np.zeros(numTargetPoints, dtype=np.float32)
EC = np.zeros((3, 3, numTargetPoints), dtype=np.float32) # Error covariance matrix
G = np.zeros(numTargetPoints, dtype=np.float32) # Gain
for objectNum in range(numTargetPoints):
# Read the data for each object
# print("idx:{0}".format(idX))
targetId[objectNum] = np.matmul(byteBuffer[idX:idX + 4], word)
idX += 4
posX[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
posY[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
velX[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
velY[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
accX[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
accY[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[0, 0, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[0, 1, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[0, 2, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[1, 0, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[1, 1, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[1, 2, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[2, 0, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[2, 1, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
EC[2, 2, objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
G[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
idX += 4
# Store the data in the detObj dictionary
targetObj = {"targetId": targetId, "posX": posX, "posY": posY, \
"velX": velX, "velY": velY, "accX": accX, "accY": accY, \
"EC": EC, "G": G, "numTargets":numTargetPoints}
print("TARGET_OBJ_posX:{0}".format(targetObj["posX"]))
print("TARGET_OBJ_posY:{0}".format(targetObj["posY"]))
targetDetected = 1
elif tlv_type == MMWDEMO_UART_MSG_TARGET_INDEX_2D:
# Calculate the length of the index message
# print("not here maybe2")
numIndices = tlv_length - tlvHeaderLengthInBytes
indices = byteBuffer[idX:idX + numIndices]
idX += numIndices
# Remove already processed data
if idX > 0:
shiftSize = totalPacketLen
byteBuffer[:byteBufferLength - shiftSize] = byteBuffer[shiftSize:byteBufferLength]
byteBuffer[byteBufferLength - shiftSize:] = np.zeros(len(byteBuffer[byteBufferLength - shiftSize:]),dtype = 'uint8')
byteBufferLength = byteBufferLength - shiftSize
# Check that there are no errors with the buffer length
if byteBufferLength < 0:
byteBufferLength = 0
return dataOK, targetDetected, frameNumber, targetObj, pointObj
# ------------------------------------------------------------------
# Funtion to update the data and display in the plot
def update():
dataOk = 0
targetDetected = 0
global targetObj
global pointObj
x = []
y = []
# Read and parse the received data
dataOk, targetDetected, frameNumber, targetObj, pointObj = readAndParseData16xx(Dataport, configParameters)
print('check_dataOk:{0}'.format(dataOk))
print('targetDetected:{0}'.format(targetDetected))
print('frameNumber:{0}'.format(frameNumber))
if p_cnt % 1000 == 0 :
printTest()
if targetDetected:
# print(targetObj)
# print(targetObj["numTargets"])
x = -targetObj["posX"]
y = targetObj["posY"]
print('tar_x:{0}'.format(x))
print('tar_y:{0}'.format(y))
target_cnt = 0;
# LOCATION logic start #HARD CODING
if(1.5<x.any() <2.2 and 2.4<y.any() <3.2): #This line has to be changed GUI User Interface Command
print('TARGET DETECTED THEN DB IN ')
x_d = x
y_d = y
# print('target_x:{0}'.format(x))
# s3.setData(x_d,y_d)
d = datetime.today().strftime('%Y-%m-%d %H:%M:%S')
sql = "INSERT INTO data Values(%s,%s,%s)"
Cursor.execute(sql,(
'_Radar_target', time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()), 'target')
)
Maria.commit();
time.sleep(0.5)
#####################################
# s2.setData(x,y, clear = True)
# QtGui.QApplication.processEvents()
# print('dataOk: {0}, targetDetected:{1}, frameNumber:{2}, targetObj:{3}, pointobj:{4}'.format(dataOK,targetDetected, frameNumber, targetObj, pointObj))
print(dataOk)
if dataOk:
x = -pointObj["range"]*np.sin(pointObj["azimuth"])
y = pointObj["range"]*np.cos(pointObj["azimuth"])
# point_range = pointObj["range"]
# point_azimuth = pointObj["azimuth"]
# print('FINAL_Range : {0}'.format(point_range))
# print('FINAL_Azimuth : {0}'.format(point_azimuth))
# x = -pointObj["rangeUnit"]*np.sin(pointObj["azimuthUnit"])
# y = pointObj["rangeUnit"]*np.cos(pointObj["azimuthUnit"])
print('FINAL_x:{0}'.format(x))
print('FINAL_y:{0}'.format(y))
#DB logic added
point_cnt = 0;
px_tmp=[];
for po in range(len(x)):
if(1.5 < x[po]< 2.3 and 2.4< y[po] < 3.2 ): #specific targeted area
# print("HHHHHHHHHHHHHHHHH")
point_cnt += 1
print('poin_cnt:{0}'.format(point_cnt))
# ave_tmp = np.mean(x[po])
# p_tmp.append(x[po])
# print(ave_tmp)
# px_tmp.append(ave_tmp)
# if(ave_tmp<)
# print(px_tmp)
if(point_cnt >3):
print("PEOPLE DETECDED ") #ok
# d = datetime.today().strftime('%Y-%m-%d %H:%M:%S')
sql = "INSERT INTO data Values(%s,%s,%s)"
Cursor.execute(sql,(
'People Detected ', time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()), 'mainDoor' )
)
Maria.commit();
print("DB saved")
# f = open('log.txt','w')
# # data = "%Y-%m-%d %H:%M:%S", time.localtime()
# f.write(time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()))
# f.close()
time.sleep(0.5)
# s1.setData(x,y, clear=True)
# QtGui.QApplication.processEvents()
print('dataok:{0}'.format(dataOk))
return dataOk
# ------------------------- MAIN -----------------------------------------
#try
#### class
# Configurate the serial port
CLIport, Dataport = serialConfig(configFileName)
# Get the configuration parameters from the configuration file
configParameters = parseConfigFile(configFileName)
# START QtAPPfor the plot
# app = QtGui.QApplication([])
# Set the plot
# pg.setConfigOption('background','w')
# win = pg.GraphicsWindow(title="2D scatter plot")
# p = win.addPlot()
# # p.setXRange(-0.5,0.5)
# p.setXRange(-3,3)
# p.setYRange(0,6)
# p.setLabel('left',text = 'Y position (m)')
# p.setLabel('bottom', text= 'X position (m)')
# s1 = p.plot([],[],pen=None,symbol='o')
# s1 = p.plot([],[],pen=None,symbol='o')
# s2 = p.plot([],[],pen=(0,0,255),symbol='star')
# s3 = p.plot([],[],pen=(0,0,255),symbol='x')
# Main loop
targetObj = {}
pointObj = {}
frameData = {}
currentIndex = 0
while True:
p_cnt+=1
print(p_cnt)
try:
# Update the data and check if the data is okay
dataOk = update()
if(p_cnt%50000 == 0):
printTest()
if dataOk:
# Store the current frame into frameData
# print('print_takget : {0}'.format(targetObj))
frameData[currentIndex] = targetObj
currentIndex += 1
time.sleep(0.033) # Sampling frequency of 30 Hz
# Stop the program and close everything if Ctrl + c is pressed
except KeyboardInterrupt:
CLIport.write(('sensorStop\n').encode())
CLIport.close()
Dataport.close()
win.close()
break