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@2:7d8d349f52ce, 2021-04-30 (annotated)

Committer:
johnAlexander
Date:
Fri Apr 30 14:29:25 2021 +0000
Revision:
2:7d8d349f52ce
Parent:
1:52cac9f5ed6a
Child:
3:6ecc831dc9fa
Child:
9:57c341d81a9f
Updated serial port labels. Aligned with MbedOS v6.10.

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