File content as of revision 2:c537f1ebad7b:

#include"mbed.h"
#include <ros.h>

#include "Sensor.h"

Sensor::Sensor(PinName sda, PinName scl, PinName shdn)
: i2c(sda, scl), SHDN(shdn) 
{
    addr = DEFAULT_DEVICE_ADDRESS << 1; 
}

///////////////////////////////////////////////////////////////////
// TODO
///////////////////////////////////////////////////////////////////
int Sensor::init()
{
    char reset;

    // check to see has it be Initialised already
    reset = readByte(0x016);
    if (reset==1) {
        // Mandatory settings : private registers
        setRegister(0x0207, 0x01);
        setRegister(0x0208, 0x01);
        setRegister(0x0096, 0x00);
        setRegister(0x0097, 0xfd);
        setRegister(0x00e3, 0x01);
        setRegister(0x00e4, 0x03);
        setRegister(0x00e5, 0x02);
        setRegister(0x00e6, 0x01);
        setRegister(0x00e7, 0x03);
        setRegister(0x00f5, 0x02);
        setRegister(0x00d9, 0x05);
        setRegister(0x00db, 0xce);
        setRegister(0x00dc, 0x03);
        setRegister(0x00dd, 0xf8);
        setRegister(0x009f, 0x00);
        setRegister(0x00a3, 0x3c);
        setRegister(0x00b7, 0x00);
        setRegister(0x00bb, 0x3c);
        setRegister(0x00b2, 0x09);
        setRegister(0x00ca, 0x09);
        setRegister(0x0198, 0x01);
        setRegister(0x01b0, 0x17);
        setRegister(0x01ad, 0x00);
        setRegister(0x00ff, 0x05);
        setRegister(0x0100, 0x05);
        setRegister(0x0199, 0x05);
        setRegister(0x01a6, 0x1b);
        setRegister(0x01ac, 0x3e);
        setRegister(0x01a7, 0x1f);
        setRegister(0x0030, 0x00);

        setRegister(SYSTEM_FRESH_OUT_OF_RESET, 0x00); //change fresh out of set status to 0
    } else {
        return -1;
    }
    return 0;
}

void Sensor::changeAddress(char address)
{
    setRegister(I2C_SLAVE_DEVICE_ADDRESS, address);
    addr = address << 1;
}

///////////////////////////////////////////////////////////////////
// New measurement
///////////////////////////////////////////////////////////////////
int Sensor::read()
{
    int range;

    // Start range measurement
    startRange();

    // Poll the VL6180 till new sample ready
    pollRange();

    // Read range result
    range = readRange();

    // Clear the interrupt on VL6180
    clearInterrupts();

    // Display result
    return range;
}

void Sensor::startRange()
{
    writeByte(0x018,0x01);
}

void Sensor::pollRange()
{
    char status;
    char range_status;

    // check the status
    status = readByte(RESULT_INTERRUPT_STATUS_GPIO);
    range_status = status & 0x07;

    // wait for new measurement ready status
    while (range_status !=  0x00) {
        status = readByte(RESULT_INTERRUPT_STATUS_GPIO);
        range_status = status & 0x07;
    }
}

int Sensor::readRange()
{
    int range;
    range = readByte(RESULT_RANGE_VAL);
    return range;
}

void Sensor::clearInterrupts()
{
    writeByte(0x015,0x07);
}

void Sensor::turnOff()
{
    SHDN = 0;   
}

void Sensor::turnOn()
{
    SHDN = 1;   
}

///////////////////////////////////////////////////////////////////
// Update a VL6180X register
///////////////////////////////////////////////////////////////////
void Sensor::setRegister(wchar_t reg, char data)
{
    writeByte(reg, data);
}

///////////////////////////////////////////////////////////////////
// Split 16-bit register address into two bytes and write
// the address + data via I²C
///////////////////////////////////////////////////////////////////
void Sensor::writeByte(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(addr, data_write, 3);
}

///////////////////////////////////////////////////////////////////
// Split 16-bit register address into two bytes and write
// required register address to VL6180 and read the data back
///////////////////////////////////////////////////////////////////
char Sensor::readByte(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(addr, data_write, 2);
    i2c.read(addr, data_read, 1);
    return data_read[0];
}