Diff: main.cpp

Revision:

3:09f23aad108a

Parent:

2:25bcfa4b1aca

Child:

4:738d308ed16e

--- a/main.cpp	Wed May 12 14:30:46 2021 +0000
+++ b/main.cpp	Thu May 13 08:31:52 2021 +0000
@@ -136,51 +136,93 @@
 {
     int status = 0;
     bool current = false;
+    
     uint16_t distance = 0;
+    
+    VL53L1_MultiRangingData_t MultiRangingData;
+    VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
 
     /* Handle the interrupt and output the range from the centre sensor */
     if (centerSensor) {
         centerSensor = false;
-//        board->sensor_centre->handle_irq(&distance);
+
         status = board->sensor_centre->VL53L1_GetDistance(&distance);
-        status = board->sensor_centre->VL53L1_ClearInterrupt();
-        board->sensor_centre->enable_interrupt_measure_detection_irq();
         
         current = (currentSensor == 0);
         if (current) {
             printf("Centre: %d\r\n", distance);
         }
+        
+        status = board->sensor_centre->VL53L1_ClearInterrupt();
+        board->sensor_centre->enable_interrupt_measure_detection_irq();        
+        
     }
 
     /* Handle the interrupt and output the range from the left sensor */
     if (leftSensor) {
         leftSensor = false;
-//        board->sensor_left->handle_irq(&distance);
+        
         status = board->sensor_left->VL53L1_GetDistance(&distance);
-        status = board->sensor_left->VL53L1_ClearInterrupt();
-        board->sensor_left->enable_interrupt_measure_detection_irq();
         
         current = (installedSensors[currentSensor] == 'L');
         if (current) {
             printf("Left: %d\r\n", distance);
         }
+        status = board->sensor_left->VL53L1_ClearInterrupt();
+        board->sensor_left->enable_interrupt_measure_detection_irq();
+        
     }
 
     /* Handle the interrupt and output the range from the right sensor */
     if (rightSensor) {
         rightSensor = false;
-//        board->sensor_right->handle_irq(&distance);
+        
         status = board->sensor_right->VL53L1_GetDistance(&distance);
-        status = board->sensor_right->VL53L1_ClearInterrupt();
-        board->sensor_right->enable_interrupt_measure_detection_irq();
         
         current = (installedSensors[currentSensor] == 'R');
         if (current) {
             printf("Right: %d\r\n", distance);
         }
+        status = board->sensor_right->VL53L1_ClearInterrupt();
+        board->sensor_right->enable_interrupt_measure_detection_irq();
+        
     }    
 }
 
+int configure_sensor(VL53L1 *Sensor)
+{
+    int status = 0;
+    
+// Device Initialization and setting 
+    status = Sensor->vl53L1_DataInit();
+    status = Sensor->vl53L1_StaticInit();
+    status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
+    
+    //configure the regions of interest for each sensor
+    VL53L1_RoiConfig_t roiConfig;
+    
+    roiConfig.NumberOfRoi = 3;
+    roiConfig.UserRois[0].TopLeftX = 0;
+    roiConfig.UserRois[0].TopLeftY = 9; 
+    roiConfig.UserRois[0].BotRightX = 4;
+    roiConfig.UserRois[0].BotRightY = 5; 
+    roiConfig.UserRois[1].TopLeftX = 5;
+    roiConfig.UserRois[1].TopLeftY = 9; 
+    roiConfig.UserRois[1].BotRightX = 9;
+    roiConfig.UserRois[1].BotRightY = 4; 
+    roiConfig.UserRois[2].TopLeftX = 11;
+    roiConfig.UserRois[2].TopLeftY = 9; 
+    roiConfig.UserRois[2].BotRightX = 15;
+    roiConfig.UserRois[2].BotRightY = 5; 
+    status = Sensor->vl53L1_SetROI( &roiConfig);
+    
+    
+    status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
+//       status = Sensor->VL53L1_SetMeasurementTimingBudgetMicroSeconds( 100 * 500); // error -21 is because of this. Don't know why
+
+    return status;
+}
+
 /*
  * Add to an array a character that represents the sensor and start ranging
  */
@@ -195,51 +237,23 @@
     if (board->sensor_centre != NULL) {
         Dev = &devCentre;
         Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
-        Sensor = board->sensor_centre;
+        status = configure_sensor(board->sensor_centre);
         printf("configuring centre channel \n");
     }
     
     if (board->sensor_left != NULL) {
         Dev = &devLeft; 
         Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
-        Sensor = board->sensor_left;
+        status = configure_sensor(board->sensor_left);
         printf("configuring left channel \n");
     }
     
     if (board->sensor_right != NULL) {
         Dev = &devRight;  
         Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
-        Sensor = board->sensor_right;
+        status = configure_sensor(board->sensor_right);
         printf("configuring right channel \n");
     }
