前言

常规的Out Box of Demo的食用方法TI官方给的已经很多了总结下来就是:

电脑装emupack(设备和电脑通过USB通信的驱动程序)
Uniflash给设备烧录demo的预编译文件
使用demo_visualizer与设备通过UART通信并读取数据, 可视化数据

但是这样使用官方软件的方法并不适合我们做更进一步的处理, 比如目标识别, 跟踪等等信息处理. 所以我们要做的就是写一个脚本来替代demo_visualizer. 那为了完成这样一件事儿我们需要知道demo_visualizer的工作机制, 具体通信的方式, 数据格式等等细节.

在这篇博客中会有许多概念我们默认为读者已经提前了解了, 比如通用异步收发传输器(Universal Asynchronous Receiver/Transmitter, UART), 基本的LFMCW雷达原理等. 如果有不清楚的概念搜索引擎搜索就可以得到需要的信息.

在这篇博客中我们将讲首先介绍out of box demo的数据传输方式以及格式, 之后会分别使用matlab和python给出一个例程, 这个例程可以读取雷达的数据还可以对数据进行可视化.

数据传输方式以及格式

数据传输方式

xWR设备使用UART与上位机通信,熟悉嵌入式的读者应该清楚从xWR SoC上的UART电平信号想通过USB传输一定需要一个转换电路将其转换为USB的差分信号, 同时还要能告诉USB host传输信号的源数据类型为UART. 那么这件工作是由谁来完成的呢, 对于xWR1642boost和xWR1843boost 是通过评估板上的一块XDS芯片完成的, 而对于xWR6843AoP, xWR6843ISK-ODS和xWR6843ISK 则是通过一块名为CP2015的UART bridge芯片完成的. 当你安装emupack这个驱动程序时, 其实你就安装了XDS的芯片驱动程序. 那CP2015的驱动程序呢? 由于CP2015是一块”烂大街”的芯片(使用极其广泛), 大多数Windows发行版在安装时就已经包含了这个驱动程序, 如果你的电脑不幸miss了CP2015的驱动程序, 通过网络你也可以很轻易的得到.

当你通过micro-USB的数据线将xWR评估板连接到你的电脑上时, 你会在设备管理器中看到检测到新插入的两个串行接口, 后面还会显示相应的COM标号:

xWR1642boost和xWR1843boost会显示为
xWR6843AoP, xWR6843ISK-ODS和xWR6843ISK会显示为

他们分别时CLI port和 Data port, CLI port传输你对demo程序的配置, 比如LFMCW雷达波形基础配置, 检测算法阈值的配置等等, 同时demo程序也会积极的与你交互, 告知你的配置格式数据是否合规. Data port传输经过雷达数据处理得到的检测点云数据, 芯片工作负载信息等等. 正是由于这两个串口不同的工作任务, 他们对信息传输的速度要求不同, CLI port工作在115200的波特率而Data port工作在926000的波特率. 当然这两个串口也是工作在全双工模式下的.

CLI port数据格式

CLI port是传输你对Demo程序的配置的端口, 配置信息的格式及相应的含义可以在

1	/ti/mmwave_sdk_<ver>/docs/mmwave_sdk_user_guide.pdf

中的

1	3.4 Configuration (.cfg) File Format

中找到

Data port数据格式

Data port的数据格式可以在

1	> \ti\mmwave_sdk_<ver>\packages\ti\demo\xwr68xx\mmw\docs

中找到,如下图所示

Data port数据输出格式是一个TLV格式的协议, 即就是每一个大的数据块(TLV)由 Tag, Length 和 Value 三个部分组成. 并且在 Data port 一次输出数据的过程中, 可能输出多个TLV, 这样在总的数据块头部, 需要一个总的Header数据段以告知该数据包TLV的数目以及这个Packet的长度(in bytes). 那么每个TLV传输的是什么数据呢? 比如检测的点云数据, 距离多普勒数据, 距离幅度的一维数据等等. 熟悉C语言的读者可以发现, 这样的多段线性的存储结构可以使用结构体来实现, 这也是TI 的官方Demo程序的做法. Packet 的header结构体叫 MmwDemo_output_message_header_t

typedef struct MmwDemo_output_message_header_t
{
	uint16_t    magicWord[4];
	uint32_t     version;
	uint32_t    totalPacketLen;
	uint32_t    platform;
	uint32_t    frameNumber;
	uint32_t    timeCpuCycles;
	uint32_t    numDetectedObj;
	uint32_t    numTLVs;
#ifdef SOC_XWR16XX
	uint32_t    subFrameNumber;
#endif
} MmwDemo_output_message_header;

