Sample ranging application, using X_Nucleo_53L3A2 expansion board and supporting Satellite boards, to ranging from each sensor, using interrupts. Targetting MbedOS v6.10.
Dependencies: X_NUCLEO_53L3A2
main.cpp@7:5b8a91c3bafc, 2021-05-11 (annotated)
- Committer:
- johnAlexander
- Date:
- Tue May 11 08:51:35 2021 +0000
- Revision:
- 7:5b8a91c3bafc
- Parent:
- 5:92a861d48253
Optimise performance.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
charlesmn | 0:c77591fc308d | 1 | /* |
johnAlexander | 4:360dc34c3769 | 2 | * This VL53L3 Expansion board test application performs range measurements |
johnAlexander | 7:5b8a91c3bafc | 3 | * using the onboard embedded sensor, and satellite boards, in interrupt mode. |
johnAlexander | 7:5b8a91c3bafc | 4 | * Measured ranges are output on the Serial Port, running at 115200 baud. |
charlesmn | 0:c77591fc308d | 5 | * |
johnAlexander | 4:360dc34c3769 | 6 | * The Reset button can be used to restart the program. |
charlesmn | 0:c77591fc308d | 7 | * |
johnAlexander | 5:92a861d48253 | 8 | * *** Note : |
johnAlexander | 7:5b8a91c3bafc | 9 | * Default Mbed build system settings disable printf floating-point support. |
johnAlexander | 7:5b8a91c3bafc | 10 | * Online builds seem unable to configure this. |
johnAlexander | 7:5b8a91c3bafc | 11 | * Offline builds can enable printf floating-point support. |
johnAlexander | 4:360dc34c3769 | 12 | * https://github.com/ARMmbed/mbed-os/blob/master/platform/source/minimal-printf/README.md |
johnAlexander | 4:360dc34c3769 | 13 | * .\mbed-os\platform\mbed_lib.json |
johnAlexander | 4:360dc34c3769 | 14 | * |
charlesmn | 0:c77591fc308d | 15 | */ |
johnAlexander | 5:92a861d48253 | 16 | |
charlesmn | 0:c77591fc308d | 17 | #include <stdio.h> |
johnAlexander | 3:42c96c9c627b | 18 | #include <time.h> |
charlesmn | 0:c77591fc308d | 19 | |
charlesmn | 0:c77591fc308d | 20 | #include "mbed.h" |
johnAlexander | 4:360dc34c3769 | 21 | |
johnAlexander | 3:42c96c9c627b | 22 | #include "XNucleo53L3A2.h" |
charlesmn | 2:86876e590f28 | 23 | #include "vl53L3_I2c.h" |
charlesmn | 0:c77591fc308d | 24 | |
charlesmn | 0:c77591fc308d | 25 | |
johnAlexander | 5:92a861d48253 | 26 | #define I2C_SDA D14 |
johnAlexander | 5:92a861d48253 | 27 | #define I2C_SCL D15 |
charlesmn | 0:c77591fc308d | 28 | |
johnAlexander | 7:5b8a91c3bafc | 29 | #define MEASUREMENTTIMING 2 // 5 // 10 // 20 // 30 // 55 |
charlesmn | 0:c77591fc308d | 30 | |
johnAlexander | 3:42c96c9c627b | 31 | static XNucleo53L3A2 *board=NULL; |
johnAlexander | 3:42c96c9c627b | 32 | #if (MBED_VERSION > 60300) |
johnAlexander | 5:92a861d48253 | 33 | UnbufferedSerial pc(USBTX, USBRX); |
johnAlexander | 5:92a861d48253 | 34 | extern "C" void wait_ms(int ms); |
johnAlexander | 3:42c96c9c627b | 35 | #else |
johnAlexander | 5:92a861d48253 | 36 | Serial pc(SERIAL_TX, SERIAL_RX); |
johnAlexander | 3:42c96c9c627b | 37 | #endif |
charlesmn | 0:c77591fc308d | 38 | |
charlesmn | 0:c77591fc308d | 39 | static int int_centre_result = 0; |
charlesmn | 0:c77591fc308d | 40 | static int int_left_result = 0; |
charlesmn | 0:c77591fc308d | 41 | static int int_right_result = 0; |
charlesmn | 0:c77591fc308d | 42 | |
charlesmn | 0:c77591fc308d | 43 | |
johnAlexander | 5:92a861d48253 | 44 | class WaitForMeasurement |
johnAlexander | 5:92a861d48253 | 45 | { |
johnAlexander | 5:92a861d48253 | 46 | |
charlesmn | 0:c77591fc308d | 47 | public: |
charlesmn | 0:c77591fc308d | 48 | |
charlesmn | 0:c77591fc308d | 49 | |
charlesmn | 0:c77591fc308d | 50 | // this class services the interrupts from the ToF sensors. |
charlesmn | 0:c77591fc308d | 51 | // There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things. |
charlesmn | 0:c77591fc308d | 52 | // We keep things simple by only raising a flag so all the real work is done outside the interrupt. |
charlesmn | 0:c77591fc308d | 53 | // This is designed around MBED V2 which doesn't have the RTOS features that would make this work nicely e.g. semaphores/queues. |
johnAlexander | 5:92a861d48253 | 54 | WaitForMeasurement(): _interrupt(A1) |
johnAlexander | 5:92a861d48253 | 55 | { |
johnAlexander | 5:92a861d48253 | 56 | } |
charlesmn | 0:c77591fc308d | 57 | |
charlesmn | 0:c77591fc308d | 58 | |
charlesmn | 0:c77591fc308d | 59 | // constructor - Sensor is not used and can be removed |
charlesmn | 0:c77591fc308d | 60 | WaitForMeasurement(PinName pin,VL53LX_DEV Dev) : _interrupt(pin) // create the InterruptIn on the pin specified to Counter |
charlesmn | 0:c77591fc308d | 61 | { |
johnAlexander | 5:92a861d48253 | 62 | Devlocal = Dev; |
charlesmn | 0:c77591fc308d | 63 | _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach increment function of this counter instance |
johnAlexander | 5:92a861d48253 | 64 | |
charlesmn | 0:c77591fc308d | 65 | } |
johnAlexander | 5:92a861d48253 | 66 | |
charlesmn | 0:c77591fc308d | 67 | void process_right_interrupt() |
charlesmn | 0:c77591fc308d | 68 | { |
johnAlexander | 5:92a861d48253 | 69 | printf("processing right interrupt\n"); |
charlesmn | 0:c77591fc308d | 70 | } |
charlesmn | 0:c77591fc308d | 71 | |
johnAlexander | 5:92a861d48253 | 72 | // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt. |
charlesmn | 0:c77591fc308d | 73 | void got_interrupt() |
charlesmn | 0:c77591fc308d | 74 | { |
charlesmn | 0:c77591fc308d | 75 | _count++; |
charlesmn | 0:c77591fc308d | 76 | |
charlesmn | 0:c77591fc308d | 77 | if (Devlocal->I2cDevAddr == NEW_SENSOR_CENTRE_ADDRESS) |
charlesmn | 0:c77591fc308d | 78 | int_centre_result = 1; //flag to main that interrupt happened |
charlesmn | 0:c77591fc308d | 79 | if (Devlocal->I2cDevAddr == NEW_SENSOR_LEFT_ADDRESS) |
charlesmn | 0:c77591fc308d | 80 | int_left_result = 1; //flag to main that interrupt happened |
charlesmn | 0:c77591fc308d | 81 | if (Devlocal->I2cDevAddr == NEW_SENSOR_RIGHT_ADDRESS) |
charlesmn | 0:c77591fc308d | 82 | int_right_result = 1; //flag to main that interrupt happened |
charlesmn | 0:c77591fc308d | 83 | } |
charlesmn | 0:c77591fc308d | 84 | |
johnAlexander | 5:92a861d48253 | 85 | |
charlesmn | 0:c77591fc308d | 86 | //destructor |
charlesmn | 0:c77591fc308d | 87 | ~WaitForMeasurement() |
charlesmn | 0:c77591fc308d | 88 | { |
charlesmn | 0:c77591fc308d | 89 | printf("destruction \n"); |
charlesmn | 0:c77591fc308d | 90 | } |
charlesmn | 0:c77591fc308d | 91 | |
charlesmn | 0:c77591fc308d | 92 | private: |
johnAlexander | 4:360dc34c3769 | 93 | |
charlesmn | 0:c77591fc308d | 94 | InterruptIn _interrupt; |
charlesmn | 0:c77591fc308d | 95 | volatile int _count; |
charlesmn | 0:c77591fc308d | 96 | VL53LX_DEV Devlocal; |
johnAlexander | 5:92a861d48253 | 97 | int status; |
charlesmn | 0:c77591fc308d | 98 | }; |
charlesmn | 0:c77591fc308d | 99 | |
charlesmn | 0:c77591fc308d | 100 | |
charlesmn | 0:c77591fc308d | 101 | |
johnAlexander | 7:5b8a91c3bafc | 102 | VL53LX_Dev_t devCentre; |
johnAlexander | 7:5b8a91c3bafc | 103 | VL53LX_Dev_t devLeft; |
johnAlexander | 7:5b8a91c3bafc | 104 | VL53LX_Dev_t devRight; |
johnAlexander | 7:5b8a91c3bafc | 105 | VL53LX_DEV Dev = &devCentre; |
charlesmn | 0:c77591fc308d | 106 | |
charlesmn | 0:c77591fc308d | 107 | |
charlesmn | 0:c77591fc308d | 108 | |
charlesmn | 0:c77591fc308d | 109 | |
johnAlexander | 5:92a861d48253 | 110 | |
charlesmn | 0:c77591fc308d | 111 | /*=================================== Main ================================== |
charlesmn | 0:c77591fc308d | 112 | =============================================================================*/ |
charlesmn | 0:c77591fc308d | 113 | int main() |
johnAlexander | 5:92a861d48253 | 114 | { |
charlesmn | 0:c77591fc308d | 115 | int status; |
johnAlexander | 7:5b8a91c3bafc | 116 | VL53L3 *Sensor; |
charlesmn | 0:c77591fc308d | 117 | uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right |
johnAlexander | 5:92a861d48253 | 118 | |
johnAlexander | 7:5b8a91c3bafc | 119 | WaitForMeasurement *int2; |
johnAlexander | 7:5b8a91c3bafc | 120 | WaitForMeasurement *int1; |
johnAlexander | 7:5b8a91c3bafc | 121 | WaitForMeasurement *int3; |
johnAlexander | 7:5b8a91c3bafc | 122 | |
johnAlexander | 5:92a861d48253 | 123 | |
charlesmn | 2:86876e590f28 | 124 | vl53L3_DevI2C *dev_I2C = new vl53L3_DevI2C(I2C_SDA, I2C_SCL); |
charlesmn | 0:c77591fc308d | 125 | /* creates the 53L1A1 expansion board singleton obj */ |
johnAlexander | 3:42c96c9c627b | 126 | board = XNucleo53L3A2::instance(dev_I2C, A2, D8, D2); |
charlesmn | 0:c77591fc308d | 127 | printf("board created!\r\n"); |
charlesmn | 0:c77591fc308d | 128 | |
johnAlexander | 7:5b8a91c3bafc | 129 | pc.baud(115200); // baud rate is important as printf statements take a lot of time |
johnAlexander | 7:5b8a91c3bafc | 130 | |
charlesmn | 0:c77591fc308d | 131 | /* init the 53L1A1 expansion board with default values */ |
charlesmn | 0:c77591fc308d | 132 | status = board->init_board(); |
charlesmn | 0:c77591fc308d | 133 | if (status) { |
charlesmn | 0:c77591fc308d | 134 | printf("Failed to init board!\r\n"); |
charlesmn | 0:c77591fc308d | 135 | return 0; |
charlesmn | 0:c77591fc308d | 136 | } |
johnAlexander | 5:92a861d48253 | 137 | |
charlesmn | 0:c77591fc308d | 138 | printf("board initiated! - %d\r\n", status); |
johnAlexander | 5:92a861d48253 | 139 | |
johnAlexander | 7:5b8a91c3bafc | 140 | for (ToFSensor = 0; ToFSensor < 3; ToFSensor++) { |
charlesmn | 0:c77591fc308d | 141 | wait_ms(15); |
johnAlexander | 5:92a861d48253 | 142 | switch(ToFSensor) { |
charlesmn | 0:c77591fc308d | 143 | case 0: |
johnAlexander | 7:5b8a91c3bafc | 144 | if (board->sensor_centre == NULL ) continue; |
charlesmn | 0:c77591fc308d | 145 | Dev=&devCentre; |
charlesmn | 0:c77591fc308d | 146 | Dev->I2cDevAddr = NEW_SENSOR_CENTRE_ADDRESS; |
johnAlexander | 7:5b8a91c3bafc | 147 | Sensor = board->sensor_centre; |
charlesmn | 0:c77591fc308d | 148 | printf("configuring centre channel \n"); |
charlesmn | 0:c77591fc308d | 149 | break; |
charlesmn | 0:c77591fc308d | 150 | case 1: |
johnAlexander | 7:5b8a91c3bafc | 151 | if (board->sensor_left == NULL ) continue; |
johnAlexander | 5:92a861d48253 | 152 | Dev=&devLeft; |
charlesmn | 0:c77591fc308d | 153 | Dev->I2cDevAddr = NEW_SENSOR_LEFT_ADDRESS; |
johnAlexander | 7:5b8a91c3bafc | 154 | Sensor = board->sensor_left; |
charlesmn | 0:c77591fc308d | 155 | printf("configuring left channel \n"); |
charlesmn | 0:c77591fc308d | 156 | break; |
charlesmn | 0:c77591fc308d | 157 | case 2: |
johnAlexander | 7:5b8a91c3bafc | 158 | if (board->sensor_right == NULL ) continue; |
johnAlexander | 5:92a861d48253 | 159 | Dev=&devRight; |
charlesmn | 0:c77591fc308d | 160 | Dev->I2cDevAddr = NEW_SENSOR_RIGHT_ADDRESS; |
johnAlexander | 7:5b8a91c3bafc | 161 | Sensor = board->sensor_right; |
charlesmn | 0:c77591fc308d | 162 | printf("configuring right channel \n"); |
johnAlexander | 5:92a861d48253 | 163 | break; |
charlesmn | 0:c77591fc308d | 164 | default: |
johnAlexander | 5:92a861d48253 | 165 | printf(" error in switch, invalid ToF sensor \n"); |
charlesmn | 0:c77591fc308d | 166 | } |
johnAlexander | 5:92a861d48253 | 167 | |
charlesmn | 0:c77591fc308d | 168 | // configure the sensors |
charlesmn | 0:c77591fc308d | 169 | Dev->comms_speed_khz = 400; |
charlesmn | 0:c77591fc308d | 170 | Dev->comms_type = 1; |
charlesmn | 0:c77591fc308d | 171 | |
johnAlexander | 5:92a861d48253 | 172 | /* Device Initialization and setting */ |
charlesmn | 0:c77591fc308d | 173 | status = Sensor->VL53LX_DataInit(); |
charlesmn | 0:c77591fc308d | 174 | status = Sensor->VL53LX_SetDistanceMode(VL53LX_DISTANCEMODE_LONG); |
johnAlexander | 5:92a861d48253 | 175 | status = Sensor->VL53LX_SetMeasurementTimingBudgetMicroSeconds( MEASUREMENTTIMING * 1000); |
charlesmn | 0:c77591fc308d | 176 | status = Sensor->VL53LX_SmudgeCorrectionEnable(VL53LX_SMUDGE_CORRECTION_SINGLE); |
charlesmn | 0:c77591fc308d | 177 | status = Sensor->VL53LX_SetXTalkCompensationEnable(1); |
johnAlexander | 5:92a861d48253 | 178 | } |
johnAlexander | 5:92a861d48253 | 179 | |
johnAlexander | 5:92a861d48253 | 180 | |
johnAlexander | 7:5b8a91c3bafc | 181 | if (board->sensor_centre != NULL ) { |
johnAlexander | 7:5b8a91c3bafc | 182 | printf("starting interrupt centre\n"); |
johnAlexander | 7:5b8a91c3bafc | 183 | Sensor = board->sensor_centre; |
johnAlexander | 7:5b8a91c3bafc | 184 | devCentre.I2cDevAddr = NEW_SENSOR_CENTRE_ADDRESS; |
johnAlexander | 7:5b8a91c3bafc | 185 | int1 = new WaitForMeasurement(A2,&devCentre); |
johnAlexander | 7:5b8a91c3bafc | 186 | status = Sensor->VL53LX_StartMeasurement(); |
johnAlexander | 7:5b8a91c3bafc | 187 | printf("VL53L1_StartMeasurement %d \n",status); |
johnAlexander | 7:5b8a91c3bafc | 188 | status = Sensor->VL53LX_ClearInterruptAndStartMeasurement(); |
johnAlexander | 7:5b8a91c3bafc | 189 | } |
johnAlexander | 7:5b8a91c3bafc | 190 | |
johnAlexander | 7:5b8a91c3bafc | 191 | |
johnAlexander | 7:5b8a91c3bafc | 192 | if (board->sensor_left != NULL ) { |
johnAlexander | 5:92a861d48253 | 193 | printf("starting interrupt left\n"); |
johnAlexander | 7:5b8a91c3bafc | 194 | Sensor = board->sensor_left; |
johnAlexander | 5:92a861d48253 | 195 | devLeft.I2cDevAddr = NEW_SENSOR_LEFT_ADDRESS; |
johnAlexander | 5:92a861d48253 | 196 | int2 = new WaitForMeasurement(D8,&devLeft); |
johnAlexander | 5:92a861d48253 | 197 | status = Sensor->VL53LX_StartMeasurement(); |
johnAlexander | 5:92a861d48253 | 198 | printf("VL53L1_StartMeasurement %d \n",status); |
johnAlexander | 7:5b8a91c3bafc | 199 | status = Sensor->VL53LX_ClearInterruptAndStartMeasurement(); |
johnAlexander | 5:92a861d48253 | 200 | } |
johnAlexander | 5:92a861d48253 | 201 | |
johnAlexander | 7:5b8a91c3bafc | 202 | if (board->sensor_right != NULL ) { |
johnAlexander | 5:92a861d48253 | 203 | printf("starting interrupt right\n"); |
johnAlexander | 7:5b8a91c3bafc | 204 | Sensor = board->sensor_right; |
johnAlexander | 5:92a861d48253 | 205 | devRight.I2cDevAddr = NEW_SENSOR_RIGHT_ADDRESS; |
johnAlexander | 5:92a861d48253 | 206 | int3 = new WaitForMeasurement(D2,&devRight); |
johnAlexander | 5:92a861d48253 | 207 | status = Sensor->VL53LX_StartMeasurement(); |
johnAlexander | 5:92a861d48253 | 208 | printf("VL53L1_StartMeasurement %d \n",status); |
johnAlexander | 7:5b8a91c3bafc | 209 | status = Sensor->VL53LX_ClearInterruptAndStartMeasurement(); |
johnAlexander | 5:92a861d48253 | 210 | } |
johnAlexander | 5:92a861d48253 | 211 | |
johnAlexander | 5:92a861d48253 | 212 | |
johnAlexander | 7:5b8a91c3bafc | 213 | VL53LX_MultiRangingData_t MultiRangingData; |
johnAlexander | 7:5b8a91c3bafc | 214 | VL53LX_MultiRangingData_t *pMultiRangingData; |
johnAlexander | 7:5b8a91c3bafc | 215 | |
johnAlexander | 5:92a861d48253 | 216 | // loop waiting for interrupts to happen. This is signaled by int_centre_result,int_left_result or int_right_result |
johnAlexander | 5:92a861d48253 | 217 | // being non zero. The are set back to zero when processing is completed |
johnAlexander | 5:92a861d48253 | 218 | while (1) { |
johnAlexander | 7:5b8a91c3bafc | 219 | pMultiRangingData = &MultiRangingData; |
johnAlexander | 5:92a861d48253 | 220 | |
johnAlexander | 7:5b8a91c3bafc | 221 | wait_ms(5); // 10 |
johnAlexander | 5:92a861d48253 | 222 | |
johnAlexander | 5:92a861d48253 | 223 | if (int_centre_result != 0) { |
johnAlexander | 7:5b8a91c3bafc | 224 | status = board->sensor_centre->VL53LX_GetMultiRangingData(pMultiRangingData); |
johnAlexander | 7:5b8a91c3bafc | 225 | int no_of_object_found = pMultiRangingData->NumberOfObjectsFound; |
johnAlexander | 7:5b8a91c3bafc | 226 | if ((no_of_object_found < 10) && (no_of_object_found != 0)) { |
johnAlexander | 7:5b8a91c3bafc | 227 | for(int j = 0; j < no_of_object_found; j++) { |
johnAlexander | 5:92a861d48253 | 228 | if (pMultiRangingData->RangeData[j].RangeStatus == 0) { |
johnAlexander | 5:92a861d48253 | 229 | printf("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n", |
johnAlexander | 5:92a861d48253 | 230 | pMultiRangingData->RangeData[j].RangeStatus, |
johnAlexander | 5:92a861d48253 | 231 | pMultiRangingData->RangeData[j].RangeMilliMeter, |
johnAlexander | 7:5b8a91c3bafc | 232 | (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535.0), |
johnAlexander | 7:5b8a91c3bafc | 233 | (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535.0)); |
johnAlexander | 5:92a861d48253 | 234 | } |
johnAlexander | 5:92a861d48253 | 235 | } |
johnAlexander | 5:92a861d48253 | 236 | } |
johnAlexander | 7:5b8a91c3bafc | 237 | // clear the interrupt and wait for another result |
johnAlexander | 5:92a861d48253 | 238 | int_centre_result = 0; |
johnAlexander | 5:92a861d48253 | 239 | wait_ms( MEASUREMENTTIMING ); |
charlesmn | 0:c77591fc308d | 240 | status = board->sensor_centre->VL53LX_ClearInterruptAndStartMeasurement(); |
charlesmn | 0:c77591fc308d | 241 | } |
johnAlexander | 5:92a861d48253 | 242 | |
charlesmn | 0:c77591fc308d | 243 | |
johnAlexander | 5:92a861d48253 | 244 | if (int_left_result != 0) { |
johnAlexander | 7:5b8a91c3bafc | 245 | status = board->sensor_left->VL53LX_GetMultiRangingData(pMultiRangingData); |
johnAlexander | 7:5b8a91c3bafc | 246 | if (status == 0) { |
johnAlexander | 7:5b8a91c3bafc | 247 | int no_of_object_found = pMultiRangingData->NumberOfObjectsFound; |
johnAlexander | 7:5b8a91c3bafc | 248 | if ((no_of_object_found < 10) && (no_of_object_found != 0)) { |
johnAlexander | 7:5b8a91c3bafc | 249 | for(int j = 0; j < no_of_object_found; j++) { |
johnAlexander | 5:92a861d48253 | 250 | if (pMultiRangingData->RangeData[j].RangeStatus == 0) { |
johnAlexander | 5:92a861d48253 | 251 | printf("left \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n", |
johnAlexander | 5:92a861d48253 | 252 | pMultiRangingData->RangeData[j].RangeStatus, |
johnAlexander | 5:92a861d48253 | 253 | pMultiRangingData->RangeData[j].RangeMilliMeter, |
johnAlexander | 5:92a861d48253 | 254 | (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535.0), |
johnAlexander | 5:92a861d48253 | 255 | (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535.0) ); |
johnAlexander | 5:92a861d48253 | 256 | } |
johnAlexander | 5:92a861d48253 | 257 | } |
johnAlexander | 5:92a861d48253 | 258 | } |
johnAlexander | 5:92a861d48253 | 259 | } |
johnAlexander | 7:5b8a91c3bafc | 260 | // clear the interrupt and wait for another result |
johnAlexander | 7:5b8a91c3bafc | 261 | int_left_result = 0; |
johnAlexander | 5:92a861d48253 | 262 | wait_ms( MEASUREMENTTIMING ); |
charlesmn | 0:c77591fc308d | 263 | status = board->sensor_left->VL53LX_ClearInterruptAndStartMeasurement(); |
charlesmn | 0:c77591fc308d | 264 | } |
charlesmn | 0:c77591fc308d | 265 | |
charlesmn | 0:c77591fc308d | 266 | |
johnAlexander | 5:92a861d48253 | 267 | if (int_right_result != 0) { |
johnAlexander | 7:5b8a91c3bafc | 268 | status = board->sensor_right->VL53LX_GetMultiRangingData(pMultiRangingData); |
johnAlexander | 7:5b8a91c3bafc | 269 | if (status == 0) { |
johnAlexander | 5:92a861d48253 | 270 | // if valid result print it |
johnAlexander | 7:5b8a91c3bafc | 271 | int no_of_object_found = pMultiRangingData->NumberOfObjectsFound; |
johnAlexander | 7:5b8a91c3bafc | 272 | if ((no_of_object_found < 10) && (no_of_object_found != 0)) { |
johnAlexander | 7:5b8a91c3bafc | 273 | for(int j = 0; j < no_of_object_found; j++) { |
johnAlexander | 5:92a861d48253 | 274 | if (pMultiRangingData->RangeData[j].RangeStatus == 0) { |
johnAlexander | 5:92a861d48253 | 275 | printf("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n", |
johnAlexander | 5:92a861d48253 | 276 | pMultiRangingData->RangeData[j].RangeStatus, |
johnAlexander | 5:92a861d48253 | 277 | pMultiRangingData->RangeData[j].RangeMilliMeter, |
johnAlexander | 7:5b8a91c3bafc | 278 | (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535.0), |
johnAlexander | 7:5b8a91c3bafc | 279 | (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535.0) ); |
charlesmn | 0:c77591fc308d | 280 | } |
charlesmn | 0:c77591fc308d | 281 | } |
charlesmn | 0:c77591fc308d | 282 | } |
charlesmn | 0:c77591fc308d | 283 | } |
johnAlexander | 7:5b8a91c3bafc | 284 | // clear the interrupt and wait for another result |
johnAlexander | 7:5b8a91c3bafc | 285 | int_right_result = 0; |
johnAlexander | 5:92a861d48253 | 286 | wait_ms( MEASUREMENTTIMING ); |
johnAlexander | 5:92a861d48253 | 287 | status = board->sensor_right->VL53LX_ClearInterruptAndStartMeasurement(); |
johnAlexander | 5:92a861d48253 | 288 | } |
johnAlexander | 5:92a861d48253 | 289 | } |
johnAlexander | 5:92a861d48253 | 290 | printf("terminated"); |
johnAlexander | 5:92a861d48253 | 291 | } |
charlesmn | 0:c77591fc308d | 292 | |
johnAlexander | 3:42c96c9c627b | 293 | #if (MBED_VERSION > 60300) |
johnAlexander | 3:42c96c9c627b | 294 | extern "C" void wait_ms(int ms) |
johnAlexander | 5:92a861d48253 | 295 | { |
johnAlexander | 3:42c96c9c627b | 296 | thread_sleep_for(ms); |
johnAlexander | 5:92a861d48253 | 297 | } |
johnAlexander | 3:42c96c9c627b | 298 | #endif |
charlesmn | 0:c77591fc308d | 299 |