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@1:6bbe4976bc7b, 2020-11-08 (annotated)

Committer:
charlesmn
Date:
Sun Nov 08 18:09:34 2020 +0000
Revision:
1:6bbe4976bc7b
Parent:
0:47d5c900dab9
Child:
2:014f6e21d7b3
Sample MBed program for use with ST VL53L1CB sensor directly connected to the F401 board. Uses polling to cope with a sensors in multizone mode. Includes MBed V6.4

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 1:6bbe4976bc7b 16 //#include "NoShield53L1A1.h"
charlesmn 0:47d5c900dab9 17 #include "ToF_I2C.h"
charlesmn 0:47d5c900dab9 18 #include <time.h>
charlesmn 1:6bbe4976bc7b 19 //#include "vl53lx_class.h"
charlesmn 1:6bbe4976bc7b 20 #include "vl53l1_def.h"
charlesmn 1:6bbe4976bc7b 21 #include "vl53l1_platform_user_data.h"
charlesmn 1:6bbe4976bc7b 22 #include "vl53l1x.h"
charlesmn 0:47d5c900dab9 23
charlesmn 0:47d5c900dab9 24
charlesmn 0:47d5c900dab9 25 // define the i2c comms pins
charlesmn 0:47d5c900dab9 26 #define I2C_SDA D14
charlesmn 0:47d5c900dab9 27 #define I2C_SCL D15
charlesmn 0:47d5c900dab9 28
charlesmn 1:6bbe4976bc7b 29 #define SENSOR_DEFAULT_ADDRESS 0x52
charlesmn 1:6bbe4976bc7b 30 #define NEW_SENSOR_CENTRE_ADDRESS 0x56
charlesmn 1:6bbe4976bc7b 31 #define NEW_SENSOR_LEFT_ADDRESS 0x58
charlesmn 1:6bbe4976bc7b 32 #define NEW_SENSOR_RIGHT_ADDRESS 0x5a
charlesmn 1:6bbe4976bc7b 33
charlesmn 0:47d5c900dab9 34
charlesmn 0:47d5c900dab9 35
charlesmn 1:6bbe4976bc7b 36 //static NoShield53L1 *board=NULL;
charlesmn 0:47d5c900dab9 37
charlesmn 1:6bbe4976bc7b 38 // MBed V6.4 has renamed wait_ms and UnbufferedSerial replaces Serial
charlesmn 1:6bbe4976bc7b 39 #if (MBED_VERSION > 60300)
charlesmn 1:6bbe4976bc7b 40 UnbufferedSerial pc(SERIAL_TX, SERIAL_RX);
charlesmn 1:6bbe4976bc7b 41 extern "C" void wait_ms(int ms);
charlesmn 1:6bbe4976bc7b 42 #else
charlesmn 1:6bbe4976bc7b 43 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 1:6bbe4976bc7b 44 #endif
charlesmn 1:6bbe4976bc7b 45
charlesmn 1:6bbe4976bc7b 46 //void process_interrupt( VL53L1X * sensor,VL53L1_DEV dev );
charlesmn 0:47d5c900dab9 47 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
charlesmn 0:47d5c900dab9 48
charlesmn 1:6bbe4976bc7b 49
charlesmn 0:47d5c900dab9 50 VL53L1_Dev_t devCentre;
charlesmn 0:47d5c900dab9 51 VL53L1_Dev_t devLeft;
charlesmn 0:47d5c900dab9 52 VL53L1_Dev_t devRight;
charlesmn 0:47d5c900dab9 53 VL53L1_DEV Dev = &devCentre;
charlesmn 0:47d5c900dab9 54
charlesmn 0:47d5c900dab9 55
charlesmn 0:47d5c900dab9 56 /*=================================== Main ==================================
charlesmn 0:47d5c900dab9 57 =============================================================================*/
charlesmn 0:47d5c900dab9 58 int main()
charlesmn 0:47d5c900dab9 59 {
charlesmn 0:47d5c900dab9 60 int status;
charlesmn 0:47d5c900dab9 61 VL53L1X * Sensor;
charlesmn 0:47d5c900dab9 62 uint16_t wordData;
charlesmn 1:6bbe4976bc7b 63 DigitalOut xshutdown(D9);
charlesmn 0:47d5c900dab9 64
charlesmn 0:47d5c900dab9 65
charlesmn 0:47d5c900dab9 66 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 1:6bbe4976bc7b 67 printf("Polling single multizone mbed = %d \r\n",MBED_VERSION);
charlesmn 0:47d5c900dab9 68
charlesmn 0:47d5c900dab9 69 // create i2c interface
charlesmn 0:47d5c900dab9 70 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:47d5c900dab9 71
charlesmn 0:47d5c900dab9 72 /* creates the 53L1A1 expansion board singleton obj */
charlesmn 1:6bbe4976bc7b 73 // board = XNucleo51L1::instance(dev_I2C, A2, D8, D2);
charlesmn 1:6bbe4976bc7b 74
charlesmn 1:6bbe4976bc7b 75 Sensor = new VL53L1X(dev_I2C,&xshutdown,A2);
charlesmn 0:47d5c900dab9 76
charlesmn 0:47d5c900dab9 77 printf("board created!\r\n");
charlesmn 0:47d5c900dab9 78
charlesmn 0:47d5c900dab9 79 /* define the shutdown pins */
charlesmn 1:6bbe4976bc7b 80 // status = board->init_board();
charlesmn 1:6bbe4976bc7b 81 status = Sensor->InitSensor(SENSOR_DEFAULT_ADDRESS);
charlesmn 0:47d5c900dab9 82 if (status) {
charlesmn 1:6bbe4976bc7b 83 printf("Failed to init sensor!\r\n");
charlesmn 0:47d5c900dab9 84 return 0;
charlesmn 0:47d5c900dab9 85 }
charlesmn 1:6bbe4976bc7b 86 wait_ms(250); // wait for the sensor to initialise
charlesmn 0:47d5c900dab9 87
charlesmn 1:6bbe4976bc7b 88 printf("sensor initiated! - %d\r\n", status);
charlesmn 0:47d5c900dab9 89 // create the sensor controller classes
charlesmn 1:6bbe4976bc7b 90
charlesmn 0:47d5c900dab9 91 Dev=&devCentre;
charlesmn 0:47d5c900dab9 92 Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
charlesmn 0:47d5c900dab9 93 printf("VL53L1X: %02X %d\n\r", wordData,Dev->i2c_slave_address);
charlesmn 0:47d5c900dab9 94 /* Device Initialization and setting */
charlesmn 0:47d5c900dab9 95 status = Sensor->vl53L1_DataInit();
charlesmn 0:47d5c900dab9 96 status = Sensor->vl53L1_StaticInit();
charlesmn 0:47d5c900dab9 97 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
charlesmn 1:6bbe4976bc7b 98 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
charlesmn 1:6bbe4976bc7b 99 status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 50 * 1000);
charlesmn 0:47d5c900dab9 100
charlesmn 1:6bbe4976bc7b 101 // configure the region of interests
charlesmn 0:47d5c900dab9 102 VL53L1_RoiConfig_t roiConfig;
charlesmn 0:47d5c900dab9 103 roiConfig.NumberOfRoi =3;
charlesmn 0:47d5c900dab9 104 roiConfig.UserRois[0].TopLeftX = 0;
charlesmn 0:47d5c900dab9 105 roiConfig.UserRois[0].TopLeftY = 9;
charlesmn 0:47d5c900dab9 106 roiConfig.UserRois[0].BotRightX = 4;
charlesmn 0:47d5c900dab9 107 roiConfig.UserRois[0].BotRightY = 5;
charlesmn 0:47d5c900dab9 108 roiConfig.UserRois[1].TopLeftX = 5;
charlesmn 0:47d5c900dab9 109 roiConfig.UserRois[1].TopLeftY = 9;
charlesmn 0:47d5c900dab9 110 roiConfig.UserRois[1].BotRightX = 9;
charlesmn 0:47d5c900dab9 111 roiConfig.UserRois[1].BotRightY = 4;
charlesmn 0:47d5c900dab9 112 roiConfig.UserRois[2].TopLeftX = 11;
charlesmn 0:47d5c900dab9 113 roiConfig.UserRois[2].TopLeftY = 9;
charlesmn 0:47d5c900dab9 114 roiConfig.UserRois[2].BotRightX = 15;
charlesmn 0:47d5c900dab9 115 roiConfig.UserRois[2].BotRightY = 5;
charlesmn 0:47d5c900dab9 116 status = Sensor->vl53L1_SetROI( &roiConfig);
charlesmn 0:47d5c900dab9 117 printf("VL53L1_SetROI %d \n",status);
charlesmn 0:47d5c900dab9 118
charlesmn 1:6bbe4976bc7b 119 status = Sensor->vl53L1_StartMeasurement();
charlesmn 0:47d5c900dab9 120
charlesmn 0:47d5c900dab9 121 // looping polling for results
charlesmn 0:47d5c900dab9 122 while (1)
charlesmn 0:47d5c900dab9 123 {
charlesmn 0:47d5c900dab9 124 VL53L1_MultiRangingData_t MultiRangingData;
charlesmn 0:47d5c900dab9 125 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
charlesmn 0:47d5c900dab9 126
charlesmn 0:47d5c900dab9 127 // wait for result
charlesmn 1:6bbe4976bc7b 128 status = Sensor->vl53L1_WaitMeasurementDataReady();
charlesmn 0:47d5c900dab9 129 // get the result
charlesmn 1:6bbe4976bc7b 130 status = Sensor->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:47d5c900dab9 131 // if valid, print it
charlesmn 0:47d5c900dab9 132 if(status==0) {
charlesmn 0:47d5c900dab9 133 print_results(devCentre.i2c_slave_address, pMultiRangingData );
charlesmn 1:6bbe4976bc7b 134 status = Sensor->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:47d5c900dab9 135
charlesmn 0:47d5c900dab9 136 } //if(status==0)
charlesmn 0:47d5c900dab9 137 else
charlesmn 0:47d5c900dab9 138 {
charlesmn 0:47d5c900dab9 139 printf("VL53L1_GetMultiRangingData centre %d \n",status);
charlesmn 0:47d5c900dab9 140 }
charlesmn 0:47d5c900dab9 141
charlesmn 0:47d5c900dab9 142 wait_ms( 1 * 10);
charlesmn 0:47d5c900dab9 143 }
charlesmn 0:47d5c900dab9 144 }
charlesmn 0:47d5c900dab9 145
charlesmn 0:47d5c900dab9 146
charlesmn 0:47d5c900dab9 147
charlesmn 0:47d5c900dab9 148 // prints the range result seen above
charlesmn 0:47d5c900dab9 149 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
charlesmn 0:47d5c900dab9 150 {
charlesmn 1:6bbe4976bc7b 151 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:47d5c900dab9 152
charlesmn 1:6bbe4976bc7b 153 int RoiNumber=pMultiRangingData->RoiNumber;
charlesmn 0:47d5c900dab9 154 // int RoiStatus=pMultiRangingData->RoiStatus;
charlesmn 0:47d5c900dab9 155
charlesmn 1:6bbe4976bc7b 156 if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 1:6bbe4976bc7b 157 {
charlesmn 1:6bbe4976bc7b 158
charlesmn 1:6bbe4976bc7b 159 // printf("MZI Count=%5d, ", pMultiRangingData->StreamCount);
charlesmn 1:6bbe4976bc7b 160 // printf("RoiNumber%1d, ", RoiNumber);
charlesmn 1:6bbe4976bc7b 161 // printf("RoiStatus=%1d, \n", RoiStatus);
charlesmn 1:6bbe4976bc7b 162 for(int j=0;j<no_of_object_found;j++){
charlesmn 1:6bbe4976bc7b 163 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 1:6bbe4976bc7b 164 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 1:6bbe4976bc7b 165 {
charlesmn 1:6bbe4976bc7b 166 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 1:6bbe4976bc7b 167 devSpiNumber,
charlesmn 1:6bbe4976bc7b 168 RoiNumber,
charlesmn 1:6bbe4976bc7b 169 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 1:6bbe4976bc7b 170 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 1:6bbe4976bc7b 171 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 1:6bbe4976bc7b 172 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 1:6bbe4976bc7b 173 }
charlesmn 1:6bbe4976bc7b 174 else
charlesmn 1:6bbe4976bc7b 175 {
charlesmn 0:47d5c900dab9 176 // printf("RangeStatus %d %d\n",j, pMultiRangingData->RangeData[j].RangeStatus);
charlesmn 0:47d5c900dab9 177 }
charlesmn 0:47d5c900dab9 178 }
charlesmn 1:6bbe4976bc7b 179 } // if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 1:6bbe4976bc7b 180
charlesmn 0:47d5c900dab9 181
charlesmn 0:47d5c900dab9 182 }
charlesmn 0:47d5c900dab9 183
charlesmn 1:6bbe4976bc7b 184
charlesmn 1:6bbe4976bc7b 185 #if (MBED_VERSION > 60300)
charlesmn 1:6bbe4976bc7b 186 void wait_ms(int ms)
charlesmn 1:6bbe4976bc7b 187 {
charlesmn 1:6bbe4976bc7b 188 thread_sleep_for(ms);
charlesmn 1:6bbe4976bc7b 189 }
charlesmn 1:6bbe4976bc7b 190 #endif