mmWave_Tutorial6-mmWave原始数据获取

前言

在之前的5篇博客中我们介绍了, TI out of box demo, Ti 3D people counting demo 以及使用他们的数据进行的目标跟踪与航迹管理, 还有TI out of box demo的源码程序, 那按照理论上来说, 我们其实就可以在CCS中对mmWave SoC 进行编程从而实现我们自己所设计的信号处理流程, 通过直接去写C语言来对 ARM 和 DSP 编程的方式需要比较长的时间, 而在进行算法验证的初期我们更多是使用 Matlab, Python 等一些高级语言进行算法验证, 在性能达到我们预期的情况下, 我们才会考虑将这样的算法移植到SOC的平台中, 这样能够为我们的开发节省出很多的时间.

Ti 官方已经提供了 mmWaveStudio 这样的工具, 在搭配 iwr设备, mmWave ICBoost承载板卡, DCA1000数据采集办卡后, 我们可以通过LVDS获得雷达各个接收通道的中频信号(也可以叫差拍信号). 使用 mmWaveStudio 工具进行原始数据获取TI官方给了非常详尽的文档, 如果还没有使用过 mmWaveStudio 的读者, 请先使用官方方式进行数据采集然后再来参考本篇博客

/ti/mmwave_studio_02_01_01_00/docs/mmwave_studio_user_guide.pdf

在这篇博客当中, 我将介绍如何使用 python 脚本对 mmWave radar 的原始数据进行获取.

DCA1000EVM Overview

硬件连接

相关代码(python)

# Copyright 2019 The OpenRadar Authors. All Rights Reserved.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#     http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================

import codecs
import socket
import struct
import serial
import time
import matplotlib.pyplot as plt
# import pyqtgraph as pg
# from mpl_toolkits.mplot3d import Axes3D
from enum import Enum
# from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
# from matplotlib.figure import Figure
# from matplotlib.lines import Line2D
# import matplotlib.cbook as cbook
import numpy as np

configFileName = "IWR6843_cfg.cfg"


class CMD(Enum):
    RESET_FPGA_CMD_CODE = '0100'
    RESET_AR_DEV_CMD_CODE = '0200'
    CONFIG_FPGA_GEN_CMD_CODE = '0300'
    CONFIG_EEPROM_CMD_CODE = '0400'
    RECORD_START_CMD_CODE = '0500'
    RECORD_STOP_CMD_CODE = '0600'
    PLAYBACK_START_CMD_CODE = '0700'
    PLAYBACK_STOP_CMD_CODE = '0800'
    SYSTEM_CONNECT_CMD_CODE = '0900'
    SYSTEM_ERROR_CMD_CODE = '0a00'
    CONFIG_PACKET_DATA_CMD_CODE = '0b00'
    CONFIG_DATA_MODE_AR_DEV_CMD_CODE = '0c00'
    INIT_FPGA_PLAYBACK_CMD_CODE = '0d00'
    READ_FPGA_VERSION_CMD_CODE = '0e00'

    def __str__(self):
        return str(self.value)


# MESSAGE = codecs.decode(b'5aa509000000aaee', 'hex')
CONFIG_HEADER = '5aa5'
CONFIG_STATUS = '0000'
CONFIG_FOOTER = 'aaee'
ADC_PARAMS = {
    'chirps': 96,
    'rx': 4,
    'tx': 3,
    'samples': 96,
    'IQ': 2,
    'bytes': 2
}
# STATIC
MAX_PACKET_SIZE = 2097152
# 默认数据包长度
BYTES_IN_PACKET = 1456
# DYNAMIC
BYTES_IN_FRAME = (ADC_PARAMS['chirps'] * ADC_PARAMS['rx'] * ADC_PARAMS['tx'] *
                  ADC_PARAMS['IQ'] * ADC_PARAMS['samples'] *
                  ADC_PARAMS['bytes'])
