John Alexander
/
VL53L1CB_shield_3sensor_polling_multizone
3 sensors, polling
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?
User | Revision | Line number | New 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 |