每个结构体成员变量的含义可以在以下html文档中查看

1	/ti/mmwave_sdk_02_01_00_04/packages/ti/demo/xwr16xx/mmw/docs/doxygen/html/struct_mmw_demo__output__message__header__t.html

仅仅通过变量的名称我们可以发现, Packet的header包含了SDK的版本, Packet的长度, mmWave Device的名称, Packet的编号, 检测点的个数以及TLV的个数. 那么之后就是要确定每个TLV的格式了, Demo 中给出了几种功能的TLV

List of detected objects
Range profile
Noise floor profile
Azimuth static heatmap
Range/Doppler heatmap
Stats information

每种TLV的功能可以参见如下文档的 Output information sent to host 部分

1	/ti/mmwave_sdk_02_01_00_04/packages/ti/demo/xwr16xx/mmw/docs/doxygen/html/index.html

在这里着重要说明的是检测点云(List of detected objects)的输出格式, 也是我们需要重点需要从 Demo 中获得的数据

List of detected objects TLV 的数据结构如图

Tag和Length的结构体定义如下

typedef struct MmwDemo_output_message_tl_t
{
	uint32_t    type;
	uint32_t    length;
} MmwDemo_output_message_tl;

而List of detected objects TLV的第二个结构体包含了检测到的点的个数与坐标存储的格式(Q-format, 即定点数)

typedef struct MmwDemo_output_message_dataObjDescr_t
{
	uint16_t     numDetetedObj;
	uint16_t     xyzQFormat;
} MmwDemo_output_message_dataObjDescr;

在MmwDemo_output_message_dataObjDescr之后紧跟着numDetetedObj个检测的点云数据

typedef volatile struct MmwDemo_detectedObj_t
{
	uint16_t  rangeIdx;     
	int16_t   dopplerIdx;   
	uint16_t  peakVal;      
	int16_t   x;             
	int16_t   y;             
	int16_t   z;             
} MmwDemo_detectedObj;

相关代码(matlab)

打开传输串口

function [CLI_sphandle,DATA_sphandle] = setupUART(CLIcomPortString,DATAcomPortString)
    
    if strcmp(CLIcomPortString(1:3),'COM') || strcmp(DATAcomPortString(1:3),'COM')

        CLI_sphandle = serial(CLIcomPortString,'BaudRate',115200);
        set(CLI_sphandle,'Parity','none')
        set(CLI_sphandle,'Terminator','LF')
        fopen(CLI_sphandle);

        DATA_sphandle = serial(DATAcomPortString,'BaudRate',921600);
        set(DATA_sphandle,'Terminator', '');
        set(DATA_sphandle,'InputBufferSize', 65536);
        set(DATA_sphandle,'Timeout',10);
        set(DATA_sphandle,'ErrorFcn',@dispError);
        set(DATA_sphandle,'BytesAvailableFcnMode','byte');
        set(DATA_sphandle,'BytesAvailableFcnCount', 2^16+1);%BYTES_AVAILABLE_FCN_CNT);
        set(DATA_sphandle,'BytesAvailableFcn',@readUartCallbackFcn);
        fopen(DATA_sphandle);
    else
        fprintf('COM port string format wrong\n');
    end
end

CLI port配置数据

