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

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?

UserRevisionLine numberNew 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