Revision 11:96465baec780, committed 2021-05-18

Comitter:

johnAlexander

Date:

Tue May 18 08:36:02 2021 +0000

Parent:

10:cf7d563200fc

Commit message:

forking, to share internally

Changed in this revision

--- a/main.cpp	Thu May 13 09:17:02 2021 +0000
+++ b/main.cpp	Tue May 18 08:36:02 2021 +0000
@@ -29,6 +29,18 @@
 #define I2C_SDA   D14 
 #define I2C_SCL   D15 
 
+#define NUM_SENSORS 3
+
+// define interrupt pins
+PinName CentreIntPin = A2;
+// the satellite pins depend on solder blobs on the back of the shield.
+// they may not exist or may be one of two sets.
+// the centre pin always exists
+//PinName LeftIntPin = D8;
+PinName RightIntPin = D2;
+// alternate set
+PinName LeftIntPin = D9;
+//PinName RightIntPin = D4;
 
 
 static XNucleo53L1A2 *board=NULL;
@@ -39,6 +51,13 @@
     Serial pc(SERIAL_TX, SERIAL_RX); 
 #endif
 
+#if TARGET_STM  // we are cross compiling for an STM32-Nucleo
+    InterruptIn stop_button(BUTTON1);
+#endif
+#if TARGET_Freescale // we are cross-compiling for NXP FRDM boards.
+    InterruptIn stop_button(SW2);
+#endif
+
 void process_interrupt( VL53L1 * sensor,VL53L1_DEV dev );
 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
 
@@ -47,24 +66,257 @@
 VL53L1_Dev_t                   devRight;
 VL53L1_DEV                     Dev = &devCentre;
 
+/* Installed sensors count */
+int sensorCnt = 0;
+
+/* installed sensors prefixes */
+char installedSensors[3];
+
+/* interrupt requests */
+volatile bool centerSensor = false;
+volatile bool leftSensor = false;
+volatile bool rightSensor = false;
+volatile bool int_measuring_stop = false;
+
+/* Current sensor number*/
+volatile int currentSensor = 0;
+
+/* current displayed sensor change IRQ */
+volatile bool switchChanged = false;
+
+/* ISR callback function of the centre sensor */
+void sensor_centre_irq(void)
+{
+    centerSensor = true;
+    board->sensor_centre->disable_interrupt_measure_detection_irq();
+}
+
+/* ISR callback function of the left sensor */
+void sensor_left_irq(void)
+{
+    leftSensor = true;
+    board->sensor_left->disable_interrupt_measure_detection_irq();
+}
+
+/* ISR callback function of the right sensor */
+void sensor_right_irq(void)
+{
+    rightSensor = true;
+    board->sensor_right->disable_interrupt_measure_detection_irq();
+}
+
+/* ISR callback function of the user blue button to switch measuring sensor. */
+void switch_measuring_sensor_irq(void)
+{
+    stop_button.disable_irq();
+    switchChanged = true;
+}
+
+/*
+ * This function calls the interrupt handler for each sensor
+ * and outputs the range
+ */
+inline void measure_sensors()
+{
+    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;
+
+        status = board->sensor_centre->VL53L1_GetDistance(&distance);
+        
+        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;
+        
+        status = board->sensor_left->VL53L1_GetDistance(&distance);
+        
+        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;
+        
+        status = board->sensor_right->VL53L1_GetDistance(&distance);
+        
+        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;
+/*    
+    //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);
+*/    
+// Device Initialization and setting 
+    status = Sensor->vl53L1_DataInit();
+    status = Sensor->vl53L1_StaticInit();
+
+    status = Sensor->vl53L1_SetPresetMode(VL53L1_PRESETMODE_MULTIZONES_SCANNING);
+    status = Sensor->vl53L1_SetDistanceMode(VL53L1_DISTANCEMODE_LONG);
+    
+    status = Sensor->vl53L1_StartMeasurement();  
+
+    return status;
+}
+
+/*
+ * Add to an array a character that represents the sensor and start ranging
+ */
+int init_sensors_array()
+{
+    int status = 0;
+    VL53L1 *Sensor;
+    uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
+
+    sensorCnt = 0;
+    
+    Dev->comms_speed_khz = 400;
+    Dev->comms_type = 1;
+    
+    if (board->sensor_centre != NULL) {
+        Dev = &devCentre;
+        Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
+        devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
+        
+        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;
+        devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
+    
+        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;
+        devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
+        
+        status = configure_sensor(board->sensor_right);
+        printf("configuring right channel \n");
+    }
+    
+    /* start the measure on the center sensor */
+    if (NULL != board->sensor_centre) {
+        installedSensors[sensorCnt] = 'C';
+        status = board->sensor_centre->stop_measurement();
+        if (status != 0) {
+            return status;
+        }
+        status = board->sensor_centre->start_measurement(&sensor_centre_irq);
+        if (status != 0) {
+            return status;
+        }
+        ++sensorCnt;
+    }
+    /* start the measure on the left sensor */
+    if (NULL != board->sensor_left) {
+        installedSensors[sensorCnt] = 'L';
+        status = board->sensor_left->stop_measurement();
+        if (status != 0) {
+            return status;
+        }
+        status = board->sensor_left->start_measurement(&sensor_left_irq);
+        if (status != 0) {
+            return status;
+        }
+        ++sensorCnt;
+    }
+    /* start the measure on the right sensor */
+    if (NULL != board->sensor_right) {
+        installedSensors[sensorCnt] = 'R';
+        status = board->sensor_right->stop_measurement();
+        if (status != 0) {
+            return status;
+        }
+        status = board->sensor_right->start_measurement(&sensor_right_irq);
+        if (status != 0) {
+            return status;
+        }
+        ++sensorCnt;
+    }
+    currentSensor = 0;
+    return status;
+}
+
+
  
 /*=================================== Main ==================================
 =============================================================================*/
 int main()
 {   
     int status;
-    VL53L1 * Sensor;
-    uint16_t wordData;
-
+    VL53L1 *Sensor;
+//    uint16_t wordData;
+    
+    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
-    printf("Polling single multizone mbed = %d \r\n",MBED_VERSION);
+    printf("mbed version = %d \r\n",MBED_VERSION);
 
 // create i2c interface
     ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
     
     /* creates the 53L1A1 expansion board singleton obj */
-   board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2);
+//   board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2);
+   board = XNucleo53L1A2::instance(dev_I2C, A2, D9, D2);
     
     printf("board created!\r\n");
 