BYTES_IN_FRAME_CLIPPED = (BYTES_IN_FRAME // BYTES_IN_PACKET) * BYTES_IN_PACKET
PACKETS_IN_FRAME = BYTES_IN_FRAME / BYTES_IN_PACKET
PACKETS_IN_FRAME_CLIPPED = BYTES_IN_FRAME // BYTES_IN_PACKET
UINT16_IN_PACKET = BYTES_IN_PACKET // 2
UINT16_IN_FRAME = BYTES_IN_FRAME // 2
# a = []
# b = []


class DCA1000:
    """Software interface to the DCA1000 EVM board via ethernet.

    Attributes:
        static_ip (str): IP to receive data from the FPGA
        adc_ip (str): IP to send configuration commands to the FPGA
        data_port (int): Port that the FPGA is using to send data
        config_port (int): Port that the FPGA is using to read configuration commands from


    General steps are as follows:
        1. Power cycle DCA1000 and XWR1xxx sensor
        2. Open mmWaveStudio and setup normally until tab SensorConfig or use lua script
        3. Make sure to connect mmWaveStudio to the board via ethernet
        4. Start streaming data
        5. Read in frames using class

    Examples:
        >>> dca = DCA1000()
        >>> adc_data = dca.read(timeout=.1)
        >>> frame = dca.organize(adc_data, 128, 4, 256)

    """
    def __init__(self,
                 configfilename,
                 static_ip='192.168.33.30',
                 adc_ip='192.168.33.180',
                 data_port=4098,
                 config_port=4096):
        # Save network data
        # self.static_ip = static_ip
        # self.adc_ip = adc_ip
        # self.data_port = data_port
        # self.config_port = config_port

        # Create configuration and data destinations
        self.cfg_dest = (adc_ip, config_port)
        self.cfg_recv = (static_ip, config_port)
        self.data_recv = (static_ip, data_port)

        # Create sockets
        self.config_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
                                           socket.IPPROTO_UDP)
        self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
                                         socket.IPPROTO_UDP)

        # Bind data socket to fpga
        self.data_socket.bind(self.data_recv)

        # Bind config socket to fpga
        self.config_socket.bind(self.cfg_recv)

        self.CLIport = serial.Serial('COM7', 115200)
        self.Dataport = serial.Serial('COM8', 921600)

        self.CfgFilename = configfilename

        self.data = []
        self.packet_count = []
        self.byte_count = []

        self.frame_buff = []

        self.curr_buff = None
        self.last_frame = None
        self.lost_packets = None

    def _serialConfig(self):
        # Raspberry pi
        # CLIport = serial.Serial('/dev/ttyACM0', 115200)
        # Dataport = serial.Serial('/dev/ttyACM1', 921600)

        # Windows
        # self.CLIport = serial.Serial('COM11', 115200)
        # self.Dataport = serial.Serial('COM12', 921600)

        config = [line.rstrip('\r\n') for line in open(self.CfgFilename)]
        for i in config:
            if i == '' or i[0] == "#" or i[0] == "%":
                continue
            self.CLIport.write((i + '\n').encode())
            print('sent->' + i)
            time.sleep(0.1)
            reply = self.CLIport.read(self.CLIport.in_waiting).decode()
            print(reply)
        return self.CLIport, self.Dataport

    def _sensor_start(self):
        self.CLIport.write("sensorStart\n".encode())
        print("sent->" + "sensorStart")
        time.sleep(0.1)
        reply = self.CLIport.read(self.CLIport.in_waiting).decode()
        print(reply)

    def _sensor_stop(self):
        self.CLIport.write("sensorStop\n".encode())
        print("sent->" + "sensorStop")
        time.sleep(0.1)
        reply = self.CLIport.read(self.CLIport.in_waiting).decode()
        print(reply)

    def start(self):
        # RECORD_START_CMD_CODE
        # 5a a5 05 00 00 00 aa ee
        print("->网口发送记录开始指令：\n{}".format(
            self._send_command(CMD.RECORD_START_CMD_CODE)))
        self._sensor_start()

        print('commands send -> Start ')

    def stop(self):
        self._sensor_stop()
        # RECORD_START_CMD_CODE
        # 5a a5 06 00 00 00 aa ee
        print("->网口发送记录终止指令：\n{}".format(
            self._send_command(CMD.RECORD_STOP_CMD_CODE)))
        self._close()

    def _close(self):
        # self.data_socket.close()
        self.config_socket.close()
        self.CLIport.close()
        self.Dataport.close()

    def configure(self):
        # # SYSTEM_CONNECT_CMD_CODE
        # # 5a a5 09 00 00 00 aa ee
        # print("->连接确认:")
        # print("{}".format(self.send_command(CMD.SYSTEM_CONNECT_CMD_CODE)))

        # CONFIG_FPGA_GEN_CMD_CODE
        # 5a a5 01 00 00 00 aa ee
        print("->重置FPGA：")
        print("{}".format(self._send_command(CMD.RESET_FPGA_CMD_CODE)))

        # CONFIG_FPGA_GEN_CMD_CODE
        # 5a a5 02 00 00 00 aa ee
        print("->重置AR：")
        print("{}".format(self._send_command(CMD.RESET_AR_DEV_CMD_CODE)))

        # CONFIG_FPGA_GEN_CMD_CODE
        # 5a a5 03 00 06 00 01 02 01 02 03 1e aa ee
        print("->配置FPGA：")
        print("{}".format(
            self._send_command(CMD.CONFIG_FPGA_GEN_CMD_CODE, '0600',
                               '01020102031e')))

        # CONFIG_PACKET_DATA_CMD_CODE
        # 5a a5 0b 00 06 00 be 05 35 0c 00 00 aa ee
        print("->配置PACKET：")
        print("{}".format(
            self._send_command(CMD.CONFIG_PACKET_DATA_CMD_CODE, '0600',
                               'c005350c0000')))

        # # READ_FPGA_VERSION_CMD_CODE
        # # 5a a5 0e 00 00 00 aa ee
        # print("->读取FPGA版本：")
        # print("{}".format(self.send_command_web(CMD.READ_FPGA_VERSION_CMD_CODE)))

    def close(self):
        """Closes the sockets that are used for receiving and sending data

        Returns:
            None

        """
        self.data_socket.close()
        self.config_socket.close()

    def read(self, timeout=1):
        """ Read in a single packet via UDP

        Args:
            timeout (float): Time to wait for packet before moving on

        Returns:
            Full frame as array if successful, else None

        """
        # Configure

        self.data_socket.settimeout(timeout)

        # Frame buffer
        ret_frame = np.zeros(UINT16_IN_FRAME, dtype=np.int16)

        # Wait for start of next frame
        while True:
            packet_num, byte_count, packet_data = self._read_data_packet()
            # a.append(packet_num) # 获取包数组
            # print(packet_num,byte_count)
            if (packet_num - 1) % PACKETS_IN_FRAME_CLIPPED == 0:
                packets_read = 1
                ret_frame[0:UINT16_IN_PACKET] = packet_data
                break

        # Read in the rest of the frame
        while True:
            packet_num, byte_count, packet_data = self._read_data_packet()
            # print(packet_num,byte_count)
            # b.append(packet_num) # 获取包数组
            packets_read += 1
            if (packet_num - 1) % PACKETS_IN_FRAME_CLIPPED == 0:
                self.lost_packets = PACKETS_IN_FRAME_CLIPPED - packets_read
                # 返回-1 基本不丢包
                # print("lost_packets:%d" %self.lost_packets)
                return ret_frame

            curr_idx = ((packet_num - 1) % PACKETS_IN_FRAME_CLIPPED)
            try:
                ret_frame[curr_idx * UINT16_IN_PACKET:(curr_idx + 1) *
                          UINT16_IN_PACKET] = packet_data
            except:
                pass

            if packets_read > PACKETS_IN_FRAME_CLIPPED:
                packets_read = 0

    def _send_command(self, cmd, length='0000', body='', timeout=1):
        """Helper function to send a single commmand to the FPGA

        Args:
            cmd (CMD): Command code to send to the FPGA
            length (str): Length of the body of the command (if any)
            body (str): Body information of the command
            timeout (int): Time in seconds to wait for socket data until timeout

        Returns:
            str: Response message

        """
        # Create timeout exception
        self.config_socket.settimeout(timeout)

        # Create and send message
        resp = ''
        msg = codecs.decode(
            ''.join((CONFIG_HEADER, str(cmd), length, body, CONFIG_FOOTER)),
            'hex')
        try:
            self.config_socket.sendto(msg, self.cfg_dest)
            resp, addr = self.config_socket.recvfrom(MAX_PACKET_SIZE)
        except socket.timeout as e:
            print(e)
        return resp

    def _read_data_packet(self):
        """Helper function to read in a single ADC packet via UDP

        Returns:
 
           int: Current packet number, byte count of data that has already been read, raw ADC data in current packet
        """
        data, addr = self.data_socket.recvfrom(MAX_PACKET_SIZE)
        packet_num = struct.unpack('<1l', data[:4])[0]
        byte_count = struct.unpack('>Q', b'\x00\x00' + data[4:10][::-1])[0]
        packet_data = np.frombuffer(data[10:], dtype=np.int16)
        return packet_num, byte_count, packet_data

    def _listen_for_error(self):
        """Helper function to try and read in for an error message from the FPGA

        Returns:
            None

        """
        self.config_socket.settimeout(None)
        msg = self.config_socket.recvfrom(MAX_PACKET_SIZE)
        if msg == b'5aa50a000300aaee':
            print('stopped:', msg)

    def _stop_stream(self):
        """Helper function to send the stop command to the FPGA

        Returns:
            str: Response Message

        """
        return self._send_command(CMD.RECORD_STOP_CMD_CODE)

    @staticmethod
    def organize(raw_frame, num_chirps, num_rx, num_samples):
        """Reorganizes raw ADC data into a full frame

        Args:
            raw_frame (ndarray): Data to format
            num_chirps: Number of chirps included in the frame
            num_rx: Number of receivers used in the frame
            num_samples: Number of ADC samples included in each chirp

        Returns:
            ndarray: Reformatted frame of raw data of shape (num_chirps, num_rx, num_samples)

        """

        # ret = np.zeros(len(raw_frame) // 2, dtype=complex)

        # # Separate IQ data
        # ret[0::2] = raw_frame[0::4] + 1j * raw_frame[2::4]
        # ret[1::2] = raw_frame[1::4] + 1j * raw_frame[3::4]
        # return ret.reshape((num_chirps, num_rx, num_samples))

        
        sampleI = np.zeros(len(raw_frame) // 2, dtype=complex)
        sampleQ = np.zeros(len(raw_frame) // 2, dtype=complex)
        
        sampleI[0::2] = raw_frame[0::4]
        sampleI[1::2] = raw_frame[1::4]
        sampleQ[0::2] = raw_frame[2::4]
        sampleQ[1::2] = raw_frame[3::4]

        sample = sampleQ  + 1j * sampleI 

        return sample.reshape((num_chirps, num_rx, num_samples))

import mmwave.dsp as dsp
import mmwave.clustering as clu
from demo.visualizer import ellipse_visualize

if __name__ == "__main__":
    DCA = DCA1000(configFileName)
    DCA.configure()
    time.sleep(3)
    DCA._serialConfig()
    DCA.start()
    fig = plt.figure()

    numADCSamples = 96
    numTxAntennas = 3
    numRxAntennas = 4
    numLoopsPerFrame = 96
    numChirpsPerFrame = numTxAntennas * numLoopsPerFrame

    numRangeBins = numADCSamples
    numDopplerBins = numLoopsPerFrame
    numAngleBins = 64

    range_resolution, bandwidth = dsp.range_resolution(numADCSamples)
    doppler_resolution = dsp.doppler_resolution(bandwidth)

    plotRangeDopp = False  
    plot2DscatterXY = True  
    plot2DscatterXZ = False  
    plot3Dscatter = False  
    plotCustomPlt = False

    visTrigger = plot2DscatterXY + plot2DscatterXZ + plot3Dscatter + plotRangeDopp + plotCustomPlt
    assert visTrigger < 2, "Can only choose to plot one type of plot at once"

    singFrameView = False

    # (1.5) Required Plot Declarations
    # if plot2DscatterXY or plot2DscatterXZ:
    #     fig, axes = plt.subplots(1, 2)
    # elif plot3Dscatter:
    #     fig = plt.figure()
    # elif plotRangeDopp:
    #     fig = plt.figure()
    # elif plotCustomPlt:
    #     print("Using Custom Plotting")

    rx_index = [3,1,2,0,11,9,7,5,10,8,6,4];
    sensor_number=12;
    sensor_array = np.array([[1,1,0,0],[1,1,0,0],[1,1,1,1],[1,1,1,1]])

    index = 0;
    frame = np.zeros((100,96,12,96),dtype=complex)
    while True:
        try:
            # (1) Reading in adc data
            # start0 = time.time()
            adc_data = DCA.read(timeout=.5)
            # DCA.organize基本不占用时间
            frame[index] = DCA.organize(adc_data, ADC_PARAMS['chirps'],ADC_PARAMS['rx'] * ADC_PARAMS['tx'],ADC_PARAMS['samples'])
            if index == 99:
                import scipy.io as io
                mat_path = 'rx.mat'
                io.savemat(mat_path, {'rx': frame})
            else:
                index = index + 1
            # end0 = time.time()
            # print('capture data is :%s seconds'%(end0 - start0))

        except KeyboardInterrupt:
            print("结束！")
            DCA.stop()
            plt.close()
            break

    plt.show()
    exit(0)