VL53L1X.cpp

00001 // Most of the functionality of this library is based on the VL53L1X API
00002 // provided by ST (STSW-IMG007), and some of the explanatory comments are quoted
00003 // or paraphrased from the API source code, API user manual (UM2356), and
00004 // VL53L1X datasheet.
00005  
00006 #include "VL53L1X.h"
00007 #include "mbed.h"
00008  
00009 // Constructors ////////////////////////////////////////////////////////////////
00010 VL53L1X::VL53L1X(PinName SDA, PinName SCL, PinName shutDown) : 
00011   _i2c(SDA,SCL), _shutDown(shutDown)  
00012   , io_timeout(0) // no timeout
00013   , did_timeout(false)
00014   , calibrated(false)
00015   , saved_vhv_init(0)
00016   , saved_vhv_timeout(0)
00017   , distance_mode(Unknown){
00018     //Set I2C fast and bring reset line high
00019    _i2c.frequency(400000);
00020     address = AddressDefault << 1;
00021     turnOff();
00022     }
00023     
00024 /*VL53L1X::VL53L1X()
00025   : address(AddressDefault)
00026 {
00027 }*/
00028  
00029 // Public Methods //////////////////////////////////////////////////////////////
00030  
00031 void VL53L1X::setAddress(uint8_t new_addr)
00032 {
00033   writeReg(I2C_SLAVE__DEVICE_ADDRESS, new_addr & 0x7F);
00034   wait(.02);
00035   address = new_addr << 1;
00036   printf("%x\r\n", readReg(I2C_SLAVE__DEVICE_ADDRESS));
00037 }
00038  
00039 // Initialize sensor using settings taken mostly from VL53L1_DataInit() and
00040 // VL53L1_StaticInit().
00041 // If io_2v8 (optional) is true or not given, the sensor is configured for 2V8
00042 // mode.
00043 bool VL53L1X::init(bool io_2v8)
00044 {
00045   // check model ID and module type registers (values specified in datasheet)
00046   int tempRegister = readReg16Bit(IDENTIFICATION__MODEL_ID);
00047   printf("temporary %x\r\n", tempRegister);
00048   if (tempRegister != 0xEACC) {  
00049     return false; 
00050   }
00051  
00052   // VL53L1_software_reset() begin
00053  
00054   writeReg(SOFT_RESET, 0x00);
00055   wait(.001);
00056   writeReg(SOFT_RESET, 0x01);
00057  
00058   // VL53L1_poll_for_boot_completion() begin
00059  
00060   startTimeout();
00061   int firmware = (readReg16Bit(FIRMWARE__SYSTEM_STATUS));
00062   printf("firmware : %x\r\n", firmware);
00063   while ((readReg(FIRMWARE__SYSTEM_STATUS) & 0x01) == 0)
00064   {
00065     printf("stuck\r\n");
00066     if (checkTimeoutExpired())
00067     {
00068       did_timeout = true;
00069       return false;
00070     }
00071   }
00072   // VL53L1_poll_for_boot_completion() end
00073  
00074   // VL53L1_software_reset() end
00075  
00076   // VL53L1_DataInit() begin
00077  
00078   // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
00079   if (io_2v8)
00080   {
00081     writeReg(PAD_I2C_HV__EXTSUP_CONFIG,
00082       readReg(PAD_I2C_HV__EXTSUP_CONFIG) | 0x01);
00083   }
00084  
00085   // store oscillator info for later use
00086   fast_osc_frequency = readReg16Bit(OSC_MEASURED__FAST_OSC__FREQUENCY);
00087   osc_calibrate_val = readReg16Bit(RESULT__OSC_CALIBRATE_VAL);
00088  
00089   // VL53L1_DataInit() end
00090  
00091   // VL53L1_StaticInit() begin
00092  
00093   // Note that the API does not actually apply the configuration settings below
00094   // when VL53L1_StaticInit() is called: it keeps a copy of the sensor's
00095   // register contents in memory and doesn't actually write them until a
00096   // measurement is started. Writing the configuration here means we don't have
00097   // to keep it all in memory and avoids a lot of redundant writes later.
00098  
00099   // the API sets the preset mode to LOWPOWER_AUTONOMOUS here:
00100   // VL53L1_set_preset_mode() begin
00101  
00102   // VL53L1_preset_mode_standard_ranging() begin
00103  
00104   // values labeled "tuning parm default" are from vl53l1_tuning_parm_defaults.h
00105   // (API uses these in VL53L1_init_tuning_parm_storage_struct())
00106  
00107   // static config
00108   // API resets PAD_I2C_HV__EXTSUP_CONFIG here, but maybe we don't want to do
00109   // that? (seems like it would disable 2V8 mode)
00110   writeReg16Bit(DSS_CONFIG__TARGET_TOTAL_RATE_MCPS, TargetRate); // should already be this value after reset
00111   writeReg(GPIO__TIO_HV_STATUS, 0x02);
00112   writeReg(SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS, 8); // tuning parm default
00113   writeReg(SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS, 16); // tuning parm default
00114   writeReg(ALGO__CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0x01);
00115   writeReg(ALGO__RANGE_IGNORE_VALID_HEIGHT_MM, 0xFF);
00116   writeReg(ALGO__RANGE_MIN_CLIP, 0); // tuning parm default
00117   writeReg(ALGO__CONSISTENCY_CHECK__TOLERANCE, 2); // tuning parm default
00118  
00119   // general config
00120   writeReg16Bit(SYSTEM__THRESH_RATE_HIGH, 0x0000);
00121   writeReg16Bit(SYSTEM__THRESH_RATE_LOW, 0x0000);
00122   writeReg(DSS_CONFIG__APERTURE_ATTENUATION, 0x38);
00123  
00124   // timing config
00125   // most of these settings will be determined later by distance and timing
00126   // budget configuration
00127   writeReg16Bit(RANGE_CONFIG__SIGMA_THRESH, 360); // tuning parm default
00128   writeReg16Bit(RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192); // tuning parm default
00129  
00130   // dynamic config
00131  
00132   writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_0, 0x01);
00133   writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_1, 0x01);
00134   writeReg(SD_CONFIG__QUANTIFIER, 2); // tuning parm default
00135  
00136   // VL53L1_preset_mode_standard_ranging() end
00137  
00138   // from VL53L1_preset_mode_timed_ranging_*
00139   // GPH is 0 after reset, but writing GPH0 and GPH1 above seem to set GPH to 1,
00140   // and things don't seem to work if we don't set GPH back to 0 (which the API
00141   // does here).
00142   writeReg(SYSTEM__GROUPED_PARAMETER_HOLD, 0x00);
00143   writeReg(SYSTEM__SEED_CONFIG, 1); // tuning parm default
00144  
00145   // from VL53L1_config_low_power_auto_mode
00146   writeReg(SYSTEM__SEQUENCE_CONFIG, 0x8B); // VHV, PHASECAL, DSS1, RANGE
00147   writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8);
00148   writeReg(DSS_CONFIG__ROI_MODE_CONTROL, 2); // REQUESTED_EFFFECTIVE_SPADS
00149  
00150   // VL53L1_set_preset_mode() end
00151  
00152   // default to long range, 50 ms timing budget
00153   // note that this is different than what the API defaults to
00154   setDistanceMode(Short);
00155   setMeasurementTimingBudget(50000);
00156  
00157   // VL53L1_StaticInit() end
00158  
00159   // the API triggers this change in VL53L1_init_and_start_range() once a
00160   // measurement is started; assumes MM1 and MM2 are disabled
00161   writeReg16Bit(ALGO__PART_TO_PART_RANGE_OFFSET_MM,
00162     readReg16Bit(MM_CONFIG__OUTER_OFFSET_MM) * 4);
00163  t.start();
00164   return true;
00165 }
00166  
00167 // Write an 8-bit register
00168 void VL53L1X::writeReg(uint16_t registerAddr, uint8_t data)
00169 {
00170     char data_write[3];
00171     data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00172     data_write[1] = registerAddr & 0xFF; //LSB of register address 
00173     data_write[2] = data & 0xFF; 
00174     _i2c.write(address, data_write, 3); 
00175 }
00176  
00177 void VL53L1X::writeReg16Bit(uint16_t registerAddr, uint16_t data)
00178 {
00179     char data_write[4];
00180     data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00181     data_write[1] = registerAddr & 0xFF; //LSB of register address 
00182     data_write[2] = (data >> 8) & 0xFF;
00183     data_write[3] = data & 0xFF; 
00184     _i2c.write(address, data_write, 4); 
00185 }
00186  
00187  
00188 // Write a 32-bit register
00189 /*
00190 void VL53L1X::writeReg32Bit(uint16_t registerAddr, uint32_t data)
00191 {
00192     char data_write[5];
00193     data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00194     data_write[1] = registerAddr & 0xFF; //LSB of register address 
00195     data_write[2] = (data >> 16) & 0xFF;
00196     data_write[3] = (data >> 8) & 0xFF; 
00197     data_write[4] =  data & 0xFF;
00198     _i2c.write(address, data_write, 5); 
00199 }
00200 */
00201 void VL53L1X::writeReg32Bit(uint16_t registerAddr, uint32_t data)
00202 {
00203     char data_write[6];
00204     data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00205     data_write[1] = registerAddr & 0xFF; //LSB of register address 
00206     data_write[2] = (data >> 24) & 0xFF;
00207     data_write[3] = (data >> 16) & 0xFF; 
00208     data_write[4] = (data >> 8) & 0xFF;;
00209     data_write[5] =  data & 0xFF;
00210     _i2c.write(address, data_write, 6); 
00211 }
00212  
00213  
00214 // Read an 8-bit register
00215 uint8_t VL53L1X::readReg(uint16_t registerAddr)
00216 {
00217   uint8_t data;
00218   char data_write[2];
00219   char data_read[1];
00220   data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00221   data_write[1] = registerAddr & 0xFF; //LSB of register address 
00222   _i2c.write(address, data_write, 2,0); 
00223   _i2c.read(address,data_read,1,1);
00224   //Read Data from selected register
00225   data=data_read[0];
00226   return data;
00227 }
00228  
00229 uint16_t VL53L1X::readReg16Bit(uint16_t registerAddr)
00230 {
00231   uint8_t data_low;
00232   uint8_t data_high;
00233   uint16_t data;
00234  
00235   char data_write[2];
00236   char data_read[2];
00237   data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00238   data_write[1] = registerAddr & 0xFF; //LSB of register address 
00239   _i2c.write(address, data_write, 2,0); 
00240   _i2c.read(address,data_read,2,1);
00241   data_high = data_read[0]; //Read Data from selected register
00242   data_low = data_read[1]; //Read Data from selected register
00243   data = (data_high << 8)|data_low;
00244  
00245   return data;
00246 }
00247 // Read a 32-bit register
00248 uint32_t VL53L1X::readReg32Bit(uint16_t reg)
00249 {
00250   uint32_t value;
00251 /*
00252   _i2c.beginTransmission(address);
00253   _i2c.write((reg >> 8) & 0xFF); // reg high byte
00254   _i2c.write( reg       & 0xFF); // reg low byte
00255   last_status = _i2c.endTransmission();
00256  
00257   _i2c.requestFrom(address, (uint8_t)4);
00258   value  = (uint32_t)_i2c.read() << 24; // value highest byte
00259   value |= (uint32_t)_i2c.read() << 16;
00260   value |= (uint16_t)_i2c.read() <<  8;
00261   value |=           _i2c.read();       // value lowest byte
00262 */
00263   return value;
00264 }
00265  
00266 // set distance mode to Short, Medium, or Long
00267 // based on VL53L1_SetDistanceMode()
00268 bool VL53L1X::setDistanceMode(DistanceMode mode)
00269 {
00270   // save existing timing budget
00271   uint32_t budget_us = getMeasurementTimingBudget();
00272   switch (mode)
00273   {
00274     case Short:
00275       // from VL53L1_preset_mode_standard_ranging_short_range()
00276  
00277       // timing config
00278       writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
00279       writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
00280       writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
00281  
00282       // dynamic config
00283       writeReg(SD_CONFIG__WOI_SD0, 0x07);
00284       writeReg(SD_CONFIG__WOI_SD1, 0x05);
00285       writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 6); // tuning parm default
00286       writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 6); // tuning parm default
00287  
00288       break;
00289  
00290     case Medium:
00291       // from VL53L1_preset_mode_standard_ranging()
00292  
00293       // timing config
00294       writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0B);
00295       writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x09);
00296       writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x78);
00297  
00298       // dynamic config
00299       writeReg(SD_CONFIG__WOI_SD0, 0x0B);
00300       writeReg(SD_CONFIG__WOI_SD1, 0x09);
00301       writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 10); // tuning parm default
00302       writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 10); // tuning parm default
00303  
00304       break;
00305  
00306     case Long: // long
00307       // from VL53L1_preset_mode_standard_ranging_long_range()
00308  
00309       // timing config
00310       writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
00311       writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
00312       writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
00313  
00314       // dynamic config
00315       writeReg(SD_CONFIG__WOI_SD0, 0x0F);
00316       writeReg(SD_CONFIG__WOI_SD1, 0x0D);
00317       writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 14); // tuning parm default
00318       writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 14); // tuning parm default
00319  
00320       break;
00321  
00322     default:
00323       // unrecognized mode - do nothing
00324       return false;
00325   }
00326  
00327   // reapply timing budget
00328   setMeasurementTimingBudget(budget_us);
00329  
00330   // save mode so it can be returned by getDistanceMode()
00331   distance_mode = mode;
00332  
00333   return true;
00334 }
00335  
00336 // Set the measurement timing budget in microseconds, which is the time allowed
00337 // for one measurement. A longer timing budget allows for more accurate
00338 // measurements.
00339 // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
00340 bool VL53L1X::setMeasurementTimingBudget(uint32_t budget_us)
00341 {
00342   // assumes PresetMode is LOWPOWER_AUTONOMOUS
00343  
00344   if (budget_us <= TimingGuard) { return false; }
00345  
00346   uint32_t range_config_timeout_us = budget_us -= TimingGuard;
00347   if (range_config_timeout_us > 1100000) { return false; } // FDA_MAX_TIMING_BUDGET_US * 2
00348  
00349   range_config_timeout_us /= 2;
00350  
00351   // VL53L1_calc_timeout_register_values() begin
00352  
00353   uint32_t macro_period_us;
00354  
00355   // "Update Macro Period for Range A VCSEL Period"
00356   macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_A));
00357  
00358   // "Update Phase timeout - uses Timing A"
00359   // Timeout of 1000 is tuning parm default (TIMED_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT)
00360   // via VL53L1_get_preset_mode_timing_cfg().
00361   uint32_t phasecal_timeout_mclks = timeoutMicrosecondsToMclks(1000, macro_period_us);
00362   if (phasecal_timeout_mclks > 0xFF) { phasecal_timeout_mclks = 0xFF; }
00363   writeReg(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks);
00364  
00365   // "Update MM Timing A timeout"
00366   // Timeout of 1 is tuning parm default (LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT)
00367   // via VL53L1_get_preset_mode_timing_cfg(). With the API, the register
00368   // actually ends up with a slightly different value because it gets assigned,
00369   // retrieved, recalculated with a different macro period, and reassigned,
00370   // but it probably doesn't matter because it seems like the MM ("mode
00371   // mitigation"?) sequence steps are disabled in low power auto mode anyway.
00372   writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
00373     timeoutMicrosecondsToMclks(1, macro_period_us)));
00374  
00375   // "Update Range Timing A timeout"
00376   writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
00377     timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us)));
00378  
00379   // "Update Macro Period for Range B VCSEL Period"
00380   macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_B));
00381  
00382   // "Update MM Timing B timeout"
00383   // (See earlier comment about MM Timing A timeout.)
00384   writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
00385     timeoutMicrosecondsToMclks(1, macro_period_us)));
00386  
00387   // "Update Range Timing B timeout"
00388   writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
00389     timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us)));
00390   // VL53L1_calc_timeout_register_values() end
00391  
00392   return true;
00393 }
00394  
00395 // Get the measurement timing budget in microseconds
00396 // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
00397 uint32_t VL53L1X::getMeasurementTimingBudget()
00398 {
00399   // assumes PresetMode is LOWPOWER_AUTONOMOUS and these sequence steps are
00400   // enabled: VHV, PHASECAL, DSS1, RANGE
00401  
00402   // VL53L1_get_timeouts_us() begin
00403  
00404   // "Update Macro Period for Range A VCSEL Period"
00405   uint32_t macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_A));
00406  
00407   // "Get Range Timing A timeout"
00408  
00409   uint32_t range_config_timeout_us = timeoutMclksToMicroseconds(decodeTimeout(
00410     readReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A)), macro_period_us);
00411  
00412   // VL53L1_get_timeouts_us() end
00413  
00414   return  2 * range_config_timeout_us + TimingGuard;
00415 }
00416  
00417 // Start continuous ranging measurements, with the given inter-measurement
00418 // period in milliseconds determining how often the sensor takes a measurement.
00419 void VL53L1X::startContinuous(uint32_t period_ms)
00420 {
00421   // from VL53L1_set_inter_measurement_period_ms()
00422   writeReg32Bit(SYSTEM__INTERMEASUREMENT_PERIOD, period_ms * osc_calibrate_val);
00423   writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
00424   writeReg(SYSTEM__MODE_START, 0x40); // mode_range__timed
00425 }
00426  
00427 // Stop continuous measurements
00428 // based on VL53L1_stop_range()
00429 void VL53L1X::stopContinuous()
00430 {
00431   writeReg(SYSTEM__MODE_START, 0x80); // mode_range__abort
00432  
00433   // VL53L1_low_power_auto_data_stop_range() begin
00434  
00435   calibrated = false;
00436  
00437   // "restore vhv configs"
00438   if (saved_vhv_init != 0)
00439   {
00440     writeReg(VHV_CONFIG__INIT, saved_vhv_init);
00441   }
00442   if (saved_vhv_timeout != 0)
00443   {
00444      writeReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, saved_vhv_timeout);
00445   }
00446  
00447   // "remove phasecal override"
00448   writeReg(PHASECAL_CONFIG__OVERRIDE, 0x00);
00449  
00450   // VL53L1_low_power_auto_data_stop_range() end
00451 }
00452  
00453 // Returns a range reading in millimeters when continuous mode is active
00454 // (readRangeSingleMillimetersx () also calls this function after starting a
00455 // single-shot range measurement)
00456 uint16_t VL53L1X::read(bool blocking)
00457 {
00458   if (blocking)
00459   {
00460   //  startTimeout();
00461     
00462     /* dataReady returns 0. Loop is never entered. */
00463     /*
00464     while (dataReady())
00465     {
00466       if (checkTimeoutExpired())
00467       {
00468         did_timeout = true;
00469         ranging_data.range_status = None;
00470         ranging_data.range_mm = 0;
00471         ranging_data.peak_signal_count_rate_MCPS = 0;
00472         ranging_data.ambient_count_rate_MCPS = 0;
00473         return ranging_data.range_mm;
00474       }
00475     }*/
00476   }
00477  
00478   readResults();
00479  
00480   if (!calibrated)
00481   {
00482     setupManualCalibration();
00483     calibrated = true;
00484   }
00485  
00486   updateDSS();
00487  
00488   getRangingData();
00489  
00490   writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
00491     
00492   return ranging_data.range_mm;
00493 }
00494  
00495 // convert a RangeStatus to a readable string
00496 // Note that on an AVR, these strings are stored in RAM (dynamic memory), which
00497 // makes working with them easier but uses up 200+ bytes of RAM (many AVR-based
00498 // Arduinos only have about 2000 bytes of RAM). You can avoid this memory usage
00499 // if you do not call this function in your sketch.
00500 const char * VL53L1X::rangeStatusToString(RangeStatus status)
00501 {
00502   switch (status)
00503   {
00504     case RangeValid:
00505       return "range valid";
00506  
00507     case SigmaFail:
00508       return "sigma fail";
00509  
00510     case SignalFail:
00511       return "signal fail";
00512  
00513     case RangeValidMinRangeClipped:
00514       return "range valid, min range clipped";
00515  
00516     case OutOfBoundsFail:
00517       return "out of bounds fail";
00518  
00519     case HardwareFail:
00520       return "hardware fail";
00521  
00522     case RangeValidNoWrapCheckFail:
00523       return "range valid, no wrap check fail";
00524  
00525     case WrapTargetFail:
00526       return "wrap target fail";
00527  
00528     case XtalkSignalFail:
00529       return "xtalk signal fail";
00530  
00531     case SynchronizationInt:
00532       return "synchronization int";
00533  
00534     case MinRangeFail:
00535       return "min range fail";
00536  
00537     case None:
00538       return "no update";
00539  
00540     default:
00541       return "unknown status";
00542   }
00543 }
00544  
00545 // Did a timeout occur in one of the read functions since the last call to
00546 // timeoutOccurred()?
00547 bool VL53L1X::timeoutOccurred()
00548 {
00549   bool tmp = did_timeout;
00550   did_timeout = false;
00551   return tmp;
00552 }
00553  
00554 // Private Methods /////////////////////////////////////////////////////////////
00555  
00556 // "Setup ranges after the first one in low power auto mode by turning off
00557 // FW calibration steps and programming static values"
00558 // based on VL53L1_low_power_auto_setup_manual_calibration()
00559 void VL53L1X::setupManualCalibration()
00560 {
00561   // "save original vhv configs"
00562   saved_vhv_init = readReg(VHV_CONFIG__INIT);
00563   saved_vhv_timeout = readReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND);
00564  
00565   // "disable VHV init"
00566   writeReg(VHV_CONFIG__INIT, saved_vhv_init & 0x7F);
00567  
00568   // "set loop bound to tuning param"
00569   writeReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,
00570     (saved_vhv_timeout & 0x03) + (3 << 2)); // tuning parm default (LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT)
00571  
00572   // "override phasecal"
00573   writeReg(PHASECAL_CONFIG__OVERRIDE, 0x01);
00574   writeReg(CAL_CONFIG__VCSEL_START, readReg(PHASECAL_RESULT__VCSEL_START));
00575 }
00576  
00577 // read measurement results into buffer
00578 void VL53L1X::readResults()
00579 {
00580   char infoToWrite[2];
00581   char infoToRead[18];
00582   //_i2c.beginTransmission(address);
00583   //_i2c.write(address);
00584   //_i2c.write((RESULT__RANGE_STATUS >> 8) & 0xFF); // reg high byte
00585   //_i2c.write( RESULT__RANGE_STATUS       & 0xFF); // reg low byte
00586 //  last_status = _i2c.endTransmission();
00587   infoToWrite[0] = ((RESULT__RANGE_STATUS >> 8) & 0xFF);
00588   infoToWrite[1] = ( RESULT__RANGE_STATUS       & 0xFF);
00589   _i2c.write(address, infoToWrite, 2, 1);
00590  
00591 //  _i2c.requestFrom(address, (uint8_t)17);
00592   _i2c.read(address, infoToRead, 17, 0);
00593  
00594   wait(.005);
00595   results.range_status = infoToRead[0];
00596  
00597 //  infoToRead[1]; // report_status: not used
00598  
00599   results.stream_count = infoToRead[2];
00600  
00601   results.dss_actual_effective_spads_sd0  = (uint16_t)infoToRead[3] << 8; // high byte
00602   results.dss_actual_effective_spads_sd0 |=           infoToRead[4];      // low byte
00603  
00604 //  infoToRead[5]; // peak_signal_count_rate_mcps_sd0: not used
00605 //  infoToRead[6];
00606  
00607   results.ambient_count_rate_mcps_sd0  = (uint16_t)infoToRead[7] << 8; // high byte
00608   results.ambient_count_rate_mcps_sd0 |=           infoToRead[8];      // low byte
00609  
00610 //  infoToRead[9]; // sigma_sd0: not used
00611 //  infoToRead[10];
00612  
00613 //  infoToRead[11]; // phase_sd0: not used
00614 //  infoToRead[12];
00615  
00616   results.final_crosstalk_corrected_range_mm_sd0  = (uint16_t)infoToRead[13] << 8; // high byte
00617   results.final_crosstalk_corrected_range_mm_sd0 |=           infoToRead[14];      // low byte
00618  
00619   results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0  = (uint16_t)infoToRead[15] << 8; // high byte
00620   results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 |=           infoToRead[16];      // low byte
00621 }
00622  
00623 // perform Dynamic SPAD Selection calculation/update
00624 // based on VL53L1_low_power_auto_update_DSS()
00625 void VL53L1X::updateDSS()
00626 {
00627   uint16_t spadCount = results.dss_actual_effective_spads_sd0;
00628  
00629   if (spadCount != 0)
00630   {
00631     // "Calc total rate per spad"
00632  
00633     uint32_t totalRatePerSpad =
00634       (uint32_t)results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 +
00635       results.ambient_count_rate_mcps_sd0;
00636  
00637     // "clip to 16 bits"
00638     if (totalRatePerSpad > 0xFFFF) { totalRatePerSpad = 0xFFFF; }
00639  
00640     // "shift up to take advantage of 32 bits"
00641     totalRatePerSpad <<= 16;
00642  
00643     totalRatePerSpad /= spadCount;
00644  
00645     if (totalRatePerSpad != 0)
00646     {
00647       // "get the target rate and shift up by 16"
00648       uint32_t requiredSpads = ((uint32_t)TargetRate << 16) / totalRatePerSpad;
00649  
00650       // "clip to 16 bit"
00651       if (requiredSpads > 0xFFFF) { requiredSpads = 0xFFFF; }
00652  
00653       // "override DSS config"
00654       writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, requiredSpads);
00655       // DSS_CONFIG__ROI_MODE_CONTROL should already be set to REQUESTED_EFFFECTIVE_SPADS
00656  
00657       return;
00658     }
00659   }
00660  
00661   // If we reached this point, it means something above would have resulted in a
00662   // divide by zero.
00663   // "We want to gracefully set a spad target, not just exit with an error"
00664  
00665    // "set target to mid point"
00666    writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 0x8000);
00667 }
00668  
00669 // get range, status, rates from results buffer
00670 // based on VL53L1_GetRangingMeasurementData()
00671 void VL53L1X::getRangingData()
00672 {
00673   // VL53L1_copy_sys_and_core_results_to_range_results() begin
00674  
00675   uint16_t range = results.final_crosstalk_corrected_range_mm_sd0;
00676  
00677   // "apply correction gain"
00678   // gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT)
00679   // Basically, this appears to scale the result by 2011/2048, or about 98%
00680   // (with the 1024 added for proper rounding).
00681   ranging_data.range_mm = ((uint32_t)range * 2011 + 0x0400) / 0x0800;
00682   wait(.005);
00683   // VL53L1_copy_sys_and_core_results_to_range_results() end
00684  
00685   // set range_status in ranging_data based on value of RESULT__RANGE_STATUS register
00686   // mostly based on ConvertStatusLite()
00687   switch(results.range_status)
00688   {
00689     case 17: // MULTCLIPFAIL
00690     case 2: // VCSELWATCHDOGTESTFAILURE
00691     case 1: // VCSELCONTINUITYTESTFAILURE
00692     case 3: // NOVHVVALUEFOUND
00693       // from SetSimpleData()
00694       ranging_data.range_status = HardwareFail;
00695       break;
00696  
00697     case 13: // USERROICLIP
00698      // from SetSimpleData()
00699       ranging_data.range_status = MinRangeFail;
00700       break;
00701  
00702     case 18: // GPHSTREAMCOUNT0READY
00703       ranging_data.range_status = SynchronizationInt;
00704       break;
00705  
00706     case 5: // RANGEPHASECHECK
00707       ranging_data.range_status =  OutOfBoundsFail;
00708       break;
00709  
00710     case 4: // MSRCNOTARGET
00711       ranging_data.range_status = SignalFail;
00712       break;
00713  
00714     case 6: // SIGMATHRESHOLDCHECK
00715       ranging_data.range_status = SignalFail;
00716       break;
00717  
00718     case 7: // PHASECONSISTENCY
00719       ranging_data.range_status = WrapTargetFail;
00720       break;
00721  
00722     case 12: // RANGEIGNORETHRESHOLD
00723       ranging_data.range_status = XtalkSignalFail;
00724       break;
00725  
00726     case 8: // MINCLIP
00727       ranging_data.range_status = RangeValidMinRangeClipped;
00728       break;
00729  
00730     case 9: // RANGECOMPLETE
00731       // from VL53L1_copy_sys_and_core_results_to_range_results()
00732       if (results.stream_count == 0)
00733       {
00734         ranging_data.range_status = RangeValidNoWrapCheckFail;
00735       }
00736       else
00737       {
00738         ranging_data.range_status = RangeValid;
00739       }
00740       break;
00741  
00742     default:
00743       ranging_data.range_status = None;
00744   }
00745  
00746   // from SetSimpleData()
00747   ranging_data.peak_signal_count_rate_MCPS =
00748     countRateFixedToFloat(results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0);
00749   ranging_data.ambient_count_rate_MCPS =
00750     countRateFixedToFloat(results.ambient_count_rate_mcps_sd0);
00751 }
00752  
00753 // Decode sequence step timeout in MCLKs from register value
00754 // based on VL53L1_decode_timeout()
00755 uint32_t VL53L1X::decodeTimeout(uint16_t reg_val)
00756 {
00757   return ((uint32_t)(reg_val & 0xFF) << (reg_val >> 8)) + 1;
00758 }
00759  
00760 // Encode sequence step timeout register value from timeout in MCLKs
00761 // based on VL53L1_encode_timeout()
00762 uint16_t VL53L1X::encodeTimeout(uint32_t timeout_mclks)
00763 {
00764   // encoded format: "(LSByte * 2^MSByte) + 1"
00765  
00766   uint32_t ls_byte = 0;
00767   uint16_t ms_byte = 0;
00768  
00769   if (timeout_mclks > 0)
00770   {
00771     ls_byte = timeout_mclks - 1;
00772  
00773     while ((ls_byte & 0xFFFFFF00) > 0)
00774     {
00775       ls_byte >>= 1;
00776       ms_byte++;
00777     }
00778  
00779     return (ms_byte << 8) | (ls_byte & 0xFF);
00780   }
00781   else { return 0; }
00782 }
00783  
00784 // Convert sequence step timeout from macro periods to microseconds with given
00785 // macro period in microseconds (12.12 format)
00786 // based on VL53L1_calc_timeout_us()
00787 uint32_t VL53L1X::timeoutMclksToMicroseconds(uint32_t timeout_mclks, uint32_t macro_period_us)
00788 {
00789   return ((uint64_t)timeout_mclks * macro_period_us + 0x800) >> 12;
00790 }
00791  
00792 // Convert sequence step timeout from microseconds to macro periods with given
00793 // macro period in microseconds (12.12 format)
00794 // based on VL53L1_calc_timeout_mclks()
00795 uint32_t VL53L1X::timeoutMicrosecondsToMclks(uint32_t timeout_us, uint32_t macro_period_us)
00796 {
00797   return (((uint32_t)timeout_us << 12) + (macro_period_us >> 1)) / macro_period_us;
00798 }
00799  
00800 // Calculate macro period in microseconds (12.12 format) with given VCSEL period
00801 // assumes fast_osc_frequency has been read and stored
00802 // based on VL53L1_calc_macro_period_us()
00803 uint32_t VL53L1X::calcMacroPeriod(uint8_t vcsel_period)
00804 {
00805   // from VL53L1_calc_pll_period_us()
00806   // fast osc frequency in 4.12 format; PLL period in 0.24 format
00807   uint32_t pll_period_us = ((uint32_t)0x01 << 30) / fast_osc_frequency;
00808  
00809   // from VL53L1_decode_vcsel_period()
00810   uint8_t vcsel_period_pclks = (vcsel_period + 1) << 1;
00811  
00812   // VL53L1_MACRO_PERIOD_VCSEL_PERIODS = 2304
00813   uint32_t macro_period_us = (uint32_t)2304 * pll_period_us;
00814   macro_period_us >>= 6;
00815   macro_period_us *= vcsel_period_pclks;
00816   macro_period_us >>= 6;
00817  
00818   return macro_period_us;
00819 }
00820 
00821 
00822 
00823 
00824 
00825 
00826 
00827 
00828 
00829 
00830 bool VL53L1X::initReading(int addr, int timing_budget)
00831 {
00832   turnOn();
00833   wait_ms(100);
00834   setTimeout(500);
00835   if (!init())  {
00836     didInitialize = false;
00837     return false;
00838   }
00839  // setDistanceMode(VL53L1X::Short);//Short Medium Long
00840   setAddress(addr);//change I2C address for next sensor
00841   setMeasurementTimingBudget(timing_budget);//min 20ms for Short, 33ms for Medium and Long
00842   startContinuous(50);
00843   wait_ms(100);
00844   didInitialize = true;
00845   return true;
00846 }
00847 //*************************************
00848 
00849 //*********GPIO***********
00850 void VL53L1X::turnOff(void)
00851 {
00852   //turn pin LOW
00853   _shutDown = false;
00854 }
00855 void VL53L1X::resetPin(void)
00856 {
00857   //reset pin and set it to LOW
00858     _shutDown = false;
00859     wait(.05);
00860     _shutDown = true;
00861     wait(.05); 
00862     _shutDown = false;
00863     wait(.05);  
00864 
00865 }
00866 void VL53L1X::turnOn(void)
00867 {
00868   //turn pin HIGH
00869     _shutDown = true;
00870 }
00871 int VL53L1X::readFromOneSensor(void)
00872 {
00873     if (didInitialize)   //create bool
00874         return medianFilter();
00875     else 
00876         return -1;  
00877 }
00878 int VL53L1X::medianFilter(void)
00879 {
00880     rawData[1] = rawData[0];
00881     rawData[2] = rawData[1];
00882     rawData[0] = read();
00883     if((rawData[0] < rawData[1] && rawData[0] > rawData[2]) || (rawData[0] > rawData[1] && rawData[0] < rawData[2]))
00884         return rawData[0];
00885     else if((rawData[1] < rawData[0] && rawData[1] > rawData[2]) || (rawData[1] > rawData[0] && rawData[1] < rawData[2]))
00886         return rawData[1];
00887     else
00888         return rawData[2];
00889 }