File content as of revision 4:36a04230554d:

/*--------------------------------------------------------------------------------
Filename: TOF.cpp
Description: Source file for interfacing with the Adafruit TOF sensor
--------------------------------------------------------------------------------*/

#include "mbed.h"
#include "TOF.h"

I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM board

/*--------------------------------------------------------------------------------
Function name: ServiceTOF
Input Parameters: address - address of target TOF sensor
Output Parameters: range - distance measurement in mm
Description: performs measurement routine on a given sensor
----------------------------------------------------------------------------------*/
uint8_t serviceTOF(cAdafruit_VL6180X VL6180X, uint8_t address){
        
        uint8_t range = 0;
        
        // poll the VL6180X till new sample ready
        VL6180X.Poll_Range(address);

        // read range result
        range = VL6180X.Read_Range(address);
        
        // clear the interrupt on VL6180X
        VL6180X.Clear_Interrupts(address);
        
        return range;
}

/*--------------------------------------------------------------------------------
Function name: cAdafruit_VL6180X
Input Parameters: N/A
Output Parameters: N/A
Description: Class constructor (Initialisation upon creating class)
----------------------------------------------------------------------------------*/
cAdafruit_VL6180X::cAdafruit_VL6180X(DigitalOut sensor1, DigitalOut sensor2, DigitalOut sensor3, DigitalOut sensor4)
{
    //Disable all sensors to start
    sensor1 = 0;
    sensor2 = 0;
    sensor3 = 0;
    sensor4 = 0;
    
    //short delay
    wait(0.01);

    //Enable and configure sensor 1
    sensor1 = 1;
    wait(0.01);
    Setup(DEFAULT_ADDR, ADDR1); //Change address
    Init(ADDR1);        //Perform standard initialisation routine
    Start_Range(ADDR1); //Begin sampling in continuous mode
    
    //Enable and configure sensor 2
    sensor2 = 1;
    wait(0.01);
    Setup(DEFAULT_ADDR, ADDR4); //Change address
    Init(ADDR4);        //Perform standard initialisation routine
    Start_Range(ADDR4); //Begin sampling in continuous mode
    
    //Enable and configure sensor 3
    sensor3 = 1;
    wait(0.01);
    Setup(DEFAULT_ADDR, ADDR6); //Change address
    Init(ADDR6);        //Perform standard initialisation routine
    Start_Range(ADDR6); //Begin sampling in continuous mode
    
    //Enable and configure sensor 4
    sensor4 = 1;
    wait(0.01);
    Setup(DEFAULT_ADDR, ADDR7); //Change address
    Init(ADDR7);        //Perform standard initialisation routine
    Start_Range(ADDR7); //Begin sampling in continuous mode
    
    printf("INITIALISED\n\r");
}

/*--------------------------------------------------------------------------------
Function name: WriteByte(wchar_t reg,char data)
Input Parameters: reg - the register of the data to write, data - the data to send
Output Parameters: N/A
Description: Writes a single byte to a specified address using the I2C libraries
----------------------------------------------------------------------------------*/
void cAdafruit_VL6180X::WriteByte(uint8_t address, wchar_t reg,char data)
{
    char data_write[3];
    data_write[0] = (reg >> 8) & 0xFF;; // MSB of register address
    data_write[1] = reg & 0xFF; // LSB of register address
    data_write[2] = data & 0xFF;
    i2c.write(address, data_write, 3);
}

/*--------------------------------------------------------------------------------
Function name: ReadByte(wchar_t reg)
Input Parameters: reg - The register of the data to read from
Output Parameters: data_read[0] - The read data
Description: Writes the address to be read from then reads a single byte using the I2C libraries
----------------------------------------------------------------------------------*/
char cAdafruit_VL6180X::ReadByte(uint8_t address, wchar_t reg)
{
    char data_write[2];
    char data_read[1];
    data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
    data_write[1] = reg & 0xFF; // LSB of register address
    i2c.write(address, data_write, 2);
    i2c.read(address, data_read, 1);
    return data_read[0];
}