@@ -73,12 +325,17 @@
     if (status) {
         printf("Failed to init board!\r\n");
         return 0;
-    }
-       
+    }       
         
     printf("board initiated! - %d\r\n", status);
-  // create the sensor controller classes                                              
 
+    /* init an array with chars to id the sensors */
+//    status = init_sensors_array();
+    if (status != 0) {
+        printf("Failed to init sensors!\r\n");
+        return status;
+    }
+    
     VL53L1_RoiConfig_t roiConfig;
     
     roiConfig.NumberOfRoi =3;
@@ -97,103 +354,155 @@
     roiConfig.UserRois[2].TopLeftY = 9; 
     roiConfig.UserRois[2].BotRightX = 15;
     roiConfig.UserRois[2].BotRightY = 5; 
-    
-// start of setup of central sensor
-    Dev=&devCentre;
+
     
 // configure the sensors
     Dev->comms_speed_khz = 400;
     Dev->comms_type = 1;
-    Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
-    devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
     
-    Sensor=board->sensor_centre;
-
-    printf("configuring centre channel \n");
+// start of setup of central sensor
+    if (board->sensor_centre != NULL) {
+        Dev=&devCentre;
+        Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
+        devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
+        
+        Sensor=board->sensor_centre;
     
-//    Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
-//    printf("VL53L1X: %02X   %d\n\r", wordData,Dev->i2c_slave_address);
-/* Device Initialization and setting */  
-    status = Sensor->vl53L1_DataInit();
-    status = Sensor->vl53L1_StaticInit();
-    status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
-    status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
+        printf("configuring centre channel \n");
+        
+        // Device Initialization and setting 
+        status = Sensor->vl53L1_DataInit();
+        status = Sensor->vl53L1_StaticInit();
+        
+        status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
+        status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
+        
+        status = Sensor->vl53L1_SetROI( &roiConfig);
+        
+        printf("VL53L1_SetROI %d \n",status);
     
-    
-    status = Sensor->vl53L1_SetROI( &roiConfig);
-    
-    printf("VL53L1_SetROI %d \n",status);
-
-    status = board->sensor_centre->vl53L1_StartMeasurement();  
-        
+        status = board->sensor_centre->vl53L1_StartMeasurement();  
+    }        
 
 // start of setup of left satellite
