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