l3gd20.h

Go to the documentation of this file.

#ifndef LIBPROPELLER_L3GD20_H_
#define LIBPROPELLER_L3GD20_H_

#ifndef UNIT_TEST
#include "libpropeller/i2c/i2c.h"
#else
#include "libpropeller/i2c/i2cMOCK.h"
#endif

class L3GD20 {
public:

    enum AddressLSB {
        LSB_0, LSB_1
    };
    
    L3GD20(){
        bus_ = NULL;
        status_ = false;
    }

    bool Init(I2C * i2c_bus, const AddressLSB address = LSB_0) {
        SetAddress(address);
        bus_ = i2c_bus;

        //Check to make sure the gyro is actually there.
        status_ = bus_->Ping(device_address);
        if (status_ == false) {
            return false;
        }

        bus_->Put(device_address, kCTRL_REG1, 0b11111111);
        bus_->Put(device_address, kCTRL_REG4, 0b00110000);

        return status_;
    }

    bool ReadGyro(int& x, int& y, int& z) {
        char data[6];

        if (status_ == false) {
            x = y = z = 0;
            return false;
        }

        if (bus_->Get(device_address, kOUT_X_L, data, 6) == false) {
            x = y = z = 0;
            return false;
        }
        x = ((data[0] | (data[1] << 8)) << 16) >> 16;
        y = ((data[2] | (data[3] << 8)) << 16) >> 16;
        z = ((data[4] | (data[5] << 8)) << 16) >> 16;

        return true;
    }

private:
    I2C * bus_;

    int status_;


    const static unsigned char kCTRL_REG1 = 0x20;
    const static unsigned char kCTRL_REG4 = 0x23;
    const static unsigned char kOUT_X_L = 0x28 | 0x80; //(turn on auto increment)

    unsigned char device_address;

    void SetAddress(const AddressLSB address) {
        if (address == LSB_0) {
            device_address = 0b11010100;
        } else if (address == LSB_1) {
            device_address = 0b11010110;
        }
    }

};

#endif // LIBPROPELLER_L3GD20_H_