File content as of revision 11:0b9098ec11c7:

#include "mbed.h"
#include "General.hpp"
#include "rtos.h"
#include "Pins.h"
#include "VL6180.hpp"
#include <vector>

//#define DEBUG_TOF
#ifdef DEBUG_TOF
Serial debug_term(SERIAL_TX, SERIAL_RX);
#endif


#define ROS_TOF
int Global_TOF_Ranges[num_VL6180];
bool TOF_Range_Flag;
Mutex Global_TOF_Range_Mutex;

/*------------------------------------------------------------------------------
//  Main Program loop
------------------------------------------------------------------------------*/
int TOF_Thread()
{
    /* Pins are (PC_9, PC_11, PD_2, PG_3) */
    I2C i2c_bus(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE

    //Dynamically create VL6180 objects for the TOF sensors
    PinName SHDN_Pins[num_VL6180] = SHDN_Pins_Cell;
    char Addresses[num_VL6180] = Shifted_TOF_Addresses;
    TOFsPtr * TOF = new TOFsPtr[num_VL6180];
    for (int idx = 0; idx < num_VL6180; ++idx) {
        TOF[idx] = new VL6180(i2c_bus, SHDN_Pins[idx], Addresses[idx]);
    }

    // Initialise all TOF sensors
    Init_All_TOFs(TOF);

    //Create range variable to stored read TOF values
    int Range[num_VL6180] = {};

    //note that sensor 1 can be left at the default address, since other sensors
    //are already allocated. Adding more I2C devices would require it to be
    //reassigned to a different address, however.
    while (1) {
        for(int IDX = 0; IDX < num_VL6180; IDX++) {
            TOF[IDX]->Start_Range();
            TOF[IDX]->Poll_Range();
            Range[IDX] = TOF[IDX]->Read_Range();
            TOF[IDX]->Clear_Interrupts();

            wait_ms(10);
        }
        
#ifdef ROS_TOF
    Global_TOF_Range_Mutex.lock();
    for(int IDX = 0; IDX < num_VL6180; IDX++) {
        Global_TOF_Ranges[IDX] = Range[IDX];
    }
    if(TOF_Range_Flag == false)
    {
        TOF_Range_Flag = true;
    }
    Global_TOF_Range_Mutex.unlock();
#endif

#ifdef DEBUG_TOF
        debug_term.printf("|--------------------------------\n\r"
                          "|Ranges:                         \n\r"
                          "| %d\n\r",Range[0]);
                          
                          /*
                          "| %d\n\r"
                          "| %d\n\r"
                          "| %d\n\r",Range[0], Range[1], Range[2], Range[3]);
                          */
#endif

        wait_ms(100);

    }
}


/*------------------------------------------------------------------------------
    Function turns TOF sensor ON or OFF
------------------------------------------------------------------------------*/
bool VL6180::TOF_PWR(bool State)
{
    this->shutdown = State;
    return State;   
}

/*------------------------------------------------------------------------------
    Split 16-bit register address into two bytes and write
    the address + data via I²C
------------------------------------------------------------------------------*/
void VL6180::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;
    this->i2c.write(addr, data_write, 3);
}
void VL6180::WriteByte_Uninitialized(wchar_t reg, char data, char Uninitialized_Address) {    
    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;
    this->i2c.write(Uninitialized_Address, data_write, 3);
}

/*------------------------------------------------------------------------------
    Split 16-bit register address into two bytes and write
    required register address to VL6180 and read the data back
------------------------------------------------------------------------------*/
char VL6180::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

    this->i2c.write(addr, data_write, 2);
    this->i2c.read(addr, data_read, 1);
    return data_read[0];
}
char VL6180::ReadByte_Uninitialized(wchar_t reg, char Uninitialized_Address) {
    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
   
    this->i2c.write(Uninitialized_Address, data_write, 2);
    this->i2c.read(Uninitialized_Address, data_read, 1);
    return data_read[0];
}

/*------------------------------------------------------------------------------
    Start a range measurement in single shot mode
------------------------------------------------------------------------------*/
int VL6180::Start_Range(void)
{
    this->WriteByte(0x018,0x03);
    return 0;
}

/*------------------------------------------------------------------------------
    poll for new sample ready ready
------------------------------------------------------------------------------*/
int VL6180::Poll_Range(void)
{
    char status;
    char range_status;

    // check the status
    status = this->ReadByte(0x04f);
    range_status = status & 0x07;

    // wait for new measurement ready status
    while (range_status !=  0x00) {
        status = this->ReadByte(0x04f);
        range_status = status & 0x07;
        //wait_ms(50); // (can be removed)
    }

    return 0;
}