-    Dev=&devLeft;
+    if (board->sensor_left != NULL) {
+        Dev=&devLeft;
+        Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
+        devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
+        
+        Sensor=board->sensor_left;
     
-// configure the sensors
-    Dev->comms_speed_khz = 400;
-    Dev->comms_type = 1;
-    Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
-    devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
-    
-    Sensor=board->sensor_left;
-
-    printf("configuring centre channel \n");
+        printf("configuring left channel \n");
+        
+        // Device Initialization and setting 
+        status = Sensor->vl53L1_DataInit();
+        status = Sensor->vl53L1_StaticInit();
+        
+        status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
+        status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);    
+        
+        status = Sensor->vl53L1_SetROI( &roiConfig);
+        
+        printf("VL53L1_SetROI %d \n",status);
     
-//    Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
-//    printf("VL53L1X: %02X   %d\n\r", wordData,Dev->i2c_slave_address);
-/* Device Initialization and setting */  
-    status = Sensor->vl53L1_DataInit();
-    status = Sensor->vl53L1_StaticInit();
-    status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
-    status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
+        status = board->sensor_left->vl53L1_StartMeasurement();  
+    }        
+
+    // start of setup of right satellite
+    if (board->sensor_right != NULL) {
+        Dev=&devRight;
+        Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
+        devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
+        
+        Sensor=board->sensor_right;
     
-    
-    status = Sensor->vl53L1_SetROI( &roiConfig);
+        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);
+        status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);    
+        
+        status = Sensor->vl53L1_SetROI( &roiConfig);
+        
+        printf("VL53L1_SetROI %d \n",status);
     
-    printf("VL53L1_SetROI %d \n",status);
+        status = board->sensor_right->vl53L1_StartMeasurement();  
+    }        
 
-    status = board->sensor_left->vl53L1_StartMeasurement();  
-        
 
     VL53L1_MultiRangingData_t MultiRangingData;
     VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;    
     
     // looping polling for results
-    while (1)
-    {
+    while (true) {
         pMultiRangingData = &MultiRangingData;    
-        
-        // wait for result
-        status = board->sensor_centre->vl53L1_WaitMeasurementDataReady();  
-        // get the result
-        status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
-        // if valid, print it
-        if(status==0) {
-            print_results(devCentre.i2c_slave_address, pMultiRangingData );
-            status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
-
-        } //if(status==0) 
-        else
-        {
-            printf("VL53L1_GetMultiRangingData centre %d \n",status);
+/*        
+        if (board->sensor_centre != NULL) {
+            // wait for result
+            status = board->sensor_centre->vl53L1_WaitMeasurementDataReady();  
+            // get the result
+            status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
+            // if valid, print it
+            if(status==0) {
+                print_results(devCentre.i2c_slave_address, pMultiRangingData );
+                status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
+    
+            } //if(status==0) 
+            else
+            {
+                printf("VL53L1_GetMultiRangingData centre %d \n",status);
+            }
         }
         
-        // wait for result
-        status = board->sensor_left->vl53L1_WaitMeasurementDataReady();  
-        // get the result
-        status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
-        // if valid, print it
-        if(status==0) {
-            print_results(devLeft.i2c_slave_address, pMultiRangingData );
-            status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
-
-        } //if(status==0) 
-        else
-        {
-            printf("VL53L1_GetMultiRangingData centre %d \n",status);
+        if (board->sensor_left != NULL) {
+            // wait for result
+            status = board->sensor_left->vl53L1_WaitMeasurementDataReady();  
+            // get the result
+            status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
+            // if valid, print it
+            if(status==0) {
+                print_results(devLeft.i2c_slave_address, pMultiRangingData );
+                status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
+    
+            } //if(status==0) 
+            else
+            {
+                printf("VL53L1_GetMultiRangingData left %d \n",status);
+            }
         }
         
+        if (board->sensor_right != NULL) {
+            // wait for result
+            status = board->sensor_right->vl53L1_WaitMeasurementDataReady();  
+            // get the result
+            status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData);
+            // if valid, print it
+            if(status==0) {
+                print_results(devRight.i2c_slave_address, pMultiRangingData );
+                status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement();
+    
+            } //if(status==0) 
+            else
+            {
+                printf("VL53L1_GetMultiRangingData right %d \n",status);
+            }
+        }
+*/
+    
+        measure_sensors();
+        
+        if (switchChanged) {
+            ++currentSensor;
+            if (currentSensor == sensorCnt)
+                currentSensor = 0;
+            printf("Sensor changed to %c\r\n", installedSensors[currentSensor]);
+            switchChanged = false;
+            stop_button.enable_irq();
+        }
+    
     }
 }