Diff: VL53L1X.cpp

Revision:

4:a9362bc0597a

Parent:

3:a7f10344f941

--- a/VL53L1X.cpp	Fri Jul 20 16:39:22 2018 +0000
+++ b/VL53L1X.cpp	Tue Jul 24 17:41:28 2018 +0000
@@ -2,6 +2,7 @@
 #include "mbed.h"
 
 //Serial pc(USBTX,USBRX);
+DigitalOut led1(LED1);
 
 uint8_t configBlock[] = {
   0x29, 0x02, 0x10, 0x00, 0x28, 0xBC, 0x7A, 0x81, //8
@@ -41,6 +42,7 @@
   uint16_t modelID = readRegister16(VL53L1_IDENTIFICATION__MODEL_ID);
   printf("%x \r\n", modelID);
   if (modelID != 0xEACC){
+    led1 = !led1;
     return (false);
   }
   softReset();
@@ -65,9 +67,10 @@
   //Gets trim resistors from chip
   for (uint16_t i = 0; i < 36; i++) {
       uint8_t regVal = readRegister(i + 1);
-  //    if(configBlock[i] != regVal) printf("thanks for letting me know, %d, %x\r\n", i, regVal);
+      //if(configBlock[i] != regVal) printf("thanks for letting me know, %d, %x\r\n", i, regVal);
       configBlock[i] = regVal;
   }
+  
   startMeasurement();
 
   return (true); //Sensor online!
@@ -83,28 +86,28 @@
     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(_deviceAddress);//    _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]);
- //     printf("Data sent %x\r\n", configBlock[address + x - 1 - offset]);
+      printf("Data sent %x\r\n", configBlock[address + x - 1 - offset]);
       }
 
-
- //   _i2c.endTransmission();
+    // _i2c.endTransmission();
 
     leftToSend -= toSend;
     address += toSend;
-//    printf("I actually went through %d\r\n", leftToSend);
+    // printf("I actually went through %d\r\n", leftToSend);
   }
 }
 
 bool VL53L1X::newDataReady(void)
 {
   int read = readRegister(VL53L1_GPIO__TIO_HV_STATUS);
-  printf("read register %x\r\n", read);
+  printf("read register %x \r", read);
   
   if (read != 0x03) return(true); //New measurement!
   return(false); //No new data
@@ -227,7 +230,7 @@
 #define VL53L1_RANGESTATUS_NONE        255 /*!<No Update. */
 
   //Read status
-  uint8_t measurementStatus = readRegister(VL53L1_RESULT__RANGE_STATUS) & 0x1F;
+  uint8_t measurementStatus = (readRegister(VL53L1_RESULT__RANGE_STATUS) & 0x1F);
 
   //Convert status from one to another - From vl53l1_api.c
   switch (measurementStatus) {
@@ -265,40 +268,43 @@
   return measurementStatus;
 }
 
-
 uint8_t VL53L1X::readRegister(uint16_t registerAddr)
 {
   uint8_t data;
   char data_write[2];
-  char data_read[1];
+  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, data_write, 2,0); 
-  _i2c.read(_deviceAddress,data_read,1,1);
+  _i2c.write(_deviceAddress, data_write, 2, 1); 
+  _i2c.read(_deviceAddress, data_read, 2, 0);
   //Read Data from selected register
-  data=data_read[0];
+  data = (data_read[0] << 8) | data_read[1];
   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];
+  
+  //Tell sensor to send data back
   data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
   data_write[1] = registerAddr & 0xFF; //LSB of register address 
-  _i2c.write(_deviceAddress, data_write, 2,0); 
-  _i2c.read(_deviceAddress,data_read,2,1);
+  _i2c.write(_deviceAddress, data_write, 2, 1); 
+  
+  //Read data from Sensor
+  _i2c.read(_deviceAddress, data_read, 2, 0);
   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];
@@ -307,13 +313,13 @@
     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); 
+    data_write[0] = (registerAddr >> 9) & 0xFF; //MSB of register address 
+    //data_write[1] = registerAddr & 0xFF; //LSB of register address 
+    data_write[1] = (data >> 8) & 0xFF;
+    data_write[2] = data & 0xFF; 
+    _i2c.write(_deviceAddress, data_write, 3); 
 }
\ No newline at end of file