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@3:6ecc831dc9fa, 2021-05-07 (annotated)

Committer:
johnAlexander
Date:
Fri May 07 10:11:09 2021 +0000
Revision:
3:6ecc831dc9fa
Parent:
2:7d8d349f52ce
Child:
4:d50740df0184
Include published library.

Who changed what in which revision?

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