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@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?

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