RFD77402 ToF Library


#1

Does anyone have a library for a RFD77402 time of flight sensor? There’s an arduino library out there but I’m not sure that I’m capable of converting it.


#2

Converting that Arduino library doesn’t look like it would be particularly hard. Check out our Porting Arduino Code to the imp guide for some assistance.

Couple of at-a-glance gotchas: functions like readRegister16() take an I2C register address as an integer, but the imp’s i2c objects take strings, so you’ll need to add a conversion, eg.

addr = addr.tochar();

The imp’s i2c objects do a lot more work for you than the Arduino I2C code does, so you’ll simple replace beginTransmission(), write(), endTransmission() and requestFrom() with read(), for example.


#3

Once you get me on one of these…

Here’s a quick and dirty port. The code compiles, but I don’t have the hardware to try it out. It will need debugging, but it should get you 90% to where you want to be. Just add it to the top of your device code:

const RFD77402_ICSR = 0x00;
const RFD77402_INTERRUPTS = 0x02;
const RFD77402_COMMAND = 0x04;
const RFD77402_DEVICE_STATUS = 0x06;
const RFD77402_RESULT = 0x08;
const RFD77402_RESULT_CONFIDENCE = 0x0A;
const RFD77402_CONFIGURE_A = 0x0C;
const RFD77402_CONFIGURE_B = 0x0E;
const RFD77402_HOST_TO_MCPU_MAILBOX = 0x10;
const RFD77402_MCPU_TO_HOST_MAILBOX = 0x12;
const RFD77402_CONFIGURE_PMU = 0x14;
const RFD77402_CONFIGURE_I2C = 0x1C;
const RFD77402_CONFIGURE_HW_0 = 0x20;
const RFD77402_CONFIGURE_HW_1 = 0x22;
const RFD77402_CONFIGURE_HW_2 = 0x24;
const RFD77402_CONFIGURE_HW_3 = 0x26;
const RFD77402_MOD_CHIP_ID = 0x28;

const RFD77402_MODE_MEASUREMENT = 0x01;
const RFD77402_MODE_STANDBY = 0x10;
const RFD77402_MODE_OFF = 0x11;
const RFD77402_MODE_ON = 0x12;

const CODE_VALID_DATA = 0x00;
const CODE_FAILED_PIXELS = 0x01;
const CODE_FAILED_SIGNAL = 0x02;
const CODE_FAILED_SATURATED = 0x03;
const CODE_FAILED_NOT_NEW = 0x04;
const CODE_FAILED_TIMEOUT = 0x05;

const I2C_SPEED_STANDARD = 100000;
const I2C_SPEED_FAST = 400000;

class RFD77402 {

    _i2cPort = null;
    _i2cAddr = null;
    calibrationData = null;

    distance = 0;
    validPixels = 0;
    confidenceValue = 0;

    constructor(i2cBus = null, i2cAddr = 0x4C) {
        if (i2cBus == null) throw "RFD77402() requires a non-null imp I2C bus object";

        // Store the chosen imp I2C bus
        _i2cPort = i2cBus;

        // Convert 7-bit I2C address to 8-bit
        _i2cAddr = i2cAddr << 1;

        // Set up the calibration data array
        calibrationData = array(54);

        // Initialize the device
        local r = begin();
        server.log("Sensor initialization " + (r ? "succeeded" : "failed"));
    }

