File content as of revision 11:96465baec780:

/*
 *  This VL53L1CB Expansion board test application performs range measurements
 *  using the onboard embedded centre sensor, in multizone, polling mode.
 *  Measured ranges are ouput on the Serial Port, running at 115200 baud.
 *
 *
 * This is designed to work with MBed v2, & MBedOS v5 / v6.
 *
 *
 *  The Reset button can be used to restart the program.
 *
 * *** Note : 
 * Default Mbed build system settings disable print floating-point support.
 * Offline builds can enable this, again.
 * 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 "XNucleo53L1A2.h"
#include "ToF_I2C.h"


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

#define NUM_SENSORS 3

// define interrupt pins
PinName CentreIntPin = A2;
// the satellite pins depend on solder blobs on the back of the shield.
// they may not exist or may be one of two sets.
// the centre pin always exists
//PinName LeftIntPin = D8;
PinName RightIntPin = D2;
// alternate set
PinName LeftIntPin = D9;
//PinName RightIntPin = D4;


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

#if TARGET_STM  // we are cross compiling for an STM32-Nucleo
    InterruptIn stop_button(BUTTON1);
#endif
#if TARGET_Freescale // we are cross-compiling for NXP FRDM boards.
    InterruptIn stop_button(SW2);
#endif

void process_interrupt( VL53L1 * sensor,VL53L1_DEV dev );
void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );

VL53L1_Dev_t                   devCentre;
VL53L1_Dev_t                   devLeft;
VL53L1_Dev_t                   devRight;
VL53L1_DEV                     Dev = &devCentre;

/* Installed sensors count */
int sensorCnt = 0;

/* installed sensors prefixes */
char installedSensors[3];

/* interrupt requests */
volatile bool centerSensor = false;
volatile bool leftSensor = false;
volatile bool rightSensor = false;
volatile bool int_measuring_stop = false;

/* Current sensor number*/
volatile int currentSensor = 0;

/* current displayed sensor change IRQ */
volatile bool switchChanged = false;

/* ISR callback function of the centre sensor */
void sensor_centre_irq(void)
{
    centerSensor = true;
    board->sensor_centre->disable_interrupt_measure_detection_irq();
}

/* ISR callback function of the left sensor */
void sensor_left_irq(void)
{
    leftSensor = true;
    board->sensor_left->disable_interrupt_measure_detection_irq();
}

/* ISR callback function of the right sensor */
void sensor_right_irq(void)
{
    rightSensor = true;
    board->sensor_right->disable_interrupt_measure_detection_irq();
}

/* ISR callback function of the user blue button to switch measuring sensor. */
void switch_measuring_sensor_irq(void)
{
    stop_button.disable_irq();
    switchChanged = true;
}

/*
 * This function calls the interrupt handler for each sensor
 * and outputs the range
 */
inline void measure_sensors()
{
    int status = 0;
    bool current = false;
    
    uint16_t distance = 0;
    
    VL53L1_MultiRangingData_t MultiRangingData;
    VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;

    /* Handle the interrupt and output the range from the centre sensor */
    if (centerSensor) {
        centerSensor = false;

        status = board->sensor_centre->VL53L1_GetDistance(&distance);
        
        current = (currentSensor == 0);
        if (current) {
            printf("Centre: %d\r\n", distance);
        }
        
        status = board->sensor_centre->VL53L1_ClearInterrupt();
        board->sensor_centre->enable_interrupt_measure_detection_irq();        
        
    }

    /* Handle the interrupt and output the range from the left sensor */
    if (leftSensor) {
        leftSensor = false;
        
        status = board->sensor_left->VL53L1_GetDistance(&distance);
        
        current = (installedSensors[currentSensor] == 'L');
        if (current) {
            printf("Left: %d\r\n", distance);
        }
        status = board->sensor_left->VL53L1_ClearInterrupt();
        board->sensor_left->enable_interrupt_measure_detection_irq();
        
    }

    /* Handle the interrupt and output the range from the right sensor */
    if (rightSensor) {
        rightSensor = false;
        
        status = board->sensor_right->VL53L1_GetDistance(&distance);
        
        current = (installedSensors[currentSensor] == 'R');
        if (current) {
            printf("Right: %d\r\n", distance);
        }
        status = board->sensor_right->VL53L1_ClearInterrupt();
        board->sensor_right->enable_interrupt_measure_detection_irq();
        
    }    
}

