main.cpp

00001 /*
00002  *  This VL53L1CB Expansion board test application performs range measurements
00003  *  using the onboard embedded centre sensor, in multizone, polling mode.
00004  *  Measured ranges are ouput on the Serial Port, running at 115200 baud.
00005  *
00006  *
00007  * This is designed to work with MBed v2, & MBedOS v5 / v6.
00008  *
00009  *
00010  *  The Reset button can be used to restart the program.
00011  *
00012  * *** Note : 
00013  * Default Mbed build system settings disable print floating-point support.
00014  * Offline builds can enable this, again.
00015  * https://github.com/ARMmbed/mbed-os/blob/master/platform/source/minimal-printf/README.md
00016  * .\mbed-os\platform\mbed_lib.json
00017  *
00018  */
00019  
00020 #include <stdio.h>
00021 #include <time.h>
00022 
00023 #include "mbed.h"
00024 #include "XNucleo53L1A2.h"
00025 #include "ToF_I2C.h"
00026 
00027 
00028 // define the i2c comms pins
00029 #define I2C_SDA   D14 
00030 #define I2C_SCL   D15 
00031 
00032 
00033 
00034 static XNucleo53L1A2 *board=NULL;
00035 #if (MBED_VERSION  > 60300) 
00036     UnbufferedSerial  pc(USBTX, USBRX);
00037     extern "C" void wait_ms(int ms);
00038 #else
00039     Serial pc(SERIAL_TX, SERIAL_RX); 
00040 #endif
00041 
00042 void process_interrupt( VL53L1 * sensor,VL53L1_DEV dev );
00043 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
00044 
00045 VL53L1_Dev_t                   devCentre;
00046 VL53L1_Dev_t                   devLeft;
00047 VL53L1_Dev_t                   devRight;
00048 VL53L1_DEV                     Dev = &devCentre;
00049 
00050  
00051 /*=================================== Main ==================================
00052 =============================================================================*/
00053 int main()
00054 {   
00055     int status;
00056     VL53L1 * Sensor;
00057     uint16_t wordData;
00058 
00059 
00060     pc.baud(115200);  // baud rate is important as printf statements take a lot of time
00061     printf("Polling single multizone mbed = %d \r\n",MBED_VERSION);
00062 
00063 // create i2c interface
00064     ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
00065     
00066     /* creates the 53L1A1 expansion board singleton obj */
00067    board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2);
00068     
00069     printf("board created!\r\n");
00070 
00071     /* init the 53L1A1 expansion board with default values */
00072     status = board->init_board();
00073     if (status) {
00074         printf("Failed to init board!\r\n");
00075         return 0;
00076     }
00077        
00078         
00079     printf("board initiated! - %d\r\n", status);
00080   // create the sensor controller classes                                              
00081 
00082     VL53L1_RoiConfig_t roiConfig;
00083     
00084     roiConfig.NumberOfRoi =3;
00085     
00086     roiConfig.UserRois[0].TopLeftX = 0;
00087     roiConfig.UserRois[0].TopLeftY = 9; 
00088     roiConfig.UserRois[0].BotRightX = 4;
00089     roiConfig.UserRois[0].BotRightY = 5; 
00090     
00091     roiConfig.UserRois[1].TopLeftX = 5;
00092     roiConfig.UserRois[1].TopLeftY = 9; 
00093     roiConfig.UserRois[1].BotRightX = 9;
00094     roiConfig.UserRois[1].BotRightY = 4; 
00095     
00096     roiConfig.UserRois[2].TopLeftX = 11;
00097     roiConfig.UserRois[2].TopLeftY = 9; 
00098     roiConfig.UserRois[2].BotRightX = 15;
00099     roiConfig.UserRois[2].BotRightY = 5; 
00100     
00101 // start of setup of central sensor
00102     Dev=&devCentre;
00103     
00104 // configure the sensors
00105     Dev->comms_speed_khz = 400;
00106     Dev->comms_type = 1;
00107     Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
00108     devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
00109     
00110     Sensor=board->sensor_centre;
00111 
00112     printf("configuring centre channel \n");
00113     
00114 //    Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
00115 //    printf("VL53L1X: %02X   %d\n\r", wordData,Dev->i2c_slave_address);
00116 /* Device Initialization and setting */  
00117     status = Sensor->vl53L1_DataInit();
00118     status = Sensor->vl53L1_StaticInit();
00119     status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
00120     status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
00121     
00122     
00123     status = Sensor->vl53L1_SetROI( &roiConfig);
00124     
00125     printf("VL53L1_SetROI %d \n",status);
00126 
00127     status = board->sensor_centre->vl53L1_StartMeasurement();  
00128         
00129 
00130 // start of setup of left satellite
00131     Dev=&devLeft;
00132     
00133 // configure the sensors
00134     Dev->comms_speed_khz = 400;
00135     Dev->comms_type = 1;
00136     Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
00137     devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
00138     
00139     Sensor=board->sensor_left;
00140 
00141     printf("configuring centre channel \n");
00142     
00143 //    Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
00144 //    printf("VL53L1X: %02X   %d\n\r", wordData,Dev->i2c_slave_address);
00145 /* Device Initialization and setting */  
00146     status = Sensor->vl53L1_DataInit();
00147     status = Sensor->vl53L1_StaticInit();
00148     status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
00149     status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
00150     
00151     
00152     status = Sensor->vl53L1_SetROI( &roiConfig);
00153     
00154     printf("VL53L1_SetROI %d \n",status);
00155 
00156     status = board->sensor_left->vl53L1_StartMeasurement();  
00157         
00158 
00159     VL53L1_MultiRangingData_t MultiRangingData;
00160     VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;    
00161     
00162     // looping polling for results
00163     while (1)
00164     {
00165         pMultiRangingData = &MultiRangingData;    
00166         
00167         // wait for result
00168         status = board->sensor_centre->vl53L1_WaitMeasurementDataReady();  
00169         // get the result
00170         status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
00171         // if valid, print it
00172         if(status==0) {
00173             print_results(devCentre.i2c_slave_address, pMultiRangingData );
00174             status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
00175 
00176         } //if(status==0) 
00177         else
00178         {
00179             printf("VL53L1_GetMultiRangingData centre %d \n",status);
00180         }
00181         
00182         // wait for result
00183         status = board->sensor_left->vl53L1_WaitMeasurementDataReady();  
00184         // get the result
00185         status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
00186         // if valid, print it
00187         if(status==0) {
00188             print_results(devLeft.i2c_slave_address, pMultiRangingData );
00189             status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
00190 
00191         } //if(status==0) 
00192         else
00193         {
00194             printf("VL53L1_GetMultiRangingData centre %d \n",status);
00195         }
00196         
00197     }
00198 }
00199     
00200     
00201     
00202  // prints the range result seen above   
00203 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
00204 {
00205             int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
00206             
00207             int RoiNumber=pMultiRangingData->RoiNumber;
00208 //            int RoiStatus=pMultiRangingData->RoiStatus;
00209 
00210             if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
00211             {
00212                 
00213   //              printf("MZI Count=%5d, ", pMultiRangingData->StreamCount);
00214    //             printf("RoiNumber%1d, ", RoiNumber);
00215    //               printf("RoiStatus=%1d, \n", RoiStatus);
00216                 for(int j=0;j<no_of_object_found;j++){
00217                     if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
00218                         (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
00219                     {
00220                         printf("*****************\t i2cAddr=0x%x \t RoiNumber=%d   \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
00221                         devSpiNumber,
00222                         RoiNumber,
00223                         pMultiRangingData->RangeData[j].RangeStatus,
00224                         pMultiRangingData->RangeData[j].RangeMilliMeter,
00225                         pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
00226                         pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
00227                     }
00228                     else
00229                     {
00230 //                               printf("RangeStatus %d %d\n",j, pMultiRangingData->RangeData[j].RangeStatus);
00231                     }
00232                 }
00233             } // if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
00234             else
00235             {
00236 //                       printf("no_of_object_found %d \n",no_of_object_found);
00237             }
00238 
00239 }   
00240 
00241         
00242 #if (MBED_VERSION  > 60300)
00243 extern "C" void wait_ms(int ms)
00244  {
00245     thread_sleep_for(ms);
00246  }
00247  #endif 
00248      
00249