97 lines
3 KiB
Python
97 lines
3 KiB
Python
import serial
|
|
import time
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
from parser_mmw_demo import parser_one_mmw_demo_output_packet
|
|
|
|
# Turning interactive mode on for plotting
|
|
plt.ion()
|
|
fig = plt.figure()
|
|
|
|
# Change the configuration file name
|
|
configFileName = 'hedgehog.cfg'
|
|
|
|
# ------------------------------------------------------------------
|
|
|
|
# Function to configure the serial ports and send the data from
|
|
# the configuration file to the radar
|
|
def serialConfig(configFileName):
|
|
|
|
# Serial device ports
|
|
CLIport = serial.Serial('/dev/ttyACM0', 115200)
|
|
Dataport = serial.Serial('/dev/ttyACM1', 921600)
|
|
|
|
# Read the configuration file and send it to the board
|
|
with open(configFileName, mode="rt", encoding="utf-8") as config_file:
|
|
for line in config_file:
|
|
line = line.rstrip("\r\n")
|
|
print(f"[CONFIG] writing: '{line}'")
|
|
|
|
CLIport.write((line + "\n").encode())
|
|
time.sleep(0.01)
|
|
|
|
return CLIport, Dataport
|
|
|
|
# ------------------------- MAIN -----------------------------------------
|
|
|
|
# Configurate the serial port
|
|
CLIport, Dataport = serialConfig(configFileName) # Enable when reading from sensor
|
|
# Dataport = open("sensor_data.bin", mode="rb") # Enable when reading from file
|
|
|
|
# parser_one_mmw_demo_output_packet extracts only one complete frame at a time
|
|
# so call this in a loop till end of file
|
|
|
|
def parse_frames():
|
|
|
|
ax = fig.add_subplot(projection='3d')
|
|
plt.show()
|
|
|
|
sensor_data = bytearray()
|
|
bytes_parsed = 0
|
|
while True:
|
|
read_data = Dataport.read(256)
|
|
with open("sensor_data.bin", mode="ab") as fp:
|
|
fp.write(read_data)
|
|
|
|
sensor_data += bytearray(read_data)
|
|
|
|
# parser_one_mmw_demo_output_packet function already prints the
|
|
# parsed data to stdio. So showcasing only saving the data to arrays
|
|
# here for further custom processing
|
|
parser_result, \
|
|
headerStartIndex, \
|
|
totalPacketNumBytes, \
|
|
numDetObj, \
|
|
numTlv, \
|
|
subFrameNumber, \
|
|
detectedX_array, \
|
|
detectedY_array, \
|
|
detectedZ_array, \
|
|
detectedV_array, \
|
|
detectedRange_array, \
|
|
detectedAzimuth_array, \
|
|
detectedElevation_array, \
|
|
detectedSNR_array, \
|
|
detectedNoise_array = parser_one_mmw_demo_output_packet(sensor_data[bytes_parsed::1], len(sensor_data) - bytes_parsed)
|
|
|
|
ax.clear()
|
|
ax.scatter(detectedX_array, detectedY_array, detectedZ_array)
|
|
plt.pause(0.1)
|
|
|
|
# Check the parser result
|
|
print ("Parser result: ", parser_result)
|
|
if (parser_result == 0):
|
|
bytes_parsed += (headerStartIndex+totalPacketNumBytes)
|
|
print("totalBytesParsed: ", bytes_parsed)
|
|
else:
|
|
bytes_parsed += 1
|
|
continue
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
parse_frames()
|
|
except KeyboardInterrupt:
|
|
CLIport.write("sensorStop\n".encode()) # Enable when reading from sensor
|
|
# Dataport.close() # Enable when reading from file
|
|
exit(0)
|