% Carrier frequency     GHz                          60.25
% Ramp Slope    MHz/us                               156
% Num ADC Samples                                    256
% ADC Sampling Rate Msps                             12.5
% ADC Collection Time   us                           20.48
% Extra ramp time required (start time) us           3
% Chirp time (end time - start time)    us           21
% Chirp duration (end time) us                       24
% Sweep BW (useful) MHz                              3194.88
% Total BW  MHz                                      3744
% Max beat freq (80% of ADC sampling rate)  MHz      10
% Max distance (80%)    m                            9.62
% Range resolution  m                                0.047
% Range resolution (meter per 1D-FFT bin)   m/bin    0.047
%                                                    
% Inter-chirp duration  us                           7
% Number of chirp intervals in frame    -            96
% Number of TX (TDM MIMO)                            3
% Number of Tx elevation antennas                    0
% Number of RX channels -                            4
% Max umambiguous relative velocity kmph             48.19
%   mileph                                           30.12
% Max extended relative velocity    kmph             144.56
%   mileph                                           90.35
% Frame time (total)    ms                           2.976
% Frame time (active)   ms                           2.304
% Range FFT size    -                                256
% Doppler FFT size  -                                32
% Radar data memory required    KB                   400
% Velocity resolution   m/s                          0.84
% Velocity resolution (m/s per 2D-FFT bin)  m/s/bin  0.84
% Velocity Maximum  m/s                              13.39
% Extended Maximum Velocity m/s                      40.16
% Maximum sweep accorss range bins  range bin        0.85
% 
sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 7 0
adcCfg 2 1
adcbufCfg -1 0 1 1 1
lowPower 0 0
profileCfg 0 60.25 7 3 24 0 0 156 1 256 12500 0 0 30
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 4
chirpCfg 2 2 0 0 0 0 0 2
frameCfg 0 2 32 0 100 1 0
guiMonitor -1 2 0 0 0 0 0
cfarCfg -1 0 2 8 4 3 0 11.0 0
cfarCfg -1 1 0 4 2 3 1 11.0 0
multiObjBeamForming -1 1 0.5
calibDcRangeSig -1 0 -5 8 256
clutterRemoval -1 1

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. 0.2

aoaFovCfg -1 -90 90 -90 90
cfarFovCfg -1 0 0.1 5
cfarFovCfg -1 1 -13.39 13.39
extendedMaxVelocity -1 0

CQRxSatMonitor 0 3 4 63 0
CQSigImgMonitor 0 127 4
analogMonitor 0 0
lvdsStreamCfg -1 0 0 0
bpmCfg -1 0 0 0
sensorStart

读取CLI port配置数据

function [configcmd] = readCfgFile(configFile)

configcmd = cell(1,100);
fid = fopen(configFile, 'r');
if fid == -1
    fprintf('File %s not found!\n', configFile);
    return;
else
    fprintf('Opening configuration file %s ...\n', configFile);
end
tline = fgetl(fid);
k=1;
while ischar(tline)
    if ~isempty(tline)
        configcmd{k} = tline;
        k = k + 1;
    end 
    tline = fgetl(fid);
end
configcmd = configcmd(1:k-1);
fclose(fid);
end

发送CLI port的配置数据

function sendCfg(UART_sphandle,configcmd)
mmwDemoCliPrompt = char('mmwDemo:/>');

%Send CLI configuration to IWR
fprintf('Sending configuration to IWR radar\n');
for k=1:length(configcmd)
    command = configcmd{k};
    fprintf(UART_sphandle, command);
    fprintf('%s\n', command);
    echo = fgetl(UART_sphandle); % Get an echo of a command
    done = fgetl(UART_sphandle); % Get "Done"
    fprintf('%s\n', done);
    prompt = fread(UART_sphandle, size(mmwDemoCliPrompt,2)); % Get the prompt back
    fprintf('%s\n', prompt);
end
end

读取Data port数据

