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@0:619be236cb7c, 2021-07-14 (annotated)
- Committer:
- charlesmn
- Date:
- Wed Jul 14 16:25:11 2021 +0000
- Revision:
- 0:619be236cb7c
- Child:
- 1:638ad0a69df4
Using Ayoubs Ranging library. Mbed6.
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 | 0:619be236cb7c | 3 | * using the onboard embedded sensor, and satellite boards, in interrupt 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 | |
charlesmn | 0:619be236cb7c | 38 | #include "53l3a2_ranging_sensor.h" |
charlesmn | 0:619be236cb7c | 39 | |
charlesmn | 0:619be236cb7c | 40 | VL53L3A2_SENSOR *sensor; //class for sensor commands |
charlesmn | 0:619be236cb7c | 41 | |
charlesmn | 0:619be236cb7c | 42 | |
charlesmn | 0:619be236cb7c | 43 | /* Private typedef -----------------------------------------------------------*/ |
charlesmn | 0:619be236cb7c | 44 | |
charlesmn | 0:619be236cb7c | 45 | /* Private define ------------------------------------------------------------*/ |
charlesmn | 0:619be236cb7c | 46 | #define POLLING_PERIOD (250U) |
charlesmn | 0:619be236cb7c | 47 | |
charlesmn | 0:619be236cb7c | 48 | /* Private variables ---------------------------------------------------------*/ |
charlesmn | 0:619be236cb7c | 49 | static int32_t status = 0; |
charlesmn | 0:619be236cb7c | 50 | volatile uint8_t ToF_EventDetected = 0; |
charlesmn | 0:619be236cb7c | 51 | static uint8_t ToF_Present[RANGING_SENSOR_INSTANCES_NBR] = {0}; |
charlesmn | 0:619be236cb7c | 52 | |
charlesmn | 0:619be236cb7c | 53 | |
charlesmn | 0:619be236cb7c | 54 | static const char *TofDevStr[] = |
charlesmn | 0:619be236cb7c | 55 | { |
charlesmn | 0:619be236cb7c | 56 | [VL53L3A2_DEV_LEFT] = "LEFT", |
charlesmn | 0:619be236cb7c | 57 | [VL53L3A2_DEV_CENTER] = "CENTER", |
charlesmn | 0:619be236cb7c | 58 | [VL53L3A2_DEV_RIGHT] = "RIGHT" |
charlesmn | 0:619be236cb7c | 59 | }; |
charlesmn | 0:619be236cb7c | 60 | |
charlesmn | 0:619be236cb7c | 61 | /* Private function prototypes -----------------------------------------------*/ |
charlesmn | 0:619be236cb7c | 62 | static void MX_53L3A2_MultiSensorRanging_Init(void); |
charlesmn | 0:619be236cb7c | 63 | static void MX_53L3A2_MultiSensorRanging_Process(void); |
charlesmn | 0:619be236cb7c | 64 | static void print_result(RANGING_SENSOR_Result_t *Result); |
charlesmn | 0:619be236cb7c | 65 | |
charlesmn | 0:619be236cb7c | 66 | |
charlesmn | 0:619be236cb7c | 67 | static void MX_53L3A2_MultiSensorRanging_Init(void) |
charlesmn | 0:619be236cb7c | 68 | { |
charlesmn | 0:619be236cb7c | 69 | uint8_t ToF_sensor; |
charlesmn | 0:619be236cb7c | 70 | uint16_t i2c_addr; |
charlesmn | 0:619be236cb7c | 71 | uint32_t id; |
charlesmn | 0:619be236cb7c | 72 | |
charlesmn | 0:619be236cb7c | 73 | // shut down all sensors |
charlesmn | 0:619be236cb7c | 74 | for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) |
charlesmn | 0:619be236cb7c | 75 | { |
charlesmn | 0:619be236cb7c | 76 | sensor->VL53L3A2_RANGING_SetPowerMode(ToF_sensor, RANGING_SENSOR_POWERMODE_OFF); |
charlesmn | 0:619be236cb7c | 77 | ToF_Present[ToF_sensor] = 1; |
charlesmn | 0:619be236cb7c | 78 | } |
charlesmn | 0:619be236cb7c | 79 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 80 | |
charlesmn | 0:619be236cb7c | 81 | |
charlesmn | 0:619be236cb7c | 82 | printf("53L3A2 Multi Sensor Ranging demo application\n"); |
charlesmn | 0:619be236cb7c | 83 | |
charlesmn | 0:619be236cb7c | 84 | /* power on the devices one at a time, initialize them and change their address. |
charlesmn | 0:619be236cb7c | 85 | * once the address is updated, the communication with the devices is checked |
charlesmn | 0:619be236cb7c | 86 | * reading its ID. |
charlesmn | 0:619be236cb7c | 87 | */ |
charlesmn | 0:619be236cb7c | 88 | for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) |
charlesmn | 0:619be236cb7c | 89 | { |
charlesmn | 0:619be236cb7c | 90 | printf("VL53L3A2_RANGING_SENSOR_SetPowerMode on\n"); |
charlesmn | 0:619be236cb7c | 91 | sensor->VL53L3A2_RANGING_SetPowerMode(ToF_sensor, RANGING_SENSOR_POWERMODE_ON); |
charlesmn | 0:619be236cb7c | 92 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 93 | |
charlesmn | 0:619be236cb7c | 94 | printf("VL53L3A2_RANGING_SENSOR_Init\n"); |
charlesmn | 0:619be236cb7c | 95 | sensor->VL53L3A2_RANGING_Init(ToF_sensor); |
charlesmn | 0:619be236cb7c | 96 | if (status) |
charlesmn | 0:619be236cb7c | 97 | { |
charlesmn | 0:619be236cb7c | 98 | ToF_Present[ToF_sensor] = 0; |
charlesmn | 0:619be236cb7c | 99 | printf("VL53L3A2_RANGING_SENSOR_Init failed for sensor %d \n",ToF_sensor); |
charlesmn | 0:619be236cb7c | 100 | } |
charlesmn | 0:619be236cb7c | 101 | printf("VL53L3A2_RANGING_SENSOR_SetAddress\n"); |
charlesmn | 0:619be236cb7c | 102 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 103 | i2c_addr = (0x52 + (ToF_sensor + 1) * 2); /* 0x54, 0x56, 0x58 */ |
charlesmn | 0:619be236cb7c | 104 | sensor->VL53L3A2_RANGING_SetAddress(ToF_sensor, i2c_addr); |
charlesmn | 0:619be236cb7c | 105 | printf("VL53L3A2_RANGING_SENSOR_ReadID\n"); |
charlesmn | 0:619be236cb7c | 106 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 107 | id =0; |
charlesmn | 0:619be236cb7c | 108 | |
charlesmn | 0:619be236cb7c | 109 | sensor->VL53L3A2_RANGING_ReadID(ToF_sensor, &id); |
charlesmn | 0:619be236cb7c | 110 | printf("#######ToF sensor %d - ID: %04X\n", 0, id); |
charlesmn | 0:619be236cb7c | 111 | } |
charlesmn | 0:619be236cb7c | 112 | |
charlesmn | 0:619be236cb7c | 113 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 114 | printf("53L3A2 Multi Sensor Ranging demo application2\n"); |
charlesmn | 0:619be236cb7c | 115 | } |
charlesmn | 0:619be236cb7c | 116 | |
charlesmn | 0:619be236cb7c | 117 | |
charlesmn | 0:619be236cb7c | 118 | |
charlesmn | 0:619be236cb7c | 119 | static void MX_53L3A2_MultiSensorRanging_Process(void) |
charlesmn | 0:619be236cb7c | 120 | { |
charlesmn | 0:619be236cb7c | 121 | uint8_t i; |
charlesmn | 0:619be236cb7c | 122 | uint32_t id; |
charlesmn | 0:619be236cb7c | 123 | |
charlesmn | 0:619be236cb7c | 124 | RANGING_SENSOR_Result_t Result; |
charlesmn | 0:619be236cb7c | 125 | RANGING_SENSOR_ProfileConfig_t Profile; |
charlesmn | 0:619be236cb7c | 126 | printf("MX_53L3A2_MultiSensorRanging_Process\n"); |
charlesmn | 0:619be236cb7c | 127 | Profile.RangingProfile = RS_MULTI_TARGET_LONG_RANGE; |
charlesmn | 0:619be236cb7c | 128 | Profile.TimingBudget = 30; /* 16 ms < TimingBudget < 500 ms */ |
charlesmn | 0:619be236cb7c | 129 | Profile.Frequency = 0; /* not necessary in simple ranging */ |
charlesmn | 0:619be236cb7c | 130 | Profile.EnableAmbient = 0; /* Enable: 1, Disable: 0 */ |
charlesmn | 0:619be236cb7c | 131 | Profile.EnableSignal = 0; /* Enable: 1, Disable: 0 */ |
charlesmn | 0:619be236cb7c | 132 | printf("MX_53L3A2_MultiSensorRanging_Process start sensors\n"); |
charlesmn | 0:619be236cb7c | 133 | for (i = 0; i < RANGING_SENSOR_INSTANCES_NBR; i++) |
charlesmn | 0:619be236cb7c | 134 | { |
charlesmn | 0:619be236cb7c | 135 | /* skip this device if not detected */ |
charlesmn | 0:619be236cb7c | 136 | if (ToF_Present[i] != 1) |
charlesmn | 0:619be236cb7c | 137 | { |
charlesmn | 0:619be236cb7c | 138 | continue; |
charlesmn | 0:619be236cb7c | 139 | } |
charlesmn | 0:619be236cb7c | 140 | |
charlesmn | 0:619be236cb7c | 141 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 142 | sensor->VL53L3A2_RANGING_ConfigProfile(i, &Profile); |
charlesmn | 0:619be236cb7c | 143 | if (status != BSP_ERROR_NONE) |
charlesmn | 0:619be236cb7c | 144 | { |
charlesmn | 0:619be236cb7c | 145 | printf("VL53L3A2_RANGING_SENSOR_ConfigProfile failed sensor %d status %d\n",i,status); |
charlesmn | 0:619be236cb7c | 146 | } |
charlesmn | 0:619be236cb7c | 147 | wait_ms(100); |
charlesmn | 0:619be236cb7c | 148 | status = sensor->VL53L3A2_RANGING_Start(i, RS_MODE_BLOCKING_CONTINUOUS); |
charlesmn | 0:619be236cb7c | 149 | if (status != BSP_ERROR_NONE) |
charlesmn | 0:619be236cb7c | 150 | { |
charlesmn | 0:619be236cb7c | 151 | printf("VL53L3A2_RANGING_SENSOR_Start failed sensor %d status %d\n",i,status); |
charlesmn | 0:619be236cb7c | 152 | } |
charlesmn | 0:619be236cb7c | 153 | else |
charlesmn | 0:619be236cb7c | 154 | { |
charlesmn | 0:619be236cb7c | 155 | printf("VL53L3A2_RANGING_SENSOR_Start started %d\n",i); |
charlesmn | 0:619be236cb7c | 156 | } |
charlesmn | 0:619be236cb7c | 157 | } |
charlesmn | 0:619be236cb7c | 158 | printf("MX_53L3A2_MultiSensorRanging_Processread sensors\n"); |
charlesmn | 0:619be236cb7c | 159 | while (1) |
charlesmn | 0:619be236cb7c | 160 | { |
charlesmn | 0:619be236cb7c | 161 | /* polling mode */ |
charlesmn | 0:619be236cb7c | 162 | for (i = 0; i < RANGING_SENSOR_INSTANCES_NBR; i++) |
charlesmn | 0:619be236cb7c | 163 | { |
charlesmn | 0:619be236cb7c | 164 | status = sensor->VL53L3A2_RANGING_GetDistance(i, &Result); |
charlesmn | 0:619be236cb7c | 165 | if (status == BSP_ERROR_NONE) |
charlesmn | 0:619be236cb7c | 166 | { |
charlesmn | 0:619be236cb7c | 167 | printf("\n%s\t - ", TofDevStr[i]); |
charlesmn | 0:619be236cb7c | 168 | print_result(&Result); |
charlesmn | 0:619be236cb7c | 169 | } |
charlesmn | 0:619be236cb7c | 170 | else |
charlesmn | 0:619be236cb7c | 171 | { |
charlesmn | 0:619be236cb7c | 172 | printf("VL53L3A2_RANGING_SENSOR_GetDistance error %d \n",status); |
charlesmn | 0:619be236cb7c | 173 | } |
charlesmn | 0:619be236cb7c | 174 | } |
charlesmn | 0:619be236cb7c | 175 | wait_ms(POLLING_PERIOD); |
charlesmn | 0:619be236cb7c | 176 | } |
charlesmn | 0:619be236cb7c | 177 | } |
charlesmn | 0:619be236cb7c | 178 | |
charlesmn | 0:619be236cb7c | 179 | |
charlesmn | 0:619be236cb7c | 180 | |
charlesmn | 0:619be236cb7c | 181 | static void print_result(RANGING_SENSOR_Result_t *Result) |
charlesmn | 0:619be236cb7c | 182 | { |
charlesmn | 0:619be236cb7c | 183 | uint8_t i, j; |
charlesmn | 0:619be236cb7c | 184 | |
charlesmn | 0:619be236cb7c | 185 | |
charlesmn | 0:619be236cb7c | 186 | for (i = 0; i < RANGING_SENSOR_MAX_NB_ZONES; i++) |
charlesmn | 0:619be236cb7c | 187 | { |
charlesmn | 0:619be236cb7c | 188 | |
charlesmn | 0:619be236cb7c | 189 | uint8_t target = 0; |
charlesmn | 0:619be236cb7c | 190 | |
charlesmn | 0:619be236cb7c | 191 | printf(" |---> "); |
charlesmn | 0:619be236cb7c | 192 | printf("Status = %d, Distance = %5d mm", |
charlesmn | 0:619be236cb7c | 193 | Result->ZoneResult[target].Status[j], |
charlesmn | 0:619be236cb7c | 194 | Result->ZoneResult[target].Distance[j]); |
charlesmn | 0:619be236cb7c | 195 | |
charlesmn | 0:619be236cb7c | 196 | } |
charlesmn | 0:619be236cb7c | 197 | } |
charlesmn | 0:619be236cb7c | 198 | |
charlesmn | 0:619be236cb7c | 199 | |
charlesmn | 0:619be236cb7c | 200 | |
charlesmn | 0:619be236cb7c | 201 | /*=================================== Main ================================== |
charlesmn | 0:619be236cb7c | 202 | =============================================================================*/ |
charlesmn | 0:619be236cb7c | 203 | int main() |
charlesmn | 0:619be236cb7c | 204 | { |
charlesmn | 0:619be236cb7c | 205 | |
charlesmn | 0:619be236cb7c | 206 | pc.baud(115200); // baud rate is important as printf statements take a lot of time |
charlesmn | 0:619be236cb7c | 207 | |
charlesmn | 0:619be236cb7c | 208 | sensor = new VL53L3A2_SENSOR(); |
charlesmn | 0:619be236cb7c | 209 | |
charlesmn | 0:619be236cb7c | 210 | printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\r\n"); |
charlesmn | 0:619be236cb7c | 211 | |
charlesmn | 0:619be236cb7c | 212 | MX_53L3A2_MultiSensorRanging_Init(); |
charlesmn | 0:619be236cb7c | 213 | |
charlesmn | 0:619be236cb7c | 214 | while (1) |
charlesmn | 0:619be236cb7c | 215 | { |
charlesmn | 0:619be236cb7c | 216 | MX_53L3A2_MultiSensorRanging_Process(); |
charlesmn | 0:619be236cb7c | 217 | } |
charlesmn | 0:619be236cb7c | 218 | |
charlesmn | 0:619be236cb7c | 219 | } |
charlesmn | 0:619be236cb7c | 220 | |
charlesmn | 0:619be236cb7c | 221 | #if (MBED_VERSION > 60300) |
charlesmn | 0:619be236cb7c | 222 | extern "C" void wait_ms(int ms) |
charlesmn | 0:619be236cb7c | 223 | { |
charlesmn | 0:619be236cb7c | 224 | thread_sleep_for(ms); |
charlesmn | 0:619be236cb7c | 225 | } |
charlesmn | 0:619be236cb7c | 226 | #endif |
charlesmn | 0:619be236cb7c | 227 |