    //Sets up the sensor for constant read
    //Returns false if sensor does not respond
    function begin() {
        if (getChipID() < 0xAD00) return false;

        //Put chip into standby
        if (goToStandbyMode() == false) return false; //Chip timed out before going to standby

        //Drive INT_PAD high
        local setting = readRegister(RFD77402_ICSR);
        setting = setting | (1 << 2); //Set the bit
        writeRegister(RFD77402_ICSR, setting);

        //Configure I2C Interface
        writeRegister(RFD77402_CONFIGURE_I2C, 0x65); //0b.0110.0101 = Address increment, auto increment, host debug, MCPU debug

        //Set initialization - Magic from datasheet. Write 0x05 to 0x15 location.
        writeRegister16(RFD77402_CONFIGURE_PMU, 0x0500); //0b.0000.0101.0000.0000 //Patch_code_id_en, Patch_mem_en

        if (goToOffMode() == false) return false; //MCPU never turned off

        //Set initialization - Magic from datasheet. Write 0x06 to 0x15 location.
        writeRegister16(RFD77402_CONFIGURE_PMU, 0x0600); //MCPU_Init_state, Patch_mem_en

        if (goToOnMode() == false) return (false); //MCPU never turned on

        //ToF Configuration
        setPeak(0x0E); //Suggested values from page 20
        setThreshold(0x01);

        writeRegister16(RFD77402_CONFIGURE_B, 0x10FF); //Set valid pixel. Set MSP430 default config.
        writeRegister16(RFD77402_CONFIGURE_HW_0, 0x07D0); //Set saturation threshold = 2,000.
        writeRegister16(RFD77402_CONFIGURE_HW_1, 0x5008); //Frequecy = 5. Low level threshold = 8.
        writeRegister16(RFD77402_CONFIGURE_HW_2, 0xA041); //Integration time = 10 * (6500-20)/15)+20 = 4.340ms. Plus reserved magic.
        writeRegister16(RFD77402_CONFIGURE_HW_3, 0x45D4); //Enable harmonic cancellation. Enable auto adjust of integration time. Plus reserved magic.

        if (goToStandbyMode() == false) return false; //Error - MCPU never went to standby

        //Whew! We made it through power on configuration

        //Put device into Standby mode
        if (goToStandbyMode() == false) return false; //Error - MCPU never went to standby

        //Now assume user will want sensor in measurement mode

        //Set initialization - Magic from datasheet. Write 0x05 to 0x15 location.
        writeRegister16(RFD77402_CONFIGURE_PMU, 0x0500); //Patch_code_id_en, Patch_mem_en

        if (goToOffMode() == false) return (false); //Error - MCPU never turned off

        //Set initialization - Magic from datasheet. Write 0x06 to 0x15 location.
        writeRegister16(RFD77402_CONFIGURE_PMU, 0x0600); //MCPU_Init_state, Patch_mem_en

        if (goToOnMode() == false) return false; //Error - MCPU never turned on

        return true; //Success! Sensor is ready for measurements
    }

    //Takes a single measurement and sets the global variables with new data
    //Returns zero if reading is good, otherwise return the errorCode from the result register.
    function takeMeasurement() {
        if (goToMeasurementMode() == false) return CODE_FAILED_TIMEOUT; //Error - Timeout
        //New data is now available!

        //Read result
        local resultRegister = readRegister16(RFD77402_RESULT);

        if (resultRegister & 0x7FFF) { //Reading is valid
            local errorCode = (resultRegister >> 13) & 0x03;

            if (errorCode == 0) {
                distance = (resultRegister >> 2) & 0x07FF; //Distance is good. Read it.
                server.log("Distance field valid");

                //Read confidence register
                local confidenceRegister = readRegister16(RFD77402_RESULT_CONFIDENCE);
                validPixels = confidenceRegister & 0x0F;
                confidenceValue = (confidenceRegister >> 4) & 0x07FF;
            }

            return (errorCode);
        } else {
            //Reading is not vald
            return CODE_FAILED_NOT_NEW; //Error code for reading is not new
        }
    }

    //Returns the local variable to the caller
    function getDistance() {
        return distance;
    }

    //Returns the number of valid pixels found when taking measurement
    function getValidPixels() {
        return validPixels;
    }

    //Returns the qualitative value representing how confident the sensor is about its reported distance
    function getConfidenceValue() {
        return confidenceValue;
    }

    //Read the command opcode and covert to mode
    function getMode() {
        return (readRegister(RFD77402_COMMAND) & 0x3F);
    }

    //Tell MCPU to go to standby mode
    //Return true if successful
    function goToStandbyMode() {
        //Set Low Power Standby
        writeRegister(RFD77402_COMMAND, 0x90); //0b.1001.0000 = Go to standby mode. Set valid command.

        //Check MCPU_ON Status
        for (local x = 0 ; x < 10 ; x++) {
            if ((readRegister16(RFD77402_DEVICE_STATUS) & 0x001F) == 0x0000) return true; //MCPU is now in standby
            delay(100); //Suggested timeout for status checks from datasheet
        }

        return false; //Error - MCPU never went to standby
    }

