1 | #include <Wire.h>
|
2 |
|
3 | #define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0xc0
|
4 | #define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0xc2
|
5 | #define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x50
|
6 | #define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
|
7 | #define VL53L0X_REG_SYSRANGE_START 0x00
|
8 | #define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x13
|
9 | #define VL53L0X_REG_RESULT_RANGE_STATUS 0x14
|
10 | #define address 0x29
|
11 |
|
12 | byte gbuf[16];
|
13 | uint16_t wert;
|
14 | uint16_t wert2;
|
15 | int d1;
|
16 | int d2;
|
17 |
|
18 | void setup() {
|
19 | Wire.begin(); // join i2c bus (address optional for master)
|
20 | Serial.begin(9600); // start serial for output
|
21 | Serial.println("VLX53LOX test started.");
|
22 | test();
|
23 | }
|
24 |
|
25 | void loop() {
|
26 | wert=get_dist();
|
27 | Serial.println(wert,DEC);
|
28 | delay(1100);
|
29 | }
|
30 |
|
31 | uint16_t get_dist(void) {
|
32 | write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);
|
33 |
|
34 | byte val = 0;
|
35 | int cnt = 0;
|
36 | while (cnt < 100) { // 1 second waiting time max
|
37 | delay(10);
|
38 | val = read_byte_data_at(VL53L0X_REG_RESULT_RANGE_STATUS);
|
39 | if (val & 0x01) break;
|
40 | cnt++;
|
41 | }
|
42 | if (val & 0x01) Serial.println("ready"); else Serial.println("not ready");
|
43 |
|
44 |
|
45 | read_block_data_at(0x14, 12);
|
46 | uint16_t acnt = makeuint16(gbuf[7], gbuf[6]);
|
47 | uint16_t scnt = makeuint16(gbuf[9], gbuf[8]);
|
48 | uint16_t dist = makeuint16(gbuf[11], gbuf[10]);
|
49 | byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);
|
50 |
|
51 | //Serial.print("distance "); Serial.println(dist);
|
52 | return dist;
|
53 | }
|
54 |
|
55 | void test() {
|
56 | byte val1 = read_byte_data_at(VL53L0X_REG_IDENTIFICATION_REVISION_ID);
|
57 | Serial.print("Revision ID: "); Serial.println(val1);
|
58 |
|
59 | val1 = read_byte_data_at(VL53L0X_REG_IDENTIFICATION_MODEL_ID);
|
60 | Serial.print("Device ID: "); Serial.println(val1);
|
61 |
|
62 | val1 = read_byte_data_at(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD);
|
63 | Serial.print("PRE_RANGE_CONFIG_VCSEL_PERIOD="); Serial.println(val1);
|
64 | Serial.print(" decode: "); Serial.println(VL53L0X_decode_vcsel_period(val1));
|
65 |
|
66 | val1 = read_byte_data_at(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD);
|
67 | Serial.print("FINAL_RANGE_CONFIG_VCSEL_PERIOD="); Serial.println(val1);
|
68 | Serial.print(" decode: "); Serial.println(VL53L0X_decode_vcsel_period(val1));
|
69 |
|
70 | write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);
|
71 |
|
72 | byte val = 0;
|
73 | int cnt = 0;
|
74 | while (cnt < 100) { // 1 second waiting time max
|
75 | delay(10);
|
76 | val = read_byte_data_at(VL53L0X_REG_RESULT_RANGE_STATUS);
|
77 | if (val & 0x01) break;
|
78 | cnt++;
|
79 | }
|
80 | if (val & 0x01) Serial.println("ready"); else Serial.println("not ready");
|
81 |
|
82 | read_block_data_at(0x14, 12);
|
83 | uint16_t acnt = makeuint16(gbuf[7], gbuf[6]);
|
84 | uint16_t scnt = makeuint16(gbuf[9], gbuf[8]);
|
85 | uint16_t dist = makeuint16(gbuf[11], gbuf[10]);
|
86 | byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);
|
87 |
|
88 | Serial.print("ambient count: "); Serial.println(acnt);
|
89 | Serial.print("signal count: "); Serial.println(scnt);
|
90 | Serial.print("distance "); Serial.println(dist);
|
91 | Serial.print("status: "); Serial.println(DeviceRangeStatusInternal);
|
92 | }
|
93 |
|
94 | uint16_t bswap(byte b[]) {
|
95 | // Big Endian unsigned short to little endian unsigned short
|
96 | uint16_t val = ((b[0] << 8) & b[1]);
|
97 | return val;
|
98 | }
|
99 |
|
100 | uint16_t makeuint16(int lsb, int msb) {
|
101 | return ((msb & 0xFF) << 8) | (lsb & 0xFF);
|
102 | }
|
103 |
|
104 | void write_byte_data(byte data) {
|
105 | Wire.beginTransmission(address);
|
106 | Wire.write(data);
|
107 | Wire.endTransmission();
|
108 | }
|
109 |
|
110 | void write_byte_data_at(byte reg, byte data) {
|
111 | // write data word at address and register
|
112 | Wire.beginTransmission(address);
|
113 | Wire.write(reg);
|
114 | Wire.write(data);
|
115 | Wire.endTransmission();
|
116 | }
|
117 |
|
118 | void write_word_data_at(byte reg, uint16_t data) {
|
119 | // write data word at address and register
|
120 | byte b0 = (data &0xFF);
|
121 | byte b1 = ((data >> 8) && 0xFF);
|
122 |
|
123 | Wire.beginTransmission(address);
|
124 | Wire.write(reg);
|
125 | Wire.write(b0);
|
126 | Wire.write(b1);
|
127 | Wire.endTransmission();
|
128 | }
|
129 |
|
130 | byte read_byte_data() {
|
131 | Wire.requestFrom(address, 1);
|
132 | while (Wire.available() < 1) delay(1);
|
133 | byte b = Wire.read();
|
134 | return b;
|
135 | }
|
136 |
|
137 | byte read_byte_data_at(byte reg) {
|
138 | //write_byte_data((byte)0x00);
|
139 | write_byte_data(reg);
|
140 | Wire.requestFrom(address, 1);
|
141 | while (Wire.available() < 1) delay(1);
|
142 | byte b = Wire.read();
|
143 | return b;
|
144 | }
|
145 |
|
146 | uint16_t read_word_data_at(byte reg) {
|
147 | write_byte_data(reg);
|
148 | Wire.requestFrom(address, 2);
|
149 | while (Wire.available() < 2) delay(1);
|
150 | gbuf[0] = Wire.read();
|
151 | gbuf[1] = Wire.read();
|
152 | return bswap(gbuf);
|
153 | }
|
154 |
|
155 | void read_block_data_at(byte reg, int sz) {
|
156 | int i = 0;
|
157 | write_byte_data(reg);
|
158 | Wire.requestFrom(address, sz);
|
159 | for (i=0; i<sz; i++) {
|
160 | while (Wire.available() < 1) delay(1);
|
161 | gbuf[i] = Wire.read();
|
162 | }
|
163 | }
|
164 |
|
165 |
|
166 | uint16_t VL53L0X_decode_vcsel_period(short vcsel_period_reg) {
|
167 | // Converts the encoded VCSEL period register value into the real
|
168 | // period in PLL clocks
|
169 | uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
|
170 | return vcsel_period_pclks;
|
171 | }
|
