ST Expansion SW Team / Mbed OS VL53L1CB_shield_1sensor_interrupt_auton

VL53L1CB ranging example, using embedded sensor on X-Nucleo-53L1A2 expansion board, in interrupt mode.

Dependencies:   X_NUCLEO_53L1A2

main.cpp@0:020912dfa221, 2020-11-08 (annotated)

Committer:
charlesmn
Date:
Sun Nov 08 15:35:55 2020 +0000
Revision:
0:020912dfa221
Child:
1:ff48a20de191
; Sample MBed program for use with ST XNucleo VL53L1CB board. Uses interrupts to cope with up to 3 sensors in autonomous mode. Includes MBed V6.4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:020912dfa221 1 /*
charlesmn 0:020912dfa221 2 * This VL53L1X Expansion board test application performs range measurements
charlesmn 0:020912dfa221 3 * using the onboard embedded centre sensor and two satelites, in autonomous, interrupt mode.
charlesmn 0:020912dfa221 4 * Measured ranges are ouput on the Serial Port, running at 115200 baud.
charlesmn 0:020912dfa221 5 *
charlesmn 0:020912dfa221 6 *
charlesmn 0:020912dfa221 7 * This is designed to work with MBed V2 , MBed V5 and MBed V6.
charlesmn 0:020912dfa221 8 *
charlesmn 0:020912dfa221 9 *
charlesmn 0:020912dfa221 10 * The Reset button can be used to restart the program.
charlesmn 0:020912dfa221 11 */
charlesmn 0:020912dfa221 12
charlesmn 0:020912dfa221 13 #include <stdio.h>
charlesmn 0:020912dfa221 14
charlesmn 0:020912dfa221 15 #include "mbed.h"
charlesmn 0:020912dfa221 16 #include "XNucleo53L1A1.h"
charlesmn 0:020912dfa221 17 #include "ToF_I2C.h"
charlesmn 0:020912dfa221 18 #include <time.h>
charlesmn 0:020912dfa221 19
charlesmn 0:020912dfa221 20 // i2c comms port pins
charlesmn 0:020912dfa221 21 #define I2C_SDA D14
charlesmn 0:020912dfa221 22 #define I2C_SCL D15
charlesmn 0:020912dfa221 23
charlesmn 0:020912dfa221 24
charlesmn 0:020912dfa221 25 #define NUM_SENSORS 3
charlesmn 0:020912dfa221 26
charlesmn 0:020912dfa221 27 PinName CentreIntPin = A2;
charlesmn 0:020912dfa221 28 // the satellite pins depend on solder blobs on the back of the shield.
charlesmn 0:020912dfa221 29 // they may not exist or may be one of two sets.
charlesmn 0:020912dfa221 30 // the centre pin always exists
charlesmn 0:020912dfa221 31 PinName LeftIntPin = D9;
charlesmn 0:020912dfa221 32 PinName RightIntPin = D4;
charlesmn 0:020912dfa221 33 // alternate set
charlesmn 0:020912dfa221 34 //PinName LeftIntPin = D8;
charlesmn 0:020912dfa221 35 //PinName RightIntPin = D2;
charlesmn 0:020912dfa221 36
charlesmn 0:020912dfa221 37
charlesmn 0:020912dfa221 38
charlesmn 0:020912dfa221 39 static XNucleo53L1A1 *board=NULL;
charlesmn 0:020912dfa221 40
charlesmn 0:020912dfa221 41 // MBed V6.4 has renamed wait_ms and UnbufferedSerial replaces Serial
charlesmn 0:020912dfa221 42 #if (MBED_VERSION > 60300)
charlesmn 0:020912dfa221 43 UnbufferedSerial pc(SERIAL_TX, SERIAL_RX);
charlesmn 0:020912dfa221 44 extern "C" void wait_ms(int ms);
charlesmn 0:020912dfa221 45 #else
charlesmn 0:020912dfa221 46 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 0:020912dfa221 47 #endif
charlesmn 0:020912dfa221 48
charlesmn 0:020912dfa221 49 // flags to indicate an interrupt has happened
charlesmn 0:020912dfa221 50 static int int_centre_result = 0;
charlesmn 0:020912dfa221 51 static int int_left_result = 0;
charlesmn 0:020912dfa221 52 static int int_right_result = 0;
charlesmn 0:020912dfa221 53
charlesmn 0:020912dfa221 54 // flags to indicate an interrupt has cleared
charlesmn 0:020912dfa221 55 static int int_centre_dropped = 0;
charlesmn 0:020912dfa221 56 static int int_left_dropped = 0;
charlesmn 0:020912dfa221 57 static int int_right_dropped = 0;
charlesmn 0:020912dfa221 58
charlesmn 0:020912dfa221 59 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
charlesmn 0:020912dfa221 60
charlesmn 0:020912dfa221 61 class WaitForMeasurement {
charlesmn 0:020912dfa221 62 public:
charlesmn 0:020912dfa221 63
charlesmn 0:020912dfa221 64
charlesmn 0:020912dfa221 65 // this class services the interrupts from the ToF sensors.
charlesmn 0:020912dfa221 66 // There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things.
charlesmn 0:020912dfa221 67 // We keep things simple by only raising a flag so all the real work is done outside the interrupt.
charlesmn 0:020912dfa221 68 // 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:020912dfa221 69 WaitForMeasurement(): _interrupt(A1)
charlesmn 0:020912dfa221 70 {
charlesmn 0:020912dfa221 71 }
charlesmn 0:020912dfa221 72
charlesmn 0:020912dfa221 73
charlesmn 0:020912dfa221 74 // constructor - Sensor is not used and can be removed
charlesmn 0:020912dfa221 75 WaitForMeasurement(PinName pin,VL53L1_DEV Dev) : _interrupt(pin) // create the InterruptIn on the pin specified to Counter
charlesmn 0:020912dfa221 76 {
charlesmn 0:020912dfa221 77 Devlocal = Dev;
charlesmn 0:020912dfa221 78 pinlocal = pin;
charlesmn 0:020912dfa221 79
charlesmn 0:020912dfa221 80 #include "mbed.h"
charlesmn 0:020912dfa221 81
charlesmn 0:020912dfa221 82
charlesmn 0:020912dfa221 83 _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // if interrupt happens read data
charlesmn 0:020912dfa221 84 _interrupt.fall(callback(this, &WaitForMeasurement::linedropped)); // if interupt clears, clear interrupt
charlesmn 0:020912dfa221 85
charlesmn 0:020912dfa221 86 }
charlesmn 0:020912dfa221 87
charlesmn 0:020912dfa221 88 // function is called every time an interupt is cleared. Sets flags to clear the interrupt
charlesmn 0:020912dfa221 89 void linedropped()
charlesmn 0:020912dfa221 90 {
charlesmn 0:020912dfa221 91
charlesmn 0:020912dfa221 92 if (Devlocal->i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
charlesmn 0:020912dfa221 93 int_centre_dropped = 1; //flag to main that interrupt cleared. A flag is raised which allows the main routine to service interupt.
charlesmn 0:020912dfa221 94 if (Devlocal->i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
charlesmn 0:020912dfa221 95 int_left_dropped = 1; //flag to main that interrupt cleared
charlesmn 0:020912dfa221 96 if (Devlocal->i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
charlesmn 0:020912dfa221 97 int_right_dropped = 1; //flag to main that interrupt cleared
charlesmn 0:020912dfa221 98
charlesmn 0:020912dfa221 99 }
charlesmn 0:020912dfa221 100
charlesmn 0:020912dfa221 101 // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
charlesmn 0:020912dfa221 102 void got_interrupt()
charlesmn 0:020912dfa221 103 {
charlesmn 0:020912dfa221 104 DigitalIn intp(pinlocal);
charlesmn 0:020912dfa221 105
charlesmn 0:020912dfa221 106 if (Devlocal->i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
charlesmn 0:020912dfa221 107 int_centre_result = 1; //flag to main that interrupt happened
charlesmn 0:020912dfa221 108 if (Devlocal->i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
charlesmn 0:020912dfa221 109 int_left_result = 1; //flag to main that interrupt happened7
charlesmn 0:020912dfa221 110 if (Devlocal->i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
charlesmn 0:020912dfa221 111 int_right_result = 1; //flag to main that interrupt happened
charlesmn 0:020912dfa221 112 }
charlesmn 0:020912dfa221 113
charlesmn 0:020912dfa221 114
charlesmn 0:020912dfa221 115 //destructor
charlesmn 0:020912dfa221 116 ~WaitForMeasurement()
charlesmn 0:020912dfa221 117 {
charlesmn 0:020912dfa221 118 printf("WaitForMeasurement destruction \n");
charlesmn 0:020912dfa221 119 }
charlesmn 0:020912dfa221 120
charlesmn 0:020912dfa221 121 private:
charlesmn 0:020912dfa221 122 InterruptIn _interrupt;
charlesmn 0:020912dfa221 123 PinName pinlocal;
charlesmn 0:020912dfa221 124 VL53L1_DEV Devlocal;
charlesmn 0:020912dfa221 125 int status;
charlesmn 0:020912dfa221 126
charlesmn 0:020912dfa221 127 };
charlesmn 0:020912dfa221 128
charlesmn 0:020912dfa221 129
charlesmn 0:020912dfa221 130
charlesmn 0:020912dfa221 131 VL53L1_Dev_t devCentre;
charlesmn 0:020912dfa221 132 VL53L1_Dev_t devLeft;
charlesmn 0:020912dfa221 133 VL53L1_Dev_t devRight;
charlesmn 0:020912dfa221 134 VL53L1_DEV Dev = &devCentre;
charlesmn 0:020912dfa221 135
charlesmn 0:020912dfa221 136
charlesmn 0:020912dfa221 137 /*=================================== Main ==================================
charlesmn 0:020912dfa221 138 =============================================================================*/
charlesmn 0:020912dfa221 139 int main()
charlesmn 0:020912dfa221 140 {
charlesmn 0:020912dfa221 141 int status;
charlesmn 0:020912dfa221 142 VL53L1X * Sensor;
charlesmn 0:020912dfa221 143 uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
charlesmn 0:020912dfa221 144
charlesmn 0:020912dfa221 145 //mbed compiler claims these are never used but they are.
charlesmn 0:020912dfa221 146 WaitForMeasurement* int2;
charlesmn 0:020912dfa221 147 WaitForMeasurement* int1;
charlesmn 0:020912dfa221 148 WaitForMeasurement* int3;
charlesmn 0:020912dfa221 149
charlesmn 0:020912dfa221 150
charlesmn 0:020912dfa221 151 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 0:020912dfa221 152
charlesmn 0:020912dfa221 153 printf("Autonomous Interruptmbed = %d \r\n",MBED_VERSION);
charlesmn 0:020912dfa221 154
charlesmn 0:020912dfa221 155 // create i2c interface
charlesmn 0:020912dfa221 156 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:020912dfa221 157
charlesmn 0:020912dfa221 158 dev_I2C->frequency(400000); //also needs doing in spi_interface.c
charlesmn 0:020912dfa221 159
charlesmn 0:020912dfa221 160 /* creates the 53L1A1 expansion board singleton obj */
charlesmn 0:020912dfa221 161 board = XNucleo53L1A1::instance(dev_I2C, CentreIntPin, LeftIntPin, RightIntPin);
charlesmn 0:020912dfa221 162
charlesmn 0:020912dfa221 163 printf("board created!\r\n");
charlesmn 0:020912dfa221 164
charlesmn 0:020912dfa221 165 /* init the 53L1A1 expansion board with default values */
charlesmn 0:020912dfa221 166 status = board->init_board();
charlesmn 0:020912dfa221 167 if (status) {
charlesmn 0:020912dfa221 168 printf("Failed to init board!\r\n");
charlesmn 0:020912dfa221 169 return 0;
charlesmn 0:020912dfa221 170 }
charlesmn 0:020912dfa221 171
charlesmn 0:020912dfa221 172
charlesmn 0:020912dfa221 173 printf("board initiated! - %d\r\n", status);
charlesmn 0:020912dfa221 174 // create sensor controller classes for each vl53l1 and configure each vl53l1
charlesmn 0:020912dfa221 175 for (ToFSensor=0;ToFSensor < NUM_SENSORS;ToFSensor++){
charlesmn 0:020912dfa221 176 switch(ToFSensor){
charlesmn 0:020912dfa221 177 case 0:
charlesmn 0:020912dfa221 178 if (board->sensor_centre== NULL ) continue; // don't create if sensor not detected
charlesmn 0:020912dfa221 179 Dev=&devCentre;
charlesmn 0:020912dfa221 180 Sensor=board->sensor_centre;
charlesmn 0:020912dfa221 181 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:020912dfa221 182 printf("configuring centre channel \n");
charlesmn 0:020912dfa221 183 break;
charlesmn 0:020912dfa221 184 case 1:
charlesmn 0:020912dfa221 185 if (board->sensor_left== NULL ) continue;
charlesmn 0:020912dfa221 186 Dev=&devLeft;
charlesmn 0:020912dfa221 187 Sensor=board->sensor_left;
charlesmn 0:020912dfa221 188 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
charlesmn 0:020912dfa221 189 printf("configuring left channel \n");
charlesmn 0:020912dfa221 190 break;
charlesmn 0:020912dfa221 191 case 2:
charlesmn 0:020912dfa221 192 if (board->sensor_right== NULL ) continue;
charlesmn 0:020912dfa221 193 Dev=&devRight;
charlesmn 0:020912dfa221 194 Sensor=board->sensor_right;
charlesmn 0:020912dfa221 195 Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
charlesmn 0:020912dfa221 196 printf("configuring right channel \n");
charlesmn 0:020912dfa221 197 break;
charlesmn 0:020912dfa221 198 default:
charlesmn 0:020912dfa221 199 printf(" error in switch, invalid ToF sensor \n");
charlesmn 0:020912dfa221 200 }
charlesmn 0:020912dfa221 201
charlesmn 0:020912dfa221 202 // configure the sensors
charlesmn 0:020912dfa221 203 Dev->comms_speed_khz = 400;
charlesmn 0:020912dfa221 204
charlesmn 0:020912dfa221 205 Dev->comms_type = 1;
charlesmn 0:020912dfa221 206
charlesmn 0:020912dfa221 207 /* Device Initialization and setting */
charlesmn 0:020912dfa221 208 status = Sensor->vl53L1_DataInit();
charlesmn 0:020912dfa221 209 status = Sensor->vl53L1_StaticInit();
charlesmn 0:020912dfa221 210 status = Sensor->vl53L1_SetPresetMode(VL53L1_PRESETMODE_AUTONOMOUS);
charlesmn 0:020912dfa221 211 status = Sensor->vl53L1_SetDistanceMode(VL53L1_DISTANCEMODE_LONG);
charlesmn 0:020912dfa221 212 status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 200 * 1000);
charlesmn 0:020912dfa221 213
charlesmn 0:020912dfa221 214
charlesmn 0:020912dfa221 215 // set the ranging and signal rate filter
charlesmn 0:020912dfa221 216 VL53L1_DetectionConfig_t thresholdconfig;
charlesmn 0:020912dfa221 217 thresholdconfig.DetectionMode = VL53L1_DETECTION_DISTANCE_ONLY; /// type VL53L1_DetectionMode in vl53l1_def.h
charlesmn 0:020912dfa221 218 thresholdconfig.Distance.CrossMode = VL53L1_THRESHOLD_IN_WINDOW; // type VL53L1_ThresholdMode. ignore if distance outside high and low
charlesmn 0:020912dfa221 219 thresholdconfig.Distance.High = 300; // high distance in mm
charlesmn 0:020912dfa221 220 thresholdconfig.Distance.Low = 200; // low distance in mm
charlesmn 0:020912dfa221 221 thresholdconfig.Rate.CrossMode=0; // type VL53L1_ThresholdMode VL53L1_THRESHOLD_CROSSED_LOW VL53L1_THRESHOLD_CROSSED_HIGH VL53L1_THRESHOLD_OUT_OF_WINDOW VL53L1_THRESHOLD_IN_WINDOW
charlesmn 0:020912dfa221 222 thresholdconfig.Rate.High = 0;
charlesmn 0:020912dfa221 223 thresholdconfig.Rate.Low = 0;
charlesmn 0:020912dfa221 224 thresholdconfig.IntrNoTarget = 0 ;// if 1 produce an interrupt even if there is no target found e.g out of range
charlesmn 0:020912dfa221 225 status = Sensor->vl53L1_SetThresholdConfig(&thresholdconfig);
charlesmn 0:020912dfa221 226 }
charlesmn 0:020912dfa221 227
charlesmn 0:020912dfa221 228 // create interrupt handlers for the three sensors and start measurements
charlesmn 0:020912dfa221 229 if (board->sensor_centre!= NULL )
charlesmn 0:020912dfa221 230 {
charlesmn 0:020912dfa221 231 printf("starting interrupt centre\n");
charlesmn 0:020912dfa221 232 devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:020912dfa221 233 int1 = new WaitForMeasurement(CentreIntPin,&devCentre); // create interrupt handler
charlesmn 0:020912dfa221 234 status = board->sensor_centre->vl53L1_StartMeasurement();
charlesmn 0:020912dfa221 235 }
charlesmn 0:020912dfa221 236
charlesmn 0:020912dfa221 237 if (board->sensor_left!= NULL )
charlesmn 0:020912dfa221 238 {
charlesmn 0:020912dfa221 239 printf("starting interrupt left\n");
charlesmn 0:020912dfa221 240 devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
charlesmn 0:020912dfa221 241 int2 = new WaitForMeasurement(LeftIntPin,&devLeft); // create interrupt handler
charlesmn 0:020912dfa221 242 status = board->sensor_left->vl53L1_StartMeasurement();
charlesmn 0:020912dfa221 243 printf("started interrupt left\n");
charlesmn 0:020912dfa221 244 }
charlesmn 0:020912dfa221 245
charlesmn 0:020912dfa221 246 if (board->sensor_right!= NULL )
charlesmn 0:020912dfa221 247 {
charlesmn 0:020912dfa221 248 printf("starting interrupt right\n");
charlesmn 0:020912dfa221 249 devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
charlesmn 0:020912dfa221 250 int3 = new WaitForMeasurement(RightIntPin,&devRight); // create interrupt handler
charlesmn 0:020912dfa221 251 status = board->sensor_right->vl53L1_StartMeasurement();
charlesmn 0:020912dfa221 252 }
charlesmn 0:020912dfa221 253
charlesmn 0:020912dfa221 254
charlesmn 0:020912dfa221 255 printf("loop forever\n");
charlesmn 0:020912dfa221 256
charlesmn 0:020912dfa221 257 // loop waiting for interrupts to happen. This is signaled by int_centre_result,int_left_result or int_right_result
charlesmn 0:020912dfa221 258 // being non zero. When the interrupts clear this is signaled by int_centre_dropped,int_left_dropped and int_right_dropped.
charlesmn 0:020912dfa221 259 // These are set back to zero when processing is completed
charlesmn 0:020912dfa221 260 while (1)
charlesmn 0:020912dfa221 261 {
charlesmn 0:020912dfa221 262 VL53L1_MultiRangingData_t MultiRangingData;
charlesmn 0:020912dfa221 263 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
charlesmn 0:020912dfa221 264
charlesmn 0:020912dfa221 265 if ( int_left_dropped || int_centre_dropped || int_right_dropped )
charlesmn 0:020912dfa221 266 wait_ms(30);
charlesmn 0:020912dfa221 267
charlesmn 0:020912dfa221 268 // when the interrupt pin goes loww start new measurement
charlesmn 0:020912dfa221 269 if ( int_centre_dropped != 0)
charlesmn 0:020912dfa221 270 {
charlesmn 0:020912dfa221 271 int_centre_dropped = 0;
charlesmn 0:020912dfa221 272 status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:020912dfa221 273 }
charlesmn 0:020912dfa221 274
charlesmn 0:020912dfa221 275 if ( int_left_dropped != 0)
charlesmn 0:020912dfa221 276 {
charlesmn 0:020912dfa221 277 int_left_dropped = 0;
charlesmn 0:020912dfa221 278 status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:020912dfa221 279 }
charlesmn 0:020912dfa221 280
charlesmn 0:020912dfa221 281 if ( int_right_dropped != 0)
charlesmn 0:020912dfa221 282 {
charlesmn 0:020912dfa221 283 int_right_dropped = 0;
charlesmn 0:020912dfa221 284 status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:020912dfa221 285 }
charlesmn 0:020912dfa221 286
charlesmn 0:020912dfa221 287 if (int_right_result != 0) // interrupt seen on right sensor
charlesmn 0:020912dfa221 288 {
charlesmn 0:020912dfa221 289 status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:020912dfa221 290 if ( status == 0)
charlesmn 0:020912dfa221 291 {
charlesmn 0:020912dfa221 292 print_results( devRight.i2c_slave_address, pMultiRangingData );
charlesmn 0:020912dfa221 293 }
charlesmn 0:020912dfa221 294
charlesmn 0:020912dfa221 295 // clear interrupt flag
charlesmn 0:020912dfa221 296 int_right_result = 0;
charlesmn 0:020912dfa221 297 }
charlesmn 0:020912dfa221 298
charlesmn 0:020912dfa221 299 if (int_left_result != 0) // interrupt seen on left sensor
charlesmn 0:020912dfa221 300 {
charlesmn 0:020912dfa221 301 status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:020912dfa221 302 if ( status == 0)
charlesmn 0:020912dfa221 303 {
charlesmn 0:020912dfa221 304 print_results( devLeft.i2c_slave_address, pMultiRangingData );
charlesmn 0:020912dfa221 305 }
charlesmn 0:020912dfa221 306
charlesmn 0:020912dfa221 307 // clear interrupt flag
charlesmn 0:020912dfa221 308 int_left_result = 0;
charlesmn 0:020912dfa221 309 }
charlesmn 0:020912dfa221 310
charlesmn 0:020912dfa221 311 if (int_centre_result != 0)
charlesmn 0:020912dfa221 312 {
charlesmn 0:020912dfa221 313 status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:020912dfa221 314 if ( status == 0)
charlesmn 0:020912dfa221 315 {
charlesmn 0:020912dfa221 316 print_results( devCentre.i2c_slave_address, pMultiRangingData );
charlesmn 0:020912dfa221 317 }
charlesmn 0:020912dfa221 318
charlesmn 0:020912dfa221 319 // clear interrupt flag
charlesmn 0:020912dfa221 320 int_centre_result = 0;
charlesmn 0:020912dfa221 321
charlesmn 0:020912dfa221 322 }
charlesmn 0:020912dfa221 323 wait_ms( 1 * 10);
charlesmn 0:020912dfa221 324
charlesmn 0:020912dfa221 325 }
charlesmn 0:020912dfa221 326 }
charlesmn 0:020912dfa221 327
charlesmn 0:020912dfa221 328
charlesmn 0:020912dfa221 329 // print what ever results are required
charlesmn 0:020912dfa221 330 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
charlesmn 0:020912dfa221 331 {
charlesmn 0:020912dfa221 332 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:020912dfa221 333
charlesmn 0:020912dfa221 334 int RoiNumber=pMultiRangingData->RoiNumber;
charlesmn 0:020912dfa221 335 // int RoiStatus=pMultiRangingData->RoiStatus;
charlesmn 0:020912dfa221 336
charlesmn 0:020912dfa221 337 if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:020912dfa221 338 {
charlesmn 0:020912dfa221 339 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:020912dfa221 340 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:020912dfa221 341 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:020912dfa221 342 {
charlesmn 0:020912dfa221 343 printf("\t spiAddr=%d \t RoiNumber=%d \t D=%5dmm \n",
charlesmn 0:020912dfa221 344 devSpiNumber,
charlesmn 0:020912dfa221 345 RoiNumber,
charlesmn 0:020912dfa221 346 pMultiRangingData->RangeData[j].RangeMilliMeter);
charlesmn 0:020912dfa221 347 /*
charlesmn 0:020912dfa221 348 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:020912dfa221 349 devSpiNumber,
charlesmn 0:020912dfa221 350 RoiNumber,
charlesmn 0:020912dfa221 351 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:020912dfa221 352 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:020912dfa221 353 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:020912dfa221 354 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:020912dfa221 355 */
charlesmn 0:020912dfa221 356 }
charlesmn 0:020912dfa221 357
charlesmn 0:020912dfa221 358 }
charlesmn 0:020912dfa221 359 } // if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:020912dfa221 360
charlesmn 0:020912dfa221 361 }
charlesmn 0:020912dfa221 362
charlesmn 0:020912dfa221 363
charlesmn 0:020912dfa221 364 #if (MBED_VERSION > 60300)
charlesmn 0:020912dfa221 365 extern "C" void wait_ms(int ms)
charlesmn 0:020912dfa221 366 {
charlesmn 0:020912dfa221 367 thread_sleep_for(ms);
charlesmn 0:020912dfa221 368 }
charlesmn 0:020912dfa221 369 #endif
charlesmn 0:020912dfa221 370
charlesmn 0:020912dfa221 371