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@1:ff48a20de191, 2020-11-09 (annotated)

Committer:
charlesmn
Date:
Mon Nov 09 17:36:20 2020 +0000
Revision:
1:ff48a20de191
Parent:
0:020912dfa221
Child:
2:f0ec92af4b5f
No functional change. Changed Filenames and functions names to be VL53L1

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