Jesus Fausto / VL53L1X

This is the SparkFun_VL53L1X_Arduino_Library translated into mbed, with some elements of Ian Kilburn's VL6180x mbed library.

Dependents:   Hug2Go_ver_2 Nucleo_rtos_basic_f103rb

VL53L1X.cpp@4:2c3181cc4e98, 2018-07-27 (annotated)

Committer:
jvfausto
Date:
Fri Jul 27 17:58:43 2018 +0000
Revision:
4:2c3181cc4e98
Parent:
3:a7f10344f941
Child:
5:aa8cc9e87216
At least it blinks now

Who changed what in which revision?

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