int configure_sensor(VL53L1 *Sensor)
{
    int status = 0;
/*    
    //configure the regions of interest for each sensor
    VL53L1_RoiConfig_t roiConfig;
    
    roiConfig.NumberOfRoi = 3;
    roiConfig.UserRois[0].TopLeftX = 0;
    roiConfig.UserRois[0].TopLeftY = 9; 
    roiConfig.UserRois[0].BotRightX = 4;
    roiConfig.UserRois[0].BotRightY = 5; 
    
    roiConfig.UserRois[1].TopLeftX = 5;
    roiConfig.UserRois[1].TopLeftY = 9; 
    roiConfig.UserRois[1].BotRightX = 9;
    roiConfig.UserRois[1].BotRightY = 4; 
    
    roiConfig.UserRois[2].TopLeftX = 11;
    roiConfig.UserRois[2].TopLeftY = 9; 
    roiConfig.UserRois[2].BotRightX = 15;
    roiConfig.UserRois[2].BotRightY = 5; 
    
    status = Sensor->vl53L1_SetROI( &roiConfig);
*/    
// Device Initialization and setting 
    status = Sensor->vl53L1_DataInit();
    status = Sensor->vl53L1_StaticInit();

    status = Sensor->vl53L1_SetPresetMode(VL53L1_PRESETMODE_MULTIZONES_SCANNING);
    status = Sensor->vl53L1_SetDistanceMode(VL53L1_DISTANCEMODE_LONG);
    
    status = Sensor->vl53L1_StartMeasurement();  

    return status;
}

/*
 * Add to an array a character that represents the sensor and start ranging
 */
int init_sensors_array()
{
    int status = 0;
    VL53L1 *Sensor;
    uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right

    sensorCnt = 0;
    
    Dev->comms_speed_khz = 400;
    Dev->comms_type = 1;
    
    if (board->sensor_centre != NULL) {
        Dev = &devCentre;
        Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
        devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
        
        status = configure_sensor(board->sensor_centre);
        printf("configuring centre channel \n");
    }
    
    if (board->sensor_left != NULL) {
        Dev = &devLeft; 
        Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
        devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
    
        status = configure_sensor(board->sensor_left);
        printf("configuring left channel \n");
    }
    
    if (board->sensor_right != NULL) {
        Dev = &devRight;  
        Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
        devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
        
        status = configure_sensor(board->sensor_right);
        printf("configuring right channel \n");
    }
    
    /* start the measure on the center sensor */
    if (NULL != board->sensor_centre) {
        installedSensors[sensorCnt] = 'C';
        status = board->sensor_centre->stop_measurement();
        if (status != 0) {
            return status;
        }
        status = board->sensor_centre->start_measurement(&sensor_centre_irq);
        if (status != 0) {
            return status;
        }
        ++sensorCnt;
    }
    /* start the measure on the left sensor */
    if (NULL != board->sensor_left) {
        installedSensors[sensorCnt] = 'L';
        status = board->sensor_left->stop_measurement();
        if (status != 0) {
            return status;
        }
        status = board->sensor_left->start_measurement(&sensor_left_irq);
        if (status != 0) {
            return status;
        }
        ++sensorCnt;
    }
    /* start the measure on the right sensor */
    if (NULL != board->sensor_right) {
        installedSensors[sensorCnt] = 'R';
        status = board->sensor_right->stop_measurement();
        if (status != 0) {
            return status;
        }
        status = board->sensor_right->start_measurement(&sensor_right_irq);
        if (status != 0) {
            return status;
        }
        ++sensorCnt;
    }
    currentSensor = 0;
    return status;
}


 
/*=================================== Main ==================================
=============================================================================*/
int main()
{   
    int status;
    VL53L1 *Sensor;
//    uint16_t wordData;
    
    stop_button.rise(&switch_measuring_sensor_irq);
    stop_button.enable_irq();
    

    pc.baud(115200);  // baud rate is important as printf statements take a lot of time
    printf("mbed version = %d \r\n",MBED_VERSION);

// create i2c interface
    ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
    
    /* creates the 53L1A1 expansion board singleton obj */
//   board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2);
   board = XNucleo53L1A2::instance(dev_I2C, A2, D9, D2);
    
    printf("board created!\r\n");

    /* init the 53L1A1 expansion board with default values */
    status = board->init_board();
    if (status) {
        printf("Failed to init board!\r\n");
        return 0;
    }       
        
    printf("board initiated! - %d\r\n", status);

    /* init an array with chars to id the sensors */
//    status = init_sensors_array();
    if (status != 0) {
        printf("Failed to init sensors!\r\n");
        return status;
    }
    
    VL53L1_RoiConfig_t roiConfig;
    
    roiConfig.NumberOfRoi =3;
    
    roiConfig.UserRois[0].TopLeftX = 0;
    roiConfig.UserRois[0].TopLeftY = 9; 
    roiConfig.UserRois[0].BotRightX = 4;
    roiConfig.UserRois[0].BotRightY = 5; 
    
    roiConfig.UserRois[1].TopLeftX = 5;
    roiConfig.UserRois[1].TopLeftY = 9; 
    roiConfig.UserRois[1].BotRightX = 9;
    roiConfig.UserRois[1].BotRightY = 4; 
    
    roiConfig.UserRois[2].TopLeftX = 11;
    roiConfig.UserRois[2].TopLeftY = 9; 
    roiConfig.UserRois[2].BotRightX = 15;
    roiConfig.UserRois[2].BotRightY = 5; 

    
// configure the sensors
    Dev->comms_speed_khz = 400;
    Dev->comms_type = 1;
    
// start of setup of central sensor
    if (board->sensor_centre != NULL) {
        Dev=&devCentre;
        Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
        devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
        
        Sensor=board->sensor_centre;
    
        printf("configuring centre channel \n");
        
        // Device Initialization and setting 
        status = Sensor->vl53L1_DataInit();
        status = Sensor->vl53L1_StaticInit();
        
        status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
        status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);
        
        status = Sensor->vl53L1_SetROI( &roiConfig);
        
        printf("VL53L1_SetROI %d \n",status);
    
        status = board->sensor_centre->vl53L1_StartMeasurement();  
    }        

