charles macneill / Mbed OS VL53L3CX_Shield_3Sensors_Polling

Sample Program to demonstrate use of three VL53L3CX sensors on F401 and x-nucleo board. Uses polling. MBed 6. Uses Ayoub's Ranging library.

Dependencies:   X_NUCLEO_53L3CX

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?

UserRevisionLine numberNew 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