Diff: sensor/Sensor.cpp

Revision:

0:57855aafa907

Child:

1:d5adc483bce0

diff -r 000000000000 -r 57855aafa907 sensor/Sensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensor/Sensor.cpp	Tue Oct 22 09:12:03 2019 +0000
@@ -0,0 +1,163 @@
+#include"mbed.h"
+#include <ros.h>
+
+#include "Sensor.h"
+
+
+
+Sensor::Sensor(char address, PinName sda, PinName scl, PinName shdn)
+: i2c(sda, scl), SHDN(shdn) 
+{
+    addr = DEFAULT_SENSOR_ADDRESS;
+    init(); 
+    addr = address;   
+}
+
+///////////////////////////////////////////////////////////////////
+// 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::startRange()
+{
+    writeByte(0x018,0x03);
+}
+
+///////////////////////////////////////////////////////////////////
+// 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::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);
+}
+
+///////////////////////////////////////////////////////////////////
+// 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];
+}