-
Notifications
You must be signed in to change notification settings - Fork 0
/
vL53L0X.c
309 lines (277 loc) · 15.6 KB
/
vL53L0X.c
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
/*
* File: VL53L0X.c
* Author: Jamie
*
* Created on 01 December 2018, 19:57
*/
#include "globals.h"
#include "vL53L0X.h"
#include "i2C.h"
#define SYSRANGE_START 0x00
#define SYSTEM_SEQUENCE_CONFIG 0x01
#define SYSTEM_RANGE_CONFIG 0x09
#define SYSTEM_INTERRUPT_CONFIG_GPIO 0x0A
#define SYSTEM_INTERRUPT_CLEAR 0x0B
#define SYSTEM_THRESH_LOW 0x0E
#define INTERRUPT_STATUS 0x13
#define RANGE_RESULT_HI 0x1E
#define RANGE_RESULT_LO 0x1F
#define CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x20
#define RE_RANGE_CONFIG_MIN_SNR 0x27
#define ALGO_PHASECAL_LIM 0x30
#define GLOBAL_CONFIG_VCSEL_WIDTH 0x32
#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x44
#define MSRC_CONFIG_TIMEOUT_MACROP 0x46
#define FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x47
#define FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x48
#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E
#define DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F
#define PRE_RANGE_CONFIG_VCSEL_PERIOD 0x50
#define PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x51
#define PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x52
#define PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x56
#define PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x57
#define MSRC_CONFIG_CONTROL 0x60
#define PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0x61
#define PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0x62
#define PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x64
#define FINAL_RANGE_CONFIG_MIN_SNR 0x67
#define FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x71
#define FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x72
#define POWER_MANAGEMENT_GO1_POWER_FORCE 0x80
#define SYSTEM_HISTOGRAM_BIN 0x81
#define GPIO_HV_MUX_ACTIVE_HIGH 0x84
#define VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x89
#define SYSTEM_RANGE_CONFIG_4 0x94
#define RESULT_PEAK_SIGNAL_RATE_REF_0 0xB0
#define RESULT_PEAK_SIGNAL_RATE_REF_1 0xB1
#define RESULT_PEAK_SIGNAL_RATE_REF_2 0xB2
#define RESULT_PEAK_SIGNAL_RATE_REF_3 0xB3
#define RESULT_PEAK_SIGNAL_RATE_REF_4 0xB4
#define RESULT_PEAK_SIGNAL_RATE_REF_5 0xB5
#define GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6
/*
* Send data to VL5310X using I2C
*/
void vl53l0x_I2C_SendData(uint8_t slave_address, uint8_t device_register, uint8_t value) {
uint8_t data[2] = {device_register, value};
i2C_SendData(slave_address, data, 2);
}
/*
* Receive data from VL5310X using I2C
*/
uint8_t vl53l0x_I2C_ReceiveData(uint8_t slave_address, uint8_t device_register) {
uint8_t sendData[1] = {device_register};
uint8_t recievedData[1] = {0x00};
i2C_ReceiveData(slave_address, sendData, 1, StopStart, recievedData, 1);
return recievedData[0];
}
/*
* Setup VL5310X to desired settings using I2C
*/
void vl5310x_Setup(uint8_t slave_address) {
//TODO Find documentation that gives details and configure as desired (Possibly Peter knows?)
vl5310x_Initialisation(slave_address);
//SET TO CONTINEOUS AND HIGH ACCURACY
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 0x00);
vl53l0x_I2C_SendData(slave_address, 0x45, 0x0C); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_VALID_PHASE_LOW , 0x08);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_VCSEL_PERIOD, 0x08);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0x75); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, MSRC_CONFIG_TIMEOUT_MACROP, 0x1D);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x02);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0x8B);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSTEM_INTERRUPT_CLEAR, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0xE8);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
vl53l0x_I2C_SendData(slave_address, GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
vl53l0x_I2C_SendData(slave_address, 0x30, 0x07); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x30, 0x20); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VCSEL_PERIOD, 0x06);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x01);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0xD7);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x01);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0xD7);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0x02);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSTEM_INTERRUPT_CLEAR, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0xE8);
}
/*
* Setup VL5310X with initial values
*/
void vl5310x_Initialisation(uint8_t slave_address) {
vl53l0x_I2C_SendData(slave_address, VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0x01);
vl53l0x_I2C_SendData(slave_address, 0x88, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x01);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x00);
vl53l0x_I2C_SendData(slave_address, MSRC_CONFIG_CONTROL, 0x12);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 0x00);
vl53l0x_I2C_SendData(slave_address, 0x45, 0x20); // UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0xFF);
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x01);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x06); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x83, 0x05); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x07); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSTEM_HISTOGRAM_BIN, 0x01);
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSTEM_RANGE_CONFIG_4, 0x6B);
vl53l0x_I2C_SendData(slave_address, 0x83, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x83, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSTEM_HISTOGRAM_BIN, 0x00);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x06); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x83, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x00);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
vl53l0x_I2C_SendData(slave_address, DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
vl53l0x_I2C_SendData(slave_address, RESULT_PEAK_SIGNAL_RATE_REF_0, 0x00);
vl53l0x_I2C_SendData(slave_address, RESULT_PEAK_SIGNAL_RATE_REF_1, 0xF0);
vl53l0x_I2C_SendData(slave_address, RESULT_PEAK_SIGNAL_RATE_REF_2, 0xFF);
vl53l0x_I2C_SendData(slave_address, RESULT_PEAK_SIGNAL_RATE_REF_3, 0x1E);
vl53l0x_I2C_SendData(slave_address, RESULT_PEAK_SIGNAL_RATE_REF_4, 0x00);
vl53l0x_I2C_SendData(slave_address, RESULT_PEAK_SIGNAL_RATE_REF_5, 0x00);
vl5310x_Tuning(slave_address);
vl53l0x_I2C_SendData(slave_address, SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
vl53l0x_I2C_SendData(slave_address, GPIO_HV_MUX_ACTIVE_HIGH, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSTEM_INTERRUPT_CLEAR, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0xE8);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x02);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0x94);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x41);
while ((vl53l0x_I2C_ReceiveData(slave_address, INTERRUPT_STATUS) & 0x07) == 0);
vl53l0x_I2C_SendData(slave_address, SYSTEM_INTERRUPT_CLEAR, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0x02);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
while ((vl53l0x_I2C_ReceiveData(slave_address, INTERRUPT_STATUS) & 0x07) == 0);
vl53l0x_I2C_SendData(slave_address, SYSTEM_INTERRUPT_CLEAR, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0xE8);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x05);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0x9A);
}
/*
* Setup VL5310X to recommended tuning values
*/
void vl5310x_Tuning(uint8_t slave_address) {
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSTEM_RANGE_CONFIG, 0x00);
vl53l0x_I2C_SendData(slave_address, 0x10, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x11, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x24, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x25, 0xFF); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x75, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x00);
vl53l0x_I2C_SendData(slave_address, ALGO_PHASECAL_LIM, 0x20);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, ALGO_PHASECAL_LIM, 0x09);
vl53l0x_I2C_SendData(slave_address, 0x54, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x31, 0x04); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
vl53l0x_I2C_SendData(slave_address, 0x40, 0x83); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, MSRC_CONFIG_TIMEOUT_MACROP, 0x25);
vl53l0x_I2C_SendData(slave_address, MSRC_CONFIG_CONTROL, 0x00);
vl53l0x_I2C_SendData(slave_address, RE_RANGE_CONFIG_MIN_SNR, 0x00);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_VCSEL_PERIOD, 0x06);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x00);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0x96);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_SIGMA_THRESH_HI, 0x00);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_CONFIG_SIGMA_THRESH_LO , 0x00);
vl53l0x_I2C_SendData(slave_address, PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, 0x00);
vl53l0x_I2C_SendData(slave_address, 0x65, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x66, 0xA0); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x22, 0x32); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x14);
vl53l0x_I2C_SendData(slave_address, 0x49, 0xFF); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x4A, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x7A, 0x0A); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x7B, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x78, 0x21); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x23, 0x34); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x42, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 0xFF);
vl53l0x_I2C_SendData(slave_address, 0x45, 0x26); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, MSRC_CONFIG_TIMEOUT_MACROP, 0x05);
vl53l0x_I2C_SendData(slave_address, 0x40, 0x40); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSTEM_THRESH_LOW, 0x06);
vl53l0x_I2C_SendData(slave_address, CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, 0x1A);
vl53l0x_I2C_SendData(slave_address, 0x43, 0x40); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x34, 0x03); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x35, 0x44); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x31, 0x04); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x4B, 0x09); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x4C, 0x05); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x4D, 0x04); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 0x00);
vl53l0x_I2C_SendData(slave_address, 0x45, 0x20); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_MIN_SNR, 0x00);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_VCSEL_PERIOD, 0x04);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 0x01);
vl53l0x_I2C_SendData(slave_address, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO, 0xFE);
vl53l0x_I2C_SendData(slave_address, 0x76, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x77, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x0D, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x01);
vl53l0x_I2C_SendData(slave_address, SYSTEM_SEQUENCE_CONFIG, 0xF8);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, 0x8E, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x00);
}
/*
* Read range from VL5310X
*/
uint16_t vl5310x_ReadRange(uint8_t slave_address) {
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x01);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x01); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x00);
vl53l0x_I2C_SendData(slave_address, 0x91, 0x3C); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
vl53l0x_I2C_SendData(slave_address, 0xFF, 0x00); //UNKNOWN
vl53l0x_I2C_SendData(slave_address, POWER_MANAGEMENT_GO1_POWER_FORCE, 0x00);
vl53l0x_I2C_SendData(slave_address, SYSRANGE_START, 0x01);
while ((vl53l0x_I2C_ReceiveData(slave_address, INTERRUPT_STATUS) & 0x07) == 0);
uint8_t valueHI = vl53l0x_I2C_ReceiveData(slave_address, RANGE_RESULT_HI);
uint8_t valueLO = vl53l0x_I2C_ReceiveData(slave_address, RANGE_RESULT_LO);
vl53l0x_I2C_SendData(slave_address, SYSTEM_INTERRUPT_CLEAR, 0x01);
return (valueHI << 8) + valueLO;
}