// start of setup of left satellite
    if (board->sensor_left != NULL) {
        Dev=&devLeft;
        Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
        devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
        
        Sensor=board->sensor_left;
    
        printf("configuring left channel \n");
        
        // Device Initialization and setting 
        status = Sensor->vl53L1_DataInit();
        status = Sensor->vl53L1_StaticInit();
        
        status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
        status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);    
        
        status = Sensor->vl53L1_SetROI( &roiConfig);
        
        printf("VL53L1_SetROI %d \n",status);
    
        status = board->sensor_left->vl53L1_StartMeasurement();  
    }        

    // start of setup of right satellite
    if (board->sensor_right != NULL) {
        Dev=&devRight;
        Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
        devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
        
        Sensor=board->sensor_right;
    
        printf("configuring right channel \n");
        
        // Device Initialization and setting 
        status = Sensor->vl53L1_DataInit();
        status = Sensor->vl53L1_StaticInit();
        
        status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING);
        status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG);    
        
        status = Sensor->vl53L1_SetROI( &roiConfig);
        
        printf("VL53L1_SetROI %d \n",status);
    
        status = board->sensor_right->vl53L1_StartMeasurement();  
    }        


    VL53L1_MultiRangingData_t MultiRangingData;
    VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;    
    
    // looping polling for results
    while (true) {
        pMultiRangingData = &MultiRangingData;    
/*        
        if (board->sensor_centre != NULL) {
            // wait for result
            status = board->sensor_centre->vl53L1_WaitMeasurementDataReady();  
            // get the result
            status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
            // if valid, print it
            if(status==0) {
                print_results(devCentre.i2c_slave_address, pMultiRangingData );
                status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
    
            } //if(status==0) 
            else
            {
                printf("VL53L1_GetMultiRangingData centre %d \n",status);
            }
        }
        
        if (board->sensor_left != NULL) {
            // wait for result
            status = board->sensor_left->vl53L1_WaitMeasurementDataReady();  
            // get the result
            status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
            // if valid, print it
            if(status==0) {
                print_results(devLeft.i2c_slave_address, pMultiRangingData );
                status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
    
            } //if(status==0) 
            else
            {
                printf("VL53L1_GetMultiRangingData left %d \n",status);
            }
        }
        
        if (board->sensor_right != NULL) {
            // wait for result
            status = board->sensor_right->vl53L1_WaitMeasurementDataReady();  
            // get the result
            status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData);
            // if valid, print it
            if(status==0) {
                print_results(devRight.i2c_slave_address, pMultiRangingData );
                status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement();
    
            } //if(status==0) 
            else
            {
                printf("VL53L1_GetMultiRangingData right %d \n",status);
            }
        }
*/
    
        measure_sensors();
        
        if (switchChanged) {
            ++currentSensor;
            if (currentSensor == sensorCnt)
                currentSensor = 0;
            printf("Sensor changed to %c\r\n", installedSensors[currentSensor]);
            switchChanged = false;
            stop_button.enable_irq();
        }
    
    }
}
    
    
    
 // prints the range result seen above   
void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
{
            int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
            
            int RoiNumber=pMultiRangingData->RoiNumber;
//            int RoiStatus=pMultiRangingData->RoiStatus;

            if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
            {
                
  //              printf("MZI Count=%5d, ", pMultiRangingData->StreamCount);
   //             printf("RoiNumber%1d, ", RoiNumber);
   //               printf("RoiStatus=%1d, \n", RoiStatus);
                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("*****************\t i2cAddr=0x%x \t RoiNumber=%d   \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
                        devSpiNumber,
                        RoiNumber,
                        pMultiRangingData->RangeData[j].RangeStatus,
                        pMultiRangingData->RangeData[j].RangeMilliMeter,
                        pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
                        pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
                    }
                    else
                    {
//                               printf("RangeStatus %d %d\n",j, pMultiRangingData->RangeData[j].RangeStatus);
                    }
                }
            } // if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
            else
            {
//                       printf("no_of_object_found %d \n",no_of_object_found);
            }

}   

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