Diff: VL6180.cpp

Revision:

10:c752a8d76ee2

Child:

11:0b9098ec11c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VL6180.cpp	Tue Dec 17 16:33:46 2019 +0000
@@ -0,0 +1,263 @@
+#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
+
+/*------------------------------------------------------------------------------
+//  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 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;
+}