Arnaud VALLEY / Mbed 2 deprecated Pinscape_Controller_V2_arnoz

Mirror with some correction

Dependencies:   mbed FastIO FastPWM USBDevice

VL6180X/VL6180X.cpp@116:7a67265d7c19, 2021-10-01 (annotated)

Committer:
arnoz
Date:
Fri Oct 01 08:19:46 2021 +0000
Revision:
116:7a67265d7c19
Parent:
87:8d35c74403af 
- Correct information regarding your last merge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mjr 82:4f6209cb5c33 1 // VL6180X Time of Flight sensor interface
mjr 82:4f6209cb5c33 2
mjr 82:4f6209cb5c33 3 #include "mbed.h"
mjr 82:4f6209cb5c33 4 #include "VL6180X.h"
mjr 82:4f6209cb5c33 5
mjr 87:8d35c74403af 6 VL6180X::VL6180X(PinName sda, PinName scl, uint8_t addr, PinName gpio0,
mjr 87:8d35c74403af 7 bool internalPullups)
mjr 87:8d35c74403af 8 : i2c(sda, scl, internalPullups), gpio0Pin(gpio0)
mjr 82:4f6209cb5c33 9 {
mjr 82:4f6209cb5c33 10 // remember the address
mjr 82:4f6209cb5c33 11 this->addr = addr;
mjr 82:4f6209cb5c33 12
mjr 82:4f6209cb5c33 13 // start in single-shot distance mode
mjr 82:4f6209cb5c33 14 distMode = 0;
mjr 87:8d35c74403af 15 rangeStarted = false;
mjr 82:4f6209cb5c33 16
mjr 87:8d35c74403af 17 // initially reset the sensor by holding GPIO0/CE low
mjr 87:8d35c74403af 18 gpio0Pin.mode(PullNone);
mjr 85:3c28aee81cde 19 gpio0Pin.output();
mjr 82:4f6209cb5c33 20 gpio0Pin.write(0);
mjr 82:4f6209cb5c33 21 }
mjr 82:4f6209cb5c33 22
mjr 82:4f6209cb5c33 23 VL6180X::~VL6180X()
mjr 82:4f6209cb5c33 24 {
mjr 82:4f6209cb5c33 25 }
mjr 82:4f6209cb5c33 26
mjr 82:4f6209cb5c33 27 bool VL6180X::init()
mjr 82:4f6209cb5c33 28 {
mjr 82:4f6209cb5c33 29 // hold reset low for 10ms
mjr 85:3c28aee81cde 30 gpio0Pin.output();
mjr 82:4f6209cb5c33 31 gpio0Pin.write(0);
mjr 82:4f6209cb5c33 32 wait_us(10000);
mjr 82:4f6209cb5c33 33
mjr 87:8d35c74403af 34 // release reset and allow 10ms for the sensor to reboot
mjr 85:3c28aee81cde 35 gpio0Pin.input();
mjr 82:4f6209cb5c33 36 wait_us(10000);
mjr 87:8d35c74403af 37
mjr 82:4f6209cb5c33 38 // reset the I2C bus
mjr 82:4f6209cb5c33 39 i2c.reset();
mjr 82:4f6209cb5c33 40
mjr 82:4f6209cb5c33 41 // check that the sensor's reset register reads as '1'
mjr 82:4f6209cb5c33 42 Timer t;
mjr 82:4f6209cb5c33 43 t.start();
mjr 82:4f6209cb5c33 44 while (readReg8(VL6180X_SYSTEM_FRESH_OUT_OF_RESET) != 1)
mjr 82:4f6209cb5c33 45 {
mjr 87:8d35c74403af 46 if (t.read_us() > 1000000)
mjr 82:4f6209cb5c33 47 return false;
mjr 82:4f6209cb5c33 48 }
mjr 82:4f6209cb5c33 49
mjr 82:4f6209cb5c33 50 // clear reset flag
mjr 82:4f6209cb5c33 51 writeReg8(VL6180X_SYSTEM_FRESH_OUT_OF_RESET, 0);
mjr 82:4f6209cb5c33 52
mjr 82:4f6209cb5c33 53 // give the device 50ms before sending the startup sequence
mjr 82:4f6209cb5c33 54 wait_ms(50);
mjr 82:4f6209cb5c33 55
mjr 82:4f6209cb5c33 56 // Send the mandatory initial register assignments, per the manufacturer's app notes:
mjr 82:4f6209cb5c33 57 // http://www.st.com/st-web-ui/static/active/en/resource/technical/document/application_note/DM00122600.pdf
mjr 82:4f6209cb5c33 58 writeReg8(0x0207, 0x01);
mjr 82:4f6209cb5c33 59 writeReg8(0x0208, 0x01);
mjr 82:4f6209cb5c33 60 writeReg8(0x0096, 0x00);
mjr 82:4f6209cb5c33 61 writeReg8(0x0097, 0xfd);
mjr 82:4f6209cb5c33 62 writeReg8(0x00e3, 0x00);
mjr 82:4f6209cb5c33 63 writeReg8(0x00e4, 0x04);
mjr 82:4f6209cb5c33 64 writeReg8(0x00e5, 0x02);
mjr 82:4f6209cb5c33 65 writeReg8(0x00e6, 0x01);
mjr 82:4f6209cb5c33 66 writeReg8(0x00e7, 0x03);
mjr 82:4f6209cb5c33 67 writeReg8(0x00f5, 0x02);
mjr 82:4f6209cb5c33 68 writeReg8(0x00d9, 0x05);
mjr 82:4f6209cb5c33 69 writeReg8(0x00db, 0xce);
mjr 82:4f6209cb5c33 70 writeReg8(0x00dc, 0x03);
mjr 82:4f6209cb5c33 71 writeReg8(0x00dd, 0xf8);
mjr 82:4f6209cb5c33 72 writeReg8(0x009f, 0x00);
mjr 82:4f6209cb5c33 73 writeReg8(0x00a3, 0x3c);
mjr 82:4f6209cb5c33 74 writeReg8(0x00b7, 0x00);
mjr 82:4f6209cb5c33 75 writeReg8(0x00bb, 0x3c);
mjr 82:4f6209cb5c33 76 writeReg8(0x00b2, 0x09);
mjr 82:4f6209cb5c33 77 writeReg8(0x00ca, 0x09);
mjr 82:4f6209cb5c33 78 writeReg8(0x0198, 0x01);
mjr 82:4f6209cb5c33 79 writeReg8(0x01b0, 0x17);
mjr 82:4f6209cb5c33 80 writeReg8(0x01ad, 0x00);
mjr 82:4f6209cb5c33 81 writeReg8(0x00ff, 0x05);
mjr 82:4f6209cb5c33 82 writeReg8(0x0100, 0x05);
mjr 82:4f6209cb5c33 83 writeReg8(0x0199, 0x05);
mjr 82:4f6209cb5c33 84 writeReg8(0x01a6, 0x1b);
mjr 82:4f6209cb5c33 85 writeReg8(0x01ac, 0x3e);
mjr 82:4f6209cb5c33 86 writeReg8(0x01a7, 0x1f);
mjr 82:4f6209cb5c33 87 writeReg8(0x0030, 0x00);
mjr 82:4f6209cb5c33 88
mjr 82:4f6209cb5c33 89 // allow time to settle
mjr 82:4f6209cb5c33 90 wait_us(1000);
mjr 87:8d35c74403af 91
mjr 87:8d35c74403af 92 // start the sample timer
mjr 87:8d35c74403af 93 sampleTimer.start();
mjr 87:8d35c74403af 94
mjr 82:4f6209cb5c33 95 // success
mjr 82:4f6209cb5c33 96 return true;
mjr 82:4f6209cb5c33 97 }
mjr 82:4f6209cb5c33 98
mjr 82:4f6209cb5c33 99 void VL6180X::setDefaults()
mjr 82:4f6209cb5c33 100 {
mjr 82:4f6209cb5c33 101 writeReg8(VL6180X_SYSTEM_GROUPED_PARAMETER_HOLD, 0x01); // set parameter hold while updating settings
mjr 82:4f6209cb5c33 102
mjr 87:8d35c74403af 103 writeReg8(VL6180X_SYSTEM_INTERRUPT_CONFIG_GPIO, 4); // Enable interrupts from range only
mjr 87:8d35c74403af 104 writeReg8(VL6180X_SYSTEM_MODE_GPIO1, 0x00); // Disable GPIO1
mjr 82:4f6209cb5c33 105 writeReg8(VL6180X_SYSRANGE_VHV_REPEAT_RATE, 0xFF); // Set auto calibration period (Max = 255)/(OFF = 0)
mjr 82:4f6209cb5c33 106 writeReg8(VL6180X_SYSRANGE_INTERMEASUREMENT_PERIOD, 0x09); // Set default ranging inter-measurement period to 100ms
mjr 87:8d35c74403af 107 writeReg8(VL6180X_SYSRANGE_MAX_CONVERGENCE_TIME, 63); // Max range convergence time 63ms
mjr 87:8d35c74403af 108 writeReg8(VL6180X_SYSRANGE_RANGE_CHECK_ENABLES, 0x00); // S/N disable, ignore disable, early convergence test disable
mjr 87:8d35c74403af 109 writeReg16(VL6180X_SYSRANGE_EARLY_CONVERGENCE_ESTIMATE, 0x00); // abort range measurement if convergence rate below this value
mjr 87:8d35c74403af 110 writeReg8(VL6180X_READOUT_AVERAGING_SAMPLE_PERIOD, averagingSamplePeriod); // Sample averaging period (1.3ms + N*64.5us)
mjr 87:8d35c74403af 111 writeReg8(VL6180X_SYSRANGE_THRESH_LOW, 0x00); // low threshold
mjr 87:8d35c74403af 112 writeReg8(VL6180X_SYSRANGE_THRESH_HIGH, 0xff); // high threshold
mjr 82:4f6209cb5c33 113
mjr 82:4f6209cb5c33 114 writeReg8(VL6180X_SYSTEM_GROUPED_PARAMETER_HOLD, 0x00); // end parameter hold
mjr 82:4f6209cb5c33 115
mjr 82:4f6209cb5c33 116 // perform a single calibration; wait until it's done (within reason)
mjr 82:4f6209cb5c33 117 Timer t;
mjr 82:4f6209cb5c33 118 t.start();
mjr 82:4f6209cb5c33 119 writeReg8(VL6180X_SYSRANGE_VHV_RECALIBRATE, 0x01);
mjr 82:4f6209cb5c33 120 while (readReg8(VL6180X_SYSRANGE_VHV_RECALIBRATE) != 0)
mjr 82:4f6209cb5c33 121 {
mjr 82:4f6209cb5c33 122 // if we've been waiting too long, abort
mjr 87:8d35c74403af 123 if (t.read_us() > 100000)
mjr 82:4f6209cb5c33 124 break;
mjr 82:4f6209cb5c33 125 }
mjr 82:4f6209cb5c33 126 }
mjr 82:4f6209cb5c33 127
mjr 82:4f6209cb5c33 128 void VL6180X::getID(struct VL6180X_ID &id)
mjr 82:4f6209cb5c33 129 {
mjr 82:4f6209cb5c33 130 id.model = readReg8(VL6180X_IDENTIFICATION_MODEL_ID);
mjr 82:4f6209cb5c33 131 id.modelRevMajor = readReg8(VL6180X_IDENTIFICATION_MODEL_REV_MAJOR) & 0x07;
mjr 82:4f6209cb5c33 132 id.modelRevMinor = readReg8(VL6180X_IDENTIFICATION_MODEL_REV_MINOR) & 0x07;
mjr 82:4f6209cb5c33 133 id.moduleRevMajor = readReg8(VL6180X_IDENTIFICATION_MODULE_REV_MAJOR) & 0x07;
mjr 82:4f6209cb5c33 134 id.moduleRevMinor = readReg8(VL6180X_IDENTIFICATION_MODULE_REV_MINOR) & 0x07;
mjr 82:4f6209cb5c33 135
mjr 82:4f6209cb5c33 136 uint16_t date = readReg16(VL6180X_IDENTIFICATION_DATE);
mjr 82:4f6209cb5c33 137 uint16_t time = readReg16(VL6180X_IDENTIFICATION_TIME) * 2;
mjr 82:4f6209cb5c33 138 id.manufDate.year = 2010 + ((date >> 12) & 0x0f);
mjr 82:4f6209cb5c33 139 id.manufDate.month = (date >> 8) & 0x0f;
mjr 82:4f6209cb5c33 140 id.manufDate.day = (date >> 3) & 0x1f;
mjr 82:4f6209cb5c33 141 id.manufDate.phase = uint8_t(date & 0x07);
mjr 82:4f6209cb5c33 142 id.manufDate.hh = time/3600;
mjr 82:4f6209cb5c33 143 id.manufDate.mm = (time % 3600) / 60;
mjr 82:4f6209cb5c33 144 id.manufDate.ss = time % 60;
mjr 82:4f6209cb5c33 145 }
mjr 82:4f6209cb5c33 146
mjr 82:4f6209cb5c33 147 void VL6180X::continuousDistanceMode(bool on)
mjr 82:4f6209cb5c33 148 {
mjr 82:4f6209cb5c33 149 if (distMode != on)
mjr 82:4f6209cb5c33 150 {
mjr 82:4f6209cb5c33 151 // remember the new mode
mjr 82:4f6209cb5c33 152 distMode = on;
mjr 82:4f6209cb5c33 153
mjr 82:4f6209cb5c33 154 // Set continuous or single-shot mode. If starting continuous
mjr 82:4f6209cb5c33 155 // mode, set bits 0x01 (range mode = continuous) + 0x02 (start
mjr 82:4f6209cb5c33 156 // collecting samples now). If ending the mode, set all bits
mjr 82:4f6209cb5c33 157 // to zero to select single-shot mode without starting a reading.
mjr 82:4f6209cb5c33 158 if (on)
mjr 82:4f6209cb5c33 159 {
mjr 82:4f6209cb5c33 160 writeReg8(VL6180X_SYSTEM_INTERRUPT_CONFIG_GPIO, 4); // Enable interrupts for ranging only
mjr 82:4f6209cb5c33 161 writeReg8(VL6180X_SYSALS_INTERMEASUREMENT_PERIOD, 0); // minimum measurement interval (10ms)
mjr 82:4f6209cb5c33 162 writeReg8(VL6180X_SYSRANGE_START, 0x03);
mjr 82:4f6209cb5c33 163 }
mjr 82:4f6209cb5c33 164 else
mjr 82:4f6209cb5c33 165 writeReg8(VL6180X_SYSRANGE_START, 0x00);
mjr 82:4f6209cb5c33 166 }
mjr 82:4f6209cb5c33 167 }
mjr 82:4f6209cb5c33 168
mjr 82:4f6209cb5c33 169 bool VL6180X::rangeReady()
mjr 82:4f6209cb5c33 170 {
mjr 87:8d35c74403af 171 // check if the status register says a sample is ready (bits 0-2/0x07)
mjr 87:8d35c74403af 172 // or an error has occurred (bits 6-7/0xC0)
mjr 87:8d35c74403af 173 return ((readReg8(VL6180X_RESULT_INTERRUPT_STATUS_GPIO) & 0xC7) != 0);
mjr 82:4f6209cb5c33 174 }
mjr 82:4f6209cb5c33 175
mjr 82:4f6209cb5c33 176 void VL6180X::startRangeReading()
mjr 82:4f6209cb5c33 177 {
mjr 87:8d35c74403af 178 // start a new range reading if one isn't already in progress
mjr 87:8d35c74403af 179 if (!rangeStarted)
mjr 87:8d35c74403af 180 {
mjr 87:8d35c74403af 181 tSampleStart = sampleTimer.read_us();
mjr 87:8d35c74403af 182 writeReg8(VL6180X_SYSTEM_INTERRUPT_CLEAR, 0x07);
mjr 87:8d35c74403af 183 writeReg8(VL6180X_SYSRANGE_START, 0x00);
mjr 87:8d35c74403af 184 writeReg8(VL6180X_SYSRANGE_START, 0x01);
mjr 87:8d35c74403af 185 rangeStarted = true;
mjr 87:8d35c74403af 186 }
mjr 82:4f6209cb5c33 187 }
mjr 82:4f6209cb5c33 188
mjr 87:8d35c74403af 189 int VL6180X::getRange(uint8_t &distance, uint32_t &tMid, uint32_t &dt, uint32_t timeout_us)
mjr 82:4f6209cb5c33 190 {
mjr 87:8d35c74403af 191 // start a reading if one isn't already in progress
mjr 87:8d35c74403af 192 startRangeReading();
mjr 87:8d35c74403af 193
mjr 87:8d35c74403af 194 // we're going to wait until this reading ends, so consider the
mjr 87:8d35c74403af 195 // 'start' command consumed, no matter what happens next
mjr 87:8d35c74403af 196 rangeStarted = false;
mjr 82:4f6209cb5c33 197
mjr 82:4f6209cb5c33 198 // wait for the sample
mjr 82:4f6209cb5c33 199 Timer t;
mjr 82:4f6209cb5c33 200 t.start();
mjr 82:4f6209cb5c33 201 for (;;)
mjr 82:4f6209cb5c33 202 {
mjr 87:8d35c74403af 203 // check for a sample
mjr 82:4f6209cb5c33 204 if (rangeReady())
mjr 82:4f6209cb5c33 205 break;
mjr 82:4f6209cb5c33 206
mjr 82:4f6209cb5c33 207 // if we've exceeded the timeout, return failure
mjr 82:4f6209cb5c33 208 if (t.read_us() > timeout_us)
mjr 87:8d35c74403af 209 {
mjr 87:8d35c74403af 210 writeReg8(VL6180X_SYSRANGE_START, 0x00);
mjr 82:4f6209cb5c33 211 return -1;
mjr 87:8d35c74403af 212 }
mjr 82:4f6209cb5c33 213 }
mjr 82:4f6209cb5c33 214
mjr 82:4f6209cb5c33 215 // check for errors
mjr 82:4f6209cb5c33 216 uint8_t err = (readReg8(VL6180X_RESULT_RANGE_STATUS) >> 4) & 0x0F;
mjr 82:4f6209cb5c33 217
mjr 82:4f6209cb5c33 218 // read the distance
mjr 82:4f6209cb5c33 219 distance = readReg8(VL6180X_RESULT_RANGE_VAL);
mjr 82:4f6209cb5c33 220
mjr 87:8d35c74403af 221 // Read the convergence time, and compute the overall sample time.
mjr 87:8d35c74403af 222 // Per the data sheet, the total execution time is the sum of the
mjr 87:8d35c74403af 223 // fixed 3.2ms pre-calculation time, the convergence time, and the
mjr 87:8d35c74403af 224 // readout averaging time. We can query the convergence time for
mjr 87:8d35c74403af 225 // each reading from the sensor. The averaging time is a controlled
mjr 87:8d35c74403af 226 // by the READOUT_AVERAGING_SAMPLE_PERIOD setting, which we set to
mjr 87:8d35c74403af 227 // our constant value averagingSamplePeriod.
mjr 87:8d35c74403af 228 dt =
mjr 87:8d35c74403af 229 3200 // fixed 3.2ms pre-calculation period
mjr 87:8d35c74403af 230 + readReg32(VL6180X_RESULT_RANGE_RETURN_CONV_TIME) // convergence time
mjr 87:8d35c74403af 231 + (1300 + 48*averagingSamplePeriod); // readout averaging period
mjr 87:8d35c74403af 232
mjr 87:8d35c74403af 233 // figure the midpoint of the sample time - the starting time
mjr 87:8d35c74403af 234 // plus half the collection time
mjr 87:8d35c74403af 235 tMid = tSampleStart + dt/2;
mjr 87:8d35c74403af 236
mjr 82:4f6209cb5c33 237 // clear the data-ready interrupt
mjr 82:4f6209cb5c33 238 writeReg8(VL6180X_SYSTEM_INTERRUPT_CLEAR, 0x07);
mjr 82:4f6209cb5c33 239
mjr 82:4f6209cb5c33 240 // return the error code
mjr 82:4f6209cb5c33 241 return err;
mjr 82:4f6209cb5c33 242 }
mjr 82:4f6209cb5c33 243
mjr 82:4f6209cb5c33 244 void VL6180X::getRangeStats(VL6180X_RangeStats &stats)
mjr 82:4f6209cb5c33 245 {
mjr 82:4f6209cb5c33 246 stats.returnRate = readReg16(VL6180X_RESULT_RANGE_RETURN_RATE);
mjr 82:4f6209cb5c33 247 stats.refReturnRate = readReg16(VL6180X_RESULT_RANGE_REFERENCE_RATE);
mjr 82:4f6209cb5c33 248 stats.returnCnt = readReg32(VL6180X_RESULT_RANGE_RETURN_SIGNAL_COUNT);
mjr 82:4f6209cb5c33 249 stats.refReturnCnt = readReg32(VL6180X_RESULT_RANGE_REFERENCE_SIGNAL_COUNT);
mjr 82:4f6209cb5c33 250 stats.ambCnt = readReg32(VL6180X_RESULT_RANGE_RETURN_AMB_COUNT);
mjr 82:4f6209cb5c33 251 stats.refAmbCnt = readReg32(VL6180X_RESULT_RANGE_REFERENCE_AMB_COUNT);
mjr 82:4f6209cb5c33 252 stats.convTime = readReg32(VL6180X_RESULT_RANGE_RETURN_CONV_TIME);
mjr 82:4f6209cb5c33 253 stats.refConvTime = readReg32(VL6180X_RESULT_RANGE_REFERENCE_CONV_TIME);
mjr 82:4f6209cb5c33 254 }
mjr 82:4f6209cb5c33 255
mjr 82:4f6209cb5c33 256 uint8_t VL6180X::readReg8(uint16_t registerAddr)
mjr 82:4f6209cb5c33 257 {
mjr 82:4f6209cb5c33 258 // write the request - MSB+LSB of register address
mjr 82:4f6209cb5c33 259 uint8_t data_write[2];
mjr 82:4f6209cb5c33 260 data_write[0] = (registerAddr >> 8) & 0xFF;
mjr 82:4f6209cb5c33 261 data_write[1] = registerAddr & 0xFF;
mjr 87:8d35c74403af 262 if (i2c.write(addr << 1, data_write, 2, false))
mjr 82:4f6209cb5c33 263 return 0x00;
mjr 82:4f6209cb5c33 264
mjr 82:4f6209cb5c33 265 // read the result
mjr 82:4f6209cb5c33 266 uint8_t data_read[1];
mjr 82:4f6209cb5c33 267 if (i2c.read(addr << 1, data_read, 1))
mjr 82:4f6209cb5c33 268 return 0x00;
mjr 82:4f6209cb5c33 269
mjr 82:4f6209cb5c33 270 // return the result
mjr 82:4f6209cb5c33 271 return data_read[0];
mjr 82:4f6209cb5c33 272 }
mjr 82:4f6209cb5c33 273
mjr 82:4f6209cb5c33 274 uint16_t VL6180X::readReg16(uint16_t registerAddr)
mjr 82:4f6209cb5c33 275 {
mjr 82:4f6209cb5c33 276 // write the request - MSB+LSB of register address
mjr 82:4f6209cb5c33 277 uint8_t data_write[2];
mjr 82:4f6209cb5c33 278 data_write[0] = (registerAddr >> 8) & 0xFF;
mjr 82:4f6209cb5c33 279 data_write[1] = registerAddr & 0xFF;
mjr 87:8d35c74403af 280 if (i2c.write(addr << 1, data_write, 2, false))
mjr 82:4f6209cb5c33 281 return 0;
mjr 82:4f6209cb5c33 282
mjr 82:4f6209cb5c33 283 // read the result
mjr 82:4f6209cb5c33 284 uint8_t data_read[2];
mjr 82:4f6209cb5c33 285 if (i2c.read(addr << 1, data_read, 2))
mjr 82:4f6209cb5c33 286 return 00;
mjr 82:4f6209cb5c33 287
mjr 82:4f6209cb5c33 288 // return the result
mjr 82:4f6209cb5c33 289 return (data_read[0] << 8) | data_read[1];
mjr 82:4f6209cb5c33 290 }
mjr 82:4f6209cb5c33 291
mjr 82:4f6209cb5c33 292 uint32_t VL6180X::readReg32(uint16_t registerAddr)
mjr 82:4f6209cb5c33 293 {
mjr 82:4f6209cb5c33 294 // write the request - MSB+LSB of register address
mjr 82:4f6209cb5c33 295 uint8_t data_write[2];
mjr 82:4f6209cb5c33 296 data_write[0] = (registerAddr >> 8) & 0xFF;
mjr 82:4f6209cb5c33 297 data_write[1] = registerAddr & 0xFF;
mjr 82:4f6209cb5c33 298 if (i2c.write(addr << 1, data_write, 2, false))
mjr 82:4f6209cb5c33 299 return 0;
mjr 82:4f6209cb5c33 300
mjr 82:4f6209cb5c33 301 // read the result
mjr 82:4f6209cb5c33 302 uint8_t data_read[4];
mjr 82:4f6209cb5c33 303 if (i2c.read(addr << 1, data_read, 4))
mjr 82:4f6209cb5c33 304 return 0;
mjr 82:4f6209cb5c33 305
mjr 82:4f6209cb5c33 306 // return the result
mjr 82:4f6209cb5c33 307 return (data_read[0] << 24) | (data_read[1] << 16) | (data_read[2] << 8) | data_read[1];
mjr 82:4f6209cb5c33 308 }
mjr 82:4f6209cb5c33 309
mjr 82:4f6209cb5c33 310 void VL6180X::writeReg8(uint16_t registerAddr, uint8_t data)
mjr 82:4f6209cb5c33 311 {
mjr 82:4f6209cb5c33 312 uint8_t data_write[3];
mjr 82:4f6209cb5c33 313 data_write[0] = (registerAddr >> 8) & 0xFF;
mjr 82:4f6209cb5c33 314 data_write[1] = registerAddr & 0xFF;
mjr 82:4f6209cb5c33 315 data_write[2] = data & 0xFF;
mjr 82:4f6209cb5c33 316 i2c.write(addr << 1, data_write, 3);
mjr 82:4f6209cb5c33 317 }
mjr 82:4f6209cb5c33 318
mjr 82:4f6209cb5c33 319 void VL6180X::writeReg16(uint16_t registerAddr, uint16_t data)
mjr 82:4f6209cb5c33 320 {
mjr 82:4f6209cb5c33 321 uint8_t data_write[4];
mjr 82:4f6209cb5c33 322 data_write[0] = (registerAddr >> 8) & 0xFF;
mjr 82:4f6209cb5c33 323 data_write[1] = registerAddr & 0xFF;
mjr 82:4f6209cb5c33 324 data_write[2] = (data >> 8) & 0xFF;
mjr 82:4f6209cb5c33 325 data_write[3] = data & 0xFF;
mjr 82:4f6209cb5c33 326 i2c.write(addr << 1, data_write, 4);
mjr 82:4f6209cb5c33 327 }