ST Expansion SW Team / Mbed OS VL53L1CB_NoShield_1sensor_polling_multizone_MB6

A sample program getting measurements from a VL53L1 ToF sensor which is directly connected to the STM32F401. Only copes with one sensor. Polls for measurements. VL53L1 is operated in multizone mode. MBed V6.3 but will work with other MBed versions.

Dependencies:   X_NUCLEO_53L1A2

main.cpp@0:47d5c900dab9, 2020-10-27 (annotated)

Committer:
charlesmn
Date:
Tue Oct 27 09:01:08 2020 +0000
Revision:
0:47d5c900dab9
Child:
1:6bbe4976bc7b
A sample program getting measurements from a VL53L1 ToF sensor which is directly connected to the STM32F401. Only copes with one sensor. Polls for measurements. VL53L1 is operated in multizone mode. MBed V6.3 but will work with other MBed versions.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:47d5c900dab9 1 /*
charlesmn 0:47d5c900dab9 2 * This application performs range measurements
charlesmn 0:47d5c900dab9 3 * connecting directly to the onboard embedded centre sensor, in multizone, polling mode.
charlesmn 0:47d5c900dab9 4 * Measured ranges are ouput on the Serial Port, running at 115200 baud.
charlesmn 0:47d5c900dab9 5 *
charlesmn 0:47d5c900dab9 6 *
charlesmn 0:47d5c900dab9 7 * This is designed to work with MBed V2 ,MBed V5 and MBed V6.
charlesmn 0:47d5c900dab9 8 *
charlesmn 0:47d5c900dab9 9 *
charlesmn 0:47d5c900dab9 10 * The Reset button can be used to restart the program.
charlesmn 0:47d5c900dab9 11 */
charlesmn 0:47d5c900dab9 12
charlesmn 0:47d5c900dab9 13 #include <stdio.h>
charlesmn 0:47d5c900dab9 14
charlesmn 0:47d5c900dab9 15 #include "mbed.h"
charlesmn 0:47d5c900dab9 16 #include "NoShield53L1A1.h"
charlesmn 0:47d5c900dab9 17 #include "ToF_I2C.h"
charlesmn 0:47d5c900dab9 18 #include <time.h>
charlesmn 0:47d5c900dab9 19
charlesmn 0:47d5c900dab9 20
charlesmn 0:47d5c900dab9 21 // define the i2c comms pins
charlesmn 0:47d5c900dab9 22 #define I2C_SDA D14
charlesmn 0:47d5c900dab9 23 #define I2C_SCL D15
charlesmn 0:47d5c900dab9 24
charlesmn 0:47d5c900dab9 25
charlesmn 0:47d5c900dab9 26
charlesmn 0:47d5c900dab9 27 static NoShield53L1 *board=NULL;
charlesmn 0:47d5c900dab9 28 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 0:47d5c900dab9 29
charlesmn 0:47d5c900dab9 30 void process_interrupt( VL53L1X * sensor,VL53L1_DEV dev );
charlesmn 0:47d5c900dab9 31 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
charlesmn 0:47d5c900dab9 32
charlesmn 0:47d5c900dab9 33 VL53L1_Dev_t devCentre;
charlesmn 0:47d5c900dab9 34 VL53L1_Dev_t devLeft;
charlesmn 0:47d5c900dab9 35 VL53L1_Dev_t devRight;
charlesmn 0:47d5c900dab9 36 VL53L1_DEV Dev = &devCentre;
charlesmn 0:47d5c900dab9 37
charlesmn 0:47d5c900dab9 38
charlesmn 0:47d5c900dab9 39 /*=================================== Main ==================================
charlesmn 0:47d5c900dab9 40 =============================================================================*/
charlesmn 0:47d5c900dab9 41 int main()
charlesmn 0:47d5c900dab9 42 {
charlesmn 0:47d5c900dab9 43 int status;
charlesmn 0:47d5c900dab9 44 VL53L1X * Sensor;
charlesmn 0:47d5c900dab9 45 uint16_t wordData;
charlesmn 0:47d5c900dab9 46
charlesmn 0:47d5c900dab9 47
charlesmn 0:47d5c900dab9 48 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 0:47d5c900dab9 49 printf("Polling single multizone\r\n");
charlesmn 0:47d5c900dab9 50
charlesmn 0:47d5c900dab9 51 // create i2c interface
charlesmn 0:47d5c900dab9 52 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:47d5c900dab9 53
charlesmn 0:47d5c900dab9 54 /* creates the 53L1A1 expansion board singleton obj */
charlesmn 0:47d5c900dab9 55 board = NoShield53L1::instance(dev_I2C, A2, D8, D2);
charlesmn 0:47d5c900dab9 56
charlesmn 0:47d5c900dab9 57 printf("board created!\r\n");
charlesmn 0:47d5c900dab9 58
charlesmn 0:47d5c900dab9 59 /* define the shutdown pins */
charlesmn 0:47d5c900dab9 60 status = board->init_board(D9,D4,D3);
charlesmn 0:47d5c900dab9 61 if (status) {
charlesmn 0:47d5c900dab9 62 printf("Failed to init board!\r\n");
charlesmn 0:47d5c900dab9 63 return 0;
charlesmn 0:47d5c900dab9 64 }
charlesmn 0:47d5c900dab9 65
charlesmn 0:47d5c900dab9 66
charlesmn 0:47d5c900dab9 67 printf("board initiated! - %d\r\n", status);
charlesmn 0:47d5c900dab9 68 // create the sensor controller classes
charlesmn 0:47d5c900dab9 69
charlesmn 0:47d5c900dab9 70
charlesmn 0:47d5c900dab9 71
charlesmn 0:47d5c900dab9 72 Dev=&devCentre;
charlesmn 0:47d5c900dab9 73 Sensor=board->sensor_centre;
charlesmn 0:47d5c900dab9 74 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:47d5c900dab9 75 printf("configuring centre channel \n");
charlesmn 0:47d5c900dab9 76
charlesmn 0:47d5c900dab9 77
charlesmn 0:47d5c900dab9 78 // configure the sensors
charlesmn 0:47d5c900dab9 79 Dev->comms_speed_khz = 400;
charlesmn 0:47d5c900dab9 80
charlesmn 0:47d5c900dab9 81 Dev->comms_type = 1;
charlesmn 0:47d5c900dab9 82
charlesmn 0:47d5c900dab9 83 Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
charlesmn 0:47d5c900dab9 84 printf("VL53L1X: %02X %d\n\r", wordData,Dev->i2c_slave_address);
charlesmn 0:47d5c900dab9 85 /* Device Initialization and setting */
charlesmn 0:47d5c900dab9 86 status = Sensor->vl53L1_DataInit();
charlesmn 0:47d5c900dab9 87 status = Sensor->vl53L1_StaticInit();
charlesmn 0:47d5c900dab9 88 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
charlesmn 0:47d5c900dab9 89 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
charlesmn 0:47d5c900dab9 90 // status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 100 * 1000);
charlesmn 0:47d5c900dab9 91 // status = Sensor->vl53L1_SetInterMeasurementPeriodMilliSeconds( 100);
charlesmn 0:47d5c900dab9 92
charlesmn 0:47d5c900dab9 93
charlesmn 0:47d5c900dab9 94 VL53L1_RoiConfig_t roiConfig;
charlesmn 0:47d5c900dab9 95 roiConfig.NumberOfRoi =3;
charlesmn 0:47d5c900dab9 96 roiConfig.UserRois[0].TopLeftX = 0;
charlesmn 0:47d5c900dab9 97 roiConfig.UserRois[0].TopLeftY = 9;
charlesmn 0:47d5c900dab9 98 roiConfig.UserRois[0].BotRightX = 4;
charlesmn 0:47d5c900dab9 99 roiConfig.UserRois[0].BotRightY = 5;
charlesmn 0:47d5c900dab9 100 roiConfig.UserRois[1].TopLeftX = 5;
charlesmn 0:47d5c900dab9 101 roiConfig.UserRois[1].TopLeftY = 9;
charlesmn 0:47d5c900dab9 102 roiConfig.UserRois[1].BotRightX = 9;
charlesmn 0:47d5c900dab9 103 roiConfig.UserRois[1].BotRightY = 4;
charlesmn 0:47d5c900dab9 104 roiConfig.UserRois[2].TopLeftX = 11;
charlesmn 0:47d5c900dab9 105 roiConfig.UserRois[2].TopLeftY = 9;
charlesmn 0:47d5c900dab9 106 roiConfig.UserRois[2].BotRightX = 15;
charlesmn 0:47d5c900dab9 107 roiConfig.UserRois[2].BotRightY = 5;
charlesmn 0:47d5c900dab9 108 status = Sensor->vl53L1_SetROI( &roiConfig);
charlesmn 0:47d5c900dab9 109 printf("VL53L1_SetROI %d \n",status);
charlesmn 0:47d5c900dab9 110
charlesmn 0:47d5c900dab9 111 devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:47d5c900dab9 112 status = board->sensor_centre->vl53L1_StartMeasurement();
charlesmn 0:47d5c900dab9 113
charlesmn 0:47d5c900dab9 114 // looping polling for results
charlesmn 0:47d5c900dab9 115 while (1)
charlesmn 0:47d5c900dab9 116 {
charlesmn 0:47d5c900dab9 117 VL53L1_MultiRangingData_t MultiRangingData;
charlesmn 0:47d5c900dab9 118 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
charlesmn 0:47d5c900dab9 119
charlesmn 0:47d5c900dab9 120 // wait for result
charlesmn 0:47d5c900dab9 121 status = board->sensor_centre->vl53L1_WaitMeasurementDataReady();
charlesmn 0:47d5c900dab9 122 // get the result
charlesmn 0:47d5c900dab9 123 status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:47d5c900dab9 124 // if valid, print it
charlesmn 0:47d5c900dab9 125 if(status==0) {
charlesmn 0:47d5c900dab9 126 print_results(devCentre.i2c_slave_address, pMultiRangingData );
charlesmn 0:47d5c900dab9 127 status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:47d5c900dab9 128
charlesmn 0:47d5c900dab9 129 } //if(status==0)
charlesmn 0:47d5c900dab9 130 else
charlesmn 0:47d5c900dab9 131 {
charlesmn 0:47d5c900dab9 132 printf("VL53L1_GetMultiRangingData centre %d \n",status);
charlesmn 0:47d5c900dab9 133 }
charlesmn 0:47d5c900dab9 134
charlesmn 0:47d5c900dab9 135 wait_ms( 1 * 10);
charlesmn 0:47d5c900dab9 136 }
charlesmn 0:47d5c900dab9 137 }
charlesmn 0:47d5c900dab9 138
charlesmn 0:47d5c900dab9 139
charlesmn 0:47d5c900dab9 140
charlesmn 0:47d5c900dab9 141 // prints the range result seen above
charlesmn 0:47d5c900dab9 142 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
charlesmn 0:47d5c900dab9 143 {
charlesmn 0:47d5c900dab9 144 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:47d5c900dab9 145
charlesmn 0:47d5c900dab9 146 int RoiNumber=pMultiRangingData->RoiNumber;
charlesmn 0:47d5c900dab9 147 // int RoiStatus=pMultiRangingData->RoiStatus;
charlesmn 0:47d5c900dab9 148
charlesmn 0:47d5c900dab9 149 if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:47d5c900dab9 150 {
charlesmn 0:47d5c900dab9 151
charlesmn 0:47d5c900dab9 152 // printf("MZI Count=%5d, ", pMultiRangingData->StreamCount);
charlesmn 0:47d5c900dab9 153 // printf("RoiNumber%1d, ", RoiNumber);
charlesmn 0:47d5c900dab9 154 // printf("RoiStatus=%1d, \n", RoiStatus);
charlesmn 0:47d5c900dab9 155 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:47d5c900dab9 156 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:47d5c900dab9 157 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:47d5c900dab9 158 {
charlesmn 0:47d5c900dab9 159 printf("*****************\t spiAddr=%d \t RoiNumber=%d \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:47d5c900dab9 160 devSpiNumber,
charlesmn 0:47d5c900dab9 161 RoiNumber,
charlesmn 0:47d5c900dab9 162 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:47d5c900dab9 163 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:47d5c900dab9 164 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:47d5c900dab9 165 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:47d5c900dab9 166 }
charlesmn 0:47d5c900dab9 167 else
charlesmn 0:47d5c900dab9 168 {
charlesmn 0:47d5c900dab9 169 // printf("RangeStatus %d %d\n",j, pMultiRangingData->RangeData[j].RangeStatus);
charlesmn 0:47d5c900dab9 170 }
charlesmn 0:47d5c900dab9 171 }
charlesmn 0:47d5c900dab9 172 } // if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:47d5c900dab9 173 else
charlesmn 0:47d5c900dab9 174 {
charlesmn 0:47d5c900dab9 175 // printf("no_of_object_found %d \n",no_of_object_found);
charlesmn 0:47d5c900dab9 176 }
charlesmn 0:47d5c900dab9 177
charlesmn 0:47d5c900dab9 178 }
charlesmn 0:47d5c900dab9 179
charlesmn 0:47d5c900dab9 180