ST Expansion SW Team / Mbed 2 deprecated VL53L1_shield_MB2_3sensors_interrupt_ranging

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed VL53L1ExpansionBoard

main.cpp@0:0754d1901845, 2020-10-27 (annotated)

Committer:
charlesmn
Date:
Tue Oct 27 14:15:50 2020 +0000
Revision:
0:0754d1901845
A sample program that talks to up to 3 VL53L1 ToF sensors on a nucleo shield. The sensors are in ranging mode and and interrupts occur when a measurement is available. MBed V2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:0754d1901845 1 /*
charlesmn 0:0754d1901845 2 * This VL53L1X Expansion board test application performs range measurements
charlesmn 0:0754d1901845 3 * using the onboard embedded centre sensor and two satelites, in ranging, interrupt mode.
charlesmn 0:0754d1901845 4 * Measured ranges are ouput on the Serial Port, running at 9600 baud.
charlesmn 0:0754d1901845 5 *
charlesmn 0:0754d1901845 6 * This is designed to work with MBed V2 , MBed V5 and MBed V6.
charlesmn 0:0754d1901845 7 *
charlesmn 0:0754d1901845 8 *
charlesmn 0:0754d1901845 9 * The Reset button can be used to restart the program.
charlesmn 0:0754d1901845 10 */
charlesmn 0:0754d1901845 11
charlesmn 0:0754d1901845 12 #include <stdio.h>
charlesmn 0:0754d1901845 13
charlesmn 0:0754d1901845 14 #include "mbed.h"
charlesmn 0:0754d1901845 15 #include "XNucleo53L1A1.h"
charlesmn 0:0754d1901845 16 #include "ToF_I2C.h"
charlesmn 0:0754d1901845 17 #include <time.h>
charlesmn 0:0754d1901845 18
charlesmn 0:0754d1901845 19 // i2c comms pins
charlesmn 0:0754d1901845 20 #define I2C_SDA D14
charlesmn 0:0754d1901845 21 #define I2C_SCL D15
charlesmn 0:0754d1901845 22
charlesmn 0:0754d1901845 23 #define NUM_SENSORS 3
charlesmn 0:0754d1901845 24
charlesmn 0:0754d1901845 25 static XNucleo53L1A1 *board=NULL;
charlesmn 0:0754d1901845 26 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 0:0754d1901845 27
charlesmn 0:0754d1901845 28 static int int_centre_result = 0;
charlesmn 0:0754d1901845 29 static int int_left_result = 0;
charlesmn 0:0754d1901845 30 static int int_right_result = 0;
charlesmn 0:0754d1901845 31
charlesmn 0:0754d1901845 32
charlesmn 0:0754d1901845 33 class WaitForMeasurement {
charlesmn 0:0754d1901845 34 public:
charlesmn 0:0754d1901845 35
charlesmn 0:0754d1901845 36
charlesmn 0:0754d1901845 37 // this class services the interrupts from the ToF sensors.
charlesmn 0:0754d1901845 38 // There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things.
charlesmn 0:0754d1901845 39 // We keep things simple by only raising a flag so all the real work is done outside the interrupt.
charlesmn 0:0754d1901845 40 // This is designed around MBED V2 which doesn't have the RTOS features that would make this work nicely e.g. semaphores/queues.
charlesmn 0:0754d1901845 41 WaitForMeasurement(): _interrupt(A1)
charlesmn 0:0754d1901845 42 {
charlesmn 0:0754d1901845 43 }
charlesmn 0:0754d1901845 44
charlesmn 0:0754d1901845 45
charlesmn 0:0754d1901845 46 // constructor - Sensor is not used and can be removed
charlesmn 0:0754d1901845 47 WaitForMeasurement(PinName pin,VL53L1_DEV Dev) : _interrupt(pin) // create the InterruptIn on the pin specified to Counter
charlesmn 0:0754d1901845 48 {
charlesmn 0:0754d1901845 49 Devlocal = Dev;
charlesmn 0:0754d1901845 50 _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach increment function of this counter instance
charlesmn 0:0754d1901845 51
charlesmn 0:0754d1901845 52 }
charlesmn 0:0754d1901845 53
charlesmn 0:0754d1901845 54 void process_right_interrupt()
charlesmn 0:0754d1901845 55 {
charlesmn 0:0754d1901845 56 printf("processing right interrupt\n");
charlesmn 0:0754d1901845 57 }
charlesmn 0:0754d1901845 58
charlesmn 0:0754d1901845 59 // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
charlesmn 0:0754d1901845 60 void got_interrupt()
charlesmn 0:0754d1901845 61 {
charlesmn 0:0754d1901845 62
charlesmn 0:0754d1901845 63 _count++;
charlesmn 0:0754d1901845 64
charlesmn 0:0754d1901845 65 if (Devlocal->i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
charlesmn 0:0754d1901845 66 int_centre_result = 1; //flag to main that interrupt happened
charlesmn 0:0754d1901845 67 if (Devlocal->i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
charlesmn 0:0754d1901845 68 int_left_result = 1; //flag to main that interrupt happened
charlesmn 0:0754d1901845 69 if (Devlocal->i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
charlesmn 0:0754d1901845 70 {
charlesmn 0:0754d1901845 71 int_right_result = 1; //flag to main that interrupt happened
charlesmn 0:0754d1901845 72 }
charlesmn 0:0754d1901845 73 }
charlesmn 0:0754d1901845 74
charlesmn 0:0754d1901845 75
charlesmn 0:0754d1901845 76 //destructor
charlesmn 0:0754d1901845 77 ~WaitForMeasurement()
charlesmn 0:0754d1901845 78 {
charlesmn 0:0754d1901845 79 printf("destruction \n");
charlesmn 0:0754d1901845 80 }
charlesmn 0:0754d1901845 81
charlesmn 0:0754d1901845 82 private:
charlesmn 0:0754d1901845 83 InterruptIn _interrupt;
charlesmn 0:0754d1901845 84 volatile int _count;
charlesmn 0:0754d1901845 85 VL53L1_DEV Devlocal;
charlesmn 0:0754d1901845 86 int status;
charlesmn 0:0754d1901845 87
charlesmn 0:0754d1901845 88 };
charlesmn 0:0754d1901845 89
charlesmn 0:0754d1901845 90
charlesmn 0:0754d1901845 91
charlesmn 0:0754d1901845 92 VL53L1_Dev_t devCentre;
charlesmn 0:0754d1901845 93 VL53L1_Dev_t devLeft;
charlesmn 0:0754d1901845 94 VL53L1_Dev_t devRight;
charlesmn 0:0754d1901845 95 VL53L1_DEV Dev = &devCentre;
charlesmn 0:0754d1901845 96
charlesmn 0:0754d1901845 97 /*=================================== Main ==================================
charlesmn 0:0754d1901845 98 =============================================================================*/
charlesmn 0:0754d1901845 99 int main()
charlesmn 0:0754d1901845 100 {
charlesmn 0:0754d1901845 101 int status;
charlesmn 0:0754d1901845 102 VL53L1X * Sensor;
charlesmn 0:0754d1901845 103 uint16_t wordData;
charlesmn 0:0754d1901845 104 uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
charlesmn 0:0754d1901845 105
charlesmn 0:0754d1901845 106
charlesmn 0:0754d1901845 107 WaitForMeasurement* int2;
charlesmn 0:0754d1901845 108 WaitForMeasurement* int1;
charlesmn 0:0754d1901845 109 WaitForMeasurement* int3;
charlesmn 0:0754d1901845 110
charlesmn 0:0754d1901845 111 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 0:0754d1901845 112
charlesmn 0:0754d1901845 113 printf("Hello world!\r\n");
charlesmn 0:0754d1901845 114
charlesmn 0:0754d1901845 115 // create the i2c instance
charlesmn 0:0754d1901845 116 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:0754d1901845 117
charlesmn 0:0754d1901845 118 // printf("I2C device created! %d %d\r\n",dev_I2C,*dev_I2C);
charlesmn 0:0754d1901845 119
charlesmn 0:0754d1901845 120 /* creates the 53L1A1 expansion board singleton obj */
charlesmn 0:0754d1901845 121 board = XNucleo53L1A1::instance(dev_I2C, A2, D8, D2);
charlesmn 0:0754d1901845 122 printf("board created!\r\n");
charlesmn 0:0754d1901845 123
charlesmn 0:0754d1901845 124 /* init the 53L1A1 expansion board with default values */
charlesmn 0:0754d1901845 125 status = board->init_board();
charlesmn 0:0754d1901845 126 if (status) {
charlesmn 0:0754d1901845 127 printf("Failed to init board!\r\n");
charlesmn 0:0754d1901845 128 return 0;
charlesmn 0:0754d1901845 129 }
charlesmn 0:0754d1901845 130
charlesmn 0:0754d1901845 131
charlesmn 0:0754d1901845 132 printf("board initiated! - %d\r\n", status);
charlesmn 0:0754d1901845 133
charlesmn 0:0754d1901845 134 for (ToFSensor=0;ToFSensor< NUM_SENSORS ;ToFSensor++){
charlesmn 0:0754d1901845 135 switch(ToFSensor){
charlesmn 0:0754d1901845 136 case 1:
charlesmn 0:0754d1901845 137 if (board->sensor_centre== NULL ) continue;
charlesmn 0:0754d1901845 138 Dev=&devCentre;
charlesmn 0:0754d1901845 139 Sensor=board->sensor_centre;
charlesmn 0:0754d1901845 140 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:0754d1901845 141 printf("configuring centre channel \n");
charlesmn 0:0754d1901845 142 break;
charlesmn 0:0754d1901845 143 case 0:
charlesmn 0:0754d1901845 144 if (board->sensor_left== NULL ) continue;
charlesmn 0:0754d1901845 145 Dev=&devLeft;
charlesmn 0:0754d1901845 146 Sensor=board->sensor_left;
charlesmn 0:0754d1901845 147 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
charlesmn 0:0754d1901845 148 printf("configuring left channel \n");
charlesmn 0:0754d1901845 149 break;
charlesmn 0:0754d1901845 150 case 2:
charlesmn 0:0754d1901845 151 if (board->sensor_right== NULL ) continue;
charlesmn 0:0754d1901845 152 Dev=&devRight;
charlesmn 0:0754d1901845 153 Sensor=board->sensor_right;
charlesmn 0:0754d1901845 154 Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
charlesmn 0:0754d1901845 155 printf("configuring right channel \n");
charlesmn 0:0754d1901845 156 break;
charlesmn 0:0754d1901845 157 default:
charlesmn 0:0754d1901845 158 printf(" error in switch, invalid ToF sensor \n");
charlesmn 0:0754d1901845 159 }
charlesmn 0:0754d1901845 160
charlesmn 0:0754d1901845 161 // configure the sensors
charlesmn 0:0754d1901845 162 Dev->comms_speed_khz = 400;
charlesmn 0:0754d1901845 163
charlesmn 0:0754d1901845 164 Dev->comms_type = 1;
charlesmn 0:0754d1901845 165
charlesmn 0:0754d1901845 166 Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
charlesmn 0:0754d1901845 167 printf("VL53L1X: %02X %d\n\r", wordData,Dev->i2c_slave_address);
charlesmn 0:0754d1901845 168 /* Device Initialization and setting */
charlesmn 0:0754d1901845 169 status = Sensor->vl53L1_DataInit();
charlesmn 0:0754d1901845 170 status = Sensor->vl53L1_StaticInit();
charlesmn 0:0754d1901845 171 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_RANGING);
charlesmn 0:0754d1901845 172 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
charlesmn 0:0754d1901845 173 status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 50000);
charlesmn 0:0754d1901845 174 status = Sensor->vl53L1_SetInterMeasurementPeriodMilliSeconds( 100);
charlesmn 0:0754d1901845 175 }
charlesmn 0:0754d1901845 176
charlesmn 0:0754d1901845 177 if (board->sensor_centre!= NULL )
charlesmn 0:0754d1901845 178 {
charlesmn 0:0754d1901845 179 printf("starting interrupt centre\n");
charlesmn 0:0754d1901845 180 Sensor=board->sensor_centre;
charlesmn 0:0754d1901845 181 devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:0754d1901845 182 int1 = new WaitForMeasurement(A2,&devCentre);
charlesmn 0:0754d1901845 183 status = Sensor->vl53L1_StartMeasurement();
charlesmn 0:0754d1901845 184 }
charlesmn 0:0754d1901845 185
charlesmn 0:0754d1901845 186
charlesmn 0:0754d1901845 187 if (board->sensor_left!= NULL )
charlesmn 0:0754d1901845 188 {
charlesmn 0:0754d1901845 189 printf("starting interrupt left\n");
charlesmn 0:0754d1901845 190 Sensor=board->sensor_left;
charlesmn 0:0754d1901845 191 devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
charlesmn 0:0754d1901845 192 int2 = new WaitForMeasurement(D8,&devLeft);
charlesmn 0:0754d1901845 193 status = Sensor->vl53L1_StartMeasurement();
charlesmn 0:0754d1901845 194 }
charlesmn 0:0754d1901845 195
charlesmn 0:0754d1901845 196 if (board->sensor_right!= NULL )
charlesmn 0:0754d1901845 197 {
charlesmn 0:0754d1901845 198 printf("starting interrupt right\n");
charlesmn 0:0754d1901845 199 Sensor=board->sensor_right;
charlesmn 0:0754d1901845 200 devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
charlesmn 0:0754d1901845 201 int3 = new WaitForMeasurement(D2,&devRight);
charlesmn 0:0754d1901845 202 status = Sensor->vl53L1_StartMeasurement();
charlesmn 0:0754d1901845 203 }
charlesmn 0:0754d1901845 204
charlesmn 0:0754d1901845 205 // loop waiting for interrupts to happen. This is signaled by int_centre_result,int_left_result or int_right_result
charlesmn 0:0754d1901845 206 // being non zero. The are set back to zero when processing is completed
charlesmn 0:0754d1901845 207 while (1)
charlesmn 0:0754d1901845 208 {
charlesmn 0:0754d1901845 209 static VL53L1_RangingMeasurementData_t RangingData;
charlesmn 0:0754d1901845 210 VL53L1_MultiRangingData_t MultiRangingData;
charlesmn 0:0754d1901845 211 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
charlesmn 0:0754d1901845 212
charlesmn 0:0754d1901845 213 wait_ms( 1 * 100);
charlesmn 0:0754d1901845 214 if (int_centre_result != 0)
charlesmn 0:0754d1901845 215 {
charlesmn 0:0754d1901845 216 status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:0754d1901845 217 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:0754d1901845 218 if ( no_of_object_found < 10 )
charlesmn 0:0754d1901845 219 {
charlesmn 0:0754d1901845 220 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:0754d1901845 221 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:0754d1901845 222 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:0754d1901845 223 {
charlesmn 0:0754d1901845 224 printf("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:0754d1901845 225 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:0754d1901845 226 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:0754d1901845 227 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:0754d1901845 228 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:0754d1901845 229 }
charlesmn 0:0754d1901845 230 }
charlesmn 0:0754d1901845 231 }
charlesmn 0:0754d1901845 232 int_centre_result = 0;
charlesmn 0:0754d1901845 233 status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:0754d1901845 234 }
charlesmn 0:0754d1901845 235
charlesmn 0:0754d1901845 236
charlesmn 0:0754d1901845 237 if (int_left_result != 0)
charlesmn 0:0754d1901845 238 {
charlesmn 0:0754d1901845 239 status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:0754d1901845 240 if ( status == 0)
charlesmn 0:0754d1901845 241 {
charlesmn 0:0754d1901845 242 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:0754d1901845 243 if ( no_of_object_found < 10 )
charlesmn 0:0754d1901845 244 {
charlesmn 0:0754d1901845 245 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:0754d1901845 246 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:0754d1901845 247 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:0754d1901845 248 {
charlesmn 0:0754d1901845 249
charlesmn 0:0754d1901845 250 printf("left \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:0754d1901845 251 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:0754d1901845 252 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:0754d1901845 253 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:0754d1901845 254 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:0754d1901845 255 }
charlesmn 0:0754d1901845 256 }
charlesmn 0:0754d1901845 257 }
charlesmn 0:0754d1901845 258 }
charlesmn 0:0754d1901845 259
charlesmn 0:0754d1901845 260 int_left_result = 0;
charlesmn 0:0754d1901845 261 status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:0754d1901845 262
charlesmn 0:0754d1901845 263 }
charlesmn 0:0754d1901845 264
charlesmn 0:0754d1901845 265
charlesmn 0:0754d1901845 266 if (int_right_result != 0)
charlesmn 0:0754d1901845 267 {
charlesmn 0:0754d1901845 268 status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:0754d1901845 269 if ( status == 0)
charlesmn 0:0754d1901845 270 {
charlesmn 0:0754d1901845 271 // if valid result print it
charlesmn 0:0754d1901845 272 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:0754d1901845 273 if ( no_of_object_found < 10 )
charlesmn 0:0754d1901845 274 {
charlesmn 0:0754d1901845 275 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:0754d1901845 276 if (pMultiRangingData->RangeData[j].RangeStatus == 0)
charlesmn 0:0754d1901845 277 {
charlesmn 0:0754d1901845 278 printf("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:0754d1901845 279 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:0754d1901845 280 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:0754d1901845 281 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:0754d1901845 282 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:0754d1901845 283 }
charlesmn 0:0754d1901845 284 }
charlesmn 0:0754d1901845 285 }
charlesmn 0:0754d1901845 286 }
charlesmn 0:0754d1901845 287 // clear interrupt flag
charlesmn 0:0754d1901845 288 int_right_result = 0;
charlesmn 0:0754d1901845 289 // clear theinterrupt and wait for another result
charlesmn 0:0754d1901845 290 status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:0754d1901845 291
charlesmn 0:0754d1901845 292 }
charlesmn 0:0754d1901845 293 }
charlesmn 0:0754d1901845 294 printf("terminated");
charlesmn 0:0754d1901845 295 }
charlesmn 0:0754d1901845 296