Danny Nichols / Mbed OS ENGR6002_P001unk

stilldk

Dependencies:   vl53l0x_api SRF05

Simplifier/VL53L0XSimplifier.cpp@0:041c1089b6e5, 2022-03-10 (annotated)

Committer:
dnikolz
Date:
Thu Mar 10 08:40:14 2022 +0000
Revision:
0:041c1089b6e5
idk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dnikolz 0:041c1089b6e5 1 #include "mbed.h"
dnikolz 0:041c1089b6e5 2 #include "vl53l0x_api.h"
dnikolz 0:041c1089b6e5 3 #include "vl53l0x_platform.h"
dnikolz 0:041c1089b6e5 4 #include "vl53l0x_i2c_platform.h"
dnikolz 0:041c1089b6e5 5 #include "VL53L0XSimplifier.h"
dnikolz 0:041c1089b6e5 6 //CAMERA SENSOR 1FT
dnikolz 0:041c1089b6e5 7 Serial pc(SERIAL_TX, SERIAL_RX);
dnikolz 0:041c1089b6e5 8
dnikolz 0:041c1089b6e5 9 void VL53Simplifier::print_pal_error(VL53L0X_Error Status){
dnikolz 0:041c1089b6e5 10 char buf[VL53L0X_MAX_STRING_LENGTH];
dnikolz 0:041c1089b6e5 11 VL53L0X_GetPalErrorString(Status, buf);
dnikolz 0:041c1089b6e5 12 printf("API Status: %i : %s\n", Status, buf);
dnikolz 0:041c1089b6e5 13 }
dnikolz 0:041c1089b6e5 14
dnikolz 0:041c1089b6e5 15 VL53L0X_Error VL53Simplifier::WaitMeasurementDataReady(VL53L0X_DEV Dev) {
dnikolz 0:041c1089b6e5 16 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
dnikolz 0:041c1089b6e5 17 uint8_t NewDatReady=0;
dnikolz 0:041c1089b6e5 18 uint32_t LoopNb;
dnikolz 0:041c1089b6e5 19
dnikolz 0:041c1089b6e5 20 // Wait until it finished
dnikolz 0:041c1089b6e5 21 // use timeout to avoid deadlock
dnikolz 0:041c1089b6e5 22 if (Status == VL53L0X_ERROR_NONE) {
dnikolz 0:041c1089b6e5 23 LoopNb = 0;
dnikolz 0:041c1089b6e5 24 do {
dnikolz 0:041c1089b6e5 25 Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady);
dnikolz 0:041c1089b6e5 26 if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) {
dnikolz 0:041c1089b6e5 27 break;
dnikolz 0:041c1089b6e5 28 }
dnikolz 0:041c1089b6e5 29 LoopNb = LoopNb + 1;
dnikolz 0:041c1089b6e5 30 VL53L0X_PollingDelay(Dev);
dnikolz 0:041c1089b6e5 31 } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
dnikolz 0:041c1089b6e5 32
dnikolz 0:041c1089b6e5 33 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
dnikolz 0:041c1089b6e5 34 Status = VL53L0X_ERROR_TIME_OUT;
dnikolz 0:041c1089b6e5 35 }
dnikolz 0:041c1089b6e5 36 }
dnikolz 0:041c1089b6e5 37
dnikolz 0:041c1089b6e5 38 return Status;
dnikolz 0:041c1089b6e5 39 }
dnikolz 0:041c1089b6e5 40
dnikolz 0:041c1089b6e5 41
dnikolz 0:041c1089b6e5 42 VL53L0X_Error VL53Simplifier::WaitStopCompleted(VL53L0X_DEV Dev) {
dnikolz 0:041c1089b6e5 43 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
dnikolz 0:041c1089b6e5 44 uint32_t StopCompleted=0;
dnikolz 0:041c1089b6e5 45 uint32_t LoopNb;
dnikolz 0:041c1089b6e5 46
dnikolz 0:041c1089b6e5 47 // Wait until it finished
dnikolz 0:041c1089b6e5 48 // use timeout to avoid deadlock
dnikolz 0:041c1089b6e5 49 if (Status == VL53L0X_ERROR_NONE) {
dnikolz 0:041c1089b6e5 50 LoopNb = 0;
dnikolz 0:041c1089b6e5 51 do {
dnikolz 0:041c1089b6e5 52 Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted);
dnikolz 0:041c1089b6e5 53 if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) {
dnikolz 0:041c1089b6e5 54 break;
dnikolz 0:041c1089b6e5 55 }
dnikolz 0:041c1089b6e5 56 LoopNb = LoopNb + 1;
dnikolz 0:041c1089b6e5 57 VL53L0X_PollingDelay(Dev);
dnikolz 0:041c1089b6e5 58 } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
dnikolz 0:041c1089b6e5 59
dnikolz 0:041c1089b6e5 60 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
dnikolz 0:041c1089b6e5 61 Status = VL53L0X_ERROR_TIME_OUT;
dnikolz 0:041c1089b6e5 62 }
dnikolz 0:041c1089b6e5 63
dnikolz 0:041c1089b6e5 64 }
dnikolz 0:041c1089b6e5 65
dnikolz 0:041c1089b6e5 66 return Status;
dnikolz 0:041c1089b6e5 67 }
dnikolz 0:041c1089b6e5 68 void VL53Simplifier::print_range_status(VL53L0X_RangingMeasurementData_t* pRangingMeasurementData){
dnikolz 0:041c1089b6e5 69 char buf[VL53L0X_MAX_STRING_LENGTH];
dnikolz 0:041c1089b6e5 70 uint8_t RangeStatus;
dnikolz 0:041c1089b6e5 71
dnikolz 0:041c1089b6e5 72 /*
dnikolz 0:041c1089b6e5 73 * New Range Status: data is valid when pRangingMeasurementData->RangeStatus = 0
dnikolz 0:041c1089b6e5 74 */
dnikolz 0:041c1089b6e5 75
dnikolz 0:041c1089b6e5 76 RangeStatus = pRangingMeasurementData->RangeStatus;
dnikolz 0:041c1089b6e5 77
dnikolz 0:041c1089b6e5 78 VL53L0X_GetRangeStatusString(RangeStatus, buf);
dnikolz 0:041c1089b6e5 79 printf("Range Status: %i : %s\n", RangeStatus, buf);
dnikolz 0:041c1089b6e5 80
dnikolz 0:041c1089b6e5 81 }
dnikolz 0:041c1089b6e5 82 void VL53Simplifier::runVL53() {
dnikolz 0:041c1089b6e5 83 // sensor camera
dnikolz 0:041c1089b6e5 84
dnikolz 0:041c1089b6e5 85 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
dnikolz 0:041c1089b6e5 86 VL53L0X_Dev_t MyDevice;
dnikolz 0:041c1089b6e5 87 VL53L0X_Dev_t *pMyDevice = &MyDevice;
dnikolz 0:041c1089b6e5 88 VL53L0X_Version_t Version;
dnikolz 0:041c1089b6e5 89 VL53L0X_Version_t *pVersion = &Version;
dnikolz 0:041c1089b6e5 90 VL53L0X_DeviceInfo_t DeviceInfo;
dnikolz 0:041c1089b6e5 91
dnikolz 0:041c1089b6e5 92 int32_t status_int;
dnikolz 0:041c1089b6e5 93
dnikolz 0:041c1089b6e5 94 pc.printf("VL53L0X API Simple Ranging Example\r\n");
dnikolz 0:041c1089b6e5 95
dnikolz 0:041c1089b6e5 96 // Initialize Comms
dnikolz 0:041c1089b6e5 97 pMyDevice->I2cDevAddr = 0x52;
dnikolz 0:041c1089b6e5 98 pMyDevice->comms_type = 1;
dnikolz 0:041c1089b6e5 99 pMyDevice->comms_speed_khz = 400;
dnikolz 0:041c1089b6e5 100
dnikolz 0:041c1089b6e5 101 pc.printf("Init comms\r\n");
dnikolz 0:041c1089b6e5 102
dnikolz 0:041c1089b6e5 103 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 104 {
dnikolz 0:041c1089b6e5 105 status_int = VL53L0X_GetVersion(pVersion);
dnikolz 0:041c1089b6e5 106 if (status_int != 0)
dnikolz 0:041c1089b6e5 107 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
dnikolz 0:041c1089b6e5 108 }
dnikolz 0:041c1089b6e5 109 pc.printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major, pVersion->minor ,pVersion->build, pVersion->revision);
dnikolz 0:041c1089b6e5 110
dnikolz 0:041c1089b6e5 111 int addr;
dnikolz 0:041c1089b6e5 112
dnikolz 0:041c1089b6e5 113 addr = VL53L0X_scan();
dnikolz 0:041c1089b6e5 114 printf("Device found at: %i\r\n", addr);
dnikolz 0:041c1089b6e5 115 //uint8_t data;
dnikolz 0:041c1089b6e5 116 //data=0;
dnikolz 0:041c1089b6e5 117 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 118 {
dnikolz 0:041c1089b6e5 119 printf ("Call of VL53L0X_DataInit\n");
dnikolz 0:041c1089b6e5 120 uint16_t osc_calibrate_val=0;
dnikolz 0:041c1089b6e5 121 Status = VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,&osc_calibrate_val);
dnikolz 0:041c1089b6e5 122 printf("%i\n",osc_calibrate_val);
dnikolz 0:041c1089b6e5 123 Status = VL53L0X_DataInit(&MyDevice); // Data initialization
dnikolz 0:041c1089b6e5 124 VL53Simplifier::print_pal_error(Status);
dnikolz 0:041c1089b6e5 125 }
dnikolz 0:041c1089b6e5 126
dnikolz 0:041c1089b6e5 127 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 128 {
dnikolz 0:041c1089b6e5 129 Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
dnikolz 0:041c1089b6e5 130 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 131 {
dnikolz 0:041c1089b6e5 132 printf("VL53L0X_GetDeviceInfo:\n");
dnikolz 0:041c1089b6e5 133 printf("Device Name : %s\n", DeviceInfo.Name);
dnikolz 0:041c1089b6e5 134 printf("Device Type : %s\n", DeviceInfo.Type);
dnikolz 0:041c1089b6e5 135 printf("Device ID : %s\n", DeviceInfo.ProductId);
dnikolz 0:041c1089b6e5 136 printf("ProductRevisionMajor : %d\n", DeviceInfo.ProductRevisionMajor);
dnikolz 0:041c1089b6e5 137 printf("ProductRevisionMinor : %d\n", DeviceInfo.ProductRevisionMinor);
dnikolz 0:041c1089b6e5 138
dnikolz 0:041c1089b6e5 139 if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) {
dnikolz 0:041c1089b6e5 140 printf("Error expected cut 1.1 but found cut %d.%d\n",
dnikolz 0:041c1089b6e5 141 DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor);
dnikolz 0:041c1089b6e5 142 Status = VL53L0X_ERROR_NOT_SUPPORTED;
dnikolz 0:041c1089b6e5 143 }
dnikolz 0:041c1089b6e5 144 }
dnikolz 0:041c1089b6e5 145 VL53Simplifier::print_pal_error(Status);
dnikolz 0:041c1089b6e5 146 }
dnikolz 0:041c1089b6e5 147
dnikolz 0:041c1089b6e5 148 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
dnikolz 0:041c1089b6e5 149 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData = &RangingMeasurementData;
dnikolz 0:041c1089b6e5 150 Status = VL53L0X_ERROR_NONE;
dnikolz 0:041c1089b6e5 151 uint32_t refSpadCount;
dnikolz 0:041c1089b6e5 152 uint8_t isApertureSpads;
dnikolz 0:041c1089b6e5 153 uint8_t VhvSettings;
dnikolz 0:041c1089b6e5 154 uint8_t PhaseCal;
dnikolz 0:041c1089b6e5 155
dnikolz 0:041c1089b6e5 156 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 157 {
dnikolz 0:041c1089b6e5 158 printf ("Call of VL53L0X_StaticInit\n");
dnikolz 0:041c1089b6e5 159 Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
dnikolz 0:041c1089b6e5 160 // StaticInit will set interrupt by default
dnikolz 0:041c1089b6e5 161 VL53Simplifier::print_pal_error(Status);
dnikolz 0:041c1089b6e5 162 }
dnikolz 0:041c1089b6e5 163
dnikolz 0:041c1089b6e5 164 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 165 {
dnikolz 0:041c1089b6e5 166 printf ("Call of VL53L0X_PerformRefCalibration\n");
dnikolz 0:041c1089b6e5 167 Status = VL53L0X_PerformRefCalibration(pMyDevice,
dnikolz 0:041c1089b6e5 168 &VhvSettings, &PhaseCal); // Device Initialization
dnikolz 0:041c1089b6e5 169 VL53Simplifier::print_pal_error(Status);
dnikolz 0:041c1089b6e5 170 }
dnikolz 0:041c1089b6e5 171
dnikolz 0:041c1089b6e5 172 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 173 {
dnikolz 0:041c1089b6e5 174 printf ("Call of VL53L0X_PerformRefSpadManagement\n");
dnikolz 0:041c1089b6e5 175 Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
dnikolz 0:041c1089b6e5 176 &refSpadCount, &isApertureSpads); // Device Initialization
dnikolz 0:041c1089b6e5 177 VL53Simplifier::print_pal_error(Status);
dnikolz 0:041c1089b6e5 178 }
dnikolz 0:041c1089b6e5 179
dnikolz 0:041c1089b6e5 180 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 181 {
dnikolz 0:041c1089b6e5 182
dnikolz 0:041c1089b6e5 183 printf ("Call of VL53L0X_SetDeviceMode\n");
dnikolz 0:041c1089b6e5 184 Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode
dnikolz 0:041c1089b6e5 185 VL53Simplifier::print_pal_error(Status);
dnikolz 0:041c1089b6e5 186 }
dnikolz 0:041c1089b6e5 187
dnikolz 0:041c1089b6e5 188 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 189 {
dnikolz 0:041c1089b6e5 190 printf ("Call of VL53L0X_StartMeasurement\n");
dnikolz 0:041c1089b6e5 191 Status = VL53L0X_StartMeasurement(pMyDevice);
dnikolz 0:041c1089b6e5 192 VL53Simplifier::print_pal_error(Status);
dnikolz 0:041c1089b6e5 193 }
dnikolz 0:041c1089b6e5 194
dnikolz 0:041c1089b6e5 195 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 196 {
dnikolz 0:041c1089b6e5 197 uint32_t measurement;
dnikolz 0:041c1089b6e5 198 uint32_t no_of_measurements = 32;
dnikolz 0:041c1089b6e5 199
dnikolz 0:041c1089b6e5 200 uint16_t* pResults = (uint16_t*)malloc(sizeof(uint16_t) * no_of_measurements);
dnikolz 0:041c1089b6e5 201
dnikolz 0:041c1089b6e5 202 for(measurement=0; measurement<no_of_measurements; measurement++)
dnikolz 0:041c1089b6e5 203 {
dnikolz 0:041c1089b6e5 204
dnikolz 0:041c1089b6e5 205 Status = VL53Simplifier::WaitMeasurementDataReady(pMyDevice);
dnikolz 0:041c1089b6e5 206
dnikolz 0:041c1089b6e5 207 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 208 {
dnikolz 0:041c1089b6e5 209 Status = VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData);
dnikolz 0:041c1089b6e5 210
dnikolz 0:041c1089b6e5 211 *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter;
dnikolz 0:041c1089b6e5 212 printf("In loop measurement %lu: %d\n", measurement, pRangingMeasurementData->RangeMilliMeter);
dnikolz 0:041c1089b6e5 213
dnikolz 0:041c1089b6e5 214 // Clear the interrupt
dnikolz 0:041c1089b6e5 215 VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
dnikolz 0:041c1089b6e5 216 VL53L0X_PollingDelay(pMyDevice);
dnikolz 0:041c1089b6e5 217 } else {
dnikolz 0:041c1089b6e5 218 break;
dnikolz 0:041c1089b6e5 219 }
dnikolz 0:041c1089b6e5 220 }
dnikolz 0:041c1089b6e5 221
dnikolz 0:041c1089b6e5 222 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 223 {
dnikolz 0:041c1089b6e5 224 for(measurement=0; measurement<no_of_measurements; measurement++)
dnikolz 0:041c1089b6e5 225 {
dnikolz 0:041c1089b6e5 226 printf("measurement %lu: %d\n", measurement, *(pResults + measurement));
dnikolz 0:041c1089b6e5 227 }
dnikolz 0:041c1089b6e5 228 }
dnikolz 0:041c1089b6e5 229
dnikolz 0:041c1089b6e5 230 free(pResults);
dnikolz 0:041c1089b6e5 231 }
dnikolz 0:041c1089b6e5 232
dnikolz 0:041c1089b6e5 233
dnikolz 0:041c1089b6e5 234 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 235 {
dnikolz 0:041c1089b6e5 236 printf ("Call of VL53L0X_StopMeasurement\n");
dnikolz 0:041c1089b6e5 237 Status = VL53L0X_StopMeasurement(pMyDevice);
dnikolz 0:041c1089b6e5 238 }
dnikolz 0:041c1089b6e5 239
dnikolz 0:041c1089b6e5 240 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 241 {
dnikolz 0:041c1089b6e5 242 printf ("Wait Stop to be competed\n");
dnikolz 0:041c1089b6e5 243 Status = VL53Simplifier::WaitStopCompleted(pMyDevice);
dnikolz 0:041c1089b6e5 244 }
dnikolz 0:041c1089b6e5 245
dnikolz 0:041c1089b6e5 246 if(Status == VL53L0X_ERROR_NONE)
dnikolz 0:041c1089b6e5 247 Status = VL53L0X_ClearInterruptMask(pMyDevice,
dnikolz 0:041c1089b6e5 248 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
dnikolz 0:041c1089b6e5 249 while (true)
dnikolz 0:041c1089b6e5 250 if (pc.readable())
dnikolz 0:041c1089b6e5 251 pc.putc(pc.getc());
dnikolz 0:041c1089b6e5 252 }