ST Expansion SW Team / Mbed 2 deprecated VL53L1CB_MB2_NoShield_3sensors_interrupt_ranging

A sample program getting measurements from a VL53L1CB ToF sensors which are directly connected to the STM32F401. Copes with three sensors. Interrupt mode. VL53L1 is operated in ranging mode. MBed V2.

Dependencies:   mbed X_NUCLEO_53L1CB

main.cpp@1:02f2b9e04b46, 2021-01-19 (annotated)

Committer:
charlesmn
Date:
Tue Jan 19 12:22:01 2021 +0000
Revision:
1:02f2b9e04b46
Parent:
0:2b5a44a0d2ec 
Change int pins to not conflict with xshutdown pins

Who changed what in which revision?

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