    //Tell MCPU to go to off state
    //Return true if successful
    function goToOffMode() {
        //Set MCPU_OFF
        writeRegister(RFD77402_COMMAND, 0x91); //0b.1001.0001 = Go MCPU off state. Set valid command.

        //Check MCPU_OFF Status
        for (local x = 0 ; x < 10 ; x++) {
            if ((readRegister16(RFD77402_DEVICE_STATUS) & 0x001F) == 0x0010) return true; //MCPU is now off
            delay(10); //Suggested timeout for status checks from datasheet
        }

        return false; //Error - MCPU never turned off
    }

    //Tell MCPU to go to on state
    //Return true if successful
    function goToOnMode() {
        //Set MCPU_ON
        writeRegister(RFD77402_COMMAND, 0x92); //0b.1001.0010 = Wake up MCPU to ON mode. Set valid command.

        //Check MCPU_ON Status
        for (local x = 0 ; x < 10 ; x++) {
            if ((readRegister16(RFD77402_DEVICE_STATUS) & 0x001F) == 0x0018) return true; //MCPU is now on
            delay(10); //Suggested timeout for status checks from datasheet
        }

        return false; //Error - MCPU never turned on
    }

    //Tell MCPU to go to measurement mode
    //Takes a measurement. If measurement data is ready, return true
    function goToMeasurementMode() {
        //Single measure command
        writeRegister(RFD77402_COMMAND, 0x81); //0b.1000.0001 = Single measurement. Set valid command.

        //Read ICSR Register - Check to see if measurement data is ready
        for (local x = 0 ; x < 10 ; x++) {
            if ((readRegister(RFD77402_ICSR) & (1 << 4)) != 0) return true; //Data is ready!
            delay(10); //Suggested timeout for status checks from datasheet
        }

        return false; //Error - Timeout
    }

    //Returns the VCSEL peak 4-bit value
    function getPeak() {
        local configValue = readRegister16(RFD77402_CONFIGURE_A);
        return (configValue >> 12) & 0x0F;
    }

    //Sets the VCSEL peak 4-bit value
    function setPeak(peakValue) {
        local configValue = readRegister16(RFD77402_CONFIGURE_A); //Read
        configValue = configValue & ~0xF000;// Zero out the peak configuration bits
        configValue = configValue | (peakValue << 12); //Mask in user's settings
        writeRegister16(RFD77402_CONFIGURE_A, configValue); //Write in this new value
    }

    //Returns the VCSEL Threshold 4-bit value
    function getThreshold() {
        local configValue = readRegister16(RFD77402_CONFIGURE_A);
        return (configValue >> 8) & 0x0F;
    }

    //Sets the VCSEL Threshold 4-bit value
    function setThreshold(thresholdValue) {
        local configValue = readRegister16(RFD77402_CONFIGURE_A); //Read
        configValue = configValue & ~0x0F00;// Zero out the threshold configuration bits
        configValue = configValue | (thresholdValue << 8); //Mask in user's settings
        writeRegister16(RFD77402_CONFIGURE_A, configValue); //Write in this new value
    }

    //Returns the VCSEL Frequency 4-bit value
    function getFrequency() {
        local configValue = readRegister16(RFD77402_CONFIGURE_HW_1);
        return (configValue >> 12) & 0x0F;
    }

    //Sets the VCSEL Frequency 4-bit value
    function setFrequency(thresholdValue) {
        local configValue = readRegister16(RFD77402_CONFIGURE_HW_1); //Read
        configValue = configValue & ~0xF000;// Zero out the threshold configuration bits
        configValue = configValue | (thresholdValue << 12); //Mask in user's settings
        writeRegister16(RFD77402_CONFIGURE_HW_1, configValue); //Write in this new value
    }

    //Gets whatever is in the 'MCPU to Host' mailbox
    //Check ICSR bit 5 before reading
    function getMailbox() {
        return (readRegister16(RFD77402_MCPU_TO_HOST_MAILBOX));
    }

