-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdistance_sensor.py
231 lines (178 loc) · 7.29 KB
/
distance_sensor.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
#CODE BASED ON rpisensors/proximity.py
#https://github.com/altosz/rpisensors
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import logging
import time
from rpisensors.eeprom16 import Eeprom16
# pylint: disable=C0103
# Default address
VL6180X_I2CADDRESS = 0x29
VL_IDENTIFICATION_MODEL_ID = 0x000
VL_SYSTEM_INTERRUPT_CONFIG_GPIO = 0x014
VL_SYSTEM_INTERRUPT_CLEAR = 0x015
VL_SYSTEM_FRESH_OUT_OF_RESET = 0x016
VL_SYSTEM_MODE_GPIO1 = 0x011
VL_SYSRANGE_START = 0x018
VL_SYSRANGE_INTERMEASUREMENT_PERIOD = 0x01B
VL_SYSRANGE_VHV_RECALIBRATE = 0x02E
VL_SYSRANGE_VHV_REPEAT_RATE = 0x031
VL_SYSALS_START = 0x038
VL_SYSALS_INTERMEASUREMENT_PERIOD = 0x03E
VL_SYSALS_ANALOGUE_GAIN = 0x03F
VL_SYSALS_INTEGRATION_PERIOD = 0x040
VL_RESULT_INTERRUPT_STATUS_GPIO = 0x04F
VL_RESULT_ALS_VAL = 0x050
VL_RESULT_RANGE_VAL = 0x062
VL_READOUT_AVERAGING_SAMPLE_PERIOD = 0x10A
VL_IDENTIFICATION_MODEL_ID_VALUE = 0xB4
VL_ALS_GAIN_1 = 0x06
VL_ALS_GAIN_1_25 = 0x05
VL_ALS_GAIN_1_67 = 0x04
VL_ALS_GAIN_2_5 = 0x03
VL_ALS_GAIN_5 = 0x02
VL_ALS_GAIN_10 = 0x01
VL_ALS_GAIN_20 = 0x00
VL_ALS_GAIN_40 = 0x07
class VL6180X(Eeprom16):
def __init__(self, bus_id, address=VL6180X_I2CADDRESS, **kwargs):
super(VL6180X, self).__init__(bus_id, address, **kwargs)
self.verify()
self.prepare()
def verify(self):
chip_id = self.read_byte(VL_IDENTIFICATION_MODEL_ID)
is_vl6180x = chip_id == VL_IDENTIFICATION_MODEL_ID_VALUE
if is_vl6180x:
self.logger.debug("VL6180X sensor found: chip id is 0xB4 ")
else:
self.logger.warning(
'No VL6180X sensor found: chip id is 0x%02X, '
'should be 0x%02X', chip_id, VL_IDENTIFICATION_MODEL_ID_VALUE)
def prepare(self, force=False):
status = self.read_byte(VL_SYSTEM_FRESH_OUT_OF_RESET)
if status == 1:
self.logger.debug("System is fresh out of reset")
if status == 1 or force is True:
self.logger.debug("Setting sensor parameters, force = %s", force)
self.write_byte(0x0207, 0x01)
self.write_byte(0x0208, 0x01)
self.write_byte(0x0096, 0x00)
self.write_byte(0x0097, 0xfd)
self.write_byte(0x00e3, 0x00)
self.write_byte(0x00e4, 0x04)
self.write_byte(0x00e5, 0x02)
self.write_byte(0x00e6, 0x01)
self.write_byte(0x00e7, 0x03)
self.write_byte(0x00f5, 0x02)
self.write_byte(0x00d9, 0x05)
self.write_byte(0x00db, 0xce)
self.write_byte(0x00dc, 0x03)
self.write_byte(0x00dd, 0xf8)
self.write_byte(0x009f, 0x00)
self.write_byte(0x00a3, 0x3c)
self.write_byte(0x00b7, 0x00)
self.write_byte(0x00bb, 0x3c)
self.write_byte(0x00b2, 0x09)
self.write_byte(0x00ca, 0x09)
self.write_byte(0x0198, 0x01)
self.write_byte(0x01b0, 0x17)
self.write_byte(0x01ad, 0x00)
self.write_byte(0x00ff, 0x05)
self.write_byte(0x0100, 0x05)
self.write_byte(0x0199, 0x05)
self.write_byte(0x01a6, 0x1b)
self.write_byte(0x01ac, 0x3e)
self.write_byte(0x01a7, 0x1f)
self.write_byte(0x0030, 0x00)
# Enables polling for ‘New Sample ready’ when measurement completes
self.write_byte(VL_SYSTEM_MODE_GPIO1, 0x10)
# Set the averaging sample period (compromise between lower noise
# and increased execution time)
self.write_byte(VL_READOUT_AVERAGING_SAMPLE_PERIOD, 0x30)
# Set the light and dark gain (upper nibble).
# Dark gain should not be changed.
self.write_byte(VL_SYSALS_ANALOGUE_GAIN, 0x40 | VL_ALS_GAIN_1)
# Set the number of range measurements after which auto calibration
# of system is performed
self.write_byte(VL_SYSRANGE_VHV_REPEAT_RATE, 0xFF)
# Set ALS integration time to 100ms
self.write_byte(VL_SYSALS_INTEGRATION_PERIOD, 0x63)
# Perform a single temperature calibratio of the ranging sensor
self.write_byte(VL_SYSRANGE_VHV_RECALIBRATE, 0x01)
# Set default ranging inter-measurement period to 100ms
self.write_byte(VL_SYSRANGE_INTERMEASUREMENT_PERIOD, 0x09)
# Set default ALS inter-measurement period to 500ms
self.write_byte(VL_SYSALS_INTERMEASUREMENT_PERIOD, 0x31)
# Configure interrupt on ‘New Sample Ready threshold event’
self.write_byte(VL_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x24)
self.write_byte(VL_SYSTEM_FRESH_OUT_OF_RESET, 0x00)
def read_distance(self):
self.write_byte(VL_SYSRANGE_START, 0x01)
value = None
i = 0
while i < 10:
i += 1
status = self.read_byte(VL_RESULT_INTERRUPT_STATUS_GPIO)
range_status = status & 0x07
if range_status == 0x04:
value = self.read_byte(VL_RESULT_RANGE_VAL)
break
time.sleep(0.1)
self.write_byte(VL_SYSTEM_INTERRUPT_CLEAR, 0x07)
self.logger.debug("Distance is %d mm", value)
return value
def read_lux(self, gain=VL_ALS_GAIN_1):
reg = self.read_byte(VL_SYSTEM_INTERRUPT_CONFIG_GPIO)
reg = reg & (~0x38)
reg = reg | (0x4 << 3)
self.write_byte(VL_SYSTEM_INTERRUPT_CONFIG_GPIO, reg)
integration_time = 100
self.write_byte(VL_SYSALS_INTEGRATION_PERIOD, 0)
self.write_byte(VL_SYSALS_INTEGRATION_PERIOD + 1, integration_time)
if gain > VL_ALS_GAIN_40:
gain = VL_ALS_GAIN_40
self.write_byte(VL_SYSALS_ANALOGUE_GAIN, 0x40 | gain)
# start ALS
self.write_byte(VL_SYSALS_START, 0x01)
value = None
i = 0
while i < 10:
i += 1
status = self.read_byte(VL_RESULT_INTERRUPT_STATUS_GPIO)
als_status = (status >> 3) & 0x07
if als_status == 0x04:
# reg = Eeprom16.address_to_bytes(VL_RESULT_ALS_VAL)
# self.bus.write_byte_data(self.address, *reg)
# data = self.bus.read_i2c_block_data(self.address, reg[0], 2)
# print data
value = self.read_word(VL_RESULT_ALS_VAL)
break
time.sleep(0.1)
self.write_byte(VL_SYSTEM_INTERRUPT_CLEAR, 0x07)
if value is None:
self.logger.warning("Failed to get lux value")
return None
# The factory calibrated ALS lux resolution is 0.32 lux/count
# for an analog gain of 1 & 100ms integration time (calibrated
# without glass).
lux = value * 0.32
analog_gain = {
VL_ALS_GAIN_1: 1.0,
VL_ALS_GAIN_1_25: 1.25,
VL_ALS_GAIN_1_67: 1.67,
VL_ALS_GAIN_2_5: 2.5,
VL_ALS_GAIN_5: 5.0,
VL_ALS_GAIN_10: 10.0,
VL_ALS_GAIN_20: 20.0,
VL_ALS_GAIN_40: 40.0,
}.get(gain)
lux = lux * 100 / (analog_gain * integration_time)
self.logger.debug("Lux is %d", lux)
return lux
if __name__ == "__main__":
logging.basicConfig(
level=logging.DEBUG,
format="%(name)-20s %(levelname)-8s %(message)s")
sensor = VL6180X(1)
sensor.read_lux(VL_ALS_GAIN_20)
sensor.read_distance()