File content as of revision 4:f74c066a9d05:

/*
 * This VL53L3 Expansion board test application performs range measurements
 * using the onboard embedded sensor, and satellite boards, in polling mode.
 * Measured ranges are output on the Serial Port, running at 115200 baud.
 *
 * The Reset button can be used to restart the program.
 *
 * *** Note :
 * Default Mbed build system settings disable printf floating-point support.
 * Online builds seem unable to configure this.
 * Offline builds can enable printf floating-point support.
 * https://github.com/ARMmbed/mbed-os/blob/master/platform/source/minimal-printf/README.md
 * .\mbed-os\platform\mbed_lib.json
 *
 */

#include <stdio.h>
#include <time.h>

#include "mbed.h"

#include "vl53L3_I2c.h"
#include "vl53lx_platform_user_data.h"
#include "53l3a2_ranging_sensor.h"
#include "VL53L3A2_RangingClass.h"

#define BSP_ERROR_NONE                         0


#if (MBED_VERSION  > 60300)
UnbufferedSerial  pc(USBTX, USBRX);
extern "C" void wait_ms(int ms);
#else
Serial pc(SERIAL_TX, SERIAL_RX);
#endif

VL53L3A2_SENSOR *sensor; //class for sensor commands

/* Private define ------------------------------------------------------------*/
#define POLLING_PERIOD              (250U)

#define SENSOR_LEFT_NUMBER 0
#define SENSOR_CENTRE_NUMBER 1
#define SENSOR_RIGHT_NUMBER 2

/* Private variables ---------------------------------------------------------*/
static int32_t status = 0;
volatile uint8_t ToF_EventDetected = 0;
static uint8_t ToF_Present[RANGING_SENSOR_INSTANCES_NBR] = {0};

/*
static const char *TofDevStr[] = {
    [VL53L3A2_DEV_LEFT] = "LEFT",
    [VL53L3A2_DEV_CENTER] = "CENTER",
    [VL53L3A2_DEV_RIGHT] = "RIGHT"
};
*/
static const char *TofDevStr[] = {"LEFT","CENTER","RIGHT"};

/* Private function prototypes -----------------------------------------------*/
static void MX_53L3A2_MultiSensorRanging_Init(void);
static void MX_53L3A2_MultiSensorRanging_Process(void);
static void print_result(uint8_t ToF_sensor,RANGING_SENSOR_Result_t *Result);


static void MX_53L3A2_MultiSensorRanging_Init(void)
{
    uint8_t ToF_sensor;
    uint16_t i2c_addr;
    uint32_t id;

    // shut down all sensors
    for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
        sensor->VL53L3A2_RANGING_SetPowerMode(ToF_sensor, RANGING_SENSOR_POWERMODE_OFF);
        ToF_Present[ToF_sensor] = 1;
    }
    wait_ms(100);


    printf("53L3A2 Multi Sensor Ranging demo application\n");

    /* power on the devices one at a time, initialize them and change their address.
     */
    for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
        printf("VL53L3A2_RANGING_SENSOR_Init\n");
        sensor->VL53L3A2_RANGING_Init(ToF_sensor);
        if (status) {
            ToF_Present[ToF_sensor] = 0;
            printf("VL53L3A2_RANGING_SENSOR_Init failed for sensor %d \n",ToF_sensor);
        }
        // give each sensor a unique address
        printf("VL53L3A2_RANGING_SENSOR_SetAddress\n");
        wait_ms(100);
        i2c_addr = (0x52 + (ToF_sensor + 1) * 2);	/* 0x54, 0x56, 0x58 */
        sensor->VL53L3A2_RANGING_SetAddress(ToF_sensor, i2c_addr);
    }

    wait_ms(100);
    printf("53L3A2 Multi Sensor Ranging demo application2\n");
}



static void MX_53L3A2_MultiSensorRanging_Process(void)
{
    uint8_t ToF_sensor;
    uint32_t id;

    RANGING_SENSOR_Result_t Result;
    RANGING_SENSOR_ProfileConfig_t Profile;
    printf("MX_53L3A2_MultiSensorRanging_Process\n");
    Profile.RangingProfile = RS_MULTI_TARGET_LONG_RANGE;
    Profile.TimingBudget = 30; /* 16 ms < TimingBudget < 500 ms */
    Profile.Frequency = 0; /* not necessary in simple ranging */
    Profile.EnableAmbient = 0; /* Enable: 1, Disable: 0 */
    Profile.EnableSignal = 0; /* Enable: 1, Disable: 0 */
    Profile.EnableInterrupt = 0;  // disables interupts
    printf("MX_53L3A2_MultiSensorRanging_Process start sensors\n");
    for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
        /* skip this device if not detected */
        if (ToF_Present[ToF_sensor] != 1) {
            continue;
        }

        wait_ms(100);
        sensor->VL53L3A2_RANGING_ConfigProfile(ToF_sensor, &Profile);
        if (status != BSP_ERROR_NONE) {
            printf("VL53L3A2_RANGING_SENSOR_ConfigProfile  failed sensor %d status %d\n",ToF_sensor,status);
        }
        wait_ms(100);
        status = sensor->VL53L3A2_RANGING_Start(ToF_sensor, RS_MODE_BLOCKING_CONTINUOUS);
        if (status != BSP_ERROR_NONE) {
            printf("VL53L3A2_RANGING_SENSOR_Start failed sensor %d status %d\n",ToF_sensor,status);
        } else {
            printf("VL53L3A2_RANGING_SENSOR_Start started %d\n",ToF_sensor);
        }
    }
    printf("MX_53L3A2_MultiSensorRanging_Process read sensors\n");
    // repeatedly read data and start next measurement
    while (1) {
        /* polling mode */
        for (ToF_sensor = 0; ToF_sensor < RANGING_SENSOR_INSTANCES_NBR; ToF_sensor++) {
            status = sensor->VL53L3A2_RANGING_GetDistance(ToF_sensor, &Result);
            if ((status == BSP_ERROR_NONE) && 
                (Result.ZoneResult[0].NumberOfTargets != 0)) 
            {
                print_result(ToF_sensor,&Result);
            } 
        }
        wait_ms(POLLING_PERIOD);
    }
}


// Note in VL53L3CX there is only one Zone so NumberOfZones is not used and is always assumed to be 1
static void print_result(uint8_t ToF_sensor,RANGING_SENSOR_Result_t *Result)
{
    uint8_t zone = 0; // in VL53L3CX there is only one zone
    uint8_t target = 0;

    for (target = 0; target < Result->ZoneResult[zone].NumberOfTargets; target++) {
        if (Result->ZoneResult[zone].Status[target] == VL53LX_RANGESTATUS_RANGE_VALID )
        {
	        printf("\n%s\t - ", TofDevStr[ToF_sensor]);
	        printf(" |---> ");
	        printf("Status = %d, Target %d, Distance = %5d mm",
	               Result->ZoneResult[zone].Status[target],
	               target,
	               Result->ZoneResult[zone].Distance[target]);
        }

    }
}



/*=================================== Main ==================================
=============================================================================*/
int main()
{

    pc.baud(115200);  // baud rate is important as printf statements take a lot of time

    sensor = new VL53L3A2_SENSOR();

    printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\r\n");
    printf("VL53L3CX_Shield_3Sensors_Polling\n");

    MX_53L3A2_MultiSensorRanging_Init();

    while (1) {
        MX_53L3A2_MultiSensorRanging_Process();
    }

}

#if (MBED_VERSION  > 60300)
extern "C" void wait_ms(int ms)
{
    thread_sleep_for(ms);
}
#endif