    //Software reset the device
    function reset() {
        writeRegister(RFD77402_COMMAND, 1 << 6);
        delay(100);
    }

    //Returns the chip ID
    //Should be 0xAD01 or higher
    function getChipID() {
        return(readRegister16(RFD77402_MOD_CHIP_ID));
    }

    //Retreive 2*27 bytes from MCPU for computation of calibration parameters
    //This is 9.2.2 from datasheet
    //Reads 54 bytes into the calibration[] array
    //Returns true if new cal data is loaded
    function getCalibrationData() {
        if (goToOnMode() == false) return false; //Error - sensor timed out before getting to On Mode

        //Check ICSR Register and read Mailbox until it is empty
        local messages = 0;
        while (1) {
            if ((readRegister(RFD77402_ICSR) & (1 << 5)) == 0) break; //Mailbox interrupt is cleared

            //Mailbox interrupt (Bit 5) is set so read the M2H mailbox register
            getMailbox(); //Throw it out. Just read to clear the register.

            if (messages++ > 27) return false; //Error - Too many messages

            delay(10); //Suggested timeout for status checks from datasheet
        }

        //Issue mailbox command
        writeRegister16(RFD77402_HOST_TO_MCPU_MAILBOX, 0x0006); //Send 0x0006 mailbox command

        //Check to see if Mailbox can be read
        //Read 54 bytes of payload into the calibration[54] array
        for (local message = 0 ; message < 27 ; message++) {
            //Wait for bit to be set
            local x = 0;
            while (1) {
                local icsr = readRegister(RFD77402_ICSR);
                if ((icsr & (1 << 5)) != 0) break; //New message in available

                if (x++ > 10) return false; //Error - Timeout

                delay(10); //Suggested timeout for status checks from datasheet
            }

            local incoming = getMailbox(); //Get 16-bit message

            //Put message into larger calibrationData array
            calibrationData[message * 2] = incoming >> 8;
            calibrationData[message * 2 + 1] = incoming & 0xFF;
        }
    }

    //Reads two bytes from a given location from the RFD77402
    function readRegister16(addr) {
        local data = _i2cPort.read(_i2cAddr, addr.tochar(), 2);
        local lower = data[0];
        local higher = data[1];
        return ((higher << 8) | lower);
    }

    //Reads from a given location from the RFD77402
    function readRegister(addr) {
        local data = _i2cPort.read(_i2cAddr, addr.tochar(), 1);
        if (data != null) return data;
        return 0xFF; //Error
    }

    //Write a 16 bit value to a spot in the RFD77402
    function writeRegister16(addr, val) {
        local lower = (val & 0xFF).tointeger();
        local higher = (val >> 8).tointeger();
        _i2cPort.write(_i2cAddr, addr.tochar() + lower.tochar() + higher.tochar());
    }

    //Write a value to a spot in the RFD77402
    function writeRegister(addr, val) {
        _i2cPort.write(_i2cAddr, addr.tochar() + val.tochar());
    }

    // Blocks for ms milliseconds - replicates Arduino delay()
    function delay(ms) {
        local a = hardware.millis() + ms;
        do {
            // NOP
        } while (hardware.millis() <= a);
    }
}

You’ll need to configure your I2C bus first, and then instantiate the class:

local i2c = hardware.i2c89; // Assuming you're using an imp001
i2c.configure(CLOCK_SPEED_400_KHZ);

local tof = RFD77402(i2c);

function loop() {
    // Read and display a distance measurement every 5 seconds
    if (tof.readMeasurement() == 0) server.log(rof.getDistance());
    imp.wakeup(5, loop);
}

loop();

#4

@smittytone – you are the man. Much appreciated.

I saw your original response and rolled up my sleeves this morning and began porting it myself (sorry work). There were some spots where I didn’t completely understand how to port it (mostly bit shifting stuff), so I would like to take what you sent and compare to the original library to educate myself on this. This isn’t the first library I wish I could have ported. I’ll go ahead and order the hardware and post back on my results.


#5

To learn, the best thing is to try it yourself, but fall back on my code when you get stuck, or you’re unsure how to proceed. As I say, my code isn’t tested, so you may very well need to adjust the code when you run it on real hardware.