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