A sample program getting measurements from a VL53L1CB ToF sensors which are directly connected to the STM32F401. Copes with three sensors. Interrupt mode. VL53L1 is operated in ranging mode. MBed V2.

Dependencies:   mbed X_NUCLEO_53L1CB

main.cpp

Committer:
charlesmn
Date:
2021-01-19
Revision:
1:02f2b9e04b46
Parent:
0:2b5a44a0d2ec

File content as of revision 1:02f2b9e04b46:

/*
 *  This VL53L1 test application performs range measurements
 *  using the onboard embedded centre sensor and two satelites, in ranging, interrupt mode.
 *  This program does not use the expansion board and the sensors a directly connected to the F401.
 *  Measured ranges are ouput on the Serial Port, running at 115200 baud.
 *
 * This is designed to work with MBed V2 , MBed V5 and MBed V6.
 *
 *
 *  The Reset button can be used to restart the program.
 */
 
#include <stdio.h>

#include "mbed.h"
#include "XNucleo53L1A1.h"
#include "ToF_I2C.h"
#include <time.h>

// i2c comms pins
#define I2C_SDA   D14 
#define I2C_SCL   D15 

// i2c addresses the sensors are configured to be
#define NEW_SENSOR_CENTRE_ADDRESS   0x56
#define NEW_SENSOR_LEFT_ADDRESS     0x58
#define NEW_SENSOR_RIGHT_ADDRESS    0x5a


#define NUM_SENSORS 3

// define the sensor interrupt pins. This must match the wiring!
PinName CentreIntPin = A2;
PinName LeftIntPin = A3;
PinName RightIntPin = A4;


// MBed V6.4 has renamed wait_ms and UnbufferedSerial replaces Serial
#if (MBED_VERSION  > 60300) 
UnbufferedSerial  pc(SERIAL_TX, SERIAL_RX); 
extern "C" void wait_ms(int ms);
#else
Serial pc(SERIAL_TX, SERIAL_RX); 
#endif

static int int_centre_result = 0;
static int int_left_result = 0;
static int int_right_result = 0;


class WaitForMeasurement {
public:


// this class services the interrupts from the ToF sensors.
// There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things.
// We keep things simple by only raising a flag so all the real work is done outside the interrupt.
// This is designed around MBED V2 which doesn't have the RTOS features that would make this work nicely e.g. semaphores/queues.
WaitForMeasurement(): _interrupt(A1)
{
}


    // constructor 
    WaitForMeasurement(PinName pin, uint8_t slave_address) : _interrupt(pin)          // create the InterruptIn on the pin specified 
    {
        i2c_slave_address = slave_address;
        _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach routine to deal with interrupt
        
    }
    

  // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
    void got_interrupt()
    {
    
        // identify which sensor this instance relates to and flag an interrupt has occured
        if (i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
            int_centre_result = 1;  //flag to main that interrupt happened
        if (i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
            int_left_result = 1;   //flag to main that interrupt happened
        if (i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
            int_right_result = 1;  //flag to main that interrupt happened
    }

    
    //destructor
    ~WaitForMeasurement()
    {
        printf("destruction \n");
    }

private:
    InterruptIn _interrupt;
    int status;
    uint8_t   i2c_slave_address;
    
};

 
/*=================================== Main ==================================
=============================================================================*/
int main()
{   
    int status;
    VL53L1X * Sensor;
    VL53L1X * SensorCentre;
    VL53L1X * SensorLeft;
    VL53L1X * SensorRight;
    DigitalOut xshutdownCentre(D9);
    DigitalOut xshutdownLeft(D4);
    DigitalOut xshutdownRight(D3);

    uint8_t ToFSensor = 1; 
  
    
    WaitForMeasurement* int2;
    WaitForMeasurement* int1;
    WaitForMeasurement* int3;

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

    printf("VL53L1CB_NoShield_3sensors_interrupt_ranging mbed = %d \r\n",MBED_VERSION);

// create the i2c instance
    ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
// create the sensor instances    
    SensorCentre = new VL53L1X(dev_I2C, &xshutdownCentre, CentreIntPin); 
    SensorLeft = new VL53L1X(dev_I2C, &xshutdownLeft, LeftIntPin); 
    SensorRight = new VL53L1X(dev_I2C, &xshutdownRight, RightIntPin); 
    
    printf("board created!\r\n");
 // initialise sensors                                               
    for (ToFSensor=0;ToFSensor< NUM_SENSORS ;ToFSensor++){
    switch(ToFSensor){
        case 0:
            
            if (SensorCentre == NULL ) continue;
            // try to initialise the sensor. returns -1 if sensor is not there.
            if (SensorCentre->InitSensor(NEW_SENSOR_CENTRE_ADDRESS) ==-1) 
            {
                 printf("centre not found \n");
                 SensorCentre = NULL;
                 continue;
            }
            Sensor=SensorCentre;
            printf("configuring centre channel \n");
            break;
        case 1:
            if (SensorLeft== NULL ) continue;
            if (SensorLeft->InitSensor(NEW_SENSOR_LEFT_ADDRESS)==-1)
            {
                 printf("left not found \n");
                 SensorLeft = NULL;
                 continue;
            }
            Sensor=SensorLeft;
            printf("configuring left channel \n");
            break;
        case 2:
            if (SensorRight == NULL ) continue;
            if(SensorRight->InitSensor(NEW_SENSOR_RIGHT_ADDRESS)==-1)
            {
                 printf("right not found \n");
                 SensorRight = NULL;
                 continue;
            }
            Sensor=SensorRight;
            printf("configuring right channel \n");
            break;      
        default:
           printf(" error in switch, invalid ToF sensor \n");
    }
    wait_ms(250);  // wait for the sensor to come alive
        
// configure the sensors

/* Device Initialization and setting */  
    status = Sensor->vl53L1_DataInit();
    status = Sensor->vl53L1_StaticInit();
    status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_RANGING);
    status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
    status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 50000);
    status = Sensor->vl53L1_SetInterMeasurementPeriodMilliSeconds( 100);
    }
    
    if (SensorCentre != NULL )
    {
        printf("starting interrupt centre\n");
        // create interrupt process for the centre sensor
        int1 =  new WaitForMeasurement(CentreIntPin,NEW_SENSOR_CENTRE_ADDRESS);
        // start the snesor measuring
        status = SensorCentre->vl53L1_StartMeasurement();
                    printf("end interrupt centre\n");
    }
    

    if (SensorLeft!= NULL )
    {
        printf("starting interrupt left\n");
        int2 = new WaitForMeasurement(LeftIntPin,NEW_SENSOR_LEFT_ADDRESS);
        status = SensorLeft->vl53L1_StartMeasurement();
    }

    if (SensorRight!= NULL )
    {
        printf("starting interrupt right\n");
        int3 = new WaitForMeasurement(RightIntPin,NEW_SENSOR_RIGHT_ADDRESS);
        status = SensorRight->vl53L1_StartMeasurement();
    }
            
    printf("loop forever \n");
   // loop waiting for interrupts to happen. This is signaled by   int_centre_result,int_left_result or int_right_result
   // being non zero. The are set back to zero when processing is completed
    while (1)
    {
        static VL53L1_RangingMeasurementData_t RangingData;  
        VL53L1_MultiRangingData_t MultiRangingData;
        VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;   
        
        wait_ms( 1 * 50);
        if (int_centre_result != 0)
        {
            // as there has been an interrupt there is data waiting so get it
            status = SensorCentre->vl53L1_GetMultiRangingData( pMultiRangingData);
            if ( status == VL53L1_ERROR_NONE)
            {
                int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
                if ( no_of_object_found < 10 ) 
                {
                    for(int j=0;j<no_of_object_found;j++){
                        if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
                            (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
                        {
                            printf("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
                            pMultiRangingData->RangeData[j].RangeStatus,
                            pMultiRangingData->RangeData[j].RangeMilliMeter,
                            pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
                            pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
                        }
                    }
                }
            }
                int_centre_result = 0;
                status = SensorCentre->vl53L1_ClearInterruptAndStartMeasurement();
        }


        if (int_left_result != 0)
        {
            status = SensorLeft->vl53L1_GetMultiRangingData( pMultiRangingData);
            if ( status == VL53L1_ERROR_NONE)
            {
                int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
                if ( no_of_object_found < 10 ) 
                {
                    for(int j=0;j<no_of_object_found;j++){
                            if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
                                (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
                            {

                                printf("left  \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
                                pMultiRangingData->RangeData[j].RangeStatus,
                                pMultiRangingData->RangeData[j].RangeMilliMeter,
                                pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
                                pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
                            }
                    }
                }
            }
 
                int_left_result = 0;
                status = SensorLeft->vl53L1_ClearInterruptAndStartMeasurement();

        }
            
            
        if (int_right_result != 0)
        {
            status = SensorRight->vl53L1_GetMultiRangingData( pMultiRangingData);
            if ( status == VL53L1_ERROR_NONE)
            {
                // if valid result print it
                int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
                if ( no_of_object_found < 10 ) 
                {
                    for(int j=0;j<no_of_object_found;j++){
                        if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
                            (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
                            {
                                printf("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
                                pMultiRangingData->RangeData[j].RangeStatus,
                                pMultiRangingData->RangeData[j].RangeMilliMeter,
                                pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
                                pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
                            }
                    }
                }
            }
            // clear interrupt flag
            int_right_result = 0;
            // clear theinterrupt and wait for another result
            status = SensorRight->vl53L1_ClearInterruptAndStartMeasurement();

        }
    }
    printf("terminated");
}
    
  
  // this function doesn't exist in MBed6.4 onwards. It is required for the F401 drivers.
  // addded here as it allows the file to be compatible with all mbed versions          
#if (MBED_VERSION  > 60300) 
void wait_ms(int ms)
 {
    thread_sleep_for(ms);
 }
#endif