collab/pb-hal

git clone https://git.julia.blue/collab/pb-hal

Joseph RyanAdd prep code for integrating working init routine64c6e28

tof
6.6 KiB153 linesraw
1#include <pigpio.h>
2#include <stdio.h>
3#include "I2Cdev.h"
4
5const short __VL6180X_IDENTIFICATION_MODEL_ID               = 0x0000;
6const short __VL6180X_IDENTIFICATION_MODEL_REV_MAJOR        = 0x0001;
7const short __VL6180X_IDENTIFICATION_MODEL_REV_MINOR        = 0x0002;
8const short __VL6180X_IDENTIFICATION_MODULE_REV_MAJOR       = 0x0003;
9const short __VL6180X_IDENTIFICATION_MODULE_REV_MINOR       = 0x0004;
10const short __VL6180X_IDENTIFICATION_DATE                   = 0x0006;
11const short __VL6180X_IDENTIFICATION_TIME                   = 0x0008;
12
13const short __VL6180X_SYSTEM_MODE_GPIO0                     = 0x0010;
14const short __VL6180X_SYSTEM_MODE_GPIO1                     = 0x0011;
15const short __VL6180X_SYSTEM_HISTORY_CTRL                   = 0x0012;
16const short __VL6180X_SYSTEM_INTERRUPT_CONFIG_GPIO          = 0x0014;
17const short __VL6180X_SYSTEM_INTERRUPT_CLEAR                = 0x0015;
18const short __VL6180X_SYSTEM_FRESH_OUT_OF_RESET             = 0x0016;
19const short __VL6180X_SYSTEM_GROUPED_PARAMETER_HOLD         = 0x0017;
20
21const short __VL6180X_SYSRANGE_START                        = 0x0018;
22const short __VL6180X_SYSRANGE_THRESH_HIGH                  = 0x0019;
23const short __VL6180X_SYSRANGE_THRESH_LOW                   = 0x001A;
24const short __VL6180X_SYSRANGE_INTERMEASUREMENT_PERIOD      = 0x001B;
25const short __VL6180X_SYSRANGE_MAX_CONVERGENCE_TIME         = 0x001C;
26const short __VL6180X_SYSRANGE_CROSSTALK_COMPENSATION_RATE  = 0x001E;
27const short __VL6180X_SYSRANGE_CROSSTALK_VALID_HEIGHT       = 0x0021;
28const short __VL6180X_SYSRANGE_EARLY_CONVERGENCE_ESTIMATE   = 0x0022;
29const short __VL6180X_SYSRANGE_PART_TO_PART_RANGE_OFFSET    = 0x0024;
30const short __VL6180X_SYSRANGE_RANGE_IGNORE_VALID_HEIGHT    = 0x0025;
31const short __VL6180X_SYSRANGE_RANGE_IGNORE_THRESHOLD       = 0x0026;
32const short __VL6180X_SYSRANGE_MAX_AMBIENT_LEVEL_MULT       = 0x002C;
33const short __VL6180X_SYSRANGE_RANGE_CHECK_ENABLES          = 0x002D;
34const short __VL6180X_SYSRANGE_VHV_RECALIBRATE              = 0x002E;
35const short __VL6180X_SYSRANGE_VHV_REPEAT_RATE              = 0x0031;
36
37const short __VL6180X_SYSALS_START                          = 0x0038;
38const short __VL6180X_SYSALS_THRESH_HIGH                    = 0x003A;
39const short __VL6180X_SYSALS_THRESH_LOW                     = 0x003C;
40const short __VL6180X_SYSALS_INTERMEASUREMENT_PERIOD        = 0x003E;
41const short __VL6180X_SYSALS_ANALOGUE_GAIN                  = 0x003F;
42const short __VL6180X_SYSALS_INTEGRATION_PERIOD             = 0x0040;
43
44const short __VL6180X_RESULT_RANGE_STATUS                   = 0x004D;
45const short __VL6180X_RESULT_ALS_STATUS                     = 0x004E;
46const short __VL6180X_RESULT_INTERRUPT_STATUS_GPIO          = 0x004F;
47const short __VL6180X_RESULT_ALS_VAL                        = 0x0050;
48const short __VL6180X_RESULT_HISTORY_BUFFER                 = 0x0052;
49const short __VL6180X_RESULT_RANGE_VAL                      = 0x0062;
50const short __VL6180X_RESULT_RANGE_RAW                      = 0x0064;
51const short __VL6180X_RESULT_RANGE_RETURN_RATE              = 0x0066;
52const short __VL6180X_RESULT_RANGE_REFERENCE_RATE           = 0x0068;
53const short __VL6180X_RESULT_RANGE_RETURN_SIGNAL_COUNT      = 0x006C;
54const short __VL6180X_RESULT_RANGE_REFERENCE_SIGNAL_COUNT   = 0x0070;
55const short __VL6180X_RESULT_RANGE_RETURN_AMB_COUNT         = 0x0074;
56const short __VL6180X_RESULT_RANGE_REFERENCE_AMB_COUNT      = 0x0078;
57const short __VL6180X_RESULT_RANGE_RETURN_CONV_TIME         = 0x007C;
58const short __VL6180X_RESULT_RANGE_REFERENCE_CONV_TIME      = 0x0080;
59
60const short __VL6180X_READOUT_AVERAGING_SAMPLE_PERIOD       = 0x010A;
61const short __VL6180X_FIRMWARE_BOOTUP                       = 0x0119;
62const short __VL6180X_FIRMWARE_RESULT_SCALER                = 0x0120;
63const short __VL6180X_I2C_SLAVE_DEVICE_ADDRESS              = 0x0212;
64const short __VL6180X_INTERLEAVED_MODE_ENABLE               = 0x02A3;
65
66int enablePins[4] = {12, 23, 24, 24};
67int sensors[4] = {0x29, 0x2a, 0x2b, 0x2c};
68
69// first list is addresses, second is values
70int initPairs[2][29] = {
71    {
72        0x208, 0x96, 0x97, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xf5, 0xd9,
73        0xdb, 0xdc, 0xdd, 0x9f, 0xa3, 0xb7, 0xbb, 0xb2, 0xca, 0x198,
74        0x1b0, 0x1ad, 0xff, 0x100, 0x199, 0x1a6, 0x1ac, 0x1a7, 0x30
75    },
76    {
77        0x01, 0x00, 0xfd, 0x00, 0x04, 0x02, 0x01, 0x03, 0x02, 0x05,
78        0xce, 0x03, 0xf8, 0x00, 0x3c, 0x00, 0x3c, 0x09, 0x09, 0x01,
79        0x17, 0x00, 0x05, 0x05, 0x05, 0x1b, 0x3e, 0x1f, 0x00
80    }
81};
82
83int WriteByte(unsigned addr, uint16_t reg, uint8_t data) {
84    //printf("register: %i, data: %i\n", reg, data);
85    return I2Cdev::writeBytes(addr, reg, 1, &data);
86
87}
88
89char ReadByte(unsigned addr, uint16_t reg) {
90    uint8_t data_read;
91    I2Cdev::readBytes(addr, reg, 1, &data_read);
92    return data_read;
93}
94
95//reinitialize all distance sensors to address 0x29
96void resetAddr() {
97    for(int i = 0; i < 4; ++i) {
98        gpioWrite(enablePins[i], 1);
99    }
100    for(int i = 0; i < 4; ++i) {
101        WriteByte(sensors[i], __VL6180X_I2C_SLAVE_DEVICE_ADDRESS, 0x29);
102    }
103}
104
105void initSensor(sensor) {
106    //enable one sensor at a time
107    printf("Sensor %d enabling\n", sensor);
108    gpioWrite(enablePins[sensor], 1);
109    //i2cReadByteData(sensor, __VL6180X_SYSTEM_FRESH_OUT_OF_RESET);
110    // proprietary setup stuff
111    for(int j = 0; j < 29; ++j) {
112        WriteByte(0x29, initPairs[0][sensor], initPairs[1][sensor]);
113    }
114    // recommended setup stuff
115    // WriteByte(sensor, __VL6180X_SYSTEM_MODE_GPIO1, 0x10);
116    WriteByte(0x29, __VL6180X_READOUT_AVERAGING_SAMPLE_PERIOD, 0x30);
117    WriteByte(0x29, __VL6180X_SYSALS_ANALOGUE_GAIN, 0x46);
118    WriteByte(0x29, __VL6180X_SYSRANGE_VHV_REPEAT_RATE, 0xff);
119    WriteByte(0x29, __VL6180X_SYSALS_INTEGRATION_PERIOD, 0x63);
120    WriteByte(0x29, __VL6180X_SYSRANGE_VHV_RECALIBRATE, 0x01);
121    // optional settings todo: add more settings
122    WriteByte(0x29, 0x001b, 0x09);
123    WriteByte(0x29, 0x003e, 0x31);
124    WriteByte(0x29, 0x0014, 0x24);
125    // assign new register
126    WriteByte(0x29, __VL6180X_I2C_SLAVE_DEVICE_ADDRESS, sensors[sensor]);
127    printf("finished sensor %d\n", i);
128}
129
130int main() {
131    gpioInitialise();
132    I2Cdev::initialize();
133    I2Cdev::enable(true);
134    resetAddr();
135    //must open address that tof is currently on
136    for(int i = 0; i < 4; ++i) {
137        initSensor(i);
138    }
139    while(1) {
140        for(int i = 0; i < 4; ++i) {
141            // wait until ready
142            //while(!ReadByte(sensor1, 0x004d) & (1<<0));
143            // start measurment 1
144            WriteByte(sensors[i], 0x0018, 0x01);
145            //while(!ReadByte(sensor1, 0x004f) & (1<<2));
146            int reading = ReadByte(sensors[i], 0x0062);
147            printf("%04dmm\t", reading);
148        }
149        printf("\n");
150    }
151    return 0;
152}
153