Sample Program to demonstrate use of three VL53L3CX sensors on F401 and x-nucleo board. Uses polling. MBed 6. Uses Ayoub's Ranging library.
main.cpp@2:b1af0870b56a, 2021-07-21 (annotated)
- Committer:
- charlesmn
- Date:
- Wed Jul 21 09:43:30 2021 +0000
- Revision:
- 2:b1af0870b56a
- Parent:
- 1:638ad0a69df4
- Child:
- 3:9129850679e6
Correct NumberOfZones issue and fix printing of results
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
charlesmn | 0:619be236cb7c | 1 | /* |
charlesmn | 0:619be236cb7c | 2 | * This VL53L3 Expansion board test application performs range measurements |
charlesmn | 1:638ad0a69df4 | 3 | * using the onboard embedded sensor, and satellite boards, in polling mode. |
charlesmn | 0:619be236cb7c | 4 | * Measured ranges are output on the Serial Port, running at 115200 baud. |
charlesmn | 0:619be236cb7c | 5 | * |
charlesmn | 0:619be236cb7c | 6 | * The Reset button can be used to restart the program. |
charlesmn | 0:619be236cb7c | 7 | * |
charlesmn | 0:619be236cb7c | 8 | * *** Note : |
charlesmn | 0:619be236cb7c | 9 | * Default Mbed build system settings disable printf floating-point support. |
charlesmn | 0:619be236cb7c | 10 | * Online builds seem unable to configure this. |
charlesmn | 0:619be236cb7c | 11 | * Offline builds can enable printf floating-point support. |
charlesmn | 0:619be236cb7c | 12 | * https://github.com/ARMmbed/mbed-os/blob/master/platform/source/minimal-printf/README.md |
charlesmn | 0:619be236cb7c | 13 | * .\mbed-os\platform\mbed_lib.json |
charlesmn | 0:619be236cb7c | 14 | * |
charlesmn | 0:619be236cb7c | 15 | */ |
charlesmn | 0:619be236cb7c | 16 | |
charlesmn | 0:619be236cb7c | 17 | #include <stdio.h> |
charlesmn | 0:619be236cb7c | 18 | #include <time.h> |
charlesmn | 0:619be236cb7c | 19 | |
charlesmn | 0:619be236cb7c | 20 | #include "mbed.h" |
charlesmn | 0:619be236cb7c | 21 | |
charlesmn | 0:619be236cb7c | 22 | #include "vl53L3_I2c.h" |
charlesmn | 0:619be236cb7c | 23 | #include "vl53lx_platform_user_data.h" |
charlesmn | 0:619be236cb7c | 24 | #include "53l3a2_ranging_sensor.h" |
charlesmn | 0:619be236cb7c | 25 | #include "VL53L3A2_RangingClass.h" |
charlesmn | 0:619be236cb7c | 26 | |
charlesmn | 0:619be236cb7c | 27 | #define BSP_ERROR_NONE 0 |
charlesmn | 0:619be236cb7c | 28 | |
charlesmn | 0:619be236cb7c | 29 | |
charlesmn | 0:619be236cb7c | 30 | #if (MBED_VERSION > 60300) |
charlesmn | 0:619be236cb7c | 31 | UnbufferedSerial pc(USBTX, USBRX); |
charlesmn | 0:619be236cb7c | 32 | extern "C" void wait_ms(int ms); |
charlesmn | 0:619be236cb7c | 33 | #else |
charlesmn | 0:619be236cb7c | 34 | Serial pc(SERIAL_TX, SERIAL_RX); |
charlesmn | 0:619be236cb7c | 35 | #endif |
charlesmn | 0:619be236cb7c | 36 | |
charlesmn | 0:619be236cb7c | 37 | VL53L3A2_SENSOR *sensor; //class for sensor commands |
charlesmn | 0:619be236cb7c | 38 | |
charlesmn | 0:619be236cb7c | 39 | /* Private define ------------------------------------------------------------*/ |
charlesmn | 0:619be236cb7c | 40 | #define POLLING_PERIOD (250U) |
charlesmn | 0:619be236cb7c | 41 | |
charlesmn | 1:638ad0a69df4 | 42 | #define SENSOR_LEFT_NUMBER 0 |
charlesmn | 1:638ad0a69df4 | 43 | #define SENSOR_CENTRE_NUMBER 1 |
charlesmn | 1:638ad0a69df4 | 44 | #define SENSOR_RIGHT_NUMBER 2 |
charlesmn | 1:638ad0a69df4 | 45 | |
charlesmn | 0:619be236cb7c | 46 | /* Private variables ---------------------------------------------------------*/ |
charlesmn | 0:619be236cb7c | 47 | static int32_t status = 0; |
charlesmn | 0:619be236cb7c | 48 | volatile uint8_t ToF_EventDetected = 0; |
charlesmn | 0:619be236cb7c | 49 | static uint8_t ToF_Present[RANGING_SENSOR_INSTANCES_NBR] = {0}; |
charlesmn | 0:619be236cb7c | 50 | |
charlesmn | 0:619be236cb7c | 51 | |
charlesmn | 1:638ad0a69df4 | 52 | static const char *TofDevStr[] = { |
charlesmn | 0:619be236cb7c | 53 | [VL53L3A2_DEV_LEFT] = "LEFT", |
charlesmn | 0:619be236cb7c | 54 | [VL53L3A2_DEV_CENTER] = "CENTER", |
charlesmn | 0:619be236cb7c | 55 | [VL53L3A2_DEV_RIGHT] = "RIGHT" |
charlesmn | 0:619be236cb7c | 56 | }; |
charlesmn | 0:619be236cb7c | 57 | |
charlesmn | 0:619be236cb7c | 58 | /* Private function prototypes -----------------------------------------------*/ |
charlesmn | 0:619be236cb7c | 59 | static void MX_53L3A2_MultiSensorRanging_Init(void); |
charlesmn | 0:619be236cb7c | 60 | static void MX_53L3A2_MultiSensorRanging_Process(void); |
charlesmn | 2:b1af0870b56a | 61 | static void print_result(uint8_t ToF_sensor,RANGING_SENSOR_Result_t *Result); |
charlesmn | 0:619be236cb7c | 62 | |
charlesmn | 0:619be236cb7c | 63 | |
charlesmn | 0:619be236cb7c | 64 | static void MX_53L3A2_MultiSensorRanging_Init(void) |
charlesmn | 0:619be236cb7c | 65 | { |
charlesmn | 1:638ad0a69df4 | 66 | uint8_t ToF_sensor; |
charlesmn | 1:638ad0a69df4 | 67 | uint16_t i2c_addr; |
charlesmn | 1:638ad0a69df4 | 68 | uint32_t id; |
charlesmn | 1:638ad0a69df4 | 69 | |
charlesmn | 1:638ad0a69df4 | 70 | // shut down all sensors |
charlesmn | 1:638ad0a69df4 | 71 | for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) { |
charlesmn | 1:638ad0a69df4 | 72 | sensor->VL53L3A2_RANGING_SetPowerMode(ToF_sensor, RANGING_SENSOR_POWERMODE_OFF); |
charlesmn | 1:638ad0a69df4 | 73 | ToF_Present[ToF_sensor] = 1; |
charlesmn | 0:619be236cb7c | 74 | } |
charlesmn | 1:638ad0a69df4 | 75 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 76 | |
charlesmn | 0:619be236cb7c | 77 | |
charlesmn | 1:638ad0a69df4 | 78 | printf("53L3A2 Multi Sensor Ranging demo application\n"); |
charlesmn | 0:619be236cb7c | 79 | |
charlesmn | 1:638ad0a69df4 | 80 | /* power on the devices one at a time, initialize them and change their address. |
charlesmn | 1:638ad0a69df4 | 81 | */ |
charlesmn | 1:638ad0a69df4 | 82 | for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) { |
charlesmn | 1:638ad0a69df4 | 83 | printf("VL53L3A2_RANGING_SENSOR_Init\n"); |
charlesmn | 1:638ad0a69df4 | 84 | sensor->VL53L3A2_RANGING_Init(ToF_sensor); |
charlesmn | 1:638ad0a69df4 | 85 | if (status) { |
charlesmn | 1:638ad0a69df4 | 86 | ToF_Present[ToF_sensor] = 0; |
charlesmn | 1:638ad0a69df4 | 87 | printf("VL53L3A2_RANGING_SENSOR_Init failed for sensor %d \n",ToF_sensor); |
charlesmn | 1:638ad0a69df4 | 88 | } |
charlesmn | 1:638ad0a69df4 | 89 | // give each sensor a unique address |
charlesmn | 1:638ad0a69df4 | 90 | printf("VL53L3A2_RANGING_SENSOR_SetAddress\n"); |
charlesmn | 0:619be236cb7c | 91 | wait_ms(100); |
charlesmn | 1:638ad0a69df4 | 92 | i2c_addr = (0x52 + (ToF_sensor + 1) * 2); /* 0x54, 0x56, 0x58 */ |
charlesmn | 1:638ad0a69df4 | 93 | sensor->VL53L3A2_RANGING_SetAddress(ToF_sensor, i2c_addr); |
charlesmn | 1:638ad0a69df4 | 94 | } |
charlesmn | 0:619be236cb7c | 95 | |
charlesmn | 1:638ad0a69df4 | 96 | wait_ms(100); |
charlesmn | 1:638ad0a69df4 | 97 | printf("53L3A2 Multi Sensor Ranging demo application2\n"); |
charlesmn | 0:619be236cb7c | 98 | } |
charlesmn | 0:619be236cb7c | 99 | |
charlesmn | 0:619be236cb7c | 100 | |
charlesmn | 0:619be236cb7c | 101 | |
charlesmn | 0:619be236cb7c | 102 | static void MX_53L3A2_MultiSensorRanging_Process(void) |
charlesmn | 0:619be236cb7c | 103 | { |
charlesmn | 1:638ad0a69df4 | 104 | uint8_t ToF_sensor; |
charlesmn | 1:638ad0a69df4 | 105 | uint32_t id; |
charlesmn | 0:619be236cb7c | 106 | |
charlesmn | 1:638ad0a69df4 | 107 | RANGING_SENSOR_Result_t Result; |
charlesmn | 1:638ad0a69df4 | 108 | RANGING_SENSOR_ProfileConfig_t Profile; |
charlesmn | 1:638ad0a69df4 | 109 | printf("MX_53L3A2_MultiSensorRanging_Process\n"); |
charlesmn | 1:638ad0a69df4 | 110 | Profile.RangingProfile = RS_MULTI_TARGET_LONG_RANGE; |
charlesmn | 1:638ad0a69df4 | 111 | Profile.TimingBudget = 30; /* 16 ms < TimingBudget < 500 ms */ |
charlesmn | 1:638ad0a69df4 | 112 | Profile.Frequency = 0; /* not necessary in simple ranging */ |
charlesmn | 1:638ad0a69df4 | 113 | Profile.EnableAmbient = 0; /* Enable: 1, Disable: 0 */ |
charlesmn | 1:638ad0a69df4 | 114 | Profile.EnableSignal = 0; /* Enable: 1, Disable: 0 */ |
charlesmn | 1:638ad0a69df4 | 115 | Profile.EnableInterrupt = 0; // disables interupts |
charlesmn | 1:638ad0a69df4 | 116 | printf("MX_53L3A2_MultiSensorRanging_Process start sensors\n"); |
charlesmn | 1:638ad0a69df4 | 117 | for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) { |
charlesmn | 1:638ad0a69df4 | 118 | /* skip this device if not detected */ |
charlesmn | 1:638ad0a69df4 | 119 | if (ToF_Present[ToF_sensor] != 1) { |
charlesmn | 1:638ad0a69df4 | 120 | continue; |
charlesmn | 1:638ad0a69df4 | 121 | } |
charlesmn | 1:638ad0a69df4 | 122 | |
charlesmn | 1:638ad0a69df4 | 123 | wait_ms(100); |
charlesmn | 1:638ad0a69df4 | 124 | sensor->VL53L3A2_RANGING_ConfigProfile(ToF_sensor, &Profile); |
charlesmn | 1:638ad0a69df4 | 125 | if (status != BSP_ERROR_NONE) { |
charlesmn | 1:638ad0a69df4 | 126 | printf("VL53L3A2_RANGING_SENSOR_ConfigProfile failed sensor %d status %d\n",ToF_sensor,status); |
charlesmn | 1:638ad0a69df4 | 127 | } |
charlesmn | 1:638ad0a69df4 | 128 | wait_ms(100); |
charlesmn | 1:638ad0a69df4 | 129 | status = sensor->VL53L3A2_RANGING_Start(ToF_sensor, RS_MODE_BLOCKING_CONTINUOUS); |
charlesmn | 1:638ad0a69df4 | 130 | if (status != BSP_ERROR_NONE) { |
charlesmn | 1:638ad0a69df4 | 131 | printf("VL53L3A2_RANGING_SENSOR_Start failed sensor %d status %d\n",ToF_sensor,status); |
charlesmn | 1:638ad0a69df4 | 132 | } else { |
charlesmn | 1:638ad0a69df4 | 133 | printf("VL53L3A2_RANGING_SENSOR_Start started %d\n",ToF_sensor); |
charlesmn | 1:638ad0a69df4 | 134 | } |
charlesmn | 0:619be236cb7c | 135 | } |
charlesmn | 1:638ad0a69df4 | 136 | printf("MX_53L3A2_MultiSensorRanging_Process read sensors\n"); |
charlesmn | 1:638ad0a69df4 | 137 | // repeatedly read data and start next measurement |
charlesmn | 1:638ad0a69df4 | 138 | while (1) { |
charlesmn | 1:638ad0a69df4 | 139 | /* polling mode */ |
charlesmn | 1:638ad0a69df4 | 140 | for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) { |
charlesmn | 1:638ad0a69df4 | 141 | status = sensor->VL53L3A2_RANGING_GetDistance(ToF_sensor, &Result); |
charlesmn | 1:638ad0a69df4 | 142 | if ((status == BSP_ERROR_NONE) && |
charlesmn | 2:b1af0870b56a | 143 | (Result.ZoneResult[0].NumberOfTargets != 0)) |
charlesmn | 1:638ad0a69df4 | 144 | { |
charlesmn | 2:b1af0870b56a | 145 | print_result(ToF_sensor,&Result); |
charlesmn | 1:638ad0a69df4 | 146 | } |
charlesmn | 1:638ad0a69df4 | 147 | } |
charlesmn | 1:638ad0a69df4 | 148 | wait_ms(POLLING_PERIOD); |
charlesmn | 0:619be236cb7c | 149 | } |
charlesmn | 0:619be236cb7c | 150 | } |
charlesmn | 0:619be236cb7c | 151 | |
charlesmn | 0:619be236cb7c | 152 | |
charlesmn | 2:b1af0870b56a | 153 | // Note in VL53L3CX there is only one Zone so NumberOfZones is not used and is always assumed to be 1 |
charlesmn | 2:b1af0870b56a | 154 | static void print_result(uint8_t ToF_sensor,RANGING_SENSOR_Result_t *Result) |
charlesmn | 0:619be236cb7c | 155 | { |
charlesmn | 2:b1af0870b56a | 156 | uint8_t zone = 0; // in VL53L3CX there is only one zone |
charlesmn | 2:b1af0870b56a | 157 | uint8_t target = 0; |
charlesmn | 1:638ad0a69df4 | 158 | |
charlesmn | 2:b1af0870b56a | 159 | for (target = 0; target < Result->ZoneResult[zone].NumberOfTargets; target++) { |
charlesmn | 2:b1af0870b56a | 160 | if (Result->ZoneResult[zone].Status[target] == VL53LX_RANGESTATUS_RANGE_VALID ) |
charlesmn | 2:b1af0870b56a | 161 | { |
charlesmn | 2:b1af0870b56a | 162 | printf("\n%s\t - ", TofDevStr[ToF_sensor]); |
charlesmn | 2:b1af0870b56a | 163 | printf(" |---> "); |
charlesmn | 2:b1af0870b56a | 164 | printf("Status = %d, Target %d, Distance = %5d mm", |
charlesmn | 2:b1af0870b56a | 165 | Result->ZoneResult[zone].Status[target], |
charlesmn | 2:b1af0870b56a | 166 | target, |
charlesmn | 2:b1af0870b56a | 167 | Result->ZoneResult[zone].Distance[target]); |
charlesmn | 2:b1af0870b56a | 168 | } |
charlesmn | 1:638ad0a69df4 | 169 | |
charlesmn | 1:638ad0a69df4 | 170 | } |
charlesmn | 0:619be236cb7c | 171 | } |
charlesmn | 1:638ad0a69df4 | 172 | |
charlesmn | 0:619be236cb7c | 173 | |
charlesmn | 0:619be236cb7c | 174 | |
charlesmn | 0:619be236cb7c | 175 | /*=================================== Main ================================== |
charlesmn | 0:619be236cb7c | 176 | =============================================================================*/ |
charlesmn | 0:619be236cb7c | 177 | int main() |
charlesmn | 0:619be236cb7c | 178 | { |
charlesmn | 1:638ad0a69df4 | 179 | |
charlesmn | 1:638ad0a69df4 | 180 | pc.baud(115200); // baud rate is important as printf statements take a lot of time |
charlesmn | 1:638ad0a69df4 | 181 | |
charlesmn | 1:638ad0a69df4 | 182 | sensor = new VL53L3A2_SENSOR(); |
charlesmn | 0:619be236cb7c | 183 | |
charlesmn | 1:638ad0a69df4 | 184 | printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\r\n"); |
charlesmn | 1:638ad0a69df4 | 185 | printf("VL53L3CX_Shield_3Sensors_Polling\n"); |
charlesmn | 0:619be236cb7c | 186 | |
charlesmn | 1:638ad0a69df4 | 187 | MX_53L3A2_MultiSensorRanging_Init(); |
charlesmn | 1:638ad0a69df4 | 188 | |
charlesmn | 1:638ad0a69df4 | 189 | while (1) { |
charlesmn | 1:638ad0a69df4 | 190 | MX_53L3A2_MultiSensorRanging_Process(); |
charlesmn | 1:638ad0a69df4 | 191 | } |
charlesmn | 0:619be236cb7c | 192 | |
charlesmn | 0:619be236cb7c | 193 | } |
charlesmn | 0:619be236cb7c | 194 | |
charlesmn | 0:619be236cb7c | 195 | #if (MBED_VERSION > 60300) |
charlesmn | 0:619be236cb7c | 196 | extern "C" void wait_ms(int ms) |
charlesmn | 0:619be236cb7c | 197 | { |
charlesmn | 0:619be236cb7c | 198 | thread_sleep_for(ms); |
charlesmn | 0:619be236cb7c | 199 | } |
charlesmn | 0:619be236cb7c | 200 | #endif |
charlesmn | 1:638ad0a69df4 | 201 |