main.cpp

00001 /*
00002  *  This VL53L1 test application performs range measurements
00003  *  using the onboard embedded centre sensor and two satelites, in ranging, interrupt mode.
00004  *  This program does not use the expansion board and the sensors a directly connected to the F401.
00005  *  Measured ranges are ouput on the Serial Port, running at 115200 baud.
00006  *
00007  * This is designed to work with MBed V2 , MBed V5 and MBed V6.
00008  *
00009  *
00010  *  The Reset button can be used to restart the program.
00011  */
00012  
00013 #include <stdio.h>
00014 
00015 #include "mbed.h"
00016 #include "XNucleo53L1A1.h"
00017 #include "ToF_I2C.h"
00018 #include <time.h>
00019 
00020 // i2c comms pins
00021 #define I2C_SDA   D14 
00022 #define I2C_SCL   D15 
00023 
00024 // i2c addresses the sensors are configured to be
00025 #define NEW_SENSOR_CENTRE_ADDRESS   0x56
00026 #define NEW_SENSOR_LEFT_ADDRESS     0x58
00027 #define NEW_SENSOR_RIGHT_ADDRESS    0x5a
00028 
00029 
00030 #define NUM_SENSORS 3
00031 
00032 // define the sensor interrupt pins. This much match the wiring!
00033 PinName CentreIntPin = A2;
00034 PinName LeftIntPin = D9;
00035 PinName RightIntPin = D4;
00036 
00037 
00038 // MBed V6.4 has renamed wait_ms and UnbufferedSerial replaces Serial
00039 #if (MBED_VERSION  > 60300) 
00040 UnbufferedSerial  pc(SERIAL_TX, SERIAL_RX); 
00041 extern "C" void wait_ms(int ms);
00042 #else
00043 Serial pc(SERIAL_TX, SERIAL_RX); 
00044 #endif
00045 
00046 static int int_centre_result = 0;
00047 static int int_left_result = 0;
00048 static int int_right_result = 0;
00049 
00050 
00051 class WaitForMeasurement {
00052 public:
00053 
00054 
00055 // this class services the interrupts from the ToF sensors.
00056 // There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things.
00057 // We keep things simple by only raising a flag so all the real work is done outside the interrupt.
00058 // This is designed around MBED V2 which doesn't have the RTOS features that would make this work nicely e.g. semaphores/queues.
00059 WaitForMeasurement(): _interrupt(A1)
00060 {
00061 }
00062 
00063 
00064     // constructor 
00065     WaitForMeasurement(PinName pin, uint8_t slave_address) : _interrupt(pin)          // create the InterruptIn on the pin specified 
00066     {
00067         i2c_slave_address = slave_address;
00068         _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach routine to deal with interrupt
00069         
00070     }
00071     
00072 
00073   // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
00074     void got_interrupt()
00075     {
00076     
00077         // identify which sensor this instance relates to and flag an interrupt has occured
00078         if (i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
00079             int_centre_result = 1;  //flag to main that interrupt happened
00080         if (i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
00081             int_left_result = 1;   //flag to main that interrupt happened
00082         if (i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
00083             int_right_result = 1;  //flag to main that interrupt happened
00084     }
00085 
00086     
00087     //destructor
00088     ~WaitForMeasurement()
00089     {
00090         printf("destruction \n");
00091     }
00092 
00093 private:
00094     InterruptIn _interrupt;
00095     int status;
00096     uint8_t   i2c_slave_address;
00097     
00098 };
00099 
00100  
00101 /*=================================== Main ==================================
00102 =============================================================================*/
00103 int main()
00104 {   
00105     int status;
00106     VL53L1X * Sensor;
00107     VL53L1X * SensorCentre;
00108     VL53L1X * SensorLeft;
00109     VL53L1X * SensorRight;
00110     DigitalOut xshutdownCentre(D9);
00111     DigitalOut xshutdownLeft(D4);
00112     DigitalOut xshutdownRight(D3);
00113 
00114     uint8_t ToFSensor = 1; 
00115   
00116     
00117     WaitForMeasurement* int2;
00118     WaitForMeasurement* int1;
00119     WaitForMeasurement* int3;
00120 
00121     pc.baud(115200);  // baud rate is important as printf statements take a lot of time
00122 
00123     printf("VL53L1CB_NoShield_3sensors_interrupt_ranging mbed = %d \r\n",MBED_VERSION);
00124 
00125 // create the i2c instance
00126     ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
00127 // create the sensor instances    
00128     SensorCentre = new VL53L1X(dev_I2C, &xshutdownCentre, CentreIntPin); 
00129     SensorLeft = new VL53L1X(dev_I2C, &xshutdownLeft, LeftIntPin); 
00130     SensorRight = new VL53L1X(dev_I2C, &xshutdownRight, RightIntPin); 
00131     
00132     printf("board created!\r\n");
00133  // initialise sensors                                               
00134     for (ToFSensor=0;ToFSensor< NUM_SENSORS ;ToFSensor++){
00135     switch(ToFSensor){
00136         case 0:
00137             
00138             if (SensorCentre == NULL ) continue;
00139             // try to initialise the sensor. returns -1 if sensor is not there.
00140             if (SensorCentre->InitSensor(NEW_SENSOR_CENTRE_ADDRESS) ==-1) 
00141             {
00142                  printf("centre not found \n");
00143                  SensorCentre = NULL;
00144                  continue;
00145             }
00146             Sensor=SensorCentre;
00147             printf("configuring centre channel \n");
00148             break;
00149         case 1:
00150             if (SensorLeft== NULL ) continue;
00151             if (SensorLeft->InitSensor(NEW_SENSOR_LEFT_ADDRESS)==-1)
00152             {
00153                  printf("left not found \n");
00154                  SensorLeft = NULL;
00155                  continue;
00156             }
00157             Sensor=SensorLeft;
00158             printf("configuring left channel \n");
00159             break;
00160         case 2:
00161             if (SensorRight == NULL ) continue;
00162             if(SensorRight->InitSensor(NEW_SENSOR_RIGHT_ADDRESS)==-1)
00163             {
00164                  printf("right not found \n");
00165                  SensorRight = NULL;
00166                  continue;
00167             }
00168             Sensor=SensorRight;
00169             printf("configuring right channel \n");
00170             break;      
00171         default:
00172            printf(" error in switch, invalid ToF sensor \n");
00173     }
00174     wait_ms(250);  // wait for the sensor to come alive
00175         
00176 // configure the sensors
00177 
00178 /* Device Initialization and setting */  
00179     status = Sensor->vl53L1_DataInit();
00180     status = Sensor->vl53L1_StaticInit();
00181     status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_RANGING);
00182     status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
00183     status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 50000);
00184     status = Sensor->vl53L1_SetInterMeasurementPeriodMilliSeconds( 100);
00185     }
00186     
00187     if (SensorCentre != NULL )
00188     {
00189         printf("starting interrupt centre\n");
00190         // create interrupt process for the centre sensor
00191         int1 =  new WaitForMeasurement(CentreIntPin,NEW_SENSOR_CENTRE_ADDRESS);
00192         // start the snesor measuring
00193         status = SensorCentre->vl53L1_StartMeasurement();
00194                     printf("end interrupt centre\n");
00195     }
00196     
00197 
00198     if (SensorLeft!= NULL )
00199     {
00200         printf("starting interrupt left\n");
00201         int2 = new WaitForMeasurement(LeftIntPin,NEW_SENSOR_LEFT_ADDRESS);
00202         status = SensorLeft->vl53L1_StartMeasurement();
00203     }
00204 
00205     if (SensorRight!= NULL )
00206     {
00207         printf("starting interrupt right\n");
00208         int3 = new WaitForMeasurement(RightIntPin,NEW_SENSOR_RIGHT_ADDRESS);
00209         status = SensorRight->vl53L1_StartMeasurement();
00210     }
00211             
00212     printf("loop forever \n");
00213    // loop waiting for interrupts to happen. This is signaled by   int_centre_result,int_left_result or int_right_result
00214    // being non zero. The are set back to zero when processing is completed
00215     while (1)
00216     {
00217         static VL53L1_RangingMeasurementData_t RangingData;  
00218         VL53L1_MultiRangingData_t MultiRangingData;
00219         VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;   
00220         
00221         wait_ms( 1 * 50);
00222         if (int_centre_result != 0)
00223         {
00224             // as there has been an interrupt there is data waiting so get it
00225             status = SensorCentre->vl53L1_GetMultiRangingData( pMultiRangingData);
00226             if ( status == VL53L1_ERROR_NONE)
00227             {
00228                 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
00229                 if ( no_of_object_found < 10 ) 
00230                 {
00231                     for(int j=0;j<no_of_object_found;j++){
00232                         if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
00233                             (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
00234                         {
00235                             printf("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
00236                             pMultiRangingData->RangeData[j].RangeStatus,
00237                             pMultiRangingData->RangeData[j].RangeMilliMeter,
00238                             pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
00239                             pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
00240                         }
00241                     }
00242                 }
00243             }
00244                 int_centre_result = 0;
00245                 status = SensorCentre->vl53L1_ClearInterruptAndStartMeasurement();
00246         }
00247 
00248 
00249         if (int_left_result != 0)
00250         {
00251             status = SensorLeft->vl53L1_GetMultiRangingData( pMultiRangingData);
00252             if ( status == VL53L1_ERROR_NONE)
00253             {
00254                 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
00255                 if ( no_of_object_found < 10 ) 
00256                 {
00257                     for(int j=0;j<no_of_object_found;j++){
00258                             if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
00259                                 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
00260                             {
00261 
00262                                 printf("left  \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
00263                                 pMultiRangingData->RangeData[j].RangeStatus,
00264                                 pMultiRangingData->RangeData[j].RangeMilliMeter,
00265                                 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
00266                                 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
00267                             }
00268                     }
00269                 }
00270             }
00271  
00272                 int_left_result = 0;
00273                 status = SensorLeft->vl53L1_ClearInterruptAndStartMeasurement();
00274 
00275         }
00276             
00277             
00278         if (int_right_result != 0)
00279         {
00280             status = SensorRight->vl53L1_GetMultiRangingData( pMultiRangingData);
00281             if ( status == VL53L1_ERROR_NONE)
00282             {
00283                 // if valid result print it
00284                 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
00285                 if ( no_of_object_found < 10 ) 
00286                 {
00287                     for(int j=0;j<no_of_object_found;j++){
00288                         if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
00289                             (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
00290                             {
00291                                 printf("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
00292                                 pMultiRangingData->RangeData[j].RangeStatus,
00293                                 pMultiRangingData->RangeData[j].RangeMilliMeter,
00294                                 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
00295                                 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
00296                             }
00297                     }
00298                 }
00299             }
00300             // clear interrupt flag
00301             int_right_result = 0;
00302             // clear theinterrupt and wait for another result
00303             status = SensorRight->vl53L1_ClearInterruptAndStartMeasurement();
00304 
00305         }
00306     }
00307     printf("terminated");
00308 }
00309     
00310   
00311   // this function doesn't exist in MBed6.4 onwards. It is required for the F401 drivers.
00312   // addded here as it allows the file to be compatible with all mbed versions          
00313 #if (MBED_VERSION  > 60300) 
00314 void wait_ms(int ms)
00315  {
00316     thread_sleep_for(ms);
00317  }
00318 #endif
00319