-/*
-// Device Initialization and setting 
-        status = Sensor->vl53L1_DataInit();
-        status = Sensor->vl53L1_StaticInit();
-        status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
-   
-   //configure the regions of interest for each sensor
-        VL53L1_RoiConfig_t roiConfig;
-        roiConfig.NumberOfRoi = 3;
-        roiConfig.UserRois[0].TopLeftX = 0;
-        roiConfig.UserRois[0].TopLeftY = 9; 
-        roiConfig.UserRois[0].BotRightX = 4;
-        roiConfig.UserRois[0].BotRightY = 5; 
-        roiConfig.UserRois[1].TopLeftX = 5;
-        roiConfig.UserRois[1].TopLeftY = 9; 
-        roiConfig.UserRois[1].BotRightX = 9;
-        roiConfig.UserRois[1].BotRightY = 4; 
-        roiConfig.UserRois[2].TopLeftX = 11;
-        roiConfig.UserRois[2].TopLeftY = 9; 
-        roiConfig.UserRois[2].BotRightX = 15;
-        roiConfig.UserRois[2].BotRightY = 5; 
-        status = Sensor->vl53L1_SetROI( &roiConfig);
-        
-        
-        status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
- //       status = Sensor->VL53L1_SetMeasurementTimingBudgetMicroSeconds( 100 * 500); // error -21 is because of this. Don't know why
-*/ 
-  
     
     /* start the measure on the center sensor */
     if (NULL != board->sensor_centre) {
@@ -295,7 +309,7 @@
     stop_button.rise(&switch_measuring_sensor_irq);
     stop_button.enable_irq();
     
-//    pc.baud(115200);  // baud rate is important as printf statements take a lot of time
+    pc.baud(115200);  // baud rate is important as printf statements take a lot of time
     
     printf("mbed version : %d \r\n", MBED_VERSION);
 
@@ -324,82 +338,11 @@
 
     printf("loop forever\n");    
     
-/*
-       // loop waiting for interrupts to happen. This is signaled by int_centre_result,int_left_result or int_right_result
-       // being non zero. When the interrupts clear this is signaled by int_centre_dropped,int_left_dropped and int_right_dropped. 
-       // These are set back to zero when processing is completed
-    while (1)
-    {
-        VL53L1_MultiRangingData_t MultiRangingData;
-        VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;   
-        
-        if ( int_left_dropped || int_centre_dropped || int_right_result)
-            wait_ms(20);        
-
-        
-        // when the interrupt pin goes low start new measurement
-        if ( int_centre_dropped != 0)
-        {
-               int_centre_dropped = 0;
-               status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();// get next measurement
-        }
-             
-        if ( int_left_dropped != 0)
-        {
-               int_left_dropped = 0;
-               status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
-        }
-        
-        if ( int_right_dropped != 0)
-        {
-               int_right_dropped = 0;
-               status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement();
-        }
-            
-        if (int_right_result != 0) // interrupt seen on right sensor
-        {
-            status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData);
-            if ( status == 0)
-            {
-                print_results( devRight.i2c_slave_address, pMultiRangingData );
-            }
-            
-            // clear interrupt flag
-            int_right_result = 0;
-        }
-
-
-        if (int_left_result != 0)  // interrupt seen on left sensor
-        {
-            status = board->sensor_left->vl53L1_GetMultiRangingData(pMultiRangingData);
-            if ( status == 0)
-            {
-                print_results( devLeft.i2c_slave_address, pMultiRangingData );
-            }
-
-            // clear interrupt flag
-            int_left_result = 0;
-        }
-          
-        if (int_centre_result != 0)
-        {                           
-            status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
-            if(status==0) {
-                print_results(devCentre.i2c_slave_address, pMultiRangingData );
-            } //if(status==0) 
-            
-            // clear interrupt flag
-            int_centre_result = 0;
-
-        }
-            
-        wait_ms( 1 * 5);
-                              
-    }
-*/
     /* Main ranging interrupt loop */
     while (true) {
+        
         measure_sensors();
+        
         if (switchChanged) {
             ++currentSensor;
             if (currentSensor == sensorCnt)
@@ -414,35 +357,39 @@
     
     
  // print out what data is required   
-void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
+void print_results( int devNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
 {
-            int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
-            
-            int RoiNumber=pMultiRangingData->RoiNumber;
-            int RoiStatus=pMultiRangingData->RoiStatus;
+    int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
+    int signal_rate = 0;
+    int ambient_rate = 0;
+
+    int RoiNumber=pMultiRangingData->RoiNumber;
 
-            if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
-            {
-                for(int j=0;j<no_of_object_found;j++){
-                    if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
-                        (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
-                    {
-                        printf("\t spiAddr=%d \t RoiNumber=%d  \t D=%5dmm \n",
-                                devSpiNumber,
-                                RoiNumber,
-                                pMultiRangingData->RangeData[j].RangeMilliMeter);
-                    }
-                    else
-                    {
-//                               printf("RangeStatus %d %d\n",j, pMultiRangingData->RangeData[j].RangeStatus);
-                    }
-                }
-            } // if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
-            else
-            {
-//                       printf("no_of_object_found %d \n",no_of_object_found);
+    if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) {
+        for(int j=0; j<no_of_object_found; j++) {
+            if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
+                    (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL)) {
+                signal_rate = pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535;
+                ambient_rate = pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535;
+                printf("\t i2cAddr=%d \t RoiNumber=%d   \t status=%d, \t D=%5dmm, \t Signal=%d Mcps, \t Ambient=%d Mcps \n",
+                       devNumber, RoiNumber,
+                       pMultiRangingData->RangeData[j].RangeStatus,
+                       pMultiRangingData->RangeData[j].RangeMilliMeter,
+                       signal_rate,
+                       ambient_rate);
+/*
+// online compiler disables printf() / floating-point support, for code-size reasons.                        
+// offline compiler can switch it on.
+                printf("\t i2cAddr=%d \t RoiNumber=%d   \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
+                       devNumber, RoiNumber,
+                       pMultiRangingData->RangeData[j].RangeStatus,
+                       pMultiRangingData->RangeData[j].RangeMilliMeter,
+                       pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535.0,
+                       pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535.0);
+*/                       
             }
-
+        }
+    } // if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0))
 }    
 
     
@@ -452,5 +399,5 @@
  {
     thread_sleep_for(ms);
  }
- #endif
+#endif