Peng Jing Xuan / Mbed OS VL53L1X-MAX

VL53L1X-MAX

VL53L1X.cpp@0:385e286b830a, 2019-10-22 (annotated)

Committer:
peng103617
Date:
Tue Oct 22 05:47:05 2019 +0000
Revision:
0:385e286b830a
MAX

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peng103617 0:385e286b830a 1 #include "VL53L1X.h"
peng103617 0:385e286b830a 2 #include "mbed.h"
peng103617 0:385e286b830a 3
peng103617 0:385e286b830a 4 //Serial pc(USBTX,USBRX);
peng103617 0:385e286b830a 5 //DigitalOut led1(LED1);
peng103617 0:385e286b830a 6
peng103617 0:385e286b830a 7 uint8_t configBlock[] = {
peng103617 0:385e286b830a 8 0x29, 0x02, 0x10, 0x00, 0x28, 0xBC, 0x7A, 0x81, //8
peng103617 0:385e286b830a 9 0x80, 0x07, 0x95, 0x00, 0xED, 0xFF, 0xF7, 0xFD, //16
peng103617 0:385e286b830a 10 0x9E, 0x0E, 0x00, 0x10, 0x01, 0x00, 0x00, 0x00, //24
peng103617 0:385e286b830a 11 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x34, 0x00, //32
peng103617 0:385e286b830a 12 0x28, 0x00, 0x0D, 0x0A, 0x00, 0x00, 0x00, 0x00, //40
peng103617 0:385e286b830a 13 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, //48
peng103617 0:385e286b830a 14 0x02, 0x00, 0x02, 0x08, 0x00, 0x08, 0x10, 0x01, //56
peng103617 0:385e286b830a 15 0x01, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x02, //64
peng103617 0:385e286b830a 16 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0B, 0x00, //72
peng103617 0:385e286b830a 17 0x00, 0x02, 0x0A, 0x21, 0x00, 0x00, 0x02, 0x00, //80
peng103617 0:385e286b830a 18 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x38, 0xFF, //88
peng103617 0:385e286b830a 19 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x91, 0x0F, //96
peng103617 0:385e286b830a 20 0x00, 0xA5, 0x0D, 0x00, 0x80, 0x00, 0x0C, 0x08, //104
peng103617 0:385e286b830a 21 0xB8, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x10, 0x00, //112
peng103617 0:385e286b830a 22 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0F, //120
peng103617 0:385e286b830a 23 0x0D, 0x0E, 0x0E, 0x01, 0x00, 0x02, 0xC7, 0xFF, //128
peng103617 0:385e286b830a 24 0x8B, 0x00, 0x00, 0x00, 0x01, 0x01, 0x40 //129 - 135 (0x81 - 0x87)
peng103617 0:385e286b830a 25 };
peng103617 0:385e286b830a 26
peng103617 0:385e286b830a 27 VL53L1X::VL53L1X(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
peng103617 0:385e286b830a 28 //Set I2C fast and bring reset line high
peng103617 0:385e286b830a 29 _i2c.frequency(400000);
peng103617 0:385e286b830a 30 _deviceAddress = defaultAddress_VL53L1X << 1;
peng103617 0:385e286b830a 31 }
peng103617 0:385e286b830a 32
peng103617 0:385e286b830a 33 bool VL53L1X::begin()
peng103617 0:385e286b830a 34 {
peng103617 0:385e286b830a 35 //Check the device ID
peng103617 0:385e286b830a 36 uint16_t modelID = readRegister16(VL53L1_IDENTIFICATION__MODEL_ID);
peng103617 0:385e286b830a 37 if (modelID != 0xEACC){
peng103617 0:385e286b830a 38 return (false);
peng103617 0:385e286b830a 39 }
peng103617 0:385e286b830a 40 softReset();
peng103617 0:385e286b830a 41
peng103617 0:385e286b830a 42 //Polls the bit 0 of the FIRMWARE__SYSTEM_STATUS register to see if the firmware is ready
peng103617 0:385e286b830a 43 int counter = 0;
peng103617 0:385e286b830a 44 int Firmware = readRegister16(VL53L1_FIRMWARE__SYSTEM_STATUS);
peng103617 0:385e286b830a 45 printf("Firmware = %x\r\n", Firmware);
peng103617 0:385e286b830a 46 while ((Firmware & 0x01) == 0)
peng103617 0:385e286b830a 47 {
peng103617 0:385e286b830a 48 Firmware = readRegister16(VL53L1_FIRMWARE__SYSTEM_STATUS);
peng103617 0:385e286b830a 49 printf("Firmware = %x\r\n", Firmware);
peng103617 0:385e286b830a 50 if (counter++ == 100) return (false); //Sensor timed out
peng103617 0:385e286b830a 51 wait(.1);
peng103617 0:385e286b830a 52 }
peng103617 0:385e286b830a 53
peng103617 0:385e286b830a 54 //Set I2C to 2.8V mode. In this mode 3.3V I2C is allowed.
peng103617 0:385e286b830a 55 uint16_t result = readRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG);
peng103617 0:385e286b830a 56 result = (result & 0xFE) | 0x01;
peng103617 0:385e286b830a 57 writeRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, result);
peng103617 0:385e286b830a 58
peng103617 0:385e286b830a 59 //Gets trim resistors from chip
peng103617 0:385e286b830a 60 for (uint16_t i = 0; i < 36; i++) {
peng103617 0:385e286b830a 61 uint8_t regVal = readRegister(i + 1);
peng103617 0:385e286b830a 62 configBlock[i] = regVal;
peng103617 0:385e286b830a 63 }
peng103617 0:385e286b830a 64
peng103617 0:385e286b830a 65 startMeasurement();
peng103617 0:385e286b830a 66 return (true); //Sensor online!
peng103617 0:385e286b830a 67
peng103617 0:385e286b830a 68 }
peng103617 0:385e286b830a 69
peng103617 0:385e286b830a 70
peng103617 0:385e286b830a 71 void VL53L1X::startMeasurement(uint8_t offset)
peng103617 0:385e286b830a 72 {
peng103617 0:385e286b830a 73 offset = 0; //Start at a location within the configBlock array
peng103617 0:385e286b830a 74 uint8_t address = 1 + offset; //Start at memory location 0x01, add offset
peng103617 0:385e286b830a 75 char data_write[32];
peng103617 0:385e286b830a 76 uint8_t leftToSend = sizeof(configBlock) - offset;
peng103617 0:385e286b830a 77 while (leftToSend > 0)
peng103617 0:385e286b830a 78 {
peng103617 0:385e286b830a 79
peng103617 0:385e286b830a 80 data_write[0] = 0; //MSB of register address
peng103617 0:385e286b830a 81 data_write[1] = address; //LSB of register address
peng103617 0:385e286b830a 82
peng103617 0:385e286b830a 83 uint8_t toSend = 30; //Max I2C buffer on Arduino is 32, and we need 2 bytes for address
peng103617 0:385e286b830a 84 if (toSend > leftToSend) toSend = leftToSend;
peng103617 0:385e286b830a 85 for(int x = 0; x < toSend; x++)
peng103617 0:385e286b830a 86 {
peng103617 0:385e286b830a 87 data_write[x+2] = configBlock[x+address-1];
peng103617 0:385e286b830a 88 }
peng103617 0:385e286b830a 89 _i2c.write(_deviceAddress, data_write, toSend+2);
peng103617 0:385e286b830a 90
peng103617 0:385e286b830a 91 leftToSend -= toSend;
peng103617 0:385e286b830a 92 address += toSend;
peng103617 0:385e286b830a 93 }
peng103617 0:385e286b830a 94 }
peng103617 0:385e286b830a 95
peng103617 0:385e286b830a 96 bool VL53L1X::newDataReady(void)
peng103617 0:385e286b830a 97 {
peng103617 0:385e286b830a 98 int read = readRegister(VL53L1_GPIO__TIO_HV_STATUS);
peng103617 0:385e286b830a 99 if (read != 0x03) return(true); //New measurement!
peng103617 0:385e286b830a 100 return(false); //No new data
peng103617 0:385e286b830a 101 }
peng103617 0:385e286b830a 102
peng103617 0:385e286b830a 103 //Reset sensor via software
peng103617 0:385e286b830a 104 void VL53L1X::softReset()
peng103617 0:385e286b830a 105 {
peng103617 0:385e286b830a 106 writeRegister(VL53L1_SOFT_RESET, 0x00); //Reset
peng103617 0:385e286b830a 107 wait(.001); //Driver uses 100us
peng103617 0:385e286b830a 108 writeRegister(VL53L1_SOFT_RESET, 0x01); //Exit reset
peng103617 0:385e286b830a 109 }
peng103617 0:385e286b830a 110
peng103617 0:385e286b830a 111 uint16_t VL53L1X::getDistance()
peng103617 0:385e286b830a 112 {
peng103617 0:385e286b830a 113 return (readRegister16(VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0));
peng103617 0:385e286b830a 114 }
peng103617 0:385e286b830a 115
peng103617 0:385e286b830a 116 uint16_t VL53L1X::getSignalRate()
peng103617 0:385e286b830a 117 {
peng103617 0:385e286b830a 118 //From vl53l1_api.c line 2041
peng103617 0:385e286b830a 119 uint16_t reading = readRegister16(VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0);// << 9; //FIXPOINT97TOFIXPOINT1616
peng103617 0:385e286b830a 120 //float signalRate = (float)reading/65536.0;
peng103617 0:385e286b830a 121 return (reading);
peng103617 0:385e286b830a 122 }
peng103617 0:385e286b830a 123
peng103617 0:385e286b830a 124 void VL53L1X::setDistanceMode(uint8_t mode)
peng103617 0:385e286b830a 125 {
peng103617 0:385e286b830a 126 uint8_t periodA;
peng103617 0:385e286b830a 127 uint8_t periodB;
peng103617 0:385e286b830a 128 uint8_t phaseHigh;
peng103617 0:385e286b830a 129 uint8_t phaseInit;
peng103617 0:385e286b830a 130
peng103617 0:385e286b830a 131 switch (mode)
peng103617 0:385e286b830a 132 {
peng103617 0:385e286b830a 133 case 0:
peng103617 0:385e286b830a 134 periodA = 0x07;
peng103617 0:385e286b830a 135 periodB = 0x05;
peng103617 0:385e286b830a 136 phaseHigh = 0x38;
peng103617 0:385e286b830a 137 phaseInit = 6;
peng103617 0:385e286b830a 138 break;
peng103617 0:385e286b830a 139 case 1:
peng103617 0:385e286b830a 140 periodA = 0x0B;
peng103617 0:385e286b830a 141 periodB = 0x09;
peng103617 0:385e286b830a 142 phaseHigh = 0x78;
peng103617 0:385e286b830a 143 phaseInit = 10;
peng103617 0:385e286b830a 144 break;
peng103617 0:385e286b830a 145 case 2:
peng103617 0:385e286b830a 146 periodA = 0x0F;
peng103617 0:385e286b830a 147 periodB = 0x0D;
peng103617 0:385e286b830a 148 phaseHigh = 0xB8;
peng103617 0:385e286b830a 149 phaseInit = 14;
peng103617 0:385e286b830a 150 break;
peng103617 0:385e286b830a 151 //If user inputs wrong range, we default to long range
peng103617 0:385e286b830a 152 default:
peng103617 0:385e286b830a 153 periodA = 0x0F;
peng103617 0:385e286b830a 154 periodB = 0x0D;
peng103617 0:385e286b830a 155 phaseHigh = 0xB8;
peng103617 0:385e286b830a 156 phaseInit = 14;
peng103617 0:385e286b830a 157 break;
peng103617 0:385e286b830a 158 }
peng103617 0:385e286b830a 159 //timing
peng103617 0:385e286b830a 160 writeRegister(VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A, periodA);
peng103617 0:385e286b830a 161 writeRegister(VL53L1_RANGE_CONFIG__VCSEL_PERIOD_B , periodB);
peng103617 0:385e286b830a 162 writeRegister(VL53L1_RANGE_CONFIG__VALID_PHASE_HIGH, phaseHigh);
peng103617 0:385e286b830a 163
peng103617 0:385e286b830a 164 //dynamic
peng103617 0:385e286b830a 165 writeRegister(VL53L1_SD_CONFIG__WOI_SD0 , periodA);
peng103617 0:385e286b830a 166 writeRegister(VL53L1_SD_CONFIG__WOI_SD1, periodB);
peng103617 0:385e286b830a 167 writeRegister(VL53L1_SD_CONFIG__INITIAL_PHASE_SD0, phaseInit);
peng103617 0:385e286b830a 168 writeRegister(VL53L1_SD_CONFIG__INITIAL_PHASE_SD1, phaseInit);
peng103617 0:385e286b830a 169
peng103617 0:385e286b830a 170 _distanceMode = mode;
peng103617 0:385e286b830a 171 }
peng103617 0:385e286b830a 172 uint8_t VL53L1X::getDistanceMode()
peng103617 0:385e286b830a 173 {
peng103617 0:385e286b830a 174 return _distanceMode;
peng103617 0:385e286b830a 175 }
peng103617 0:385e286b830a 176
peng103617 0:385e286b830a 177 uint8_t VL53L1X::getRangeStatus()
peng103617 0:385e286b830a 178 {
peng103617 0:385e286b830a 179 #define VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ( 1)
peng103617 0:385e286b830a 180 #define VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ( 2)
peng103617 0:385e286b830a 181 #define VL53L1_DEVICEERROR_NOVHVVALUEFOUND ( 3)
peng103617 0:385e286b830a 182 #define VL53L1_DEVICEERROR_MSRCNOTARGET ( 4)
peng103617 0:385e286b830a 183 #define VL53L1_DEVICEERROR_RANGEPHASECHECK ( 5)
peng103617 0:385e286b830a 184 #define VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK ( 6)
peng103617 0:385e286b830a 185 #define VL53L1_DEVICEERROR_PHASECONSISTENCY ( 7)
peng103617 0:385e286b830a 186 #define VL53L1_DEVICEERROR_MINCLIP ( 8)
peng103617 0:385e286b830a 187 #define VL53L1_DEVICEERROR_RANGECOMPLETE ( 9)
peng103617 0:385e286b830a 188 #define VL53L1_DEVICEERROR_ALGOUNDERFLOW ( 10)
peng103617 0:385e286b830a 189 #define VL53L1_DEVICEERROR_ALGOOVERFLOW ( 11)
peng103617 0:385e286b830a 190 #define VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD ( 12)
peng103617 0:385e286b830a 191 #define VL53L1_DEVICEERROR_USERROICLIP ( 13)
peng103617 0:385e286b830a 192 #define VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS ( 14)
peng103617 0:385e286b830a 193 #define VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET ( 15)
peng103617 0:385e286b830a 194 #define VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET ( 16)
peng103617 0:385e286b830a 195 #define VL53L1_DEVICEERROR_MULTCLIPFAIL ( 17)
peng103617 0:385e286b830a 196 #define VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY ( 18)
peng103617 0:385e286b830a 197 #define VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ( 19)
peng103617 0:385e286b830a 198 #define VL53L1_DEVICEERROR_EVENTCONSISTENCY ( 20)
peng103617 0:385e286b830a 199 #define VL53L1_DEVICEERROR_MINSIGNALEVENTCHECK ( 21)
peng103617 0:385e286b830a 200 #define VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE ( 22)
peng103617 0:385e286b830a 201
peng103617 0:385e286b830a 202 #define VL53L1_RANGESTATUS_RANGE_VALID 0 /*!<The Range is valid. */
peng103617 0:385e286b830a 203 #define VL53L1_RANGESTATUS_SIGMA_FAIL 1 /*!<Sigma Fail. */
peng103617 0:385e286b830a 204 #define VL53L1_RANGESTATUS_SIGNAL_FAIL 2 /*!<Signal fail. */
peng103617 0:385e286b830a 205 #define VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED 3 /*!<Target is below minimum detection threshold. */
peng103617 0:385e286b830a 206 #define VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL 4 /*!<Phase out of valid limits - different to a wrap exit. */
peng103617 0:385e286b830a 207 #define VL53L1_RANGESTATUS_HARDWARE_FAIL 5 /*!<Hardware fail. */
peng103617 0:385e286b830a 208 #define VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL 6 /*!<The Range is valid but the wraparound check has not been done. */
peng103617 0:385e286b830a 209 #define VL53L1_RANGESTATUS_WRAP_TARGET_FAIL 7 /*!<Wrapped target - no matching phase in other VCSEL period timing. */
peng103617 0:385e286b830a 210 #define VL53L1_RANGESTATUS_PROCESSING_FAIL 8 /*!<Internal algo underflow or overflow in lite ranging. */
peng103617 0:385e286b830a 211 #define VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL 9 /*!<Specific to lite ranging. */
peng103617 0:385e286b830a 212 #define VL53L1_RANGESTATUS_SYNCRONISATION_INT 10 /*!<1st interrupt when starting ranging in back to back mode. Ignore data. */
peng103617 0:385e286b830a 213 #define VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE 11 /*!<All Range ok but object is result of multiple pulses merging together.*/
peng103617 0:385e286b830a 214 #define VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL 12 /*!<Used by RQL as different to phase fail. */
peng103617 0:385e286b830a 215 #define VL53L1_RANGESTATUS_MIN_RANGE_FAIL 13 /*!<User ROI input is not valid e.g. beyond SPAD Array.*/
peng103617 0:385e286b830a 216 #define VL53L1_RANGESTATUS_RANGE_INVALID 14 /*!<lld returned valid range but negative value ! */
peng103617 0:385e286b830a 217 #define VL53L1_RANGESTATUS_NONE 255 /*!<No Update. */
peng103617 0:385e286b830a 218
peng103617 0:385e286b830a 219 //Read status
peng103617 0:385e286b830a 220 uint8_t measurementStatus = (readRegister(VL53L1_RESULT__RANGE_STATUS) & 0x1F);
peng103617 0:385e286b830a 221 //Convert status from one to another - From vl53l1_api.c
peng103617 0:385e286b830a 222 switch (measurementStatus) {
peng103617 0:385e286b830a 223 case VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY:
peng103617 0:385e286b830a 224 measurementStatus = VL53L1_RANGESTATUS_SYNCRONISATION_INT;
peng103617 0:385e286b830a 225 break;
peng103617 0:385e286b830a 226 case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
peng103617 0:385e286b830a 227 measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
peng103617 0:385e286b830a 228 break;
peng103617 0:385e286b830a 229 case VL53L1_DEVICEERROR_RANGEPHASECHECK:
peng103617 0:385e286b830a 230 measurementStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL;
peng103617 0:385e286b830a 231 break;
peng103617 0:385e286b830a 232 case VL53L1_DEVICEERROR_MSRCNOTARGET:
peng103617 0:385e286b830a 233 measurementStatus = VL53L1_RANGESTATUS_SIGNAL_FAIL;
peng103617 0:385e286b830a 234 break;
peng103617 0:385e286b830a 235 case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK:
peng103617 0:385e286b830a 236 measurementStatus = VL53L1_RANGESTATUS_SIGMA_FAIL;
peng103617 0:385e286b830a 237 break;
peng103617 0:385e286b830a 238 case VL53L1_DEVICEERROR_PHASECONSISTENCY:
peng103617 0:385e286b830a 239 measurementStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
peng103617 0:385e286b830a 240 break;
peng103617 0:385e286b830a 241 case VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD:
peng103617 0:385e286b830a 242 measurementStatus = VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL;
peng103617 0:385e286b830a 243 break;
peng103617 0:385e286b830a 244 case VL53L1_DEVICEERROR_MINCLIP:
peng103617 0:385e286b830a 245 measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED;
peng103617 0:385e286b830a 246 break;
peng103617 0:385e286b830a 247 case VL53L1_DEVICEERROR_RANGECOMPLETE:
peng103617 0:385e286b830a 248 measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID;
peng103617 0:385e286b830a 249 break;
peng103617 0:385e286b830a 250 default:
peng103617 0:385e286b830a 251 measurementStatus = VL53L1_RANGESTATUS_NONE;
peng103617 0:385e286b830a 252 }
peng103617 0:385e286b830a 253
peng103617 0:385e286b830a 254 return measurementStatus;
peng103617 0:385e286b830a 255 }
peng103617 0:385e286b830a 256
peng103617 0:385e286b830a 257 uint8_t VL53L1X::readRegister(uint16_t registerAddr)
peng103617 0:385e286b830a 258 {
peng103617 0:385e286b830a 259 uint8_t data;
peng103617 0:385e286b830a 260 char data_write[2];
peng103617 0:385e286b830a 261 char data_read[1];
peng103617 0:385e286b830a 262 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 0:385e286b830a 263 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 0:385e286b830a 264 _i2c.write(_deviceAddress, data_write, 2,0);
peng103617 0:385e286b830a 265 _i2c.read(_deviceAddress,data_read,1,1);
peng103617 0:385e286b830a 266 //Read Data from selected register
peng103617 0:385e286b830a 267 data=data_read[0];
peng103617 0:385e286b830a 268 return data;
peng103617 0:385e286b830a 269 }
peng103617 0:385e286b830a 270
peng103617 0:385e286b830a 271 uint16_t VL53L1X::readRegister16(uint16_t registerAddr)
peng103617 0:385e286b830a 272 {
peng103617 0:385e286b830a 273 uint8_t data_low;
peng103617 0:385e286b830a 274 uint8_t data_high;
peng103617 0:385e286b830a 275 uint16_t data;
peng103617 0:385e286b830a 276
peng103617 0:385e286b830a 277 char data_write[2];
peng103617 0:385e286b830a 278 char data_read[2];
peng103617 0:385e286b830a 279 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 0:385e286b830a 280 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 0:385e286b830a 281 _i2c.write(_deviceAddress, data_write, 2,0);
peng103617 0:385e286b830a 282 _i2c.read(_deviceAddress,data_read,2,1);
peng103617 0:385e286b830a 283 data_high = data_read[0]; //Read Data from selected register
peng103617 0:385e286b830a 284 data_low = data_read[1]; //Read Data from selected register
peng103617 0:385e286b830a 285 data = (data_high << 8)|data_low;
peng103617 0:385e286b830a 286
peng103617 0:385e286b830a 287 return data;
peng103617 0:385e286b830a 288 }
peng103617 0:385e286b830a 289
peng103617 0:385e286b830a 290 void VL53L1X::writeRegister(uint16_t registerAddr, uint8_t data)
peng103617 0:385e286b830a 291 {
peng103617 0:385e286b830a 292 char data_write[3];
peng103617 0:385e286b830a 293 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 0:385e286b830a 294 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 0:385e286b830a 295 data_write[2] = data & 0xFF;
peng103617 0:385e286b830a 296 _i2c.write(_deviceAddress, data_write, 3);
peng103617 0:385e286b830a 297 }
peng103617 0:385e286b830a 298
peng103617 0:385e286b830a 299 void VL53L1X::writeRegister16(uint16_t registerAddr, uint16_t data)
peng103617 0:385e286b830a 300 {
peng103617 0:385e286b830a 301 char data_write[4];
peng103617 0:385e286b830a 302 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 0:385e286b830a 303 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 0:385e286b830a 304 data_write[2] = (data >> 8) & 0xFF;
peng103617 0:385e286b830a 305 data_write[3] = data & 0xFF;
peng103617 0:385e286b830a 306 _i2c.write(_deviceAddress, data_write, 4);
peng103617 0:385e286b830a 307 }
peng103617 0:385e286b830a 308