![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
No range status update (error 255)
Fork of VL53L1X by
VL53L1X.cpp
- Committer:
- jvfausto
- Date:
- 2018-07-19
- Revision:
- 1:0d762892f7af
- Parent:
- 0:03b7e8deb3ee
- Child:
- 2:fbaee2407103
File content as of revision 1:0d762892f7af:
#include "VL53L1X.h" #include "mbed.h" //Serial pc(USBTX,USBRX); uint8_t configBlock[] = { 0x29, 0x02, 0x10, 0x00, 0x28, 0xBC, 0x7A, 0x81, //8 0x80, 0x07, 0x95, 0x00, 0xED, 0xFF, 0xF7, 0xFD, //16 0x9E, 0x0E, 0x00, 0x10, 0x01, 0x00, 0x00, 0x00, //24 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x34, 0x00, //32 0x28, 0x00, 0x0D, 0x0A, 0x00, 0x00, 0x00, 0x00, //40 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, //48 0x02, 0x00, 0x02, 0x08, 0x00, 0x08, 0x10, 0x01, //56 0x01, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x02, //64 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0B, 0x00, //72 0x00, 0x02, 0x0A, 0x21, 0x00, 0x00, 0x02, 0x00, //80 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x38, 0xFF, //88 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x91, 0x0F, //96 0x00, 0xA5, 0x0D, 0x00, 0x80, 0x00, 0x0C, 0x08, //104 0xB8, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x10, 0x00, //112 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0F, //120 0x0D, 0x0E, 0x0E, 0x01, 0x00, 0x02, 0xC7, 0xFF, //128 0x8B, 0x00, 0x00, 0x00, 0x01, 0x01, 0x40 //129 - 135 (0x81 - 0x87) }; VL53L1X::VL53L1X(PinName SDA, PinName SCL) : _i2c(SDA,SCL){ //Set I2C fast and bring reset line high _i2c.frequency(400000); _deviceAddress = defaultAddress_VL53L1X; } bool VL53L1X::begin() { // _deviceAddress = defaultAddress_VL53L1X; //If provided, store the I2C address from user //We expect caller to begin their I2C port, with the speed of their choice external to the library //But if they forget, we start the hardware here. //_i2c.begin(); //Check the device ID uint16_t modelID = readRegister16(VL53L1_IDENTIFICATION__MODEL_ID); printf("%x \r\n", modelID); if (modelID != 0xEACC){ return (false); } softReset(); //Polls the bit 0 of the FIRMWARE__SYSTEM_STATUS register to see if the firmware is ready int counter = 0; while (readRegister16(VL53L1_FIRMWARE__SYSTEM_STATUS) & 0x01 == 0) { if (counter++ == 100) return (false); //Sensor timed out wait(1); } //Set I2C to 2.8V mode. In this mode 3.3V I2C is allowed. uint16_t result = readRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG); result = (result & 0xFE) | 0x01; writeRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, result); //Gets trim resistors from chip for (uint16_t i = 0; i < 36; i++) { uint8_t regVal = readRegister(i + 1); configBlock[i] = regVal; } startMeasurement(); return (true); //Sensor online! } void VL53L1X::startMeasurement(uint8_t offset) { uint8_t address = 1 + offset; //Start at memory location 0x01, add offset uint8_t leftToSend = sizeof(configBlock) - offset; while (leftToSend > 0) { uint16_t toSend = I2C_BUFFER_LENGTH - 2; //Max I2C buffer on Arduino is 32, and we need 2 bytes for address if (toSend > leftToSend) toSend = leftToSend; // _i2c.write(address << 1);// _i2c.beginTransmission(_deviceAddress); _i2c.write(0); //We're only in lower address space. No MSB needed. _i2c.write(address); for (char x = 0 ; x < toSend ; x++) _i2c.write(configBlock[address + x - 1 - offset]); // _i2c.endTransmission(); leftToSend -= toSend; address += toSend; } } bool VL53L1X::newDataReady(void) { if (readRegister(VL53L1_GPIO__TIO_HV_STATUS) != 0x03) return(true); //New measurement! return(false); //No new data } //Reset sensor via software void VL53L1X::softReset() { writeRegister(VL53L1_SOFT_RESET, 0x00); //Reset wait(.001); //Driver uses 100us writeRegister(VL53L1_SOFT_RESET, 0x01); //Exit reset } uint16_t VL53L1X::getDistance() { return (readRegister16(VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0)); } uint16_t VL53L1X::getSignalRate() { //From vl53l1_api.c line 2041 uint16_t reading = readRegister16(VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0);// << 9; //FIXPOINT97TOFIXPOINT1616 //float signalRate = (float)reading/65536.0; return (reading); } void VL53L1X::setDistanceMode(uint8_t mode) { uint8_t periodA; uint8_t periodB; uint8_t phaseHigh; uint8_t phaseInit; switch (mode) { case 0: periodA = 0x07; periodB = 0x05; phaseHigh = 0x38; phaseInit = 6; break; case 1: periodA = 0x0B; periodB = 0x09; phaseHigh = 0x78; phaseInit = 10; break; case 2: periodA = 0x0F; periodB = 0x0D; phaseHigh = 0xB8; phaseInit = 14; break; //If user inputs wrong range, we default to long range default: periodA = 0x0F; periodB = 0x0D; phaseHigh = 0xB8; phaseInit = 14; break; } //timing writeRegister(VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A, periodA); writeRegister(VL53L1_RANGE_CONFIG__VCSEL_PERIOD_B , periodB); writeRegister(VL53L1_RANGE_CONFIG__VALID_PHASE_HIGH, phaseHigh); //dynamic writeRegister(VL53L1_SD_CONFIG__WOI_SD0 , periodA); writeRegister(VL53L1_SD_CONFIG__WOI_SD1, periodB); writeRegister(VL53L1_SD_CONFIG__INITIAL_PHASE_SD0, phaseInit); writeRegister(VL53L1_SD_CONFIG__INITIAL_PHASE_SD1, phaseInit); _distanceMode = mode; } uint8_t VL53L1X::getDistanceMode() { return _distanceMode; } uint8_t VL53L1X::getRangeStatus() { #define VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ( 1) #define VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ( 2) #define VL53L1_DEVICEERROR_NOVHVVALUEFOUND ( 3) #define VL53L1_DEVICEERROR_MSRCNOTARGET ( 4) #define VL53L1_DEVICEERROR_RANGEPHASECHECK ( 5) #define VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK ( 6) #define VL53L1_DEVICEERROR_PHASECONSISTENCY ( 7) #define VL53L1_DEVICEERROR_MINCLIP ( 8) #define VL53L1_DEVICEERROR_RANGECOMPLETE ( 9) #define VL53L1_DEVICEERROR_ALGOUNDERFLOW ( 10) #define VL53L1_DEVICEERROR_ALGOOVERFLOW ( 11) #define VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD ( 12) #define VL53L1_DEVICEERROR_USERROICLIP ( 13) #define VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS ( 14) #define VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET ( 15) #define VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET ( 16) #define VL53L1_DEVICEERROR_MULTCLIPFAIL ( 17) #define VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY ( 18) #define VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ( 19) #define VL53L1_DEVICEERROR_EVENTCONSISTENCY ( 20) #define VL53L1_DEVICEERROR_MINSIGNALEVENTCHECK ( 21) #define VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE ( 22) #define VL53L1_RANGESTATUS_RANGE_VALID 0 /*!<The Range is valid. */ #define VL53L1_RANGESTATUS_SIGMA_FAIL 1 /*!<Sigma Fail. */ #define VL53L1_RANGESTATUS_SIGNAL_FAIL 2 /*!<Signal fail. */ #define VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED 3 /*!<Target is below minimum detection threshold. */ #define VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL 4 /*!<Phase out of valid limits - different to a wrap exit. */ #define VL53L1_RANGESTATUS_HARDWARE_FAIL 5 /*!<Hardware fail. */ #define VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL 6 /*!<The Range is valid but the wraparound check has not been done. */ #define VL53L1_RANGESTATUS_WRAP_TARGET_FAIL 7 /*!<Wrapped target - no matching phase in other VCSEL period timing. */ #define VL53L1_RANGESTATUS_PROCESSING_FAIL 8 /*!<Internal algo underflow or overflow in lite ranging. */ #define VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL 9 /*!<Specific to lite ranging. */ #define VL53L1_RANGESTATUS_SYNCRONISATION_INT 10 /*!<1st interrupt when starting ranging in back to back mode. Ignore data. */ #define VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE 11 /*!<All Range ok but object is result of multiple pulses merging together.*/ #define VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL 12 /*!<Used by RQL as different to phase fail. */ #define VL53L1_RANGESTATUS_MIN_RANGE_FAIL 13 /*!<User ROI input is not valid e.g. beyond SPAD Array.*/ #define VL53L1_RANGESTATUS_RANGE_INVALID 14 /*!<lld returned valid range but negative value ! */ #define VL53L1_RANGESTATUS_NONE 255 /*!<No Update. */ //Read status uint8_t measurementStatus = readRegister(VL53L1_RESULT__RANGE_STATUS) & 0x1F; //Convert status from one to another - From vl53l1_api.c switch (measurementStatus) { case VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY: measurementStatus = VL53L1_RANGESTATUS_SYNCRONISATION_INT; break; case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK: measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL; break; case VL53L1_DEVICEERROR_RANGEPHASECHECK: measurementStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL; break; case VL53L1_DEVICEERROR_MSRCNOTARGET: measurementStatus = VL53L1_RANGESTATUS_SIGNAL_FAIL; break; case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK: measurementStatus = VL53L1_RANGESTATUS_SIGMA_FAIL; break; case VL53L1_DEVICEERROR_PHASECONSISTENCY: measurementStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL; break; case VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD: measurementStatus = VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL; break; case VL53L1_DEVICEERROR_MINCLIP: measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED; break; case VL53L1_DEVICEERROR_RANGECOMPLETE: measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID; break; default: measurementStatus = VL53L1_RANGESTATUS_NONE; } return measurementStatus; } uint8_t VL53L1X::readRegister(uint16_t registerAddr) { uint8_t data; char data_write[2]; char data_read[1]; data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address data_write[1] = registerAddr & 0xFF; //LSB of register address _i2c.write(_deviceAddress << 1, data_write, 2,0); _i2c.read(_deviceAddress << 1,data_read,1,1); //Read Data from selected register data=data_read[0]; return data; } uint16_t VL53L1X::readRegister16(uint16_t registerAddr) { uint8_t data_low; uint8_t data_high; uint16_t data; char data_write[2]; char data_read[2]; data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address data_write[1] = registerAddr & 0xFF; //LSB of register address _i2c.write(_deviceAddress << 1, data_write, 2,0); _i2c.read(_deviceAddress << 1,data_read,2,1); data_high = data_read[0]; //Read Data from selected register data_low = data_read[1]; //Read Data from selected register data = (data_high << 8)|data_low; return data; } void VL53L1X::writeRegister(uint16_t registerAddr, uint8_t data) { char data_write[3]; data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address data_write[1] = registerAddr & 0xFF; //LSB of register address data_write[2] = data & 0xFF; _i2c.write(_deviceAddress, data_write, 3); } void VL53L1X::writeRegister16(uint16_t registerAddr, uint16_t data) { char data_write[4]; data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address data_write[1] = registerAddr & 0xFF; //LSB of register address data_write[2] = (data >> 8) & 0xFF; data_write[3] = data & 0xFF; _i2c.write(_deviceAddress, data_write, 4); }