ST Expansion SW Team / Mbed OS VL53L1CB_shield_3sensor_polling_multi_ja

VL53L1CB ranging example, using expansion shield and satellites, with polling.

Dependencies:   X_NUCLEO_53L1A2

main.cpp@11:96465baec780, 2021-05-18 (annotated)

Committer:
johnAlexander
Date:
Tue May 18 08:36:02 2021 +0000
Revision:
11:96465baec780
Parent:
10:cf7d563200fc 
forking, to share internally

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:50b05f035d13 1 /*
johnAlexander 8:8f6490937346 2 * This VL53L1CB Expansion board test application performs range measurements
charlesmn 0:50b05f035d13 3 * using the onboard embedded centre sensor, in multizone, polling mode.
charlesmn 0:50b05f035d13 4 * Measured ranges are ouput on the Serial Port, running at 115200 baud.
charlesmn 0:50b05f035d13 5 *
charlesmn 0:50b05f035d13 6 *
johnAlexander 7:b5a4ad8ba844 7 * This is designed to work with MBed v2, & MBedOS v5 / v6.
charlesmn 0:50b05f035d13 8 *
charlesmn 0:50b05f035d13 9 *
charlesmn 0:50b05f035d13 10 * The Reset button can be used to restart the program.
johnAlexander 9:0a3e1affe004 11 *
johnAlexander 9:0a3e1affe004 12 * *** Note :
johnAlexander 9:0a3e1affe004 13 * Default Mbed build system settings disable print floating-point support.
johnAlexander 9:0a3e1affe004 14 * Offline builds can enable this, again.
johnAlexander 9:0a3e1affe004 15 * https://github.com/ARMmbed/mbed-os/blob/master/platform/source/minimal-printf/README.md
johnAlexander 9:0a3e1affe004 16 * .\mbed-os\platform\mbed_lib.json
johnAlexander 9:0a3e1affe004 17 *
charlesmn 0:50b05f035d13 18 */
charlesmn 0:50b05f035d13 19
charlesmn 0:50b05f035d13 20 #include <stdio.h>
johnAlexander 6:bc6ac1e294f4 21 #include <time.h>
charlesmn 0:50b05f035d13 22
charlesmn 0:50b05f035d13 23 #include "mbed.h"
johnAlexander 4:177d711cc20f 24 #include "XNucleo53L1A2.h"
charlesmn 0:50b05f035d13 25 #include "ToF_I2C.h"
charlesmn 0:50b05f035d13 26
charlesmn 0:50b05f035d13 27
charlesmn 0:50b05f035d13 28 // define the i2c comms pins
charlesmn 0:50b05f035d13 29 #define I2C_SDA D14
charlesmn 0:50b05f035d13 30 #define I2C_SCL D15
charlesmn 0:50b05f035d13 31
johnAlexander 11:96465baec780 32 #define NUM_SENSORS 3
johnAlexander 11:96465baec780 33
johnAlexander 11:96465baec780 34 // define interrupt pins
johnAlexander 11:96465baec780 35 PinName CentreIntPin = A2;
johnAlexander 11:96465baec780 36 // the satellite pins depend on solder blobs on the back of the shield.
johnAlexander 11:96465baec780 37 // they may not exist or may be one of two sets.
johnAlexander 11:96465baec780 38 // the centre pin always exists
johnAlexander 11:96465baec780 39 //PinName LeftIntPin = D8;
johnAlexander 11:96465baec780 40 PinName RightIntPin = D2;
johnAlexander 11:96465baec780 41 // alternate set
johnAlexander 11:96465baec780 42 PinName LeftIntPin = D9;
johnAlexander 11:96465baec780 43 //PinName RightIntPin = D4;
charlesmn 0:50b05f035d13 44
charlesmn 0:50b05f035d13 45
johnAlexander 4:177d711cc20f 46 static XNucleo53L1A2 *board=NULL;
charlesmn 3:d1a3d15a06ff 47 #if (MBED_VERSION > 60300)
johnAlexander 7:b5a4ad8ba844 48 UnbufferedSerial pc(USBTX, USBRX);
johnAlexander 6:bc6ac1e294f4 49 extern "C" void wait_ms(int ms);
charlesmn 2:ef5e40bad526 50 #else
johnAlexander 6:bc6ac1e294f4 51 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 2:ef5e40bad526 52 #endif
charlesmn 0:50b05f035d13 53
johnAlexander 11:96465baec780 54 #if TARGET_STM // we are cross compiling for an STM32-Nucleo
johnAlexander 11:96465baec780 55 InterruptIn stop_button(BUTTON1);
johnAlexander 11:96465baec780 56 #endif
johnAlexander 11:96465baec780 57 #if TARGET_Freescale // we are cross-compiling for NXP FRDM boards.
johnAlexander 11:96465baec780 58 InterruptIn stop_button(SW2);
johnAlexander 11:96465baec780 59 #endif
johnAlexander 11:96465baec780 60
charlesmn 5:5916ff386a94 61 void process_interrupt( VL53L1 * sensor,VL53L1_DEV dev );
charlesmn 0:50b05f035d13 62 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
charlesmn 0:50b05f035d13 63
charlesmn 0:50b05f035d13 64 VL53L1_Dev_t devCentre;
charlesmn 0:50b05f035d13 65 VL53L1_Dev_t devLeft;
charlesmn 0:50b05f035d13 66 VL53L1_Dev_t devRight;
charlesmn 0:50b05f035d13 67 VL53L1_DEV Dev = &devCentre;
charlesmn 0:50b05f035d13 68
johnAlexander 11:96465baec780 69 /* Installed sensors count */
johnAlexander 11:96465baec780 70 int sensorCnt = 0;
johnAlexander 11:96465baec780 71
johnAlexander 11:96465baec780 72 /* installed sensors prefixes */
johnAlexander 11:96465baec780 73 char installedSensors[3];
johnAlexander 11:96465baec780 74
johnAlexander 11:96465baec780 75 /* interrupt requests */
johnAlexander 11:96465baec780 76 volatile bool centerSensor = false;
johnAlexander 11:96465baec780 77 volatile bool leftSensor = false;
johnAlexander 11:96465baec780 78 volatile bool rightSensor = false;
johnAlexander 11:96465baec780 79 volatile bool int_measuring_stop = false;
johnAlexander 11:96465baec780 80
johnAlexander 11:96465baec780 81 /* Current sensor number*/
johnAlexander 11:96465baec780 82 volatile int currentSensor = 0;
johnAlexander 11:96465baec780 83
johnAlexander 11:96465baec780 84 /* current displayed sensor change IRQ */
johnAlexander 11:96465baec780 85 volatile bool switchChanged = false;
johnAlexander 11:96465baec780 86
johnAlexander 11:96465baec780 87 /* ISR callback function of the centre sensor */
johnAlexander 11:96465baec780 88 void sensor_centre_irq(void)
johnAlexander 11:96465baec780 89 {
johnAlexander 11:96465baec780 90 centerSensor = true;
johnAlexander 11:96465baec780 91 board->sensor_centre->disable_interrupt_measure_detection_irq();
johnAlexander 11:96465baec780 92 }
johnAlexander 11:96465baec780 93
johnAlexander 11:96465baec780 94 /* ISR callback function of the left sensor */
johnAlexander 11:96465baec780 95 void sensor_left_irq(void)
johnAlexander 11:96465baec780 96 {
johnAlexander 11:96465baec780 97 leftSensor = true;
johnAlexander 11:96465baec780 98 board->sensor_left->disable_interrupt_measure_detection_irq();
johnAlexander 11:96465baec780 99 }
johnAlexander 11:96465baec780 100
johnAlexander 11:96465baec780 101 /* ISR callback function of the right sensor */
johnAlexander 11:96465baec780 102 void sensor_right_irq(void)
johnAlexander 11:96465baec780 103 {
johnAlexander 11:96465baec780 104 rightSensor = true;
johnAlexander 11:96465baec780 105 board->sensor_right->disable_interrupt_measure_detection_irq();
johnAlexander 11:96465baec780 106 }
johnAlexander 11:96465baec780 107
johnAlexander 11:96465baec780 108 /* ISR callback function of the user blue button to switch measuring sensor. */
johnAlexander 11:96465baec780 109 void switch_measuring_sensor_irq(void)
johnAlexander 11:96465baec780 110 {
johnAlexander 11:96465baec780 111 stop_button.disable_irq();
johnAlexander 11:96465baec780 112 switchChanged = true;
johnAlexander 11:96465baec780 113 }
johnAlexander 11:96465baec780 114
johnAlexander 11:96465baec780 115 /*
johnAlexander 11:96465baec780 116 * This function calls the interrupt handler for each sensor
johnAlexander 11:96465baec780 117 * and outputs the range
johnAlexander 11:96465baec780 118 */
johnAlexander 11:96465baec780 119 inline void measure_sensors()
johnAlexander 11:96465baec780 120 {
johnAlexander 11:96465baec780 121 int status = 0;
johnAlexander 11:96465baec780 122 bool current = false;
johnAlexander 11:96465baec780 123
johnAlexander 11:96465baec780 124 uint16_t distance = 0;
johnAlexander 11:96465baec780 125
johnAlexander 11:96465baec780 126 VL53L1_MultiRangingData_t MultiRangingData;
johnAlexander 11:96465baec780 127 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
johnAlexander 11:96465baec780 128
johnAlexander 11:96465baec780 129 /* Handle the interrupt and output the range from the centre sensor */
johnAlexander 11:96465baec780 130 if (centerSensor) {
johnAlexander 11:96465baec780 131 centerSensor = false;
johnAlexander 11:96465baec780 132
johnAlexander 11:96465baec780 133 status = board->sensor_centre->VL53L1_GetDistance(&distance);
johnAlexander 11:96465baec780 134
johnAlexander 11:96465baec780 135 current = (currentSensor == 0);
johnAlexander 11:96465baec780 136 if (current) {
johnAlexander 11:96465baec780 137 printf("Centre: %d\r\n", distance);
johnAlexander 11:96465baec780 138 }
johnAlexander 11:96465baec780 139
johnAlexander 11:96465baec780 140 status = board->sensor_centre->VL53L1_ClearInterrupt();
johnAlexander 11:96465baec780 141 board->sensor_centre->enable_interrupt_measure_detection_irq();
johnAlexander 11:96465baec780 142
johnAlexander 11:96465baec780 143 }
johnAlexander 11:96465baec780 144
johnAlexander 11:96465baec780 145 /* Handle the interrupt and output the range from the left sensor */
johnAlexander 11:96465baec780 146 if (leftSensor) {
johnAlexander 11:96465baec780 147 leftSensor = false;
johnAlexander 11:96465baec780 148
johnAlexander 11:96465baec780 149 status = board->sensor_left->VL53L1_GetDistance(&distance);
johnAlexander 11:96465baec780 150
johnAlexander 11:96465baec780 151 current = (installedSensors[currentSensor] == 'L');
johnAlexander 11:96465baec780 152 if (current) {
johnAlexander 11:96465baec780 153 printf("Left: %d\r\n", distance);
johnAlexander 11:96465baec780 154 }
johnAlexander 11:96465baec780 155 status = board->sensor_left->VL53L1_ClearInterrupt();
johnAlexander 11:96465baec780 156 board->sensor_left->enable_interrupt_measure_detection_irq();
johnAlexander 11:96465baec780 157
johnAlexander 11:96465baec780 158 }
johnAlexander 11:96465baec780 159
johnAlexander 11:96465baec780 160 /* Handle the interrupt and output the range from the right sensor */
johnAlexander 11:96465baec780 161 if (rightSensor) {
johnAlexander 11:96465baec780 162 rightSensor = false;
johnAlexander 11:96465baec780 163
johnAlexander 11:96465baec780 164 status = board->sensor_right->VL53L1_GetDistance(&distance);
johnAlexander 11:96465baec780 165
johnAlexander 11:96465baec780 166 current = (installedSensors[currentSensor] == 'R');
johnAlexander 11:96465baec780 167 if (current) {
johnAlexander 11:96465baec780 168 printf("Right: %d\r\n", distance);
johnAlexander 11:96465baec780 169 }
johnAlexander 11:96465baec780 170 status = board->sensor_right->VL53L1_ClearInterrupt();
johnAlexander 11:96465baec780 171 board->sensor_right->enable_interrupt_measure_detection_irq();
johnAlexander 11:96465baec780 172
johnAlexander 11:96465baec780 173 }
johnAlexander 11:96465baec780 174 }
johnAlexander 11:96465baec780 175
johnAlexander 11:96465baec780 176 int configure_sensor(VL53L1 *Sensor)
johnAlexander 11:96465baec780 177 {
johnAlexander 11:96465baec780 178 int status = 0;
johnAlexander 11:96465baec780 179 /*
johnAlexander 11:96465baec780 180 //configure the regions of interest for each sensor
johnAlexander 11:96465baec780 181 VL53L1_RoiConfig_t roiConfig;
johnAlexander 11:96465baec780 182
johnAlexander 11:96465baec780 183 roiConfig.NumberOfRoi = 3;
johnAlexander 11:96465baec780 184 roiConfig.UserRois[0].TopLeftX = 0;
johnAlexander 11:96465baec780 185 roiConfig.UserRois[0].TopLeftY = 9;
johnAlexander 11:96465baec780 186 roiConfig.UserRois[0].BotRightX = 4;
johnAlexander 11:96465baec780 187 roiConfig.UserRois[0].BotRightY = 5;
johnAlexander 11:96465baec780 188
johnAlexander 11:96465baec780 189 roiConfig.UserRois[1].TopLeftX = 5;
johnAlexander 11:96465baec780 190 roiConfig.UserRois[1].TopLeftY = 9;
johnAlexander 11:96465baec780 191 roiConfig.UserRois[1].BotRightX = 9;
johnAlexander 11:96465baec780 192 roiConfig.UserRois[1].BotRightY = 4;
johnAlexander 11:96465baec780 193
johnAlexander 11:96465baec780 194 roiConfig.UserRois[2].TopLeftX = 11;
johnAlexander 11:96465baec780 195 roiConfig.UserRois[2].TopLeftY = 9;
johnAlexander 11:96465baec780 196 roiConfig.UserRois[2].BotRightX = 15;
johnAlexander 11:96465baec780 197 roiConfig.UserRois[2].BotRightY = 5;
johnAlexander 11:96465baec780 198
johnAlexander 11:96465baec780 199 status = Sensor->vl53L1_SetROI( &roiConfig);
johnAlexander 11:96465baec780 200 */
johnAlexander 11:96465baec780 201 // Device Initialization and setting
johnAlexander 11:96465baec780 202 status = Sensor->vl53L1_DataInit();
johnAlexander 11:96465baec780 203 status = Sensor->vl53L1_StaticInit();
johnAlexander 11:96465baec780 204
johnAlexander 11:96465baec780 205 status = Sensor->vl53L1_SetPresetMode(VL53L1_PRESETMODE_MULTIZONES_SCANNING);
johnAlexander 11:96465baec780 206 status = Sensor->vl53L1_SetDistanceMode(VL53L1_DISTANCEMODE_LONG);
johnAlexander 11:96465baec780 207
johnAlexander 11:96465baec780 208 status = Sensor->vl53L1_StartMeasurement();
johnAlexander 11:96465baec780 209
johnAlexander 11:96465baec780 210 return status;
johnAlexander 11:96465baec780 211 }
johnAlexander 11:96465baec780 212
johnAlexander 11:96465baec780 213 /*
johnAlexander 11:96465baec780 214 * Add to an array a character that represents the sensor and start ranging
johnAlexander 11:96465baec780 215 */
johnAlexander 11:96465baec780 216 int init_sensors_array()
johnAlexander 11:96465baec780 217 {
johnAlexander 11:96465baec780 218 int status = 0;
johnAlexander 11:96465baec780 219 VL53L1 *Sensor;
johnAlexander 11:96465baec780 220 uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
johnAlexander 11:96465baec780 221
johnAlexander 11:96465baec780 222 sensorCnt = 0;
johnAlexander 11:96465baec780 223
johnAlexander 11:96465baec780 224 Dev->comms_speed_khz = 400;
johnAlexander 11:96465baec780 225 Dev->comms_type = 1;
johnAlexander 11:96465baec780 226
johnAlexander 11:96465baec780 227 if (board->sensor_centre != NULL) {
johnAlexander 11:96465baec780 228 Dev = &devCentre;
johnAlexander 11:96465baec780 229 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
johnAlexander 11:96465baec780 230 devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
johnAlexander 11:96465baec780 231
johnAlexander 11:96465baec780 232 status = configure_sensor(board->sensor_centre);
johnAlexander 11:96465baec780 233 printf("configuring centre channel \n");
johnAlexander 11:96465baec780 234 }
johnAlexander 11:96465baec780 235
johnAlexander 11:96465baec780 236 if (board->sensor_left != NULL) {
johnAlexander 11:96465baec780 237 Dev = &devLeft;
johnAlexander 11:96465baec780 238 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
johnAlexander 11:96465baec780 239 devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
johnAlexander 11:96465baec780 240
johnAlexander 11:96465baec780 241 status = configure_sensor(board->sensor_left);
johnAlexander 11:96465baec780 242 printf("configuring left channel \n");
johnAlexander 11:96465baec780 243 }
johnAlexander 11:96465baec780 244
johnAlexander 11:96465baec780 245 if (board->sensor_right != NULL) {
johnAlexander 11:96465baec780 246 Dev = &devRight;
johnAlexander 11:96465baec780 247 Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
johnAlexander 11:96465baec780 248 devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
johnAlexander 11:96465baec780 249
johnAlexander 11:96465baec780 250 status = configure_sensor(board->sensor_right);
johnAlexander 11:96465baec780 251 printf("configuring right channel \n");
johnAlexander 11:96465baec780 252 }
johnAlexander 11:96465baec780 253
johnAlexander 11:96465baec780 254 /* start the measure on the center sensor */
johnAlexander 11:96465baec780 255 if (NULL != board->sensor_centre) {
johnAlexander 11:96465baec780 256 installedSensors[sensorCnt] = 'C';
johnAlexander 11:96465baec780 257 status = board->sensor_centre->stop_measurement();
johnAlexander 11:96465baec780 258 if (status != 0) {
johnAlexander 11:96465baec780 259 return status;
johnAlexander 11:96465baec780 260 }
johnAlexander 11:96465baec780 261 status = board->sensor_centre->start_measurement(&sensor_centre_irq);
johnAlexander 11:96465baec780 262 if (status != 0) {
johnAlexander 11:96465baec780 263 return status;
johnAlexander 11:96465baec780 264 }
johnAlexander 11:96465baec780 265 ++sensorCnt;
johnAlexander 11:96465baec780 266 }
johnAlexander 11:96465baec780 267 /* start the measure on the left sensor */
johnAlexander 11:96465baec780 268 if (NULL != board->sensor_left) {
johnAlexander 11:96465baec780 269 installedSensors[sensorCnt] = 'L';
johnAlexander 11:96465baec780 270 status = board->sensor_left->stop_measurement();
johnAlexander 11:96465baec780 271 if (status != 0) {
johnAlexander 11:96465baec780 272 return status;
johnAlexander 11:96465baec780 273 }
johnAlexander 11:96465baec780 274 status = board->sensor_left->start_measurement(&sensor_left_irq);
johnAlexander 11:96465baec780 275 if (status != 0) {
johnAlexander 11:96465baec780 276 return status;
johnAlexander 11:96465baec780 277 }
johnAlexander 11:96465baec780 278 ++sensorCnt;
johnAlexander 11:96465baec780 279 }
johnAlexander 11:96465baec780 280 /* start the measure on the right sensor */
johnAlexander 11:96465baec780 281 if (NULL != board->sensor_right) {
johnAlexander 11:96465baec780 282 installedSensors[sensorCnt] = 'R';
johnAlexander 11:96465baec780 283 status = board->sensor_right->stop_measurement();
johnAlexander 11:96465baec780 284 if (status != 0) {
johnAlexander 11:96465baec780 285 return status;
johnAlexander 11:96465baec780 286 }
johnAlexander 11:96465baec780 287 status = board->sensor_right->start_measurement(&sensor_right_irq);
johnAlexander 11:96465baec780 288 if (status != 0) {
johnAlexander 11:96465baec780 289 return status;
johnAlexander 11:96465baec780 290 }
johnAlexander 11:96465baec780 291 ++sensorCnt;
johnAlexander 11:96465baec780 292 }
johnAlexander 11:96465baec780 293 currentSensor = 0;
johnAlexander 11:96465baec780 294 return status;
johnAlexander 11:96465baec780 295 }
johnAlexander 11:96465baec780 296
johnAlexander 11:96465baec780 297
charlesmn 0:50b05f035d13 298
charlesmn 0:50b05f035d13 299 /*=================================== Main ==================================
charlesmn 0:50b05f035d13 300 =============================================================================*/
charlesmn 0:50b05f035d13 301 int main()
charlesmn 0:50b05f035d13 302 {
charlesmn 0:50b05f035d13 303 int status;
johnAlexander 11:96465baec780 304 VL53L1 *Sensor;
johnAlexander 11:96465baec780 305 // uint16_t wordData;
johnAlexander 11:96465baec780 306
johnAlexander 11:96465baec780 307 stop_button.rise(&switch_measuring_sensor_irq);
johnAlexander 11:96465baec780 308 stop_button.enable_irq();
johnAlexander 11:96465baec780 309
charlesmn 0:50b05f035d13 310
charlesmn 0:50b05f035d13 311 pc.baud(115200); // baud rate is important as printf statements take a lot of time
johnAlexander 11:96465baec780 312 printf("mbed version = %d \r\n",MBED_VERSION);
charlesmn 0:50b05f035d13 313
charlesmn 0:50b05f035d13 314 // create i2c interface
charlesmn 0:50b05f035d13 315 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:50b05f035d13 316
charlesmn 0:50b05f035d13 317 /* creates the 53L1A1 expansion board singleton obj */
johnAlexander 11:96465baec780 318 // board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2);
johnAlexander 11:96465baec780 319 board = XNucleo53L1A2::instance(dev_I2C, A2, D9, D2);
charlesmn 0:50b05f035d13 320
charlesmn 0:50b05f035d13 321 printf("board created!\r\n");
charlesmn 0:50b05f035d13 322
charlesmn 0:50b05f035d13 323 /* init the 53L1A1 expansion board with default values */
charlesmn 0:50b05f035d13 324 status = board->init_board();
charlesmn 0:50b05f035d13 325 if (status) {
charlesmn 0:50b05f035d13 326 printf("Failed to init board!\r\n");
charlesmn 0:50b05f035d13 327 return 0;
johnAlexander 11:96465baec780 328 }
charlesmn 0:50b05f035d13 329
charlesmn 0:50b05f035d13 330 printf("board initiated! - %d\r\n", status);
charlesmn 0:50b05f035d13 331
johnAlexander 11:96465baec780 332 /* init an array with chars to id the sensors */
johnAlexander 11:96465baec780 333 // status = init_sensors_array();
johnAlexander 11:96465baec780 334 if (status != 0) {
johnAlexander 11:96465baec780 335 printf("Failed to init sensors!\r\n");
johnAlexander 11:96465baec780 336 return status;
johnAlexander 11:96465baec780 337 }
johnAlexander 11:96465baec780 338
johnAlexander 10:cf7d563200fc 339 VL53L1_RoiConfig_t roiConfig;
johnAlexander 10:cf7d563200fc 340
johnAlexander 10:cf7d563200fc 341 roiConfig.NumberOfRoi =3;
johnAlexander 10:cf7d563200fc 342
johnAlexander 10:cf7d563200fc 343 roiConfig.UserRois[0].TopLeftX = 0;
johnAlexander 10:cf7d563200fc 344 roiConfig.UserRois[0].TopLeftY = 9;
johnAlexander 10:cf7d563200fc 345 roiConfig.UserRois[0].BotRightX = 4;
johnAlexander 10:cf7d563200fc 346 roiConfig.UserRois[0].BotRightY = 5;
johnAlexander 10:cf7d563200fc 347
johnAlexander 10:cf7d563200fc 348 roiConfig.UserRois[1].TopLeftX = 5;
johnAlexander 10:cf7d563200fc 349 roiConfig.UserRois[1].TopLeftY = 9;
johnAlexander 10:cf7d563200fc 350 roiConfig.UserRois[1].BotRightX = 9;
johnAlexander 10:cf7d563200fc 351 roiConfig.UserRois[1].BotRightY = 4;
johnAlexander 10:cf7d563200fc 352
johnAlexander 10:cf7d563200fc 353 roiConfig.UserRois[2].TopLeftX = 11;
johnAlexander 10:cf7d563200fc 354 roiConfig.UserRois[2].TopLeftY = 9;
johnAlexander 10:cf7d563200fc 355 roiConfig.UserRois[2].BotRightX = 15;
johnAlexander 10:cf7d563200fc 356 roiConfig.UserRois[2].BotRightY = 5;
johnAlexander 11:96465baec780 357
charlesmn 0:50b05f035d13 358
charlesmn 0:50b05f035d13 359 // configure the sensors
charlesmn 0:50b05f035d13 360 Dev->comms_speed_khz = 400;
charlesmn 0:50b05f035d13 361 Dev->comms_type = 1;
johnAlexander 10:cf7d563200fc 362
johnAlexander 11:96465baec780 363 // start of setup of central sensor
johnAlexander 11:96465baec780 364 if (board->sensor_centre != NULL) {
johnAlexander 11:96465baec780 365 Dev=&devCentre;
johnAlexander 11:96465baec780 366 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
johnAlexander 11:96465baec780 367 devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
johnAlexander 11:96465baec780 368
johnAlexander 11:96465baec780 369 Sensor=board->sensor_centre;
johnAlexander 10:cf7d563200fc 370
johnAlexander 11:96465baec780 371 printf("configuring centre channel \n");
johnAlexander 11:96465baec780 372
johnAlexander 11:96465baec780 373 // Device Initialization and setting
johnAlexander 11:96465baec780 374 status = Sensor->vl53L1_DataInit();
johnAlexander 11:96465baec780 375 status = Sensor->vl53L1_StaticInit();
johnAlexander 11:96465baec780 376
johnAlexander 11:96465baec780 377 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
johnAlexander 11:96465baec780 378 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
johnAlexander 11:96465baec780 379
johnAlexander 11:96465baec780 380 status = Sensor->vl53L1_SetROI( &roiConfig);
johnAlexander 11:96465baec780 381
johnAlexander 11:96465baec780 382 printf("VL53L1_SetROI %d \n",status);
johnAlexander 10:cf7d563200fc 383
johnAlexander 11:96465baec780 384 status = board->sensor_centre->vl53L1_StartMeasurement();
johnAlexander 11:96465baec780 385 }
charlesmn 0:50b05f035d13 386
johnAlexander 10:cf7d563200fc 387 // start of setup of left satellite
johnAlexander 11:96465baec780 388 if (board->sensor_left != NULL) {
johnAlexander 11:96465baec780 389 Dev=&devLeft;
johnAlexander 11:96465baec780 390 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
johnAlexander 11:96465baec780 391 devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
johnAlexander 11:96465baec780 392
johnAlexander 11:96465baec780 393 Sensor=board->sensor_left;
johnAlexander 10:cf7d563200fc 394
johnAlexander 11:96465baec780 395 printf("configuring left channel \n");
johnAlexander 11:96465baec780 396
johnAlexander 11:96465baec780 397 // Device Initialization and setting
johnAlexander 11:96465baec780 398 status = Sensor->vl53L1_DataInit();
johnAlexander 11:96465baec780 399 status = Sensor->vl53L1_StaticInit();
johnAlexander 11:96465baec780 400
johnAlexander 11:96465baec780 401 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
johnAlexander 11:96465baec780 402 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
johnAlexander 11:96465baec780 403
johnAlexander 11:96465baec780 404 status = Sensor->vl53L1_SetROI( &roiConfig);
johnAlexander 11:96465baec780 405
johnAlexander 11:96465baec780 406 printf("VL53L1_SetROI %d \n",status);
johnAlexander 10:cf7d563200fc 407
johnAlexander 11:96465baec780 408 status = board->sensor_left->vl53L1_StartMeasurement();
johnAlexander 11:96465baec780 409 }
johnAlexander 11:96465baec780 410
johnAlexander 11:96465baec780 411 // start of setup of right satellite
johnAlexander 11:96465baec780 412 if (board->sensor_right != NULL) {
johnAlexander 11:96465baec780 413 Dev=&devRight;
johnAlexander 11:96465baec780 414 Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
johnAlexander 11:96465baec780 415 devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
johnAlexander 11:96465baec780 416
johnAlexander 11:96465baec780 417 Sensor=board->sensor_right;
johnAlexander 10:cf7d563200fc 418
johnAlexander 11:96465baec780 419 printf("configuring right channel \n");
johnAlexander 11:96465baec780 420
johnAlexander 11:96465baec780 421 // Device Initialization and setting
johnAlexander 11:96465baec780 422 status = Sensor->vl53L1_DataInit();
johnAlexander 11:96465baec780 423 status = Sensor->vl53L1_StaticInit();
johnAlexander 11:96465baec780 424
johnAlexander 11:96465baec780 425 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
johnAlexander 11:96465baec780 426 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
johnAlexander 11:96465baec780 427
johnAlexander 11:96465baec780 428 status = Sensor->vl53L1_SetROI( &roiConfig);
johnAlexander 11:96465baec780 429
johnAlexander 11:96465baec780 430 printf("VL53L1_SetROI %d \n",status);
johnAlexander 10:cf7d563200fc 431
johnAlexander 11:96465baec780 432 status = board->sensor_right->vl53L1_StartMeasurement();
johnAlexander 11:96465baec780 433 }
johnAlexander 10:cf7d563200fc 434
johnAlexander 10:cf7d563200fc 435
johnAlexander 10:cf7d563200fc 436 VL53L1_MultiRangingData_t MultiRangingData;
johnAlexander 10:cf7d563200fc 437 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
johnAlexander 10:cf7d563200fc 438
johnAlexander 10:cf7d563200fc 439 // looping polling for results
johnAlexander 11:96465baec780 440 while (true) {
johnAlexander 10:cf7d563200fc 441 pMultiRangingData = &MultiRangingData;
johnAlexander 11:96465baec780 442 /*
johnAlexander 11:96465baec780 443 if (board->sensor_centre != NULL) {
johnAlexander 11:96465baec780 444 // wait for result
johnAlexander 11:96465baec780 445 status = board->sensor_centre->vl53L1_WaitMeasurementDataReady();
johnAlexander 11:96465baec780 446 // get the result
johnAlexander 11:96465baec780 447 status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
johnAlexander 11:96465baec780 448 // if valid, print it
johnAlexander 11:96465baec780 449 if(status==0) {
johnAlexander 11:96465baec780 450 print_results(devCentre.i2c_slave_address, pMultiRangingData );
johnAlexander 11:96465baec780 451 status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
johnAlexander 11:96465baec780 452
johnAlexander 11:96465baec780 453 } //if(status==0)
johnAlexander 11:96465baec780 454 else
johnAlexander 11:96465baec780 455 {
johnAlexander 11:96465baec780 456 printf("VL53L1_GetMultiRangingData centre %d \n",status);
johnAlexander 11:96465baec780 457 }
charlesmn 0:50b05f035d13 458 }
johnAlexander 10:cf7d563200fc 459
johnAlexander 11:96465baec780 460 if (board->sensor_left != NULL) {
johnAlexander 11:96465baec780 461 // wait for result
johnAlexander 11:96465baec780 462 status = board->sensor_left->vl53L1_WaitMeasurementDataReady();
johnAlexander 11:96465baec780 463 // get the result
johnAlexander 11:96465baec780 464 status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
johnAlexander 11:96465baec780 465 // if valid, print it
johnAlexander 11:96465baec780 466 if(status==0) {
johnAlexander 11:96465baec780 467 print_results(devLeft.i2c_slave_address, pMultiRangingData );
johnAlexander 11:96465baec780 468 status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
johnAlexander 11:96465baec780 469
johnAlexander 11:96465baec780 470 } //if(status==0)
johnAlexander 11:96465baec780 471 else
johnAlexander 11:96465baec780 472 {
johnAlexander 11:96465baec780 473 printf("VL53L1_GetMultiRangingData left %d \n",status);
johnAlexander 11:96465baec780 474 }
johnAlexander 10:cf7d563200fc 475 }
johnAlexander 10:cf7d563200fc 476
johnAlexander 11:96465baec780 477 if (board->sensor_right != NULL) {
johnAlexander 11:96465baec780 478 // wait for result
johnAlexander 11:96465baec780 479 status = board->sensor_right->vl53L1_WaitMeasurementDataReady();
johnAlexander 11:96465baec780 480 // get the result
johnAlexander 11:96465baec780 481 status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData);
johnAlexander 11:96465baec780 482 // if valid, print it
johnAlexander 11:96465baec780 483 if(status==0) {
johnAlexander 11:96465baec780 484 print_results(devRight.i2c_slave_address, pMultiRangingData );
johnAlexander 11:96465baec780 485 status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement();
johnAlexander 11:96465baec780 486
johnAlexander 11:96465baec780 487 } //if(status==0)
johnAlexander 11:96465baec780 488 else
johnAlexander 11:96465baec780 489 {
johnAlexander 11:96465baec780 490 printf("VL53L1_GetMultiRangingData right %d \n",status);
johnAlexander 11:96465baec780 491 }
johnAlexander 11:96465baec780 492 }
johnAlexander 11:96465baec780 493 */
johnAlexander 11:96465baec780 494
johnAlexander 11:96465baec780 495 measure_sensors();
johnAlexander 11:96465baec780 496
johnAlexander 11:96465baec780 497 if (switchChanged) {
johnAlexander 11:96465baec780 498 ++currentSensor;
johnAlexander 11:96465baec780 499 if (currentSensor == sensorCnt)
johnAlexander 11:96465baec780 500 currentSensor = 0;
johnAlexander 11:96465baec780 501 printf("Sensor changed to %c\r\n", installedSensors[currentSensor]);
johnAlexander 11:96465baec780 502 switchChanged = false;
johnAlexander 11:96465baec780 503 stop_button.enable_irq();
johnAlexander 11:96465baec780 504 }
johnAlexander 11:96465baec780 505
charlesmn 0:50b05f035d13 506 }
charlesmn 0:50b05f035d13 507 }
charlesmn 0:50b05f035d13 508
charlesmn 0:50b05f035d13 509
charlesmn 0:50b05f035d13 510
charlesmn 0:50b05f035d13 511 // prints the range result seen above
charlesmn 0:50b05f035d13 512 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
charlesmn 0:50b05f035d13 513 {
charlesmn 0:50b05f035d13 514 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:50b05f035d13 515
charlesmn 0:50b05f035d13 516 int RoiNumber=pMultiRangingData->RoiNumber;
charlesmn 0:50b05f035d13 517 // int RoiStatus=pMultiRangingData->RoiStatus;
charlesmn 0:50b05f035d13 518
charlesmn 0:50b05f035d13 519 if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:50b05f035d13 520 {
charlesmn 0:50b05f035d13 521
charlesmn 0:50b05f035d13 522 // printf("MZI Count=%5d, ", pMultiRangingData->StreamCount);
charlesmn 0:50b05f035d13 523 // printf("RoiNumber%1d, ", RoiNumber);
charlesmn 0:50b05f035d13 524 // printf("RoiStatus=%1d, \n", RoiStatus);
charlesmn 0:50b05f035d13 525 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:50b05f035d13 526 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:50b05f035d13 527 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:50b05f035d13 528 {
johnAlexander 8:8f6490937346 529 printf("*****************\t i2cAddr=0x%x \t RoiNumber=%d \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
charlesmn 0:50b05f035d13 530 devSpiNumber,
charlesmn 0:50b05f035d13 531 RoiNumber,
charlesmn 0:50b05f035d13 532 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:50b05f035d13 533 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:50b05f035d13 534 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:50b05f035d13 535 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:50b05f035d13 536 }
charlesmn 0:50b05f035d13 537 else
charlesmn 0:50b05f035d13 538 {
charlesmn 0:50b05f035d13 539 // printf("RangeStatus %d %d\n",j, pMultiRangingData->RangeData[j].RangeStatus);
charlesmn 0:50b05f035d13 540 }
charlesmn 0:50b05f035d13 541 }
charlesmn 0:50b05f035d13 542 } // if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:50b05f035d13 543 else
charlesmn 0:50b05f035d13 544 {
charlesmn 0:50b05f035d13 545 // printf("no_of_object_found %d \n",no_of_object_found);
charlesmn 0:50b05f035d13 546 }
charlesmn 0:50b05f035d13 547
charlesmn 2:ef5e40bad526 548 }
charlesmn 2:ef5e40bad526 549
charlesmn 2:ef5e40bad526 550
charlesmn 3:d1a3d15a06ff 551 #if (MBED_VERSION > 60300)
charlesmn 2:ef5e40bad526 552 extern "C" void wait_ms(int ms)
charlesmn 2:ef5e40bad526 553 {
charlesmn 2:ef5e40bad526 554 thread_sleep_for(ms);
charlesmn 2:ef5e40bad526 555 }
charlesmn 2:ef5e40bad526 556 #endif
charlesmn 0:50b05f035d13 557
charlesmn 0:50b05f035d13 558