/*------------------------------------------------------------------------------
    Read range result (mm)
------------------------------------------------------------------------------*/
int VL6180::Read_Range(void)
{
    int range;
    range = this->ReadByte(0x062);
    return range;
}

/*------------------------------------------------------------------------------
    clear interrupts
------------------------------------------------------------------------------*/

int VL6180::Clear_Interrupts(void)
{
    this->WriteByte(0x015,0x07);
    return 0;
}

/*------------------------------------------------------------------------------
    load settings
------------------------------------------------------------------------------*/
bool VL6180::Init(void)
{
    // check to see has if the TOF sensor has been Initialised already
    char reset = this->ReadByte_Uninitialized(0x016, 0x82);    
    if (reset==1)
    {            
        // Mandatory : private registers
        this->WriteByte_Uninitialized(0x0207, 0x01, 0x82);
        this->WriteByte_Uninitialized(0x0208, 0x01, 0x82);
        this->WriteByte_Uninitialized(0x0096, 0x00, 0x82);
        this->WriteByte_Uninitialized(0x0097, 0xfd, 0x82);
        this->WriteByte_Uninitialized(0x00e3, 0x01, 0x82);
        this->WriteByte_Uninitialized(0x00e4, 0x03, 0x82);
        this->WriteByte_Uninitialized(0x00e5, 0x02, 0x82);
        this->WriteByte_Uninitialized(0x00e6, 0x01, 0x82);
        this->WriteByte_Uninitialized(0x00e7, 0x03, 0x82);
        this->WriteByte_Uninitialized(0x00f5, 0x02, 0x82);
        this->WriteByte_Uninitialized(0x00d9, 0x05, 0x82);
        this->WriteByte_Uninitialized(0x00db, 0xce, 0x82);
        this->WriteByte_Uninitialized(0x00dc, 0x03, 0x82);
        this->WriteByte_Uninitialized(0x00dd, 0xf8, 0x82);
        this->WriteByte_Uninitialized(0x009f, 0x00, 0x82);
        this->WriteByte_Uninitialized(0x00a3, 0x3c, 0x82);
        this->WriteByte_Uninitialized(0x00b7, 0x00, 0x82);
        this->WriteByte_Uninitialized(0x00bb, 0x3c, 0x82);
        this->WriteByte_Uninitialized(0x00b2, 0x09, 0x82);
        this->WriteByte_Uninitialized(0x00ca, 0x09, 0x82);
        this->WriteByte_Uninitialized(0x0198, 0x01, 0x82);
        this->WriteByte_Uninitialized(0x01b0, 0x17, 0x82);
        this->WriteByte_Uninitialized(0x01ad, 0x00, 0x82);
        this->WriteByte_Uninitialized(0x00ff, 0x05, 0x82);
        this->WriteByte_Uninitialized(0x0100, 0x05, 0x82);
        this->WriteByte_Uninitialized(0x0199, 0x05, 0x82);
        this->WriteByte_Uninitialized(0x01a6, 0x1b, 0x82);
        this->WriteByte_Uninitialized(0x01ac, 0x3e, 0x82);
        this->WriteByte_Uninitialized(0x01a7, 0x1f, 0x82);
        this->WriteByte_Uninitialized(0x0030, 0x00, 0x82);
        this->WriteByte_Uninitialized(0x016, 0x00, 0x82); //change fresh out of set status to 0 
    } else {
        return true;
    }
    return false;
}


/*------------------------------------------------------------------------------
    Function to Initialise all TOF sensors
------------------------------------------------------------------------------*/
bool Init_All_TOFs(TOFsPtr *tof)
{
    bool Err_Flag = false;
    
    char Non_Shifted_Addresses[num_VL6180] = TOF_Addresses;
    
    for(int IDX = 0; IDX < num_VL6180; IDX++)
    {
        tof[IDX]->TOF_PWR(false);
    }
    
    wait_ms(0.5);

    if(num_VL6180 > 1)
    {
        for(int IDX = 1; IDX < num_VL6180; IDX++)
        {
            tof[IDX]->TOF_PWR(true);
            
            if(tof[IDX]->Init() == true)
            {
                Err_Flag = true;    
            }
            tof[IDX]->WriteByte(0x212, Non_Shifted_Addresses[IDX]);

        }
    }
    
    tof[0]->TOF_PWR(true);         
    tof[0]->Init();
    tof[0]->WriteByte(0x212, Non_Shifted_Addresses[0]); 

    return Err_Flag;
}