AccelSensor.cpp

00001 #include "mbed.h"
00002 #include "AccelSensor.h"
00003 Serial pc1(USBTX, USBRX); // tx, rx
00004 
00005 AccelSensor::AccelSensor(PinName sda, PinName scl) : _i2c(sda, scl) {
00006     //No need to initialise anything else.
00007 }
00008 
00009 void AccelSensor::init() {
00010     char c = readRegister(WHO_AM_I); // Read WHO_AM_I register
00011     standby(); // Must be in standby to change registers
00012     char scale = GSCALE; // Set up the full scale range to 2, 4, or 8g.
00013     if (scale > 8) scale = 8; //Easy error check
00014     scale >>= 2; // Neat trick, see page 22. 00 = 2G, 01 = 4G, 10 = 8G
00015     writeRegister(XYZ_DATA_CFG, scale);
00016     active(); // Set to active to start reading
00017 }
00018 
00019 void AccelSensor::standby() {
00020     char c = readRegister(CTRL_REG1);
00021     writeRegister(CTRL_REG1, c & ~(0x01)); //Clear the active bit to go into standby
00022 }
00023 
00024 void AccelSensor::active() {
00025     char c = readRegister(CTRL_REG1);
00026     writeRegister(CTRL_REG1, c | 0x01); //Set the active bit to begin detection
00027 }
00028 
00029 void AccelSensor::readData(int *destination) {
00030     char rawData[6]; // x/y/z accel register data stored here
00031     
00032     
00033     readRegisters(OUT_X_MSB, 6, rawData); // Read the six raw data registers into data array
00034     // Loop to calculate 12-bit ADC and g value for each axis
00035     for(int i = 0; i < 3 ; i++) {
00036         int value = (rawData[i*2] << 8) | rawData[(i*2)+1]; //Combine the two 8 bit registers into one 12-bit number
00037         value >>= 4; //The registers are left align, here we right align the 12-bit integer
00038         // If the number is negative, we have to make it so manually (no 12-bit data type)
00039         if (rawData[i*2] > 0x7F) {
00040             value |= 0xFFFFF << 12;
00041         }
00042         destination[i] = value;
00043     }
00044     pc1.printf("Raw: %d %d %d %d %d %d\r\n" , rawData[0],rawData[1],rawData[2],rawData[3],rawData[4],rawData[5]);
00045 }
00046 
00047 void AccelSensor::readRegisters(char reg, int range, char* dest) {
00048     int ack = 0;
00049     _i2c.start();
00050     ack = _i2c.write((ADDRESS << 1));
00051     ack = _i2c.write(reg);
00052     _i2c.start();
00053     ack = _i2c.write((ADDRESS << 1) | 0x01);
00054     for (int i = 0; i < range - 1; i++) dest[i] = _i2c.read(1);
00055     dest[range - 1] = _i2c.read(0);
00056     _i2c.stop();
00057 }
00058 
00059 char AccelSensor::readRegister(char reg) {
00060     int ack = 0;
00061     _i2c.start();
00062     ack = _i2c.write((ADDRESS << 1));
00063     ack = _i2c.write(reg);
00064     _i2c.start();
00065     ack = _i2c.write((ADDRESS << 1) | 0x01);
00066     char result = _i2c.read(0);
00067     _i2c.stop();
00068     return result;
00069 }
00070 
00071 void AccelSensor::writeRegister(char reg, char data) {
00072     int ack = 0;
00073     _i2c.start();
00074     ack = _i2c.write((ADDRESS << 1));
00075     ack = _i2c.write(reg);
00076     ack = _i2c.write(data);
00077     _i2c.stop();
00078 }