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@4:f74c066a9d05, 2021-07-22 (annotated)

Committer:
charlesmn
Date:
Thu Jul 22 11:32:22 2021 +0000
Revision:
4:f74c066a9d05
Parent:
3:9129850679e6 
I had changed the name of X_NUCLEO_53L3CX but not the repository where it is store. This fixes.

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 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 3:9129850679e6 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 3:9129850679e6 57 */
charlesmn 3:9129850679e6 58 static const char *TofDevStr[] = {"LEFT","CENTER","RIGHT"};
charlesmn 0:619be236cb7c 59
charlesmn 0:619be236cb7c 60 /* Private function prototypes -----------------------------------------------*/
charlesmn 0:619be236cb7c 61 static void MX_53L3A2_MultiSensorRanging_Init(void);
charlesmn 0:619be236cb7c 62 static void MX_53L3A2_MultiSensorRanging_Process(void);
charlesmn 2:b1af0870b56a 63 static void print_result(uint8_t ToF_sensor,RANGING_SENSOR_Result_t *Result);
charlesmn 0:619be236cb7c 64
charlesmn 0:619be236cb7c 65
charlesmn 0:619be236cb7c 66 static void MX_53L3A2_MultiSensorRanging_Init(void)
charlesmn 0:619be236cb7c 67 {
charlesmn 1:638ad0a69df4 68 uint8_t ToF_sensor;
charlesmn 1:638ad0a69df4 69 uint16_t i2c_addr;
charlesmn 1:638ad0a69df4 70 uint32_t id;
charlesmn 1:638ad0a69df4 71
charlesmn 1:638ad0a69df4 72 // shut down all sensors
charlesmn 1:638ad0a69df4 73 for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
charlesmn 1:638ad0a69df4 74 sensor->VL53L3A2_RANGING_SetPowerMode(ToF_sensor, RANGING_SENSOR_POWERMODE_OFF);
charlesmn 1:638ad0a69df4 75 ToF_Present[ToF_sensor] = 1;
charlesmn 0:619be236cb7c 76 }
charlesmn 1:638ad0a69df4 77 wait_ms(100);
charlesmn 0:619be236cb7c 78
charlesmn 0:619be236cb7c 79
charlesmn 1:638ad0a69df4 80 printf("53L3A2 Multi Sensor Ranging demo application\n");
charlesmn 0:619be236cb7c 81
charlesmn 1:638ad0a69df4 82 /* power on the devices one at a time, initialize them and change their address.
charlesmn 1:638ad0a69df4 83 */
charlesmn 1:638ad0a69df4 84 for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
charlesmn 1:638ad0a69df4 85 printf("VL53L3A2_RANGING_SENSOR_Init\n");
charlesmn 1:638ad0a69df4 86 sensor->VL53L3A2_RANGING_Init(ToF_sensor);
charlesmn 1:638ad0a69df4 87 if (status) {
charlesmn 1:638ad0a69df4 88 ToF_Present[ToF_sensor] = 0;
charlesmn 1:638ad0a69df4 89 printf("VL53L3A2_RANGING_SENSOR_Init failed for sensor %d \n",ToF_sensor);
charlesmn 1:638ad0a69df4 90 }
charlesmn 1:638ad0a69df4 91 // give each sensor a unique address
charlesmn 1:638ad0a69df4 92 printf("VL53L3A2_RANGING_SENSOR_SetAddress\n");
charlesmn 0:619be236cb7c 93 wait_ms(100);
charlesmn 1:638ad0a69df4 94 i2c_addr = (0x52 + (ToF_sensor + 1) * 2); /* 0x54, 0x56, 0x58 */
charlesmn 1:638ad0a69df4 95 sensor->VL53L3A2_RANGING_SetAddress(ToF_sensor, i2c_addr);
charlesmn 1:638ad0a69df4 96 }
charlesmn 0:619be236cb7c 97
charlesmn 1:638ad0a69df4 98 wait_ms(100);
charlesmn 1:638ad0a69df4 99 printf("53L3A2 Multi Sensor Ranging demo application2\n");
charlesmn 0:619be236cb7c 100 }
charlesmn 0:619be236cb7c 101
charlesmn 0:619be236cb7c 102
charlesmn 0:619be236cb7c 103
charlesmn 0:619be236cb7c 104 static void MX_53L3A2_MultiSensorRanging_Process(void)
charlesmn 0:619be236cb7c 105 {
charlesmn 1:638ad0a69df4 106 uint8_t ToF_sensor;
charlesmn 1:638ad0a69df4 107 uint32_t id;
charlesmn 0:619be236cb7c 108
charlesmn 1:638ad0a69df4 109 RANGING_SENSOR_Result_t Result;
charlesmn 1:638ad0a69df4 110 RANGING_SENSOR_ProfileConfig_t Profile;
charlesmn 1:638ad0a69df4 111 printf("MX_53L3A2_MultiSensorRanging_Process\n");
charlesmn 1:638ad0a69df4 112 Profile.RangingProfile = RS_MULTI_TARGET_LONG_RANGE;
charlesmn 1:638ad0a69df4 113 Profile.TimingBudget = 30; /* 16 ms < TimingBudget < 500 ms */
charlesmn 1:638ad0a69df4 114 Profile.Frequency = 0; /* not necessary in simple ranging */
charlesmn 1:638ad0a69df4 115 Profile.EnableAmbient = 0; /* Enable: 1, Disable: 0 */
charlesmn 1:638ad0a69df4 116 Profile.EnableSignal = 0; /* Enable: 1, Disable: 0 */
charlesmn 1:638ad0a69df4 117 Profile.EnableInterrupt = 0; // disables interupts
charlesmn 1:638ad0a69df4 118 printf("MX_53L3A2_MultiSensorRanging_Process start sensors\n");
charlesmn 1:638ad0a69df4 119 for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
charlesmn 1:638ad0a69df4 120 /* skip this device if not detected */
charlesmn 1:638ad0a69df4 121 if (ToF_Present[ToF_sensor] != 1) {
charlesmn 1:638ad0a69df4 122 continue;
charlesmn 1:638ad0a69df4 123 }
charlesmn 1:638ad0a69df4 124
charlesmn 1:638ad0a69df4 125 wait_ms(100);
charlesmn 1:638ad0a69df4 126 sensor->VL53L3A2_RANGING_ConfigProfile(ToF_sensor, &Profile);
charlesmn 1:638ad0a69df4 127 if (status != BSP_ERROR_NONE) {
charlesmn 1:638ad0a69df4 128 printf("VL53L3A2_RANGING_SENSOR_ConfigProfile failed sensor %d status %d\n",ToF_sensor,status);
charlesmn 1:638ad0a69df4 129 }
charlesmn 1:638ad0a69df4 130 wait_ms(100);
charlesmn 1:638ad0a69df4 131 status = sensor->VL53L3A2_RANGING_Start(ToF_sensor, RS_MODE_BLOCKING_CONTINUOUS);
charlesmn 1:638ad0a69df4 132 if (status != BSP_ERROR_NONE) {
charlesmn 1:638ad0a69df4 133 printf("VL53L3A2_RANGING_SENSOR_Start failed sensor %d status %d\n",ToF_sensor,status);
charlesmn 1:638ad0a69df4 134 } else {
charlesmn 1:638ad0a69df4 135 printf("VL53L3A2_RANGING_SENSOR_Start started %d\n",ToF_sensor);
charlesmn 1:638ad0a69df4 136 }
charlesmn 0:619be236cb7c 137 }
charlesmn 1:638ad0a69df4 138 printf("MX_53L3A2_MultiSensorRanging_Process read sensors\n");
charlesmn 1:638ad0a69df4 139 // repeatedly read data and start next measurement
charlesmn 1:638ad0a69df4 140 while (1) {
charlesmn 1:638ad0a69df4 141 /* polling mode */
charlesmn 1:638ad0a69df4 142 for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
charlesmn 1:638ad0a69df4 143 status = sensor->VL53L3A2_RANGING_GetDistance(ToF_sensor, &Result);
charlesmn 1:638ad0a69df4 144 if ((status == BSP_ERROR_NONE) &&
charlesmn 2:b1af0870b56a 145 (Result.ZoneResult[0].NumberOfTargets != 0))
charlesmn 1:638ad0a69df4 146 {
charlesmn 2:b1af0870b56a 147 print_result(ToF_sensor,&Result);
charlesmn 1:638ad0a69df4 148 }
charlesmn 1:638ad0a69df4 149 }
charlesmn 1:638ad0a69df4 150 wait_ms(POLLING_PERIOD);
charlesmn 0:619be236cb7c 151 }
charlesmn 0:619be236cb7c 152 }
charlesmn 0:619be236cb7c 153
charlesmn 0:619be236cb7c 154
charlesmn 2:b1af0870b56a 155 // Note in VL53L3CX there is only one Zone so NumberOfZones is not used and is always assumed to be 1
charlesmn 2:b1af0870b56a 156 static void print_result(uint8_t ToF_sensor,RANGING_SENSOR_Result_t *Result)
charlesmn 0:619be236cb7c 157 {
charlesmn 2:b1af0870b56a 158 uint8_t zone = 0; // in VL53L3CX there is only one zone
charlesmn 2:b1af0870b56a 159 uint8_t target = 0;
charlesmn 1:638ad0a69df4 160
charlesmn 2:b1af0870b56a 161 for (target = 0; target < Result->ZoneResult[zone].NumberOfTargets; target++) {
charlesmn 2:b1af0870b56a 162 if (Result->ZoneResult[zone].Status[target] == VL53LX_RANGESTATUS_RANGE_VALID )
charlesmn 2:b1af0870b56a 163 {
charlesmn 2:b1af0870b56a 164 printf("\n%s\t - ", TofDevStr[ToF_sensor]);
charlesmn 2:b1af0870b56a 165 printf(" |---> ");
charlesmn 2:b1af0870b56a 166 printf("Status = %d, Target %d, Distance = %5d mm",
charlesmn 2:b1af0870b56a 167 Result->ZoneResult[zone].Status[target],
charlesmn 2:b1af0870b56a 168 target,
charlesmn 2:b1af0870b56a 169 Result->ZoneResult[zone].Distance[target]);
charlesmn 2:b1af0870b56a 170 }
charlesmn 1:638ad0a69df4 171
charlesmn 1:638ad0a69df4 172 }
charlesmn 0:619be236cb7c 173 }
charlesmn 1:638ad0a69df4 174
charlesmn 0:619be236cb7c 175
charlesmn 0:619be236cb7c 176
charlesmn 0:619be236cb7c 177 /*=================================== Main ==================================
charlesmn 0:619be236cb7c 178 =============================================================================*/
charlesmn 0:619be236cb7c 179 int main()
charlesmn 0:619be236cb7c 180 {
charlesmn 1:638ad0a69df4 181
charlesmn 1:638ad0a69df4 182 pc.baud(115200); // baud rate is important as printf statements take a lot of time
charlesmn 1:638ad0a69df4 183
charlesmn 1:638ad0a69df4 184 sensor = new VL53L3A2_SENSOR();
charlesmn 0:619be236cb7c 185
charlesmn 1:638ad0a69df4 186 printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\r\n");
charlesmn 1:638ad0a69df4 187 printf("VL53L3CX_Shield_3Sensors_Polling\n");
charlesmn 0:619be236cb7c 188
charlesmn 1:638ad0a69df4 189 MX_53L3A2_MultiSensorRanging_Init();
charlesmn 1:638ad0a69df4 190
charlesmn 1:638ad0a69df4 191 while (1) {
charlesmn 1:638ad0a69df4 192 MX_53L3A2_MultiSensorRanging_Process();
charlesmn 1:638ad0a69df4 193 }
charlesmn 0:619be236cb7c 194
charlesmn 0:619be236cb7c 195 }
charlesmn 0:619be236cb7c 196
charlesmn 0:619be236cb7c 197 #if (MBED_VERSION > 60300)
charlesmn 0:619be236cb7c 198 extern "C" void wait_ms(int ms)
charlesmn 0:619be236cb7c 199 {
charlesmn 0:619be236cb7c 200 thread_sleep_for(ms);
charlesmn 0:619be236cb7c 201 }
charlesmn 0:619be236cb7c 202 #endif
charlesmn 1:638ad0a69df4 203