function [dataOk, frameNumber, detObj] = readAndParseData(DATA_sphandle)
    OBJ_STRUCT_SIZE_BYTES = 16;
    BYTE_VEC_ACC_MAX_SIZE = 2^16;
    MMWDEMO_UART_MSG_DETECTED_POINTS = 1;
    MMWDEMO_UART_MSG_RANGE_PROFILE   = 2;
    MMWDEMO_UART_MSG_DETECTED_POINTS_SIDE_INFO  = 7;
    maxBufferSize = BYTE_VEC_ACC_MAX_SIZE;
    
    detObj = [];
    frameNumber = 0;
      
    persistent byteBuffer
    if isempty(byteBuffer)
        byteBuffer = zeros(maxBufferSize,1);
    end
    
    persistent byteBufferLength
    if isempty(byteBufferLength)
        byteBufferLength = 0;
    end
    
    persistent magiNotOkCounter
    if isempty(magiNotOkCounter)
        magiNotOkCounter = 0;
    end
    
    magicOk = 0;
    dataOk = 0;
    
    bytesToRead = get(DATA_sphandle,'BytesAvailable');
    if (bytesToRead ~= 0)
        % Read the Data Serial Port
        [bytevec, byteCount] = fread(DATA_sphandle, bytesToRead, 'uint8');
        
         % Check if the buffer is not full, and then add the data to the buffer:
        if(byteBufferLength + byteCount < maxBufferSize)
            byteBuffer(byteBufferLength+1:byteBufferLength + byteCount) = bytevec(1:byteCount);
            byteBufferLength = byteBufferLength + byteCount;
        end
        
    end
 
    % Check that the buffer is not empty:
    if byteBufferLength > 16
        byteBufferStr = char(byteBuffer);
        
        % Search for the magic number inside the buffer and check that at least one magic number has been found:
        startIdx = strfind(byteBufferStr', char([2 1 4 3 6 5 8 7]));
        if ~isempty(startIdx)
            
            % Check the position of the first magic number and put it at
            % the beginning of the buffer
            if length(startIdx) >= 2
                if startIdx(end-1) > 1
                    byteBuffer(1:byteBufferLength-(startIdx(1)-1)) = byteBuffer(startIdx(1):byteBufferLength);
                    byteBufferLength = byteBufferLength - (startIdx(1)-1);
                end
            else
                if startIdx(1) > 1
                    byteBuffer(1:byteBufferLength-(startIdx(1)-1)) = byteBuffer(startIdx(1):byteBufferLength);
                    byteBufferLength = byteBufferLength - (startIdx(1)-1);
                end
            end
            if byteBufferLength < 0
                byteBufferLength = 0;
            end
            
            totalPacketLen = sum(byteBuffer(8+4+[1:4]) .* [1 256 65536 16777216]');
            if ((byteBufferLength >= totalPacketLen) && (byteBufferLength ~= 0)) 
                magicOk = 1;
            else
                magicOk = 0;
            end
        end
    end
    
    if (magicOk == 1)
        %%%%% HEADER
        word = [1 256 65536 16777216]';
        idx = 0;
        magicNumber = byteBuffer(idx + 1:8);
        idx = idx + 8;
        Header.version = dec2hex(sum(byteBuffer(idx+[1:4]) .* word));
        idx = idx + 4;
        Header.totalPacketLen = typecast(uint8(byteBuffer(idx+[1:4])),'uint32');
        idx = idx + 4;
        Header.platform = dec2hex(sum(byteBuffer(idx+[1:4]) .* word));
        idx = idx + 4;
        Header.frameNumber = typecast(uint8(byteBuffer(idx+[1:4])),'uint32');
        frameNumber = Header.frameNumber;
        idx = idx + 4;
        Header.timeCpuCycles = typecast(uint8(byteBuffer(idx+[1:4])),'uint32');
        idx = idx + 4;
        Header.numDetectedObj = typecast(uint8(byteBuffer(idx+[1:4])),'uint32');
        idx = idx + 4;
        Header.numTLVs = typecast(uint8(byteBuffer(idx+[1:4])),'uint32');
        idx = idx + 4;
        Header.subFrameNumber = typecast(uint8(byteBuffer(idx+[1:4])),'uint32');
        idx = idx + 4;
        
        
        %%%%% TLV
        
        % Analyze each of TLV messages:
        for tlvIdx = 1:Header.numTLVs
            word = [1 256 65536 16777216]';
            % First, analyze the TLV header (TLV type and length):
            tlv.type = sum(byteBuffer(idx+uint32(1:4)) .* word);
            idx = idx + 4;
            tlv.length = sum(byteBuffer(idx+uint32(1:4)) .* word);
            idx = idx + 4;
            
            % Check that the TLV message is of the right type (Detected objects):
            switch tlv.type
                case MMWDEMO_UART_MSG_DETECTED_POINTS
                    detObj =[];
                    
                    if tlv.length > 0                       
                        % Extract the raw data for all the detected points
                        bytes = byteBuffer(idx+(1:Header.numDetectedObj*OBJ_STRUCT_SIZE_BYTES));
                        idx = idx + Header.numDetectedObj*OBJ_STRUCT_SIZE_BYTES;
                        
                        % Reshape the array to have the data for each point
                        % (X,Y,Z,doppler) in each column
                        bytes = reshape(bytes, OBJ_STRUCT_SIZE_BYTES, Header.numDetectedObj);
                        
                        % Convert the byte matrix to float data
                        floatData = reshape(typecast(reshape(uint8(bytes), 1, []), 'single'),4,Header.numDetectedObj);
                      
                        detObj.numObj = Header.numDetectedObj;
                        detObj.x = floatData(1,:);
                        detObj.y = floatData(2,:);
                        detObj.z = floatData(3,:);
                        detObj.doppler = floatData(4,:);
                        
                        dataOk = 1;
                        
                    end
                case MMWDEMO_UART_MSG_DETECTED_POINTS_SIDE_INFO
                    
                    if tlv.length > 0   
                        bytes = byteBuffer(idx+(1:Header.numDetectedObj*4));
                        idx = idx + Header.numDetectedObj*4;
                        
                        % Reshape the array to have the data for each point
                        % (snr,noise) in each column
                        bytes = reshape(bytes, 4, Header.numDetectedObj);
                        
                        % Convert the byte matrix to float data
                        floatData = reshape(typecast(reshape(uint8(bytes), 1, []), 'int16'),2,Header.numDetectedObj);
                        detObj.snr = floatData(1,:);
                        detObj.noise = floatData(2,:);
                        
                        dataOk = 1;
                    
                    end
                case MMWDEMO_UART_MSG_RANGE_PROFILE
                    rp = byteBuffer(idx+uint32(1:tlv.length));
                    idx = idx + tlv.length;
                    rp=rp(1:2:end)+rp(2:2:end)*256;
            end
            
        end
        %Remove processed data
        if idx > 0
            shiftSize = Header.totalPacketLen;
            byteBuffer(1: byteBufferLength-shiftSize) = byteBuffer(shiftSize+1:byteBufferLength);
            byteBufferLength = byteBufferLength - shiftSize;
            if byteBufferLength < 0
                byteBufferLength = 0;
            end
        end
        
    else
        magiNotOkCounter = magiNotOkCounter + 1;
    end

示例程序

clear all; close all
delete(instrfind);

N = 10000;
limit = 5;

% Setup radar with the parameters from the configuration file
configFile = "profile_3d.cfg";
configcmd = readCfgFile(configFile);
[CLI_sphandle,DATA_sphandle] = setupUART('COM9','COM10');
sendCfg(CLI_sphandle,configcmd)

%% Initialize the figure
figure

H1 = uicontrol('Style', 'PushButton', ...
                    'String', 'Shutdown', ...
                    'Callback', 'shutVal = 1','InnerPosition',[7,30,60,20]);
H2 = uicontrol('Style', 'PushButton', ...
                    'String', 'Stop', ...
                    'Callback', 'stopVal = 1','InnerPosition',[7,60,60,20]);
H3 = uicontrol('Style', 'PushButton', ...
                    'String', 'Start', ...
                    'Callback', 'startVal = 1','InnerPosition',[7,90,60,20]);                
h = scatter3([],[],[],'filled');
axis([-limit,limit,0,limit,-limit,limit]);
xlabel('X (m)'); ylabel('Y (m)');

%% main loop

myInd = 1;
lastreadTime = 0;
dataOk = 0;
stopVal = 0;
startVal = 0;
shutVal = 0;
mask = 0;

tic
while (myInd <= N)
    dataOk = 0;
    
    % Read the data from the radar:
    if mask ==1
        pause(0.1);
    else
        [dataOk, frameNumber, detObj] = readAndParseData(DATA_sphandle);
    end
    
    if stopVal == 1
        stopVal = 0;
        mask = 1;
        fprintf(CLI_sphandle, 'sensorStop\n');
    end
    
    if startVal == 1
        startVal = 0;
        mask = 0;
        fprintf(CLI_sphandle, 'sensorStart 0\n');
    end
    
    if shutVal == 1
        shutVal = 0;
        fprintf(CLI_sphandle, 'sensorStop\n');
        break;
    end
    
    if dataOk == 1
        
        % Store all the data from the radar
        frame{myInd} = detObj;
        
        %% do process here
        
        %% above
                
        % Plot the radar points
        h.XData = detObj.x;
        h.YData = detObj.y;
        h.ZData = detObj.z;
        drawnow limitrate;
        pause(0.05);
               
        myInd = myInd + 1;     
    end
end

delete(instrfind);
close all

相关代码(Python)

依赖

在Python的代码中需要安装如下几个依赖:

pyserial: 用于通过串口与设备进行通信
numpy: 进行矩阵的操作和部分数学处理
Pyqt, Pyqtgraph, OpenGL: 对数据进行可视化操作并显示
sklearn: 聚类等操作

pandas: 数据管理

代码

在这个代码中其实进行了DBscan聚类的处理, 感兴趣的读者可以多多尝试其他的信息处理方法

#
# File:         读取IWR6843处理得到的点云数据（3维）进行目标跟踪，同时调用openGL对点云进行显示
# Author:       UESTC Yu Xuyao
# Email:        yxy19991102@163.com
#

import serial
import time
import numpy as np
import pyqtgraph as pg
from pyqtgraph.Qt import QtGui
import pyqtgraph.opengl as gl
# from IWR.MOT3D import *
import numpy
from sklearn.cluster import dbscan
import pandas

# Change the configuration file named
configFileName = 'profile_iwr6843_ods_3d.cfg'
# configFileName = 'profile_3d.cfg'

CLIport = {}
Dataport = {}
byteBuffer = np.zeros(2 ** 15, dtype='uint8')
byteBufferLength = 0

testroot = []
confirmedroot = []
Z_k_prev = numpy.mat([])

cnt = 1
show = []
changdu = 5


# ------------------------------------------------------------------
# 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/ttyACM0', 115200)
    # Dataport = serial.Serial('/dev/ttyACM1', 921600)

    # Windows
    CLIport = serial.Serial('COM3', 115200)
    Dataport = serial.Serial('COM4', 921600)
    # CLIport = serial.Serial('COM22', 115200)
    # Dataport = serial.Serial('COM21', 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:
        print(i)
        if len(i)>0:
            if i[0] != '%':
                CLIport.write((i + '\n').encode())
                time.sleep(0.1)
                reply = CLIport.read(CLIport.in_waiting).decode()
                print(reply)

    return CLIport, Dataport


# ------------------------------------------------------------------

# Function to parse the data inside2 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

        # Get the information about the profile configuration
        if "profileCfg" in splitWords[0]:
            startFreq = int(float(splitWords[2]))
            idleTime = int(splitWords[3])
            rampEndTime = float(splitWords[5])
            freqSlopeConst = float(splitWords[8])
            numAdcSamples = int(splitWords[10])
            numAdcSamplesRoundTo2 = 1

            while numAdcSamples > numAdcSamplesRoundTo2:
                numAdcSamplesRoundTo2 = numAdcSamplesRoundTo2 * 2

            digOutSampleRate = int(splitWords[11])

        # Get the information about the frame configuration
        elif "frameCfg" in splitWords[0]:
            chirpStartIdx = int(splitWords[1])
            chirpEndIdx = int(splitWords[2])
            numLoops = int(splitWords[3])
            numFrames = int(splitWords[4])
            framePeriodicity = 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 readAndParseData68xx(Dataport, configParameters):
    global byteBuffer, byteBufferLength

    # Constants
    OBJ_STRUCT_SIZE_BYTES = 12
    BYTE_VEC_ACC_MAX_SIZE = 2 ** 15
    MMWDEMO_UART_MSG_DETECTED_POINTS = 1
    MMWDEMO_UART_MSG_RANGE_PROFILE = 2
    maxBufferSize = 2 ** 15
    tlvHeaderLengthInBytes = 8
    pointLengthInBytes = 16
    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
    frameNumber = 0
    detObj = {}

    readBuffer = Dataport.read(Dataport.in_waiting)
    byteVec = np.frombuffer(readBuffer, dtype='uint8')
    byteCount = len(byteVec)

    # Check that the buffer is not full, and then add the data to the buffer
    if (byteBufferLength + byteCount) < maxBufferSize:
        byteBuffer[byteBufferLength:(byteBufferLength + byteCount)] = byteVec[0:byteCount]
        byteBufferLength = byteBufferLength + byteCount

    # Check that the buffer has some data
    if byteBufferLength > 16:

        # 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 0 < 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

            # Read the total packet length
            totalPacketLen = int.from_bytes(byteBuffer[12:12 + 4], byteorder='little')

            # Check that all the packet has been read
            if (byteBufferLength >= totalPacketLen) and (byteBufferLength != 0):
                magicOK = 1

    # If magicOK is equal to 1 then process the message
    if magicOK:

        # Initialize the pointer index
        idX = 0

        # Read the header
        magicNumber = byteBuffer[idX:idX + 8]
        idX += 8
        version = format(int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little'), 'x')
        idX += 4
        totalPacketLen = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
        idX += 4
        platform = format(int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little'), 'x')
        idX += 4
        frameNumber = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
        idX += 4
        timeCpuCycles = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
        idX += 4
        numDetectedObj = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
        idX += 4
        numTLVs = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
        idX += 4
        subFrameNumber = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
        idX += 4

        # Read the TLV messages
        for tlvIdx in range(numTLVs):

            # Check the header of the TLV message
            tlv_type = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
            idX += 4
            tlv_length = int.from_bytes(byteBuffer[idX:idX + 4], byteorder='little')
            idX += 4

            # Read the data depending on the TLV message
            if tlv_type == MMWDEMO_UART_MSG_DETECTED_POINTS:

                # Initialize the arrays
                x = np.zeros(numDetectedObj, dtype=np.float32)
                y = np.zeros(numDetectedObj, dtype=np.float32)
                z = np.zeros(numDetectedObj, dtype=np.float32)
                velocity = np.zeros(numDetectedObj, dtype=np.float32)

                for objectNum in range(numDetectedObj):
                    # Read the data for each object
                    x[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
                    idX += 4
                    y[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
                    idX += 4
                    z[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
                    idX += 4
                    velocity[objectNum] = byteBuffer[idX:idX + 4].view(dtype=np.float32)
                    idX += 4

                # Store the data in the detObj dictionary
                detObj = {"numObj": numDetectedObj, "x": x, "y": y, "z": z, "velocity": velocity}
                dataOK = 1

        # Remove already processed data
        if 0 < idX < byteBufferLength:
            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, frameNumber, detObj


# ------------------------------------------------------------------

# Funtion to update the data and display in the plot
def update():
    dataOk = 0
    global detObj, confirmedroot, testroot, Z_k_prev, GLScatterPlot
    global show, cnt
    x = []
    y = []
    z = []
    mask = 1

    # Read and parse the received data
    dataOk, frameNumber, detObj = readAndParseData68xx(Dataport, configParameters)

    if dataOk:
        x = detObj["x"]
        y = detObj["y"]
        z = detObj["z"]

        if len(x) != 0:
            try:
                core_samples, cluster_ids = dbscan(np.vstack((x, y, z)).T, eps=0.3, min_samples=5)
            except:
                # print(np.vstack((x, y, z)).T)
                return dataOk
            df = pandas.DataFrame(np.c_[np.vstack((x, y, z)).T, cluster_ids],
                                  columns=['x', 'y', 'z', 'cluster_id'])
            df = df[df.cluster_id != -1]
            count = df['cluster_id'].value_counts()

            x = []
            y = []
            z = []
            if count.shape[0] >= 1:
                for i in range(0, count.shape[0]):
                    df1 = df[df.cluster_id == i]
                    # mean = np.mean([df1['x'], df1['y'], df1['z']], 1)
                    x = numpy.hstack((x, df1['x'].values))
                    y = numpy.hstack((y, df1['y'].values))
                    z = numpy.hstack((z, df1['z'].values))
                # Z_k = np.vstack((x, y, z)).T
                # if cnt==1:
                #     show = Z_k
                #     cnt += 1
                # elif cnt<changdu:
                #     show = numpy.vstack((show,Z_k))
                #     cnt+=1
                # elif cnt==changdu:
                #     cnt = 1
                #     show = numpy.vstack((show, Z_k))
                #     GLScatterPlot.setData(pos=show)

                Z_k = np.vstack((x, y, z)).T
                GLScatterPlot.setData(pos=Z_k, color=(1., 1., 1., .9))

        # confirmedroot, testroot, Z_k_prev = MOT(Z_k, Z_k_prev, confirmedroot, testroot)

        # if len(confirmedroot) > 0:
        #     tmppos = confirmedroot[0].est[0:3, :].T.A
        #     for i in range(1,len(confirmedroot)):
        #         tmppos = numpy.vstack((tmppos,confirmedroot[i].est[0:3, :].T.A))
        #     LinePlot.setData(pos=tmppos)
        #     QtGui.QApplication.processEvents()
        # else:
        #     LinePlot.setData(pos=np.array([100, 100, 100]))
        #     QtGui.QApplication.processEvents()


# -------------------------    MAIN   -----------------------------------------

# 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([])

app = pg.mkQApp()

view = gl.GLViewWidget()

view.show()

axis = gl.GLAxisItem()
axis.scale(10, 10, 10)

view.addItem(axis)

GLScatterPlot = gl.GLScatterPlotItem()
view.addItem(GLScatterPlot)

# Main loop

timer = pg.QtCore.QTimer()
timer.timeout.connect(update)  # 定时调用plotData函数
timer.start(50)  # 多少ms调用一次

app.exec_()

Bulijiojio Blog

mmWave_Tutorial1-快速上手mmWave Out of box demo

前言