ST Expansion SW Team / Mbed OS VL53L1CB_shield_3sensors_polling_lite_ranging_MB6

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   X_NUCLEO_53L1A2

main.cpp@5:6caa39f97aad, 2021-06-10 (annotated)

Committer:
Charles MacNeill
Date:
Thu Jun 10 10:44:05 2021 +0100
Revision:
5:6caa39f97aad
Parent:
4:d50740df0184 
Use VL53L1CB libraries

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:d8fe7004e61a 1 /*
johnAlexander 4:d50740df0184 2 * This VL53L1CB Expansion board test application performs range measurements
johnAlexander 4:d50740df0184 3 * using the onboard embedded centre sensor, in singleshot, polling mode.
johnAlexander 4:d50740df0184 4 * Measured ranges are ouput on the Serial Port, running at 115200 baud.
charlesmn 0:d8fe7004e61a 5 *
charlesmn 0:d8fe7004e61a 6 * This is designed to work with MBed V2 , MBed V5 and MBed V6.
charlesmn 0:d8fe7004e61a 7 *
johnAlexander 4:d50740df0184 8 * The Reset button can be used to restart the program.
charlesmn 0:d8fe7004e61a 9 *
johnAlexander 4:d50740df0184 10 *
johnAlexander 4:d50740df0184 11 * *** Note :
johnAlexander 4:d50740df0184 12 * Default Mbed build system settings disable print floating-point support.
johnAlexander 4:d50740df0184 13 * Offline builds can enable this, again.
johnAlexander 4:d50740df0184 14 * https://github.com/ARMmbed/mbed-os/blob/master/platform/source/minimal-printf/README.md
johnAlexander 4:d50740df0184 15 * .\mbed-os\platform\mbed_lib.json
johnAlexander 4:d50740df0184 16 *
charlesmn 0:d8fe7004e61a 17 */
charlesmn 0:d8fe7004e61a 18
charlesmn 0:d8fe7004e61a 19 #include <stdio.h>
johnAlexander 3:6ecc831dc9fa 20 #include <time.h>
charlesmn 0:d8fe7004e61a 21
charlesmn 0:d8fe7004e61a 22 #include "mbed.h"
johnAlexander 3:6ecc831dc9fa 23
charlesmn 1:52cac9f5ed6a 24 #include "XNucleo53L1A2.h"
charlesmn 0:d8fe7004e61a 25 #include "ToF_I2C.h"
charlesmn 0:d8fe7004e61a 26
charlesmn 0:d8fe7004e61a 27
charlesmn 0:d8fe7004e61a 28
charlesmn 0:d8fe7004e61a 29 // define the i2c comms pins
charlesmn 0:d8fe7004e61a 30 #define I2C_SDA D14
charlesmn 0:d8fe7004e61a 31 #define I2C_SCL D15
charlesmn 0:d8fe7004e61a 32
charlesmn 0:d8fe7004e61a 33 #define NUM_SENSORS 3
charlesmn 0:d8fe7004e61a 34
charlesmn 1:52cac9f5ed6a 35 // define interrupt pins
charlesmn 0:d8fe7004e61a 36 PinName CentreIntPin = A2;
charlesmn 0:d8fe7004e61a 37 // the satellite pins depend on solder blobs on the back of the shield.
charlesmn 0:d8fe7004e61a 38 // they may not exist or may be one of two sets.
charlesmn 0:d8fe7004e61a 39 // the centre pin always exists
charlesmn 0:d8fe7004e61a 40 PinName LeftIntPin = D9;
charlesmn 0:d8fe7004e61a 41 PinName RightIntPin = D4;
charlesmn 0:d8fe7004e61a 42 // alternate set
charlesmn 0:d8fe7004e61a 43 //PinName LeftIntPin = D8;
charlesmn 0:d8fe7004e61a 44 //PinName RightIntPin = D2;
charlesmn 0:d8fe7004e61a 45
charlesmn 0:d8fe7004e61a 46
charlesmn 1:52cac9f5ed6a 47 static XNucleo53L1A2 *board=NULL;
charlesmn 0:d8fe7004e61a 48
johnAlexander 3:6ecc831dc9fa 49 // MBed V6.x has renamed wait_ms and UnbufferedSerial replaces Serial
charlesmn 0:d8fe7004e61a 50 #if (MBED_VERSION > 60300)
johnAlexander 2:7d8d349f52ce 51 UnbufferedSerial pc(USBTX, USBRX);
charlesmn 0:d8fe7004e61a 52 extern "C" void wait_ms(int ms);
charlesmn 0:d8fe7004e61a 53 #else
charlesmn 0:d8fe7004e61a 54 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 0:d8fe7004e61a 55 #endif
charlesmn 0:d8fe7004e61a 56
charlesmn 0:d8fe7004e61a 57
charlesmn 0:d8fe7004e61a 58
charlesmn 0:d8fe7004e61a 59 VL53L1_Dev_t devCentre;
charlesmn 0:d8fe7004e61a 60 VL53L1_Dev_t devLeft;
charlesmn 0:d8fe7004e61a 61 VL53L1_Dev_t devRight;
charlesmn 0:d8fe7004e61a 62 VL53L1_DEV Dev = &devCentre;
charlesmn 0:d8fe7004e61a 63
charlesmn 0:d8fe7004e61a 64 /*=================================== Main ==================================
charlesmn 0:d8fe7004e61a 65 =============================================================================*/
charlesmn 0:d8fe7004e61a 66 int main()
charlesmn 0:d8fe7004e61a 67 {
charlesmn 0:d8fe7004e61a 68 int status;
Charles MacNeill 5:6caa39f97aad 69 VL53L1CB * Sensor;
charlesmn 0:d8fe7004e61a 70 uint16_t wordData;
charlesmn 0:d8fe7004e61a 71 uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
charlesmn 0:d8fe7004e61a 72 static VL53L1_RangingMeasurementData_t RangingData;
charlesmn 0:d8fe7004e61a 73 uint32_t polling_time_ms;
charlesmn 0:d8fe7004e61a 74 uint32_t start_time_ms;
charlesmn 0:d8fe7004e61a 75 uint32_t current_time_ms;
charlesmn 0:d8fe7004e61a 76
johnAlexander 3:6ecc831dc9fa 77 double signalRate = 0.0;
johnAlexander 3:6ecc831dc9fa 78 double ambientRate = 0.0;
johnAlexander 3:6ecc831dc9fa 79
charlesmn 0:d8fe7004e61a 80 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 0:d8fe7004e61a 81
charlesmn 0:d8fe7004e61a 82 printf("Polling Lite-Ranging mbed = %d \r\n",MBED_VERSION);
charlesmn 0:d8fe7004e61a 83 // create i2c interface
charlesmn 0:d8fe7004e61a 84 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:d8fe7004e61a 85
charlesmn 0:d8fe7004e61a 86 dev_I2C->frequency(400000); //also needs doing in spi_interface.c
charlesmn 0:d8fe7004e61a 87
charlesmn 1:52cac9f5ed6a 88 /* creates the 53L1A2 expansion board singleton obj */
charlesmn 1:52cac9f5ed6a 89 board = XNucleo53L1A2::instance(dev_I2C, CentreIntPin, LeftIntPin, RightIntPin);
charlesmn 0:d8fe7004e61a 90 printf("board created!\r\n");
charlesmn 0:d8fe7004e61a 91
charlesmn 0:d8fe7004e61a 92 /* init the 53L1A1 expansion board with default values */
charlesmn 0:d8fe7004e61a 93 status = board->init_board();
charlesmn 0:d8fe7004e61a 94 if (status) {
charlesmn 0:d8fe7004e61a 95 printf("Failed to init board!\r\n");
charlesmn 0:d8fe7004e61a 96 return 0;
charlesmn 0:d8fe7004e61a 97 }
charlesmn 0:d8fe7004e61a 98
charlesmn 0:d8fe7004e61a 99
charlesmn 0:d8fe7004e61a 100 printf("board initiated! - %d\r\n", status);
charlesmn 0:d8fe7004e61a 101
charlesmn 0:d8fe7004e61a 102 // for each sensor, if it exists, initialise and configure
charlesmn 0:d8fe7004e61a 103 for (ToFSensor=0;ToFSensor< NUM_SENSORS;ToFSensor++){
charlesmn 0:d8fe7004e61a 104 switch(ToFSensor){
charlesmn 0:d8fe7004e61a 105 case 0:
charlesmn 0:d8fe7004e61a 106 if (board->sensor_centre== NULL ) continue;
charlesmn 0:d8fe7004e61a 107 Dev=&devCentre;
charlesmn 0:d8fe7004e61a 108 Sensor=board->sensor_centre;
charlesmn 0:d8fe7004e61a 109 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:d8fe7004e61a 110 printf("configuring centre channel \n");
charlesmn 0:d8fe7004e61a 111 break;
charlesmn 0:d8fe7004e61a 112 case 1:
charlesmn 0:d8fe7004e61a 113 if (board->sensor_left== NULL ) continue;
charlesmn 0:d8fe7004e61a 114 Dev=&devLeft;
charlesmn 0:d8fe7004e61a 115 Sensor=board->sensor_left;
charlesmn 0:d8fe7004e61a 116 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
charlesmn 0:d8fe7004e61a 117 printf("configuring left channel \n");
charlesmn 0:d8fe7004e61a 118 break;
charlesmn 0:d8fe7004e61a 119 case 2:
charlesmn 0:d8fe7004e61a 120 if (board->sensor_right== NULL ) continue;
charlesmn 0:d8fe7004e61a 121 Dev=&devRight;
charlesmn 0:d8fe7004e61a 122 Sensor=board->sensor_right;
charlesmn 0:d8fe7004e61a 123 Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
charlesmn 0:d8fe7004e61a 124 printf("configuring right channel \n");
charlesmn 0:d8fe7004e61a 125 break;
charlesmn 0:d8fe7004e61a 126 }
charlesmn 0:d8fe7004e61a 127
charlesmn 0:d8fe7004e61a 128 Dev->comms_speed_khz = 400;
charlesmn 0:d8fe7004e61a 129 Dev->comms_type = 1;
charlesmn 0:d8fe7004e61a 130
Charles MacNeill 5:6caa39f97aad 131 Sensor->VL53L1CB_RdWord(Dev, 0x01, &wordData);
Charles MacNeill 5:6caa39f97aad 132 printf("VL53L1CB: %02X %d\n\r", wordData,Dev->i2c_slave_address);
charlesmn 0:d8fe7004e61a 133 /* Device Initialization and setting */
charlesmn 0:d8fe7004e61a 134
Charles MacNeill 5:6caa39f97aad 135 status = Sensor->VL53L1CB_DataInit();
Charles MacNeill 5:6caa39f97aad 136 status = Sensor->VL53L1CB_StaticInit();
Charles MacNeill 5:6caa39f97aad 137 status = Sensor->VL53L1CB_SetPresetMode( VL53L1_PRESETMODE_LITE_RANGING);
Charles MacNeill 5:6caa39f97aad 138 status = Sensor->VL53L1CB_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
Charles MacNeill 5:6caa39f97aad 139 status = Sensor->VL53L1CB_SetMeasurementTimingBudgetMicroSeconds( 50000);
Charles MacNeill 5:6caa39f97aad 140 status = Sensor->VL53L1CB_SetInterMeasurementPeriodMilliSeconds( 100);
charlesmn 0:d8fe7004e61a 141 }
charlesmn 0:d8fe7004e61a 142
charlesmn 0:d8fe7004e61a 143 if (board->sensor_centre!= NULL )
charlesmn 0:d8fe7004e61a 144 {
charlesmn 0:d8fe7004e61a 145 Dev=&devCentre;
charlesmn 0:d8fe7004e61a 146 Sensor=board->sensor_centre;
charlesmn 0:d8fe7004e61a 147 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:d8fe7004e61a 148 }
charlesmn 0:d8fe7004e61a 149
charlesmn 0:d8fe7004e61a 150 if (board->sensor_left!= NULL )
charlesmn 0:d8fe7004e61a 151 {
charlesmn 0:d8fe7004e61a 152 Dev=&devLeft;
charlesmn 0:d8fe7004e61a 153 Sensor=board->sensor_left;
charlesmn 0:d8fe7004e61a 154 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
charlesmn 0:d8fe7004e61a 155 }
charlesmn 0:d8fe7004e61a 156
charlesmn 0:d8fe7004e61a 157
charlesmn 0:d8fe7004e61a 158 if (board->sensor_right!= NULL )
charlesmn 0:d8fe7004e61a 159 {
charlesmn 0:d8fe7004e61a 160 Dev=&devRight;
charlesmn 0:d8fe7004e61a 161 Sensor=board->sensor_right;
charlesmn 0:d8fe7004e61a 162 Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
charlesmn 0:d8fe7004e61a 163 }
charlesmn 0:d8fe7004e61a 164
charlesmn 0:d8fe7004e61a 165 while(1) {
charlesmn 0:d8fe7004e61a 166 for (ToFSensor=0;ToFSensor< NUM_SENSORS;ToFSensor++){
charlesmn 0:d8fe7004e61a 167 switch(ToFSensor){
charlesmn 0:d8fe7004e61a 168 case 0:
charlesmn 0:d8fe7004e61a 169 if (board->sensor_centre== NULL ) continue;
charlesmn 0:d8fe7004e61a 170 Dev=&devCentre;
charlesmn 0:d8fe7004e61a 171 Sensor=board->sensor_centre;
charlesmn 0:d8fe7004e61a 172 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
charlesmn 0:d8fe7004e61a 173 break;
charlesmn 0:d8fe7004e61a 174 case 1:
charlesmn 0:d8fe7004e61a 175 if (board->sensor_left== NULL ) continue;
charlesmn 0:d8fe7004e61a 176 Dev=&devLeft;
charlesmn 0:d8fe7004e61a 177 Sensor=board->sensor_left;
charlesmn 0:d8fe7004e61a 178 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
charlesmn 0:d8fe7004e61a 179 break;
charlesmn 0:d8fe7004e61a 180 case 2:
charlesmn 0:d8fe7004e61a 181 if (board->sensor_right== NULL ) continue;
charlesmn 0:d8fe7004e61a 182 Dev=&devRight;
charlesmn 0:d8fe7004e61a 183 Sensor=board->sensor_right;
charlesmn 0:d8fe7004e61a 184 Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
charlesmn 0:d8fe7004e61a 185 break;
charlesmn 0:d8fe7004e61a 186 default:
charlesmn 0:d8fe7004e61a 187 printf("default \n");
charlesmn 0:d8fe7004e61a 188 break;
charlesmn 0:d8fe7004e61a 189 } // switch
charlesmn 0:d8fe7004e61a 190
charlesmn 0:d8fe7004e61a 191 start_time_ms = us_ticker_read() / 1000;
Charles MacNeill 5:6caa39f97aad 192 status = Sensor->VL53L1CB_StartMeasurement();
charlesmn 0:d8fe7004e61a 193
Charles MacNeill 5:6caa39f97aad 194 status = Sensor->VL53L1CB_WaitMeasurementDataReady();
charlesmn 0:d8fe7004e61a 195
charlesmn 0:d8fe7004e61a 196 if(!status)
charlesmn 0:d8fe7004e61a 197 {
Charles MacNeill 5:6caa39f97aad 198 status = Sensor->VL53L1CB_GetRangingMeasurementData( &RangingData);
charlesmn 0:d8fe7004e61a 199 current_time_ms = us_ticker_read() / 1000;
charlesmn 0:d8fe7004e61a 200
charlesmn 0:d8fe7004e61a 201 if((status==0) && (RangingData.RangeStatus != 10)) {
johnAlexander 4:d50740df0184 202 printf("data %d,%d,%d,%2.2f,%2.2f\n", ToFSensor, RangingData.RangeStatus, RangingData.RangeMilliMeter,
johnAlexander 4:d50740df0184 203 (RangingData.SignalRateRtnMegaCps/65536.0), RangingData.AmbientRateRtnMegaCps/65336.0);
charlesmn 0:d8fe7004e61a 204 }
charlesmn 0:d8fe7004e61a 205 } // if status
charlesmn 0:d8fe7004e61a 206 else
charlesmn 0:d8fe7004e61a 207 {
charlesmn 0:d8fe7004e61a 208 printf("VL53L1_WaitMeasurementDataReady failed %d \n",status);
charlesmn 0:d8fe7004e61a 209 }
charlesmn 0:d8fe7004e61a 210
Charles MacNeill 5:6caa39f97aad 211 status = Sensor->VL53L1CB_ClearInterruptAndStartMeasurement();
Charles MacNeill 5:6caa39f97aad 212 status = Sensor->VL53L1CB_WaitMeasurementDataReady();
charlesmn 0:d8fe7004e61a 213 if(!status)
charlesmn 0:d8fe7004e61a 214 {
Charles MacNeill 5:6caa39f97aad 215 status = Sensor->VL53L1CB_GetRangingMeasurementData( &RangingData);
charlesmn 0:d8fe7004e61a 216
charlesmn 0:d8fe7004e61a 217 polling_time_ms = current_time_ms - start_time_ms;
charlesmn 0:d8fe7004e61a 218 if((status==0) && (RangingData.RangeStatus != 10)) {
johnAlexander 3:6ecc831dc9fa 219 signalRate = RangingData.SignalRateRtnMegaCps / 65535.0;
johnAlexander 3:6ecc831dc9fa 220 ambientRate = RangingData.AmbientRateRtnMegaCps / 65535.0;
johnAlexander 3:6ecc831dc9fa 221 printf("data %d, %d, %d, %2.2f, %2.2f, %d\n", ToFSensor, RangingData.RangeStatus, RangingData.RangeMilliMeter,
johnAlexander 3:6ecc831dc9fa 222 signalRate, ambientRate, polling_time_ms);
charlesmn 0:d8fe7004e61a 223 }
charlesmn 0:d8fe7004e61a 224 }
charlesmn 0:d8fe7004e61a 225
Charles MacNeill 5:6caa39f97aad 226 status = Sensor->VL53L1CB_ClearInterruptAndStartMeasurement();
charlesmn 0:d8fe7004e61a 227
charlesmn 0:d8fe7004e61a 228
Charles MacNeill 5:6caa39f97aad 229 status = Sensor->VL53L1CB_StopMeasurement();
charlesmn 0:d8fe7004e61a 230 } // endwhile(1)
charlesmn 0:d8fe7004e61a 231
charlesmn 0:d8fe7004e61a 232 }
charlesmn 0:d8fe7004e61a 233
charlesmn 0:d8fe7004e61a 234
charlesmn 0:d8fe7004e61a 235 }
charlesmn 0:d8fe7004e61a 236
charlesmn 0:d8fe7004e61a 237
charlesmn 0:d8fe7004e61a 238 #if (MBED_VERSION > 60300)
charlesmn 0:d8fe7004e61a 239 void wait_ms(int ms)
charlesmn 0:d8fe7004e61a 240 {
charlesmn 0:d8fe7004e61a 241 thread_sleep_for(ms);
charlesmn 0:d8fe7004e61a 242 }
charlesmn 0:d8fe7004e61a 243 #endif
charlesmn 0:d8fe7004e61a 244