File content as of revision 1:dc9389ccc09d:

#include "bma220.h"

Serial dbug(USBTX, USBRX);

BMA220::BMA220(I2C *_i2c)
{
    i2c = _i2c;
    G[0].Ax = 0;
    G[0].Ay = 0;
    G[0].Az = 0;
}

BMA220::BMA220()
{
    i2c = new I2C(I2C_SDA, I2C_SCL);
    i2c->frequency(400000);
    G[0].Ax = 0;
    G[0].Ay = 0;
    G[0].Az = 0;
}

bool BMA220::begin(void)
{   
    uint8_t chip_id = readRegister(CHIPID_REG);
    printf("chip_id : %x\n", chip_id);
    if(chip_id != 0xDD) {
        dbug.printf("Chip ID is not valid\n");
        return false;
    }
    dbug.printf("Chip ID is valid\n");

    resetvalue = readRegister(SOFTRESET_REG); // change mode
    if (resetvalue == 0xFF) {  // was in reset mode, is in operation mode now
        resetvalue = readRegister(SOFTRESET_REG);  // sensor should be in reset mode now
    }

    if (resetvalue != 0x00) {
        return false;
    }
    // sensor should be in reset mode
    resetvalue = readRegister(SOFTRESET_REG);
    if (resetvalue != 0xFF) {  // sensor was not in reset mode
        return false;
    }
    return true;

}

void BMA220::setRegister(uint8_t reg, uint8_t value)
{
    char data[2];
    data[0] = reg;
    data[1] = value;
    i2c->write(BMA220_ADDR, data, 2, false);    
}

uint8_t BMA220::readRegister(uint8_t reg)
{
    char buffer_out[1];
    char buffer_in[1];
    buffer_in[0] = 0x0;
    buffer_out[0] = reg;
    i2c->write(BMA220_ADDR, buffer_out, 1, true);
    i2c->read(BMA220_ADDR, buffer_in, 1,false);

    return (uint8_t)buffer_in[0];
}

void BMA220::readAcceleration(int sensitivity)
{
    G[1].Ax = G[0].Ax;
    G[1].Ay = G[0].Ay;
    G[1].Az = G[0].Az;      //Acc After= Acc Before

    this->Ax = ((float)readRegister(XAXIS))/sensitivity;
    this->Ay = ((float)readRegister(YAXIS))/sensitivity;
    this->Az = ((float)readRegister(ZAXIS))/sensitivity;

    // Calculate Roll and Pitch (rotation around X-axis, rotation around Y-axis)
    // Pitch (rho) is defined as the angle of the X-axis relative to ground. Roll (phi) is defined as the angle of the Y-axis relative to the ground. Theta is the angle of the Z axis relative to gravity.”
    int r = atan(Ay / sqrt(pow(Ax, 2) + pow(Az, 2))) * 180 / PI;
    int p = atan(Ax / sqrt(pow(Ay, 2) + pow(Az, 2))) * 180 / PI;
    int t = atan(sqrt(pow(Ax, 2) + pow(Ay, 2)) / Az) * 180 / PI;

    // Low-pass filter
    this->roll = r;//(0.94*this->roll)+0.06*r;
    this->pitch = p;//(0.94*this->pitch)+0.06*p;
    this->theta = t;//(0.94*this->theta)+0.06*t;

    G[0].Ax = Ax;
    G[0].Ay = Ay;
    G[0].Az = Az;     //Acc Before=Present Acc

    FallDetection();
}

void BMA220::FallDetection()
{
    Calculate_Q1();
    Calculate_Q2();
    Calculate_Q3();

    /*Serial.println("Q0: ");
    display(Quanterion_2_Matrix(this->Q[0]));
    Serial.println("Q1: ");
    display(Quanterion_2_Matrix(this->Q[1]));
    Serial.println("Q2: ");
    display(Quanterion_2_Matrix(this->Q[2]));*/


    std::vector<std::vector<float> > Q;
    Q.resize(4, std::vector<float>(4));

    Q=MatrixMult(MatrixMult(Quanterion_2_Matrix(this->Q[0]), Quanterion_2_Matrix(this->Q[1])), Quanterion_2_Matrix(this->Q[2]));

    this->Q_result1.q0 = Q[0][0];
    this->Q_result1.q1 = Q[1][0];
    this->Q_result1.q2 = Q[2][0];
    this->Q_result1.q3 = Q[3][0];
    this->Q_result2.q0 = Q[3][3];
    this->Q_result2.q1 = -1 * Q[2][3];
    this->Q_result2.q2 = Q[1][3];
    this->Q_result2.q3 = -1 * Q[0][3];

    //result1.display();
    //result2.display();
}

