File content as of revision 8:19dd2a538cbe:

#include "Accelerometer.h"

Accelerometer::Accelerometer(int nI2cAddress, LCD_ST7735* pDisp) : i2cAddress(nI2cAddress), i2c(P0_5, P0_4)
{   // constructor
    this->i2c.frequency(400000);        // fast I2C is 400 KHz, not 400 Hz. Default frequency is 100 KHz
    this->writeRegister(0x2A, 0x01); // initialize accelerometer (set CTRL_REG1 bit ACTIVE)
    this->pDisp=pDisp;

    this->colors[0] = Color565::Red;
    this->colors[1] = Color565::Green;
    this->colors[2] = Color565::Blue;
}

void Accelerometer::readRegisters(char address, char* buffer, int len)
{
    this->i2c.write(i2cAddress, &address, 1, true);
    this->i2c.read(i2cAddress | 1, buffer, len);
}

int Accelerometer::writeRegister(char address, char value) {    
    char buffer[2] = { address, value };
    
    return this->i2c.write(i2cAddress, buffer, 2);
}

double Accelerometer::convert(char* buffer)
{
    double val = ((buffer[0] << 2) | (buffer[1] >> 6));
            
    if (val > 511.0) 
        val -= 1024.0;
    
    return val / 512.0;
}

void Accelerometer::getXYZ(double& x, double& y, double& z) {
    char buffer[6];
    
    this->readRegisters(0x01, buffer, 6);
    
    x = this->convert(buffer);
    y = this->convert(buffer + 2);
    z = this->convert(buffer + 4);
}

//
// Accellerator graph for debug purposes
//

void Accelerometer::drawAxes()
{
    for (int i = 0; i < 3; i++) {
        this->pDisp->fillRect(0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING), this->pDisp->getWidth(), i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT, Color565::fromRGB(i==0?0x22:0, i==1?0x22:0, i==2?0x22:0));
        this->pDisp->drawLine(0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING), 0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT, Color565::White);
        this->pDisp->drawLine(0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT / 2, this->pDisp->getWidth(), i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT / 2, Color565::White);
    }
}

void Accelerometer::drawPoint(int axis, double value)
{
    if (value < -1.0)
        value = -1.0;

    if (value > 1.0)
        value = 1.0;

    value += 1.0;
    value /= 2.0;
    value = 1.0 - value;
    value *= Accelerometer::GRAPH_HEIGHT;

    this->pDisp->setPixel(this->graphX, axis * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + (int)value, this->colors[axis]);
}

void Accelerometer::resetGraph()
{
    this->graphX = 0;
    this->pDisp->clearScreen();
    this->drawAxes();
}

void Accelerometer::checkGraphReset()
{
    if (this->graphX > this->pDisp->getWidth())
    {
        this->resetGraph();
    }
}

void Accelerometer::updateGraph()
{
    double x, y, z;
    this->getXYZ(x, y, z);
    
    this->checkGraphReset();
    this->drawPoint(0, x);
    this->drawPoint(1, y);
    this->drawPoint(2, z);
    this->graphX++;
}