John Alexander / Mbed OS VL53L1CB_shield_3sensor_polling_multizone

3 sensors, polling

Dependencies:   X_NUCLEO_53L1A2

main.cpp@10:cf7d563200fc, 2021-05-13 (annotated)

Committer:
johnAlexander
Date:
Thu May 13 09:17:02 2021 +0000
Revision:
10:cf7d563200fc
Parent:
9:0a3e1affe004 
debug trials

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
charlesmn 0:50b05f035d13 32
charlesmn 0:50b05f035d13 33
johnAlexander 4:177d711cc20f 34 static XNucleo53L1A2 *board=NULL;
charlesmn 3:d1a3d15a06ff 35 #if (MBED_VERSION > 60300)
johnAlexander 7:b5a4ad8ba844 36 UnbufferedSerial pc(USBTX, USBRX);
johnAlexander 6:bc6ac1e294f4 37 extern "C" void wait_ms(int ms);
charlesmn 2:ef5e40bad526 38 #else
johnAlexander 6:bc6ac1e294f4 39 Serial pc(SERIAL_TX, SERIAL_RX);
charlesmn 2:ef5e40bad526 40 #endif
charlesmn 0:50b05f035d13 41
charlesmn 5:5916ff386a94 42 void process_interrupt( VL53L1 * sensor,VL53L1_DEV dev );
charlesmn 0:50b05f035d13 43 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
charlesmn 0:50b05f035d13 44
charlesmn 0:50b05f035d13 45 VL53L1_Dev_t devCentre;
charlesmn 0:50b05f035d13 46 VL53L1_Dev_t devLeft;
charlesmn 0:50b05f035d13 47 VL53L1_Dev_t devRight;
charlesmn 0:50b05f035d13 48 VL53L1_DEV Dev = &devCentre;
charlesmn 0:50b05f035d13 49
charlesmn 0:50b05f035d13 50
charlesmn 0:50b05f035d13 51 /*=================================== Main ==================================
charlesmn 0:50b05f035d13 52 =============================================================================*/
charlesmn 0:50b05f035d13 53 int main()
charlesmn 0:50b05f035d13 54 {
charlesmn 0:50b05f035d13 55 int status;
charlesmn 5:5916ff386a94 56 VL53L1 * Sensor;
charlesmn 0:50b05f035d13 57 uint16_t wordData;
charlesmn 0:50b05f035d13 58
charlesmn 0:50b05f035d13 59
charlesmn 0:50b05f035d13 60 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 2:ef5e40bad526 61 printf("Polling single multizone mbed = %d \r\n",MBED_VERSION);
charlesmn 0:50b05f035d13 62
charlesmn 0:50b05f035d13 63 // create i2c interface
charlesmn 0:50b05f035d13 64 ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
charlesmn 0:50b05f035d13 65
charlesmn 0:50b05f035d13 66 /* creates the 53L1A1 expansion board singleton obj */
johnAlexander 4:177d711cc20f 67 board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2);
charlesmn 0:50b05f035d13 68
charlesmn 0:50b05f035d13 69 printf("board created!\r\n");
charlesmn 0:50b05f035d13 70
charlesmn 0:50b05f035d13 71 /* init the 53L1A1 expansion board with default values */
charlesmn 0:50b05f035d13 72 status = board->init_board();
charlesmn 0:50b05f035d13 73 if (status) {
charlesmn 0:50b05f035d13 74 printf("Failed to init board!\r\n");
charlesmn 0:50b05f035d13 75 return 0;
charlesmn 0:50b05f035d13 76 }
charlesmn 0:50b05f035d13 77
charlesmn 0:50b05f035d13 78
charlesmn 0:50b05f035d13 79 printf("board initiated! - %d\r\n", status);
charlesmn 0:50b05f035d13 80 // create the sensor controller classes
charlesmn 0:50b05f035d13 81
johnAlexander 10:cf7d563200fc 82 VL53L1_RoiConfig_t roiConfig;
johnAlexander 10:cf7d563200fc 83
johnAlexander 10:cf7d563200fc 84 roiConfig.NumberOfRoi =3;
johnAlexander 10:cf7d563200fc 85
johnAlexander 10:cf7d563200fc 86 roiConfig.UserRois[0].TopLeftX = 0;
johnAlexander 10:cf7d563200fc 87 roiConfig.UserRois[0].TopLeftY = 9;
johnAlexander 10:cf7d563200fc 88 roiConfig.UserRois[0].BotRightX = 4;
johnAlexander 10:cf7d563200fc 89 roiConfig.UserRois[0].BotRightY = 5;
johnAlexander 10:cf7d563200fc 90
johnAlexander 10:cf7d563200fc 91 roiConfig.UserRois[1].TopLeftX = 5;
johnAlexander 10:cf7d563200fc 92 roiConfig.UserRois[1].TopLeftY = 9;
johnAlexander 10:cf7d563200fc 93 roiConfig.UserRois[1].BotRightX = 9;
johnAlexander 10:cf7d563200fc 94 roiConfig.UserRois[1].BotRightY = 4;
johnAlexander 10:cf7d563200fc 95
johnAlexander 10:cf7d563200fc 96 roiConfig.UserRois[2].TopLeftX = 11;
johnAlexander 10:cf7d563200fc 97 roiConfig.UserRois[2].TopLeftY = 9;
johnAlexander 10:cf7d563200fc 98 roiConfig.UserRois[2].BotRightX = 15;
johnAlexander 10:cf7d563200fc 99 roiConfig.UserRois[2].BotRightY = 5;
johnAlexander 10:cf7d563200fc 100
johnAlexander 10:cf7d563200fc 101 // start of setup of central sensor
charlesmn 0:50b05f035d13 102 Dev=&devCentre;
charlesmn 0:50b05f035d13 103
charlesmn 0:50b05f035d13 104 // configure the sensors
charlesmn 0:50b05f035d13 105 Dev->comms_speed_khz = 400;
charlesmn 0:50b05f035d13 106 Dev->comms_type = 1;
johnAlexander 10:cf7d563200fc 107 Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
johnAlexander 10:cf7d563200fc 108 devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
johnAlexander 10:cf7d563200fc 109
johnAlexander 10:cf7d563200fc 110 Sensor=board->sensor_centre;
charlesmn 0:50b05f035d13 111
johnAlexander 10:cf7d563200fc 112 printf("configuring centre channel \n");
johnAlexander 10:cf7d563200fc 113
johnAlexander 10:cf7d563200fc 114 // Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
johnAlexander 10:cf7d563200fc 115 // printf("VL53L1X: %02X %d\n\r", wordData,Dev->i2c_slave_address);
charlesmn 0:50b05f035d13 116 /* Device Initialization and setting */
charlesmn 0:50b05f035d13 117 status = Sensor->vl53L1_DataInit();
charlesmn 0:50b05f035d13 118 status = Sensor->vl53L1_StaticInit();
charlesmn 0:50b05f035d13 119 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
charlesmn 1:49e2fad70dfe 120 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
johnAlexander 10:cf7d563200fc 121
charlesmn 0:50b05f035d13 122
charlesmn 0:50b05f035d13 123 status = Sensor->vl53L1_SetROI( &roiConfig);
johnAlexander 10:cf7d563200fc 124
charlesmn 0:50b05f035d13 125 printf("VL53L1_SetROI %d \n",status);
charlesmn 0:50b05f035d13 126
charlesmn 0:50b05f035d13 127 status = board->sensor_centre->vl53L1_StartMeasurement();
johnAlexander 10:cf7d563200fc 128
charlesmn 0:50b05f035d13 129
johnAlexander 10:cf7d563200fc 130 // start of setup of left satellite
johnAlexander 10:cf7d563200fc 131 Dev=&devLeft;
johnAlexander 10:cf7d563200fc 132
johnAlexander 10:cf7d563200fc 133 // configure the sensors
johnAlexander 10:cf7d563200fc 134 Dev->comms_speed_khz = 400;
johnAlexander 10:cf7d563200fc 135 Dev->comms_type = 1;
johnAlexander 10:cf7d563200fc 136 Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
johnAlexander 10:cf7d563200fc 137 devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
johnAlexander 10:cf7d563200fc 138
johnAlexander 10:cf7d563200fc 139 Sensor=board->sensor_left;
johnAlexander 10:cf7d563200fc 140
johnAlexander 10:cf7d563200fc 141 printf("configuring centre channel \n");
johnAlexander 10:cf7d563200fc 142
johnAlexander 10:cf7d563200fc 143 // Sensor->VL53L1_RdWord(Dev, 0x01, &wordData);
johnAlexander 10:cf7d563200fc 144 // printf("VL53L1X: %02X %d\n\r", wordData,Dev->i2c_slave_address);
johnAlexander 10:cf7d563200fc 145 /* Device Initialization and setting */
johnAlexander 10:cf7d563200fc 146 status = Sensor->vl53L1_DataInit();
johnAlexander 10:cf7d563200fc 147 status = Sensor->vl53L1_StaticInit();
johnAlexander 10:cf7d563200fc 148 status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
johnAlexander 10:cf7d563200fc 149 status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
johnAlexander 10:cf7d563200fc 150
johnAlexander 10:cf7d563200fc 151
johnAlexander 10:cf7d563200fc 152 status = Sensor->vl53L1_SetROI( &roiConfig);
johnAlexander 10:cf7d563200fc 153
johnAlexander 10:cf7d563200fc 154 printf("VL53L1_SetROI %d \n",status);
johnAlexander 10:cf7d563200fc 155
johnAlexander 10:cf7d563200fc 156 status = board->sensor_left->vl53L1_StartMeasurement();
johnAlexander 10:cf7d563200fc 157
johnAlexander 10:cf7d563200fc 158
johnAlexander 10:cf7d563200fc 159 VL53L1_MultiRangingData_t MultiRangingData;
johnAlexander 10:cf7d563200fc 160 VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
johnAlexander 10:cf7d563200fc 161
johnAlexander 10:cf7d563200fc 162 // looping polling for results
charlesmn 0:50b05f035d13 163 while (1)
charlesmn 0:50b05f035d13 164 {
johnAlexander 10:cf7d563200fc 165 pMultiRangingData = &MultiRangingData;
charlesmn 0:50b05f035d13 166
charlesmn 0:50b05f035d13 167 // wait for result
charlesmn 0:50b05f035d13 168 status = board->sensor_centre->vl53L1_WaitMeasurementDataReady();
charlesmn 0:50b05f035d13 169 // get the result
charlesmn 0:50b05f035d13 170 status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
charlesmn 0:50b05f035d13 171 // if valid, print it
charlesmn 0:50b05f035d13 172 if(status==0) {
charlesmn 0:50b05f035d13 173 print_results(devCentre.i2c_slave_address, pMultiRangingData );
johnAlexander 10:cf7d563200fc 174 status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
charlesmn 0:50b05f035d13 175
charlesmn 0:50b05f035d13 176 } //if(status==0)
charlesmn 0:50b05f035d13 177 else
charlesmn 0:50b05f035d13 178 {
charlesmn 0:50b05f035d13 179 printf("VL53L1_GetMultiRangingData centre %d \n",status);
charlesmn 0:50b05f035d13 180 }
johnAlexander 10:cf7d563200fc 181
johnAlexander 10:cf7d563200fc 182 // wait for result
johnAlexander 10:cf7d563200fc 183 status = board->sensor_left->vl53L1_WaitMeasurementDataReady();
johnAlexander 10:cf7d563200fc 184 // get the result
johnAlexander 10:cf7d563200fc 185 status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
johnAlexander 10:cf7d563200fc 186 // if valid, print it
johnAlexander 10:cf7d563200fc 187 if(status==0) {
johnAlexander 10:cf7d563200fc 188 print_results(devLeft.i2c_slave_address, pMultiRangingData );
johnAlexander 10:cf7d563200fc 189 status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
johnAlexander 10:cf7d563200fc 190
johnAlexander 10:cf7d563200fc 191 } //if(status==0)
johnAlexander 10:cf7d563200fc 192 else
johnAlexander 10:cf7d563200fc 193 {
johnAlexander 10:cf7d563200fc 194 printf("VL53L1_GetMultiRangingData centre %d \n",status);
johnAlexander 10:cf7d563200fc 195 }
johnAlexander 10:cf7d563200fc 196
charlesmn 0:50b05f035d13 197 }
charlesmn 0:50b05f035d13 198 }
charlesmn 0:50b05f035d13 199
charlesmn 0:50b05f035d13 200
charlesmn 0:50b05f035d13 201
charlesmn 0:50b05f035d13 202 // prints the range result seen above
charlesmn 0:50b05f035d13 203 void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
charlesmn 0:50b05f035d13 204 {
charlesmn 0:50b05f035d13 205 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
charlesmn 0:50b05f035d13 206
charlesmn 0:50b05f035d13 207 int RoiNumber=pMultiRangingData->RoiNumber;
charlesmn 0:50b05f035d13 208 // int RoiStatus=pMultiRangingData->RoiStatus;
charlesmn 0:50b05f035d13 209
charlesmn 0:50b05f035d13 210 if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:50b05f035d13 211 {
charlesmn 0:50b05f035d13 212
charlesmn 0:50b05f035d13 213 // printf("MZI Count=%5d, ", pMultiRangingData->StreamCount);
charlesmn 0:50b05f035d13 214 // printf("RoiNumber%1d, ", RoiNumber);
charlesmn 0:50b05f035d13 215 // printf("RoiStatus=%1d, \n", RoiStatus);
charlesmn 0:50b05f035d13 216 for(int j=0;j<no_of_object_found;j++){
charlesmn 0:50b05f035d13 217 if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) ||
charlesmn 0:50b05f035d13 218 (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
charlesmn 0:50b05f035d13 219 {
johnAlexander 8:8f6490937346 220 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 221 devSpiNumber,
charlesmn 0:50b05f035d13 222 RoiNumber,
charlesmn 0:50b05f035d13 223 pMultiRangingData->RangeData[j].RangeStatus,
charlesmn 0:50b05f035d13 224 pMultiRangingData->RangeData[j].RangeMilliMeter,
charlesmn 0:50b05f035d13 225 pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
charlesmn 0:50b05f035d13 226 pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
charlesmn 0:50b05f035d13 227 }
charlesmn 0:50b05f035d13 228 else
charlesmn 0:50b05f035d13 229 {
charlesmn 0:50b05f035d13 230 // printf("RangeStatus %d %d\n",j, pMultiRangingData->RangeData[j].RangeStatus);
charlesmn 0:50b05f035d13 231 }
charlesmn 0:50b05f035d13 232 }
charlesmn 0:50b05f035d13 233 } // if (( no_of_object_found < 10 ) && ( no_of_object_found != 0))
charlesmn 0:50b05f035d13 234 else
charlesmn 0:50b05f035d13 235 {
charlesmn 0:50b05f035d13 236 // printf("no_of_object_found %d \n",no_of_object_found);
charlesmn 0:50b05f035d13 237 }
charlesmn 0:50b05f035d13 238
charlesmn 2:ef5e40bad526 239 }
charlesmn 2:ef5e40bad526 240
charlesmn 2:ef5e40bad526 241
charlesmn 3:d1a3d15a06ff 242 #if (MBED_VERSION > 60300)
charlesmn 2:ef5e40bad526 243 extern "C" void wait_ms(int ms)
charlesmn 2:ef5e40bad526 244 {
charlesmn 2:ef5e40bad526 245 thread_sleep_for(ms);
charlesmn 2:ef5e40bad526 246 }
charlesmn 2:ef5e40bad526 247 #endif
charlesmn 0:50b05f035d13 248
charlesmn 0:50b05f035d13 249