/*--------------------------------------------------------------------------------
Function name: Init()
Input Parameters: address - target TOF sensor address
Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Description: Performs initial setup of the TOF sensor, from STM recommended start up config.
----------------------------------------------------------------------------------*/
int cAdafruit_VL6180X::Init(uint8_t address)
{
    char reset;
    reset = ReadByte(address, 0x016);
    if (reset==1) { // check to see has it be Initialised already
    
        // Mandatory : private registers
        WriteByte(address,0x0207, 0x01);
        WriteByte(address,0x0208, 0x01);
        WriteByte(address,0x0096, 0x00);
        WriteByte(address,0x0097, 0xfd);
        WriteByte(address,0x00e3, 0x00);
        WriteByte(address,0x00e4, 0x04);
        WriteByte(address,0x00e5, 0x02);
        WriteByte(address,0x00e6, 0x01);
        WriteByte(address,0x00e7, 0x03);
        WriteByte(address,0x00f5, 0x02);
        WriteByte(address,0x00d9, 0x05);
        WriteByte(address,0x00db, 0xce);
        WriteByte(address,0x00dc, 0x03);
        WriteByte(address,0x00dd, 0xf8);
        WriteByte(address,0x009f, 0x00);
        WriteByte(address,0x00a3, 0x3c);
        WriteByte(address,0x00b7, 0x00);
        WriteByte(address,0x00bb, 0x3c);
        WriteByte(address,0x00b2, 0x09);
        WriteByte(address,0x00ca, 0x09);
        WriteByte(address,0x0198, 0x01);
        WriteByte(address,0x01b0, 0x17);
        WriteByte(address,0x01ad, 0x00);
        WriteByte(address,0x00ff, 0x05);
        WriteByte(address,0x0100, 0x05);
        WriteByte(address,0x0199, 0x05);
        WriteByte(address,0x01a6, 0x1b);
        WriteByte(address,0x01ac, 0x3e);
        WriteByte(address,0x01a7, 0x1f);
        WriteByte(address,0x0030, 0x00);
        
        // Recommended : Public registers - See data sheet for more detail
        WriteByte(address,0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes
        WriteByte(address,0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time)
        WriteByte(address,0x003f, 0x46); // Sets the light and dark gain (upper nibble). Dark gain should not be changed.
        WriteByte(address,0x0031, 0xFF); // sets the # of range measurements after which auto calibration of system is performed
        WriteByte(address,0x0040, 0x63); // Set ALS integration time to 100ms
        WriteByte(address,0x002e, 0x01); // perform a single temperature calibration of the ranging sensor
        WriteByte(address,0x001b, 0x09); // Set default ranging inter-measurement period to 100ms
        WriteByte(address,0x003e, 0x31); // Set default ALS inter-measurement period to 500ms
        WriteByte(address,0x0014, 0x24); // Configures interrupt on ‘New Sample Ready threshold event’
        WriteByte(address, 0x016, 0x00); //change fresh out of set status to 0
    } 
    return 0;
}

/*--------------------------------------------------------------------------------
Function name: setup()
Input Parameters: address - target TOF sensor address, newaddress - new address for selected TOF sensor
Output Parameters: N/A
Description: Allows configuration of I2C slave addresses for the TOF sensors
----------------------------------------------------------------------------------*/
void cAdafruit_VL6180X::Setup(uint8_t address, uint8_t newaddress)
{
    WriteByte(address, 0x0212, newaddress/2);
}

/*--------------------------------------------------------------------------------
Function name: Start_range()
Input Parameters: address - target TOF sensor address
Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Description: Allows range measurements to begin in continuous mode
----------------------------------------------------------------------------------*/
int cAdafruit_VL6180X::Start_Range(uint8_t address)
{
    WriteByte(address, 0x018,0x03);
    return 0;
}

/*--------------------------------------------------------------------------------
Function name: Poll_range()
Input Parameters: address - target TOF sensor address
Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Description: poll for new sample ready ready
----------------------------------------------------------------------------------*/
int cAdafruit_VL6180X::Poll_Range(uint8_t address)
{
    char status;
    char range_status;

// check the status
    status = ReadByte(address, 0x04f);
    range_status = status & 0x07;

// wait for new measurement ready status
    while (range_status != 0x04) {
        status = ReadByte(address, 0x04f);
        range_status = status & 0x07;
        wait_ms(1); // (can be removed)
    }
    return 0;
}

/*--------------------------------------------------------------------------------
Function name: Read_range()
Input Parameters: address - target TOF sensor address
Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Description: allows data to be read from a tergetted address (measurement in mm)
----------------------------------------------------------------------------------*/
int cAdafruit_VL6180X::Read_Range(uint8_t address)
{
    int range;
    range=ReadByte(address,0x062);
    return range;
}

/*--------------------------------------------------------------------------------
Function name: Read_range()
Input Parameters: address - target TOF sensor address
Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Description: allows interrupt flags to be cleared
----------------------------------------------------------------------------------*/
int cAdafruit_VL6180X::Clear_Interrupts(uint8_t address)
{
    WriteByte(address, 0x015,0x07);
    return 0;
}