-
Notifications
You must be signed in to change notification settings - Fork 1
/
radar.py
193 lines (151 loc) · 7.06 KB
/
radar.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
import numpy as np
import serial
import struct
import time
DEBUG=False
MAGIC_WORD_ARRAY = np.array([2, 1, 4, 3, 6, 5, 8, 7])
MAGIC_WORD = b'\x02\x01\x04\x03\x06\x05\x08\x07'
MSG_AZIMUT_STATIC_HEAT_MAP = 8
class TI:
def __init__(self, sdk_version=3.4, cli_loc='COM4', cli_baud=115200,
data_loc='COM3', data_baud=921600, num_rx=4, num_tx=3,
verbose=False, connect=True, mode=0):
super(TI, self).__init__()
self.connected = False
self.verbose = verbose
self.mode = mode
if connect:
self.cli_port = serial.Serial(cli_loc, cli_baud)
self.data_port = serial.Serial(data_loc, data_baud)
self.connected = True
self.sdk_version = sdk_version
self.num_rx_ant = num_rx
self.num_tx_ant = num_tx
self.num_virtual_ant = num_rx * num_tx
if mode == 0:
self._initialize()
def _configure_radar(self, config):
for i in config:
self.cli_port.write((i + '\n').encode())
print(i)
time.sleep(0.01)
def _initialize(self, config_file='./profile.cfg'):
config = [line.rstrip('\r\n') for line in open(config_file)]
if self.connected:
self._configure_radar(config)
self.config_params = {} # Initialize an empty dictionary to store the configuration parameters
for i in config:
# Split the line
split_words = i.split(" ")
# Hard code the number of antennas, change if other configuration is used
num_rx_ant = 4
num_tx_ant = 3
# Get the information about the profile configuration
if "profileCfg" in split_words[0]:
start_freq = int(split_words[2])
idle_time = int(split_words[3])
ramp_end_time = float(split_words[5])
freq_slope_const = int(split_words[8])
num_adc_samples = int(split_words[10])
num_adc_samples_round_to2 = 1
while num_adc_samples > num_adc_samples_round_to2:
num_adc_samples_round_to2 = num_adc_samples_round_to2 * 2
dig_out_sample_rate = int(split_words[11])
# Get the information about the frame configuration
elif "frameCfg" in split_words[0]:
chirp_start_idx = int(split_words[1])
chirp_end_idx = int(split_words[2])
num_loops = int(split_words[3])
num_frames = int(split_words[4])
frame_periodicity = float(split_words[5])
# Combine the read data to obtain the configuration parameters
num_chirps_per_frame = (chirp_end_idx - chirp_start_idx + 1) * num_loops
self.config_params["numDopplerBins"] = num_chirps_per_frame / num_tx_ant
self.config_params["numRangeBins"] = num_adc_samples_round_to2
self.config_params["rangeResolutionMeters"] = (3e8 * dig_out_sample_rate * 1e3) / (
2 * freq_slope_const * 1e12 * num_adc_samples)
self.config_params["rangeIdxToMeters"] = (3e8 * dig_out_sample_rate * 1e3) / (
2 * freq_slope_const * 1e12 * self.config_params["numRangeBins"])
self.config_params["dopplerResolutionMps"] = 3e8 / (
2 * start_freq * 1e9 * (idle_time + ramp_end_time) * 1e-6 * self.config_params[
"numDopplerBins"] * num_tx_ant)
self.config_params["maxRange"] = (300 * 0.9 * dig_out_sample_rate) / (2 * freq_slope_const * 1e3)
self.config_params["maxVelocity"] = 3e8 / (
4 * start_freq * 1e9 * (idle_time + ramp_end_time) * 1e-6 * num_tx_ant)
def close(self):
"""End connection between radar and machine
Returns:
None
"""
self.cli_port.write('sensorStop\n'.encode())
self.cli_port.close()
self.data_port.close()
def _read_buffer(self):
"""
Returns:
"""
byte_buffer = self.data_port.read(self.data_port.in_waiting)
return byte_buffer
def _parse_header_data(self, byte_buffer, idx):
"""Parses the byte buffer for the header of the data
Args:
byte_buffer: Buffer with TLV data
idx: Current reading index of the byte buffer
Returns:
Tuple [Tuple (int), int]
"""
magic, idx = self._unpack(byte_buffer, idx, order='>', items=1, form='Q')
(version, length, platform, frame_num, cpu_cycles, num_obj, num_tlvs), idx = self._unpack(byte_buffer, idx,
items=7, form='I')
subframe_num, idx = self._unpack(byte_buffer, idx, items=1, form='I')
return (version, length, platform, frame_num, cpu_cycles, num_obj, num_tlvs, subframe_num), idx
def _parse_header_tlv(self, byte_buffer, idx):
""" Parses the byte buffer for the header of a tlv
"""
(tlv_type, tlv_length), idx = self._unpack(byte_buffer, idx, items=2, form='I')
return (tlv_type, tlv_length), idx
def _parse_msg_azimut_static_heat_map(self, byte_buffer, idx):
""" Parses the information of the azimuth heat map
"""
(imag, real), idx = self._unpack(byte_buffer, idx, items=2, form='H')
return (imag, real), idx
def sample(self):
""" Samples byte data from the radar and converts it to decimal
"""
byte_buffer = self._read_buffer()
if len(byte_buffer) < 36:
return None
return self._process(byte_buffer)
def _process(self, byte_buffer):
"""
"""
idx = byte_buffer.index(MAGIC_WORD)
header_data, idx = self._parse_header_data(byte_buffer, idx)
# print(header_data,idx)
(tlv_type, tlv_length), idx = self._parse_header_tlv(byte_buffer, idx)
# print(tlv_type, tlv_length,idx)
azimuth_map = np.zeros((self.num_virtual_ant, self.config_params['numRangeBins'], 2),dtype=np.int16)
for bin_idx in range(self.config_params['numRangeBins']):
for ant in range(self.num_virtual_ant):
azimuth_map[ant][bin_idx][:], idx = self._parse_msg_azimut_static_heat_map(byte_buffer, idx)
return azimuth_map
@staticmethod
def _unpack(byte_buffer, idx, order='', items=1, form='I'):
"""Helper function for parsing binary byte data
Args:
byte_buffer: Buffer with data
idx: Curex in the buffer
order: Little endian or big endian
items: Number of items to be extracted
form: Data type to be extracted
Returns:rent ind
Tuple [Tuple (object), int]
"""
size = {'H': 2, 'h': 2, 'I': 4, 'Q': 8, 'f': 4}
try:
data = struct.unpack(order + str(items) + form, byte_buffer[idx:idx + (items * size[form])])
if len(data) == 1:
data = data[0]
return data, idx + (items * size[form])
except:
return None