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@0:d8fe7004e61a, 2020-11-08 (annotated)

Committer:
charlesmn
Date:
Sun Nov 08 15:59:44 2020 +0000
Revision:
0:d8fe7004e61a
Child:
1:52cac9f5ed6a
Sample MBed program for use with ST XNucleo VL53L1CB board. Uses polling to cope with up to 3 sensors in lite ranging mode. Includes MBed V6.4

Who changed what in which revision?

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