Diff: VL53L1X.cpp

Revision:

1:67ced5a2c689

Parent:

0:76ea242a637f

--- a/VL53L1X.cpp	Mon Jul 30 18:45:53 2018 +0000
+++ b/VL53L1X.cpp	Tue Jul 31 23:47:38 2018 +0000
@@ -55,7 +55,7 @@
   // VL53L1_poll_for_boot_completion() begin
 
   startTimeout();
-  int firmware = (readReg(FIRMWARE__SYSTEM_STATUS));
+  int firmware = (readReg16Bit(FIRMWARE__SYSTEM_STATUS));
   printf("firmware : %x\r\n", firmware);
   while ((readReg(FIRMWARE__SYSTEM_STATUS) & 0x01) == 0)
   {
@@ -183,6 +183,18 @@
  
 
 // Write a 32-bit register
+/*
+void VL53L1X::writeReg32Bit(uint16_t registerAddr, uint32_t data)
+{
+    char data_write[5];
+    data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
+    data_write[1] = registerAddr & 0xFF; //LSB of register address 
+    data_write[2] = (data >> 16) & 0xFF;
+    data_write[3] = (data >> 8) & 0xFF; 
+    data_write[4] =  data & 0xFF;
+    _i2c.write(address, data_write, 5); 
+}
+*/
 void VL53L1X::writeReg32Bit(uint16_t registerAddr, uint32_t data)
 {
     char data_write[6];
@@ -193,16 +205,9 @@
     data_write[4] = (data >> 8) & 0xFF;;
     data_write[5] =  data & 0xFF;
     _i2c.write(address, data_write, 6); 
- // _i2c.beginTransmission(address);
- /* _i2c.write((reg >> 8) & 0xFF); // reg high byte
-  _i2c.write( reg       & 0xFF); // reg low byte
-  _i2c.write((value >> 24) & 0xFF); // value highest byte
-  _i2c.write((value >> 16) & 0xFF);
-  _i2c.write((value >>  8) & 0xFF);
-  _i2c.write( value        & 0xFF); // value lowest byte
-  //last_status = _i2c.endTransmission();*/
 }
 
+
 // Read an 8-bit register
 uint8_t VL53L1X::readReg(uint16_t registerAddr)
 {
@@ -261,7 +266,7 @@
 {
   // save existing timing budget
   uint32_t budget_us = getMeasurementTimingBudget();
-  printf("budget_us = %d", budget_us);
+  printf("budget_us = %d\n", budget_us);
   switch (mode)
   {
     case Short:
@@ -445,17 +450,23 @@
 }
 
 // Returns a range reading in millimeters when continuous mode is active
-// (readRangeSingleMillimeters() also calls this function after starting a
+// (readRangeSingleMillimetersx () also calls this function after starting a
 // single-shot range measurement)
 uint16_t VL53L1X::read(bool blocking)
 {
   if (blocking)
   {
+    //printf("Blocked. ");
     startTimeout();
+    //printf("Timeout started. ");
+    
+    /* dataReady returns 0. Loop is never entered. */
     while (dataReady())
     {
+      printf("Data. ");
       if (checkTimeoutExpired())
       {
+        printf("Timeout expired. ");
         did_timeout = true;
         ranging_data.range_status = None;
         ranging_data.range_mm = 0;
@@ -464,14 +475,17 @@
         return ranging_data.range_mm;
       }
     }
+    printf("\n");
   }
 
   readResults();
 
   if (!calibrated)
   {
+    //printf("Not calibrated");
     setupManualCalibration();
     calibrated = true;
+    //printf(". Jk now it is.\n");
   }
 
   updateDSS();
@@ -568,18 +582,29 @@
 // read measurement results into buffer
 void VL53L1X::readResults()
 {
-//  char infoToWrite[2];
-  char infoToRead[17];
+  char infoToWrite[2];
+  char infoToRead[18];
   //_i2c.beginTransmission(address);
-  _i2c.write((RESULT__RANGE_STATUS >> 8) & 0xFF); // reg high byte
-  _i2c.write( RESULT__RANGE_STATUS       & 0xFF); // reg low byte
+  //_i2c.write(address);
+  //_i2c.write((RESULT__RANGE_STATUS >> 8) & 0xFF); // reg high byte
+  //_i2c.write( RESULT__RANGE_STATUS       & 0xFF); // reg low byte
 //  last_status = _i2c.endTransmission();
+  infoToWrite[0] = ((RESULT__RANGE_STATUS >> 8) & 0xFF);
+  infoToWrite[1] = ( RESULT__RANGE_STATUS       & 0xFF);
+  _i2c.write(address, infoToWrite, 2, 1);
 
 //  _i2c.requestFrom(address, (uint8_t)17);
   _i2c.read(address, infoToRead, 17, 0);
+ 
+  for(int i = 0; i < 17; i++){
+    printf("%x ", infoToRead[i]);
+  }
+  printf("\n");
 
+  //wait_us(79);
   results.range_status = infoToRead[0];
 
+
 //  infoToRead[1]; // report_status: not used
 
   results.stream_count = infoToRead[2];
@@ -665,7 +690,7 @@
   // Basically, this appears to scale the result by 2011/2048, or about 98%
   // (with the 1024 added for proper rounding).
   ranging_data.range_mm = ((uint32_t)range * 2011 + 0x0400) / 0x0800;
-
+  printf("Range Status: ");
   // VL53L1_copy_sys_and_core_results_to_range_results() end
 
   // set range_status in ranging_data based on value of RESULT__RANGE_STATUS register
@@ -678,39 +703,48 @@
     case 3: // NOVHVVALUEFOUND
       // from SetSimpleData()
       ranging_data.range_status = HardwareFail;
+      printf("HardwareFail\n");
       break;
 
     case 13: // USERROICLIP
      // from SetSimpleData()
       ranging_data.range_status = MinRangeFail;
+      printf("MinRangeFail\n");
       break;
 
     case 18: // GPHSTREAMCOUNT0READY
       ranging_data.range_status = SynchronizationInt;
+      printf("SynchronizationInt\n");
       break;
 
     case 5: // RANGEPHASECHECK
       ranging_data.range_status =  OutOfBoundsFail;
+      printf("OutofBoundsFail\n");
       break;
 
     case 4: // MSRCNOTARGET
       ranging_data.range_status = SignalFail;
+      printf("SignalFail\n");
       break;
 
     case 6: // SIGMATHRESHOLDCHECK
       ranging_data.range_status = SignalFail;
+      printf("SignalFail\n");
       break;
 
     case 7: // PHASECONSISTENCY
       ranging_data.range_status = WrapTargetFail;
+      printf("WrapTargetFail\n");
       break;
 
     case 12: // RANGEIGNORETHRESHOLD
       ranging_data.range_status = XtalkSignalFail;
+      printf("XtalkSignalFail\n");
       break;
 
     case 8: // MINCLIP
       ranging_data.range_status = RangeValidMinRangeClipped;
+      printf("Range Valid Min Range Clipped\n");
       break;
 
     case 9: // RANGECOMPLETE
@@ -718,15 +752,18 @@
       if (results.stream_count == 0)
       {
         ranging_data.range_status = RangeValidNoWrapCheckFail;
+        printf("Range Valid No Wrap Check Fail \n");
       }
       else
       {
         ranging_data.range_status = RangeValid;
+        printf("Range Valid\n");
       }
       break;
 
     default:
       ranging_data.range_status = None;
+      printf("None\n");
   }
 
   // from SetSimpleData()