Diff: Components/VL53L0X/vl53l0x_class.cpp

Revision:

13:615f7e38568c

Parent:

12:f6e2bad00dc7

Child:

14:8320b5ff96fa

diff -r f6e2bad00dc7 -r 615f7e38568c Components/VL53L0X/vl53l0x_class.cpp
--- a/Components/VL53L0X/vl53l0x_class.cpp	Wed Jun 14 14:39:27 2017 +0000
+++ b/Components/VL53L0X/vl53l0x_class.cpp	Thu Jun 15 13:33:35 2017 +0000
@@ -2266,6 +2266,30 @@
 	return Status;
 }
 
+VL53L0X_Error VL53L0X::VL53L0X_SetInterruptThresholds(VL53L0X_DEV Dev,
+	VL53L0X_DeviceModes DeviceMode, FixPoint1616_t ThresholdLow,
+	FixPoint1616_t ThresholdHigh)
+{
+	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
+	uint16_t Threshold16;
+	LOG_FUNCTION_START("");
+
+	/* no dependency on DeviceMode for Ewok */
+	/* Need to divide by 2 because the FW will apply a x2 */
+	Threshold16 = (uint16_t)((ThresholdLow >> 17) & 0x00fff);
+	Status = VL53L0X_WrWord(Dev, VL53L0X_REG_SYSTEM_THRESH_LOW, Threshold16);
+
+	if (Status == VL53L0X_ERROR_NONE) {
+		/* Need to divide by 2 because the FW will apply a x2 */
+		Threshold16 = (uint16_t)((ThresholdHigh >> 17) & 0x00fff);
+		Status = VL53L0X_WrWord(Dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
+			Threshold16);
+	}
+
+	LOG_FUNCTION_END(Status);
+	return Status;
+}
+
 VL53L0X_Error VL53L0X::VL53L0X_GetInterruptThresholds(VL53L0X_DEV Dev,
 	VL53L0X_DeviceModes DeviceMode, FixPoint1616_t *pThresholdLow,
 	FixPoint1616_t *pThresholdHigh)
@@ -5156,6 +5180,10 @@
 {
    int status, ClrStatus;
 
+   status = VL53L0X_StopMeasurement(Device); // it is safer to do this while sensor is stopped
+
+//   status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
+
    status = VL53L0X_SetGpioConfig(Device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
 		VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
 		VL53L0X_INTERRUPTPOLARITY_HIGH);
@@ -5169,6 +5197,7 @@
    ClrStatus=ClearInterrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS|VL53L0X_REG_RESULT_RANGE_STATUS);
    if(ClrStatus)
 	   VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
+
    if(!status)
    {
       status=RangeStartContinuousMode();
@@ -5180,6 +5209,7 @@
 int VL53L0X::StartMeasurement(OperatingMode operating_mode, void (*fptr)(void))
 {
     int Status = VL53L0X_ERROR_NONE;
+    int ClrStatus;
 
     uint8_t VhvSettings;
     uint8_t PhaseCal;
@@ -5198,22 +5228,34 @@
             printf ("GPIO1 Error\r\n");
             return 1;
         }
-        printf ("Setting up GPIO Config\r\n");
-        Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
-        
-        if (Status == VL53L0X_ERROR_NONE) {
-        	Status = RangeConfigInterrupt(VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
+
+        Status = VL53L0X_StopMeasurement(Device); // it is safer to do this while sensor is stopped
+
+//        Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
+
+        Status = VL53L0X_SetGpioConfig(Device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
+                        VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
+						VL53L0X_INTERRUPTPOLARITY_HIGH);
+
+        if (Status == VL53L0X_ERROR_NONE)
+        {
+     	   	AttachInterruptMeasureDetectionIRQ(fptr);
+     	    EnableInterruptMeasureDetectionIRQ();
         }
-        if (Status == VL53L0X_ERROR_NONE) {
-            Status = RangeMeasIntContinuousMode(fptr);
-        }
-                        
+
+        ClrStatus=ClearInterrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS|VL53L0X_REG_RESULT_RANGE_STATUS);
+        if(ClrStatus)
+     	   VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
+
         if(Status == VL53L0X_ERROR_NONE)
         {
-	        printf ("Call of VL53L0X_StartMeasurement\n");
-	        Status = VL53L0X_StartMeasurement(Device);
-        }        
-            return Status;
+            Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
+        }
+
+        if(Status == VL53L0X_ERROR_NONE)
+        {
+		    Status = VL53L0X_StartMeasurement(Device);
+        }
     }
 
     if (operating_mode == range_single_shot_polling)