void BMA220::Calculate_Q1()
{
    float theta1 = atan(G[0].Az / sqrt(pow(G[0].Ax, 2) + pow(G[0].Ay, 2))) * 180 / PI;
    float sin_alpha = (-1* (G[0].Ay / sqrt(pow(G[0].Ax, 2) + pow(G[0].Ay, 2)))) * 180 / PI;
    float cos_alpha = (G[0].Ax / sqrt(pow(G[0].Ax, 2) + pow(G[0].Ay, 2))) * 180 / PI;

    this->Q[0].q0 = cos(theta1 / 2);
    this->Q[0].q1 = sin(theta1 / 2) * sin_alpha;
    this->Q[0].q2 = sin(theta1 / 2) * cos_alpha;
    this->Q[0].q3 = 0;
}

void BMA220::Calculate_Q2()
{
    float theta2 = (atan((2 * G[1].Ay) / G[1].Ax) - atan((2 * G[0].Ay) / G[0].Ax)) * 180 / PI;

    this->Q[1].q0 = cos(theta2 / 2);
    this->Q[1].q1 = 0;
    this->Q[1].q2 = 0;
    this->Q[1].q3 = sin(theta2 / 2);
}

void BMA220::Calculate_Q3()
{
       float theta3 = -1 * atan(G[1].Az / sqrt(pow(G[1].Ax, 2) + pow(G[1].Ay, 2))) * 180 / PI;
       float sin_beta = (-1 * (G[1].Ay / sqrt(pow(G[1].Ax, 2) + pow(G[1].Ay, 2)))) * 180 / PI;
       float cos_beta = (G[1].Ax / sqrt(pow(G[1].Ax, 2) + pow(G[1].Ay, 2))) * 180 / PI;

       this->Q[2].q0 = cos(theta3 / 2);
       this->Q[2].q1 = sin(theta3 / 2) * sin_beta;
       this->Q[2].q2 = sin(theta3 / 2) * cos_beta;
       this->Q[2].q3 = 0;
}

std::vector<std::vector<float> > BMA220::Quanterion_2_Matrix(const Quanterion &Q)
{
    std::vector<std::vector<float> > result;
    result.resize(4, std::vector<float>(4));

    result[0][0] = Q.q0;
    result[0][1] = -1 * Q.q1;
    result[0][2] = -1 * Q.q2;
    result[0][3] = -1 * Q.q3;
    result[1][0] = Q.q1;
    result[1][1] = Q.q0;
    result[1][2] = -1 * Q.q3;
    result[1][3] = -1 * Q.q2;
    result[2][0] = Q.q2;
    result[2][1] = Q.q3;
    result[2][2] = Q.q0;
    result[2][3] = -1 * Q.q1;
    result[3][0] = Q.q3;
    result[3][1] = -1 * Q.q2;
    result[3][2] = Q.q1;
    result[3][3] = Q.q0;
    return result;
}

float BMA220::getAcceleration_X() const
{
    return Ax;
}

float BMA220::getAcceleration_Y() const
{
    return Ay;
}

float BMA220::getAcceleration_Z() const
{
    return Az;
}

float BMA220::getPitch() const
{
    return pitch;
}

float BMA220::getRoll() const
{
    return roll;
}

float BMA220::getTheta() const
{
    return theta;
}

float BMA220::getMag() const
{
    return  sqrt(pow(Ax, 2) + pow(Ay, 2)+ pow(Az, 2));
}

void BMA220::reset(void)
{
    ;
    resetvalue = readRegister(SOFTRESET_REG);
    if (resetvalue == 0x00) {
        // BMA220 is in reset mode now. Reading soft reset register
        // again, brings the sensor back to operation mode.
        resetvalue = readRegister(SOFTRESET_REG);
    }
}