ST Expansion SW Team / Mbed OS VL53L1CB_NoShield_MB6_3sensors_interrupt_ranging

Sample MBed program running ST XNucleo VL53L1CB ToF sensors directly connected to the the F401. Uses interrupts to cope with up to 3 sensors in multizone mode. Includes MBed V6.4

Dependencies:   X_NUCLEO_53L1A2

main.cpp@1:51e9344a1f33, 2020-11-09 (annotated)

Committer:
charlesmn
Date:
Mon Nov 09 17:53:08 2020 +0000
Revision:
1:51e9344a1f33
Parent:
0:2ee129a3b1e8
Child:
2:d480db875e4a
No functional change. Changed Filenames and functions names to be VL53L1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:2ee129a3b1e8 1 /*
charlesmn 0:2ee129a3b1e8 2 * This VL53L1 test application performs range measurements
charlesmn 0:2ee129a3b1e8 3 * using the onboard embedded centre sensor and two satelites, in ranging, interrupt mode.
charlesmn 0:2ee129a3b1e8 4 * This program does not use the expansion board and the sensors a directly connected to the F401.
charlesmn 0:2ee129a3b1e8 5 * Measured ranges are ouput on the Serial Port, running at 115200 baud.
charlesmn 0:2ee129a3b1e8 6 *
charlesmn 0:2ee129a3b1e8 7 * This is designed to work with MBed V2 , MBed V5 and MBed V6.
charlesmn 0:2ee129a3b1e8 8 *
charlesmn 0:2ee129a3b1e8 9 *
charlesmn 0:2ee129a3b1e8 10 * The Reset button can be used to restart the program.
charlesmn 0:2ee129a3b1e8 11 */
charlesmn 0:2ee129a3b1e8 12
charlesmn 0:2ee129a3b1e8 13 #include <stdio.h>
charlesmn 0:2ee129a3b1e8 14
charlesmn 0:2ee129a3b1e8 15 #include "mbed.h"
charlesmn 1:51e9344a1f33 16 #include "XNucleo53L1A2.h"
charlesmn 0:2ee129a3b1e8 17 #include "ToF_I2C.h"
charlesmn 0:2ee129a3b1e8 18 #include <time.h>
charlesmn 0:2ee129a3b1e8 19
charlesmn 0:2ee129a3b1e8 20 // i2c comms pins
charlesmn 0:2ee129a3b1e8 21 #define I2C_SDA D14
charlesmn 0:2ee129a3b1e8 22 #define I2C_SCL D15
charlesmn 0:2ee129a3b1e8 23
charlesmn 0:2ee129a3b1e8 24 // i2c addresses the sensors are configured to be
charlesmn 0:2ee129a3b1e8 25 #define NEW_SENSOR_CENTRE_ADDRESS 0x56
charlesmn 0:2ee129a3b1e8 26 #define NEW_SENSOR_LEFT_ADDRESS 0x58
charlesmn 0:2ee129a3b1e8 27 #define NEW_SENSOR_RIGHT_ADDRESS 0x5a
charlesmn 0:2ee129a3b1e8 28
charlesmn 0:2ee129a3b1e8 29
charlesmn 0:2ee129a3b1e8 30 #define NUM_SENSORS 3
charlesmn 0:2ee129a3b1e8 31
charlesmn 1:51e9344a1f33 32 // define the sensor interrupt pins. This must match the wiring!
charlesmn 0:2ee129a3b1e8 33 PinName CentreIntPin = A2;
charlesmn 0:2ee129a3b1e8 34 PinName LeftIntPin = D9;
charlesmn 0:2ee129a3b1e8 35 PinName RightIntPin = D4;
charlesmn 0:2ee129a3b1e8 36
charlesmn 0:2ee129a3b1e8 37
charlesmn 0:2ee129a3b1e8 38 // MBed V6.4 has renamed wait_ms and UnbufferedSerial replaces Serial
charlesmn 0:2ee129a3b1e8 39 #if (MBED_VERSION > 60300)
charlesmn 0:2ee129a3b1e8 40 UnbufferedSerial pc(SERIAL_TX, SERIAL_RX);
charlesmn 0:2ee129a3b1e8 41 extern "C" void wait_ms(int ms);
charlesmn 0:2ee129a3b1e8 42 #else
charlesmn 0:2ee129a3b1e8 43 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 0:2ee129a3b1e8 44 #endif
charlesmn 0:2ee129a3b1e8 45
charlesmn 0:2ee129a3b1e8 46 static int int_centre_result = 0;
charlesmn 0:2ee129a3b1e8 47 static int int_left_result = 0;
charlesmn 0:2ee129a3b1e8 48 static int int_right_result = 0;
charlesmn 0:2ee129a3b1e8 49
charlesmn 0:2ee129a3b1e8 50
charlesmn 0:2ee129a3b1e8 51 class WaitForMeasurement {
charlesmn 0:2ee129a3b1e8 52 public:
charlesmn 0:2ee129a3b1e8 53
charlesmn 0:2ee129a3b1e8 54
charlesmn 0:2ee129a3b1e8 55 // this class services the interrupts from the ToF sensors.
charlesmn 0:2ee129a3b1e8 56 // There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things.
charlesmn 0:2ee129a3b1e8 57 // We keep things simple by only raising a flag so all the real work is done outside the interrupt.
charlesmn 0:2ee129a3b1e8 58 // 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:2ee129a3b1e8 59 WaitForMeasurement(): _interrupt(A1)
charlesmn 0:2ee129a3b1e8 60 {
charlesmn 0:2ee129a3b1e8 61 }
charlesmn 0:2ee129a3b1e8 62
charlesmn 0:2ee129a3b1e8 63
charlesmn 0:2ee129a3b1e8 64 // constructor
charlesmn 0:2ee129a3b1e8 65 WaitForMeasurement(PinName pin, uint8_t slave_address) : _interrupt(pin) // create the InterruptIn on the pin specified
charlesmn 0:2ee129a3b1e8 66 {
charlesmn 0:2ee129a3b1e8 67 i2c_slave_address = slave_address;
charlesmn 0:2ee129a3b1e8 68 _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach routine to deal with interrupt
charlesmn 0:2ee129a3b1e8 69
charlesmn 0:2ee129a3b1e8 70 }
charlesmn 0:2ee129a3b1e8 71
charlesmn 0:2ee129a3b1e8 72
charlesmn 0:2ee129a3b1e8 73 // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
charlesmn 0:2ee129a3b1e8 74 void got_interrupt()
charlesmn 0:2ee129a3b1e8 75 {
charlesmn 0:2ee129a3b1e8 76
charlesmn 0:2ee129a3b1e8 77 // identify which sensor this instance relates to and flag an interrupt has occured
charlesmn 0:2ee129a3b1e8 78 if (i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
charlesmn 0:2ee129a3b1e8 79 int_centre_result = 1; //flag to main that interrupt happened
charlesmn 0:2ee129a3b1e8 80 if (i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
charlesmn 0:2ee129a3b1e8 81 int_left_result = 1; //flag to main that interrupt happened
charlesmn 0:2ee129a3b1e8 82 if (i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
charlesmn 0:2ee129a3b1e8 83 int_right_result = 1; //flag to main that interrupt happened
charlesmn 0:2ee129a3b1e8 84 }
charlesmn 0:2ee129a3b1e8 85
charlesmn 0:2ee129a3b1e8 86
charlesmn 0:2ee129a3b1e8 87 //destructor
charlesmn 0:2ee129a3b1e8 88 ~WaitForMeasurement()
charlesmn 0:2ee129a3b1e8 89 {
charlesmn 0:2ee129a3b1e8 90 printf("destruction \n");
charlesmn 0:2ee129a3b1e8 91 }
charlesmn 0:2ee129a3b1e8 92
charlesmn 0:2ee129a3b1e8 93 private:
charlesmn 0:2ee129a3b1e8 94 InterruptIn _interrupt;
charlesmn 0:2ee129a3b1e8 95 int status;
charlesmn 0:2ee129a3b1e8 96 uint8_t i2c_slave_address;
charlesmn 0:2ee129a3b1e8 97
charlesmn 0:2ee129a3b1e8 98 };
charlesmn 0:2ee129a3b1e8 99
charlesmn 0:2ee129a3b1e8 100
charlesmn 0:2ee129a3b1e8 101 /*=================================== Main ==================================
charlesmn 0:2ee129a3b1e8 102 =============================================================================*/
charlesmn 0:2ee129a3b1e8 103 int main()
charlesmn 0:2ee129a3b1e8 104 {
charlesmn 0:2ee129a3b1e8 105 int status;
charlesmn 1:51e9344a1f33 106 VL53L1 * Sensor;
charlesmn 1:51e9344a1f33 107 VL53L1 * SensorCentre;
charlesmn 1:51e9344a1f33 108 VL53L1 * SensorLeft;
charlesmn 1:51e9344a1f33 109 VL53L1 * SensorRight;
charlesmn 0:2ee129a3b1e8 110 DigitalOut xshutdownCentre(D9);
charlesmn 0:2ee129a3b1e8 111 DigitalOut xshutdownLeft(D4);
charlesmn 0:2ee129a3b1e8 112 DigitalOut xshutdownRight(D3);
charlesmn 0:2ee129a3b1e8 113
charlesmn 0:2ee129a3b1e8 114 uint8_t ToFSensor = 1;
charlesmn 0:2ee129a3b1e8 115
charlesmn 0:2ee129a3b1e8 116
charlesmn 0:2ee129a3b1e8 117 WaitForMeasurement* int2;
charlesmn 0:2ee129a3b1e8 118 WaitForMeasurement* int1;
charlesmn 0:2ee129a3b1e8 119 WaitForMeasurement* int3;
charlesmn 0:2ee129a3b1e8 120
charlesmn 0:2ee129a3b1e8 121 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 0:2ee129a3b1e8 122
charlesmn 0:2ee129a3b1e8 123 printf("VL53L1CB_NoShield_3sensors_interrupt_ranging mbed = %d \r\n",MBED_VERSION);
charlesmn 0:2ee129a3b1e8 124
charlesmn 0:2ee129a3b1e8 125 // create the i2c instance
charlesmn 0:2ee129a3b1e8 126 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:2ee129a3b1e8 127 // create the sensor instances
charlesmn 1:51e9344a1f33 128 SensorCentre = new VL53L1(dev_I2C, &xshutdownCentre, CentreIntPin);
charlesmn 1:51e9344a1f33 129 SensorLeft = new VL53L1(dev_I2C, &xshutdownLeft, LeftIntPin);
charlesmn 1:51e9344a1f33 130 SensorRight = new VL53L1(dev_I2C, &xshutdownRight, RightIntPin);
charlesmn 0:2ee129a3b1e8 131
charlesmn 0:2ee129a3b1e8 132 printf("board created!\r\n");
charlesmn 0:2ee129a3b1e8 133 // initialise sensors
charlesmn 0:2ee129a3b1e8 134 for (ToFSensor=0;ToFSensor< NUM_SENSORS ;ToFSensor++){
charlesmn 0:2ee129a3b1e8 135 switch(ToFSensor){
charlesmn 0:2ee129a3b1e8 136 case 0:
charlesmn 0:2ee129a3b1e8 137 if (SensorCentre == NULL ) continue;
charlesmn 0:2ee129a3b1e8 138 // try to initialise the sensor. returns -1 if sensor is not there.
charlesmn 0:2ee129a3b1e8 139 if (SensorCentre->InitSensor(NEW_SENSOR_CENTRE_ADDRESS) ==-1)
charlesmn 0:2ee129a3b1e8 140 {
charlesmn 0:2ee129a3b1e8 141 printf("centre not found \n");
charlesmn 0:2ee129a3b1e8 142 SensorCentre = NULL;
charlesmn 0:2ee129a3b1e8 143 continue;
charlesmn 0:2ee129a3b1e8 144 }
charlesmn 0:2ee129a3b1e8 145 Sensor=SensorCentre;
charlesmn 0:2ee129a3b1e8 146 printf("configuring centre channel \n");
charlesmn 0:2ee129a3b1e8 147 break;
charlesmn 0:2ee129a3b1e8 148 case 1:
charlesmn 0:2ee129a3b1e8 149 if (SensorLeft== NULL ) continue;
charlesmn 0:2ee129a3b1e8 150 if (SensorLeft->InitSensor(NEW_SENSOR_LEFT_ADDRESS)==-1)
charlesmn 0:2ee129a3b1e8 151 {
charlesmn 0:2ee129a3b1e8 152 printf("left not found \n");
charlesmn 0:2ee129a3b1e8 153 SensorLeft = NULL;
charlesmn 0:2ee129a3b1e8 154 continue;
charlesmn 0:2ee129a3b1e8 155 }
charlesmn 0:2ee129a3b1e8 156 Sensor=SensorLeft;
charlesmn 0:2ee129a3b1e8 157 printf("configuring left channel \n");
charlesmn 0:2ee129a3b1e8 158 break;
charlesmn 0:2ee129a3b1e8 159 case 2:
charlesmn 0:2ee129a3b1e8 160 if (SensorRight == NULL ) continue;
charlesmn 0:2ee129a3b1e8 161 if(SensorRight->InitSensor(NEW_SENSOR_RIGHT_ADDRESS)==-1)
charlesmn 0:2ee129a3b1e8 162 {
charlesmn 0:2ee129a3b1e8 163 printf("right not found \n");
charlesmn 0:2ee129a3b1e8 164 SensorRight = NULL;
charlesmn 0:2ee129a3b1e8 165 continue;
charlesmn 0:2ee129a3b1e8 166 }
charlesmn 0:2ee129a3b1e8 167 Sensor=SensorRight;
charlesmn 0:2ee129a3b1e8 168 printf("configuring right channel \n");
charlesmn 0:2ee129a3b1e8 169 break;
charlesmn 0:2ee129a3b1e8 170 default:
charlesmn 0:2ee129a3b1e8 171 printf(" error in switch, invalid ToF sensor \n");
charlesmn 0:2ee129a3b1e8 172 }
charlesmn 0:2ee129a3b1e8 173 wait_ms(250); // wait for the sensor to come alive
charlesmn 0:2ee129a3b1e8 174
charlesmn 0:2ee129a3b1e8 175 // configure the sensors
charlesmn 0:2ee129a3b1e8 176
charlesmn 0:2ee129a3b1e8 177 /* Device Initialization and setting */
charlesmn 0:2ee129a3b1e8 178 status = Sensor->vl53L1_DataInit();
charlesmn 0:2ee129a3b1e8 179 status = Sensor->vl53L1_StaticInit();
charlesmn 0:2ee129a3b1e8 180 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_RANGING);
charlesmn 0:2ee129a3b1e8 181 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
charlesmn 0:2ee129a3b1e8 182 status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 50000);
charlesmn 0:2ee129a3b1e8 183 status = Sensor->vl53L1_SetInterMeasurementPeriodMilliSeconds( 100);
charlesmn 0:2ee129a3b1e8 184 }
charlesmn 0:2ee129a3b1e8 185
charlesmn 0:2ee129a3b1e8 186 if (SensorCentre != NULL )
charlesmn 0:2ee129a3b1e8 187 {
charlesmn 0:2ee129a3b1e8 188 printf("starting interrupt centre\n");
charlesmn 0:2ee129a3b1e8 189 // create interrupt process for the centre sensor
charlesmn 0:2ee129a3b1e8 190 int1 = new WaitForMeasurement(CentreIntPin,NEW_SENSOR_CENTRE_ADDRESS);
charlesmn 0:2ee129a3b1e8 191 // start the snesor measuring
charlesmn 0:2ee129a3b1e8 192 status = SensorCentre->vl53L1_StartMeasurement();
charlesmn 0:2ee129a3b1e8 193 printf("end interrupt centre\n");
charlesmn 0:2ee129a3b1e8 194 }
charlesmn 0:2ee129a3b1e8 195
charlesmn 0:2ee129a3b1e8 196
charlesmn 0:2ee129a3b1e8 197 if (SensorLeft!= NULL )
charlesmn 0:2ee129a3b1e8 198 {
charlesmn 0:2ee129a3b1e8 199 printf("starting interrupt left\n");
charlesmn 0:2ee129a3b1e8 200 int2 = new WaitForMeasurement(LeftIntPin,NEW_SENSOR_LEFT_ADDRESS);
charlesmn 0:2ee129a3b1e8 201 status = SensorLeft->vl53L1_StartMeasurement();
charlesmn 0:2ee129a3b1e8 202 }
charlesmn 0:2ee129a3b1e8 203
charlesmn 0:2ee129a3b1e8 204 if (SensorRight!= NULL )
charlesmn 0:2ee129a3b1e8 205 {
charlesmn 0:2ee129a3b1e8 206 printf("starting interrupt right\n");
charlesmn 0:2ee129a3b1e8 207 int3 = new WaitForMeasurement(RightIntPin,NEW_SENSOR_RIGHT_ADDRESS);
charlesmn 0:2ee129a3b1e8 208 status = SensorRight->vl53L1_StartMeasurement();
charlesmn 0:2ee129a3b1e8 209 }
charlesmn 0:2ee129a3b1e8 210
charlesmn 0:2ee129a3b1e8 211 printf("loop forever \n");
charlesmn 0:2ee129a3b1e8 212 // loop waiting for interrupts to happen. This is signaled by int_centre_result,int_left_result or int_right_result
charlesmn 0:2ee129a3b1e8 213 // being non zero. The are set back to zero when processing is completed
charlesmn 0:2ee129a3b1e8 214 while (1)
charlesmn 0:2ee129a3b1e8 215 {
charlesmn 0:2ee129a3b1e8 216 static VL53L1_RangingMeasurementData_t RangingData;
charlesmn 0:2ee129a3b1e8 217 VL53L1_MultiRangingData_t MultiRangingData;
charlesmn 0:2ee129a3b1e8 218 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
charlesmn 0:2ee129a3b1e8 219
charlesmn 0:2ee129a3b1e8 220 wait_ms( 1 * 50);
charlesmn 0:2ee129a3b1e8 221 if (int_centre_result != 0)
charlesmn 0:2ee129a3b1e8 222 {
charlesmn 0:2ee129a3b1e8 223 // as there has been an interrupt there is data waiting so get it
charlesmn 0:2ee129a3b1e8 224 status = SensorCentre->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:2ee129a3b1e8 225 if ( status == VL53L1_ERROR_NONE)
charlesmn 0:2ee129a3b1e8 226 {
charlesmn 0:2ee129a3b1e8 227 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:2ee129a3b1e8 228 if ( no_of_object_found < 10 )
charlesmn 0:2ee129a3b1e8 229 {
charlesmn 0:2ee129a3b1e8 230 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:2ee129a3b1e8 231 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:2ee129a3b1e8 232 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:2ee129a3b1e8 233 {
charlesmn 0:2ee129a3b1e8 234 printf("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:2ee129a3b1e8 235 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:2ee129a3b1e8 236 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:2ee129a3b1e8 237 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:2ee129a3b1e8 238 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:2ee129a3b1e8 239 }
charlesmn 0:2ee129a3b1e8 240 }
charlesmn 0:2ee129a3b1e8 241 }
charlesmn 0:2ee129a3b1e8 242 }
charlesmn 0:2ee129a3b1e8 243 int_centre_result = 0;
charlesmn 0:2ee129a3b1e8 244 status = SensorCentre->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:2ee129a3b1e8 245 }
charlesmn 0:2ee129a3b1e8 246
charlesmn 0:2ee129a3b1e8 247
charlesmn 0:2ee129a3b1e8 248 if (int_left_result != 0)
charlesmn 0:2ee129a3b1e8 249 {
charlesmn 0:2ee129a3b1e8 250 status = SensorLeft->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:2ee129a3b1e8 251 if ( status == VL53L1_ERROR_NONE)
charlesmn 0:2ee129a3b1e8 252 {
charlesmn 0:2ee129a3b1e8 253 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:2ee129a3b1e8 254 if ( no_of_object_found < 10 )
charlesmn 0:2ee129a3b1e8 255 {
charlesmn 0:2ee129a3b1e8 256 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:2ee129a3b1e8 257 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:2ee129a3b1e8 258 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:2ee129a3b1e8 259 {
charlesmn 0:2ee129a3b1e8 260
charlesmn 0:2ee129a3b1e8 261 printf("left \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:2ee129a3b1e8 262 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:2ee129a3b1e8 263 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:2ee129a3b1e8 264 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:2ee129a3b1e8 265 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:2ee129a3b1e8 266 }
charlesmn 0:2ee129a3b1e8 267 }
charlesmn 0:2ee129a3b1e8 268 }
charlesmn 0:2ee129a3b1e8 269 }
charlesmn 0:2ee129a3b1e8 270
charlesmn 0:2ee129a3b1e8 271 int_left_result = 0;
charlesmn 0:2ee129a3b1e8 272 status = SensorLeft->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:2ee129a3b1e8 273
charlesmn 0:2ee129a3b1e8 274 }
charlesmn 0:2ee129a3b1e8 275
charlesmn 0:2ee129a3b1e8 276
charlesmn 0:2ee129a3b1e8 277 if (int_right_result != 0)
charlesmn 0:2ee129a3b1e8 278 {
charlesmn 0:2ee129a3b1e8 279 status = SensorRight->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:2ee129a3b1e8 280 if ( status == VL53L1_ERROR_NONE)
charlesmn 0:2ee129a3b1e8 281 {
charlesmn 0:2ee129a3b1e8 282 // if valid result print it
charlesmn 0:2ee129a3b1e8 283 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:2ee129a3b1e8 284 if ( no_of_object_found < 10 )
charlesmn 0:2ee129a3b1e8 285 {
charlesmn 0:2ee129a3b1e8 286 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:2ee129a3b1e8 287 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:2ee129a3b1e8 288 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:2ee129a3b1e8 289 {
charlesmn 0:2ee129a3b1e8 290 printf("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:2ee129a3b1e8 291 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:2ee129a3b1e8 292 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:2ee129a3b1e8 293 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:2ee129a3b1e8 294 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:2ee129a3b1e8 295 }
charlesmn 0:2ee129a3b1e8 296 }
charlesmn 0:2ee129a3b1e8 297 }
charlesmn 0:2ee129a3b1e8 298 }
charlesmn 0:2ee129a3b1e8 299 // clear interrupt flag
charlesmn 0:2ee129a3b1e8 300 int_right_result = 0;
charlesmn 0:2ee129a3b1e8 301 // clear theinterrupt and wait for another result
charlesmn 0:2ee129a3b1e8 302 status = SensorRight->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:2ee129a3b1e8 303
charlesmn 0:2ee129a3b1e8 304 }
charlesmn 0:2ee129a3b1e8 305 }
charlesmn 0:2ee129a3b1e8 306 printf("terminated");
charlesmn 0:2ee129a3b1e8 307 }
charlesmn 0:2ee129a3b1e8 308
charlesmn 0:2ee129a3b1e8 309
charlesmn 0:2ee129a3b1e8 310 // this function doesn't exist in MBed6.4 onwards. It is required for the F401 drivers.
charlesmn 0:2ee129a3b1e8 311 // addded here as it allows the file to be compatible with all mbed versions
charlesmn 0:2ee129a3b1e8 312 #if (MBED_VERSION > 60300)
charlesmn 0:2ee129a3b1e8 313 void wait_ms(int ms)
charlesmn 0:2ee129a3b1e8 314 {
charlesmn 0:2ee129a3b1e8 315 thread_sleep_for(ms);
charlesmn 0:2ee129a3b1e8 316 }
charlesmn 0:2ee129a3b1e8 317 #endif
charlesmn 0:2ee129a3b1e8 318