ST Expansion SW Team / Mbed OS VL53L1CB_shield_1sensor_polling_multizone_MBed6

A sample program to control one VL53L1 ToF sensor in multizone mode using polling to find out if a measurement is available. Mbed V6.3 but will run any MBed version by dropping replacing this one. Maint6 release.

Dependencies:   X_NUCLEO_53L1A2

main.cpp@15:ce041cbc8727, 2020-11-09 (annotated)

Committer:
charlesmn
Date:
Mon Nov 09 17:44:46 2020 +0000
Revision:
15:ce041cbc8727
Parent:
3:d1a3d15a06ff
Child:
16:9fd42eb225c5
No functional change. Changed Filenames and functions names to be VL53L1

Who changed what in which revision?

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