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