-
Notifications
You must be signed in to change notification settings - Fork 2
/
tfli2c.py
345 lines (305 loc) · 12 KB
/
tfli2c.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
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
'''=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
# Package: tfli2c
# Developer: Bud Ryerson
# Inception: 10 JUL 2021
# Version: 0.0.1
# Described: Python module for the Benewake TF-Luna Lidar sensor
# configured for I2C communication mode
# Last Work: 17 JUL 2021
#
# Default settings for this module are:
# 0x10 - slave device address
# 4 - host I2C port address
#
# begin( addr, port)
# sets the variables `tflAddr` and `tflPort`
# for use by the rest of the module and
# sets device sampling to `trigger` mode
#
# getData()
# reads first six registers of device and
# sets the value of three variables
- dist : distance measured by the device, in cm
- flux : signal strength, quality or confidence
If flux value too low, an error will occur.
- temp : temperature of the chip in 0.01 degrees C
Returns true, if no error occurred.
If false, error is defined by a status code,
which can be displayed using `printStatus()` function.
#
# There are several explicit commands. In general,
# the `set` commmands require a follow-on `saveSettings`
# and `softReset` commands.
#
=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-'''
import time
from smbus import SMBus
status = 0 # error status code
dist = 0 # distance to target
flux = 0 # signal quality or intensity
temp = 0 # internal chip temperature
# Timeout Limits for various functions
TFL_MAX_READS = 20 # readData() sets SERIAL error
MAX_BYTES_BEFORE_HEADER = 20 # getData() sets HEADER error
MAX_ATTEMPTS_TO_MEASURE = 20
tflAddr = 0x10 # TFLuna I2C device address
# Range: 0x08 to 0x77
tflPort = 4 # Raspberry Pi I2C port number
# 4 = /dev/i2c-4, GPIO 8/9, pins 24/21
def begin( addr, port):
global tflPort, tflAddr
tflAddr = addr # re-assign device address
tflPort = port # re-assign port number
try:
bus = SMBus( tflPort)
bus.open( tflPort) # Open I2C communication
bus.write_quick( tflAddr) # Short test transaction
# Set device to single-shot/trigger mode
bus.write_byte_data( tflAddr, TFL_SET_MODE, 1)
bus.close() # Close I2C communication
return True
except Exception:
return False
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Definitions
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# - - - Register Names, Hex Address Numbers - - -
# - - - - and whether Read, Write or both - - - -
TFL_DIST_LO = 0x00 # R centimeters
TFL_DIST_HI = 0x01 # R
TFL_FLUX_LO = 0x02 # R arbitray units
TFL_FLUX_HI = 0x03 # R
TFL_TEMP_LO = 0x04 # R degrees Celsius
TFL_TEMP_HI = 0x05 # R
TFL_TICK_LO = 0x06 # R milliseconds
TFL_TICK_HI = 0x07 # R
TFL_ERR_LO = 0x08 # R
TFL_ERR_HI = 0x09 # R
TFL_VER_REV = 0x0A # R revision
TFL_VER_MIN = 0x0B # R minor release
TFL_VER_MAJ = 0x0C # R major release
TFL_PROD_CODE = 0x10 # R - 14 byte serial number
TFL_SAVE_SETTINGS = 0x20 # W -- Write 0x01 to save
TFL_SOFT_RESET = 0x21 # W -- Write 0x02 to reboot.
# Lidar not accessible during few seconds,
# then register value resets automatically
TFL_SET_I2C_ADDR = 0x22 # W/R -- Range 0x08,0x77.
# Must save and reboot to take effect.
TFL_SET_MODE = 0x23 # W/R -- 0-continuous, 1-trigger
TFL_TRIGGER = 0x24 # W -- 1-trigger once
TFL_DISABLE = 0x25 # W/R -- 0-enable, 1-disable
TFL_FPS_LO = 0x26 # W/R -- lo byte
TFL_FPS_HI = 0x27 # W/R -- hi byte
TFL_SET_LO_PWR = 0x28 # W/R -- 0-normal, 1-low power
TFL_HARD_RESET = 0x29 # W -- 1-restore factory settings
# - - Error Status Condition definitions - -
TFL_READY = 0 # no error
TFL_SERIAL = 1 # serial timeout
TFL_HEADER = 2 # no header found
TFL_CHECKSUM = 3 # checksum doesn't match
TFL_TIMEOUT = 4 # I2C timeout
TFL_PASS = 5 # reply from some system commands
TFL_FAIL = 6 # "
TFL_I2CREAD = 7
TFL_I2CWRITE = 8
TFL_I2CLENGTH = 9
TFL_WEAK = 10 # Signal Strength ≤ 100
TFL_STRONG = 11 # Signal Strength saturation
TFL_FLOOD = 12 # Ambient Light saturation
TFL_MEASURE = 13
TFL_INVALID = 14 # Invalid operation sent to sendCommand()
# - - - - - - - - - - - - - - - - - - - - - - - - - -
# GET DATA FROM THE DEVICE
# - - - - - - - - - - - - - - - - - - - - - - - - - -
# Return `True`/`False` whether data received without
# error and set system status
def getData():
''' Get get three data values '''
# 1. Make data and status variables global
global status, dist, flux, temp
# 2. Get data from the device.
bus = SMBus( tflPort) # Open I2C communication
# Trigger a one-shot data sample
bus.write_byte_data( tflAddr, TFL_TRIGGER, 1)
# Read the first six registers
frame = bus.read_i2c_block_data( tflAddr, 0, 6)
bus.close() # Close I2C communication
# 3. Shift data from read array into the three variables
dist = frame[ 0] + ( frame[ 1] << 8)
flux = frame[ 2] + ( frame[ 3] << 8)
temp = frame[ 4] + ( frame[ 5] << 8)
# Convert temp to degrees from hundredths
temp = ( temp / 100)
# Convert Celsius to Fahrenheit
#temp = ( temp * 9 / 5) + 32
# 4. Evaluate Abnormal Data Values
if( dist == -1):
status = TFL_WEAK
return False
elif( flux < 100): # Signal strength < 100
status = TFL_WEAK
return False
elif( flux > 0x8000): # Ambient light too strong
status = TFL_FLOOD
return False
elif( flux == 0xFFFF): # Signal saturation
status = TFL_STRONG
return False
# 5. Set Ready status and go home
status = TFL_READY
return True
#
# - - - - - - End of getData() function - - - - - - - -
# - - - - - - - - - - - - - - - - - - - - - - - - - -
# EXPLICIT COMMANDS
# - - - - - - - - - - - - - - - - - - - - - - - - - -
# - - - - - SAVE SETTINGS - - - - -
def saveSettings():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_SAVE_SETTINGS, 1)
bus.close()
# - - - - SOFT RESET aka Reboot - - - -
def softReset():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_SOFT_RESET, 2)
bus.close()
# - - - - HARD RESET to Factory Defaults - - - -
def hardReset():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_HARD_RESET, 1)
bus.close()
# - - - - - - SET I2C ADDRESS - - - - - -
# Range: 0x08, 0x77. Must be followed by
# `saveSettings()` and 'softReset()` commands
def setI2Caddr( addrNew):
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_SET_I2C_ADDR, addrNew)
bus.close()
# - - - - - SET ENABLE - - - - -
# Turn on LiDAR
# Must be followed by Save and Reset commands
def setEnable():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_DISABLE, 0)
bus.close()
# - - - - - SET DISABLE - - - - -
# Turn off LiDAR
# Must be followed by Save and Reset commands
def setDisable():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_DISABLE, 1)
bus.close()
# - - - - - - SET CONTINUOUS MODE - - - - - -
# Continuous ranging mode
# Must be followed by Save and Reset commands
def setModeCont():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_SET_MODE, 0)
bus.close()
# - - - - - - SET TRIGGER MODE - - - - - -
# Sample range only once when triggered
# Must be followed by Save and Reset commands
def setModeTrig():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_SET_MODE, 1)
bus.close()
# - - - - - - GET TRIGGER MODE - - - - - -
# Return mode type as a string.
def getMode():
bus = SMBus( tflPort)
mode = bus.read_byte_data( tflAddr, TFL_SET_MODE)
bus.close()
if mode == 0: return 'continuous'
else: return 'trigger'
# - - - - - - SET TRIGGER - - - - - =
# Trigger device to sample one time.
def setTrigger():
bus = SMBus( tflPort)
bus.write_byte_data( tflAddr, TFL_TRIGGER, 1)
bus.close()
# - - - - - SET FRAME RATE - - - - - -
# Write `fps` (frames per second) to device
# Command must be followed by `saveSettings()`
# and `softReset()` commands.
def setFrameRate( fps):
bus = SMBus( tflPort)
bus.write_word_data( tflAddr, TFL_FPS_LO, ( fps))
bus.close()
# - - - - - GET FRAME RATE - - - - - -
# Return two-byte Frame Rate (frames per second) value
def getFrameRate():
bus = SMBus( tflPort)
fps = bus.read_word_data( tflAddr, TFL_FPS_LO)
bus.close()
return fps
# - - - - GET DEVICE TIME (in milliseconds) - - -
# Return two-byte value of milliseconds since last reset.
def getTime():
bus = SMBus( tflPort) # Open I2C communication
tim = bus.read_word_data( tflAddr, TFL_TICK_LO) # Get two bytes of time+
bus.close() # Close I2C communication
return tim
# - - GET PRODUCTION CODE (Serial Number) - - -
# Return 14 ascii characters of serial number
def getProdCode():
prodcode = ''
bus = SMBus( tflPort) # Open I2C communication
for i in range( 14): # Build the 'production code' string
prodcode += chr(bus.read_byte_data( tflAddr, TFL_PROD_CODE + i))
bus.close() # Close I2C communication
return prodcode
# - - - - GET FIRMWARE VERSION - - - -
# Return version as a string
def getFirmwareVersion():
version = ''
bus = SMBus( tflPort) # Open I2C communication
# Build the 'version' string
version =\
str( bus.read_byte_data( tflAddr, TFL_VER_MAJ)) + '.' +\
str( bus.read_byte_data( tflAddr, TFL_VER_MIN)) + '.' +\
str( bus.read_byte_data( tflAddr, TFL_VER_REV))
bus.close() # Close I2C communication
return version # Return the version string
# - - - - - - - - - - - - - - - - - - - - -
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# - - - - - The following are for testing purposes - - - -
# They interpret error status codes and display HEX data
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#
# Called by either 'printFrame()' or 'printReply()'
# Print status condition either 'READY' or error type
def printStatus():
''' Print status condition'''
global status
print("Status: ", end= '')
if( status == TFMP_READY): print( "READY", end= '')
elif( status == TFMP_SERIAL): print( "SERIAL", end= '')
elif( status == TFMP_HEADER): print( "HEADER", end= '')
elif( status == TFMP_CHECKSUM): print( "CHECKSUM", end= '')
elif( status == TFMP_TIMEOUT): print( "TIMEOUT", end= '')
elif( status == TFMP_PASS): print( "PASS", end= '')
elif( status == TFMP_FAIL): print( "FAIL", end= '')
elif( status == TFMP_I2CREAD): print( "I2C-READ", end= '')
elif( status == TFMP_I2CWRITE): print( "I2C-WRITE", end= '')
elif( status == TFMP_I2CLENGTH): print( "I2C-LENGTH", end= '')
elif( status == TFMP_WEAK): print( "Signal weak", end= '')
elif( status == TFMP_STRONG): print( "Signal saturation", end= '')
elif( status == TFMP_FLOOD): print( "Ambient light saturation", end= '')
else: print( "OTHER", end= '')
print()
# - - - - For test and troubleshooting: - - - -
# - - - - Format a value as hexadecimal - - - -
'''
for i in range( len(cmndData)):
print( f" {cmndData[i]:0{2}X}", end='')
print()
'''
# f"{value:#0{padding}X}"
# formats 'value' as a hex number padded with
# 0s to the length of 'padding'
# - - - - - - - - - - - - - - - - - - - - - - - - -
# If this module is executed by itself
if __name__ == "__main__":
print( "tfli2c - This Python module supports the Benewake" +\
" TFLuna Lidar device in I2C mode.")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -