对自己使用过的SP70C毫米波雷达解析程序进行整理，实现对雷达探测结果的获取
SP70C毫米波雷达
SP70C 是湖南纳雷科技有限公司研发的一款紧凑型 K 波段毫米波雷达， 采用 24GHz ISM 频段，双接收天线设计，测量距离远、体型小巧、灵敏度高、 重量轻、易于集成、性能稳定，满足工业测距与防碰撞、安防领域人员定位与 跟踪、汽车自动驾驶与主动安全等多领域应用需求，产品性能已得到众多合作 伙伴的认可。
相关参数
SP70C 采 用 高 集 成 度 MMIC 方 案 ， 极 低 的 功 耗 (1.5W) ， 较 小 尺 寸 (71x63x6mm)，40m 测量距离，轻量化设计，可满足无人机、工业机械、电动车等领域的测距与防碰撞应用需求。
该款产品采用 1 路发射天线 2 路接收天线，精确测量目标的角度信息
SP70C 采用集成平面微带阵列天线，包含有 8 个垂直极化辐射单元。雷达 天线在方位面为宽波束，在俯仰面为窄波束，方位面-6dB 波束宽度超过 100°， 因此 SP70C 能够在方位面上探测到 100°范围内的运动目标。俯仰面-6dB 波束 宽度为 17°，并且采用泰勒算法对方向图进行低副瓣综合，具有优于-18dB 的副 瓣抑制比。
为方便客户开发测试，SP70C 雷达提供了两种通信接口：UART 和 RS485
数据传输格式
SP70C 雷达传感器采用的是 UART-TTL 接口，使用预设的默认传输速率为 115200 波特，8 bit 数据位，1 位停止位，无奇偶校验位，无流控。每一个数据报文 以一个起始序列开始，同时以一个终止序列结束；在 SP70C 的每个数据循环周期 (20ms)，都会输出 SP70C 的系统状态和目标输出状态报文，如果检测到目标即目 标输出状态报文的探测到目标个数字段为 1，目标输出状态报文后会紧接着输出目 标信息报文，目标信息报文包含目标的高度参数。
其中起始序列 Start Sequence 为定值 0xAAAA，报文 Message ID 定义如下表 所示，报文 Data Payload 根据 Message ID 定义，终止序列 End Sequence 为定值 0x5555。
数据解析
串口调试助手
借助USB-TTL转换器将数据线通过USB与电脑连接，同时，将GND与POWER IN与12V直流电源连接，具体如下
雷达上电后即开始工作，使用串口调试助手，可发现雷达发送的探测报文。其中06 0A为系统状态，说明连接无误，接口已联通； 07 0B为目标状态，07 0C为目标信息，说明可探测到目标，雷达正常工作。
解析代码
借助pyserial模块，构建串口解析代码，并封装成为单独的类，方便后续调用
import threading
import serial
import time
import logging
import struct
import queue
class sp70Comm:
def __init__(self, MsgQue):
self.msgQue = MsgQue
# 构造串口的属性
self.l_serial = None
self.thread_read = None
self.alive = False
self.waitEnd = None
# 串口号
self.port = ''
# 波特率
self.baudrate = 115200
# 校验位
self.parity = serial.PARITY_NONE
# 数据位
self.bytesize = serial.EIGHTBITS
self.ID = None
self.data = None
self.device_id = 'XX'
def set_param(self, DeviceId='XX', Port='COM3', Baudrate=115200, Parity=serial.PARITY_NONE,
Bytesize=serial.EIGHTBITS):
# 串口号
self.port = Port
# 波特率
self.baudrate = Baudrate
# 校验位
self.parity = Parity
# 数据位
self.bytesize = Bytesize
self.device_id = DeviceId
# 定义串口等待的函数
def waiting(self):
logging.debug("定义串口等待的函数")
if not self.waitEnd is None:
self.waitEnd.wait()
def SetStopEvent(self):
logging.debug("设置stop event")
if not self.waitEnd is None:
self.waitEnd.set()
self.alive = False
self.stop()
# 启动串口的函数
def start(self):
self.alive = True
try:
logging.debug("启动串口的函数")
self.l_serial = serial.Serial()
self.l_serial.port = self.port
self.l_serial.baudrate = self.baudrate
self.l_serial.parity = self.parity
self.l_serial.bytesize = self.bytesize
# 设置等待时间，若超出这停止等待
logging.debug("设置等待时间，若超出这停止等待")
self.l_serial.timeout = 2
logging.debug(
'参数--串口：' + str(self.port) + ',位/秒：' + str(self.baudrate) + ',奇偶校验：' + str(
self.parity) + ',数据位：' + str(
self.bytesize))
self.l_serial.open()
# 判断串口是否已经打开
logging.debug("判断串口是否已经打开:" + str(self.l_serial.isOpen()))
if self.l_serial.isOpen():
# 这里写自己的逻辑，我这里是开了一个线程监听串口读数据
self.waitEnd = threading.Event()
self.alive = True
self.thread_read = None
logging.debug("激活标志alive:" + str(self.alive))
self.thread_read = threading.Thread(target=self.msgParse)
self.thread_read.setDaemon(True)
self.thread_read.start()
logging.debug("线程启动。。。")
print(str(self.port) + ' open, start processing')
return True
else:
return False
except Exception as e:
logging.warning(str(self.port) + "开启异常：" + str(e))
print(str(self.port) + ' not open, ' + str(e))
return False
# 发送数据
def SendData(self, send):
# lmsg = ''
isOK = False
lmsg = send.encode('gb18030')
com_ack = ''
try:
# 发送数据到相应的处理组件
self.l_serial.write(lmsg)
_cli_ack = self.l_serial.readline()
# print('[0]', _cli_ack)
_done_ack = self.l_serial.readline()
# print('[1]', _done_ack)
com_ack = _cli_ack + _done_ack
except Exception as ex:
print(lmsg, ' failed: ', ex)
pass
return com_ack
def msgParse(self):
logging.debug(":数据监控开始。。。")
# head_word, tail_word = 0xAA, 0x55
head_word, tail_word = bytes.fromhex('AA'), bytes.fromhex('55')
package_length = 14
bytes_data = bytes()
msg_cnt = 0
try:
while self.alive:
n = self.l_serial.inWaiting() # 获取接收到的数据长度
# if n == 0:
# # time.sleep(0.001)
# continue
logging.debug(f'reading {n} bytes')
# print(f'reading {n} bytes')
bytes_data = bytes_data + self.l_serial.read(n)
ready_flag = False
logging.debug(f'begin header seeking, bytes len {len(bytes_data)}')
# print(f'begin header seeking, bytes len {len(bytes_data)}')
while len(bytes_data) > package_length:
raw_word = struct.unpack('14c', bytes_data[:package_length])
if (raw_word[0] != head_word) and \
(raw_word[1] != head_word) and \
(raw_word[-1] != tail_word) and \
(raw_word[-2] != tail_word):
# wrong head word, increment pointer by 1 and try again
bytes_data = bytes_data[1:]
continue
else:
# got correct word, proceed to parse
msg_cnt += 1
# logging.warning(f'header found, before bytes len {len(bytes_data)}, frameId {msg_cnt}')
bytes_data = bytes_data[package_length:]
if len(bytes_data) > 10 * package_length:
logging.warning(f'msg remain unprocessed {len(bytes_data) / package_length}')
# logging.warning(f'header found, remain bytes len {len(bytes_data)}, frameId {msg_cnt}')
msg_str = raw_word[2:-2]
msg_tag, msg_info = extract_msg(msg_str)
if msg_tag == 'target info':
msg_pack = {'tag': gdf.TAG_SP70,
'mid': self.device_id,
'time': time.time(),
'payload': [msg_info['index'], msg_info['rcs'], msg_info['range'],
msg_info['azimuth'], msg_info['vrel'], msg_info['roll_count'],
msg_info['snr']]}
self.msgQue.put(msg_pack)
# print(f'{self.device_id} info {msg_info}')
# print(f'header found, bytes len {len(bytes_data)}, frameId {frameId} tlvNum {numTLVs} packetLength {packetLength}')
break
# QApplication.processEvents()
except Exception as e:
logging.warning('获取com数据异常2：' + str(e))
print(str(self.port) + ' parse error: ' + str(e))
self.waitEnd.set()
self.alive = False
def stop(self):
try:
self.alive = False
self.data = None
if self.thread_read is not None:
self.thread_read.join()
logging.debug(str(self.thread_read.ident) + '线程关闭。。。')
print(str(self.thread_read.ident) + ' thread terminated')
if self.l_serial is not None and self.l_serial.isOpen():
self.l_serial.close()
logging.debug(str(self.l_serial.port) + '串口关闭。。。')
print(str(self.l_serial.port) + ' port closed')
except Exception as e:
logging.warning('关闭' + str(self.port) + '线程失败：' + str(e))
print(str(self.port) + 'close failed: ' + str(e))
经过封装后，便可在主函数中指定对应的端口号，直接调用。
if __name__ == '__main__':
logging.basicConfig(level=logging.WARN)
msg_queue = queue.Queue(maxsize=2000)
sp70_t1 = sp70Comm(MsgQue=msg_queue) # sp70c #1
sp70_t1.set_param(DeviceId='001', Port='COM12') # sp70c #1
sp70_t1.start()
测试结果
使用SP70C观察公路上行驶的车辆，将得到的探测结果以散点图的形式作图。
从图中可看到近雷达侧的路面，车辆探测比较连续，散点反映了车辆直线行驶的轨迹。而在对侧车道上行驶的车辆，由于遮挡及信噪比原因，探测结果较为分散。
736
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
