Revision 1:dc9389ccc09d, committed 2019-09-11

Comitter:

zinnetyazicii53

Date:

Wed Sep 11 12:15:15 2019 +0000

Parent:

0:d861e7a56281

Commit message:

commit to pass repo another account

Changed in this revision

--- a/bma220.cpp	Sun Aug 25 08:58:34 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,192 +0,0 @@
-#include "bma220.h"
-
-I2C i2c(I2C_SDA, I2C_SCL);
-Serial dbug(USBTX, USBRX);
-
-BMA220::BMA220(){
-    G[0].Ax=0; G[0].Ay=0; G[0].Az=0;
-}
-
-bool BMA220::begin(void){
-    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;
-}
-
-bool BMA220::set(uint8_t reg, uint8_t value){
-  char ach_i2c_data[2];
-  ach_i2c_data[0]=reg;
-  ach_i2c_data[1]=value;
-  
-  if(i2c.write(BMA220_ADDR, ach_i2c_data, 2, false)==0)
-    return true;
-  else
-    return false;
-}
-
-bool BMA220::read(uint8_t reg, uint8_t *pvalue){
-  char data=reg;
-  
-  if(i2c.write(BMA220_ADDR, &data, 1, true)!=0)
-    return false;
-  if(i2c.read(BMA220_ADDR, &data, 1, false)==0)
-  {
-    *pvalue=(uint8_t) data;
-    return true;
-  }
-  else return false;
-}
-
-void BMA220::setRegister(uint8_t reg, uint8_t value){
-  if(set(reg, value)){return;}
-  dbug.printf("Error: Register Not Set\n");
-}
-
-int8_t BMA220::readRegister(uint8_t reg){
-  uint8_t pvalue;
-  char data=reg;
-  i2c.read(BMA220_ADDR, &data, 1);
-  pvalue=(uint8_t) data;
-  if(read(reg, &pvalue)){return pvalue;}
-  dbug.printf("Error: Register Not read\n");
-  return pvalue;
-}
-
-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);
-    }
-}

--- a/bma220.h	Sun Aug 25 08:58:34 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,153 +0,0 @@
-#include "mbed.h"
-
-#include <vector>
-#include <math.h>
-
-#define BMA220_ADDR     0x0A // I2C Address of the sensor
-
-#define XAXIS           0x04 // x-axis acceleration value register
-#define YAXIS           0x06 // y-axis acceleration value register
-#define ZAXIS           0x08 // z-axis acceleration value register
-
-#define SLOPE_REG       0x12 // slope detection parameter
-
-#define INTRP_MODE_REG  0x1A // interrupt selection
-#define SLEEP_REG       0x1E // sleep enable and duration
-#define SOFTRESET_REG   0x32 // softreset (triggered by reading)
-#define SENSITIVITY_REG 0x22 // sensitivity selection
-#define FILTER_REG      0x20 // filter selection
-#define CHIPID_REG      0x00 // chip id
-#define REVISIONID_REG  0x02 // revision id
-#define INTRP_RES_REG   0x1C // interrupt reset, latch mode
-
-#define SENS_2g         0x00 // Empfindlichkeit: +- 2g
-#define SENS_4g         0x01 // Empfindlichkeit: +- 4g
-#define SENS_8g         0x02 // Empfindlichkeit: +- 8g
-#define SENS_16g        0x03 // Empfindlichkeit: +- 16g
-
-#define FILTER_32Hz     0x05 // cutoff frequency: 32 Hz
-#define FILTER_64Hz     0x04 // cutoff frequency: 64 Hz
-#define FILTER_125Hz    0x03 // cutoff frequency: 125 Hz
-#define FILTER_250Hz    0x02 // cutoff frequency: 250 Hz
-#define FILTER_500Hz    0x01 // cutoff frequency: 500 Hz
-#define FILTER_1kHz     0x00 // cutoff frequency: 1 kHz
-
-#define LATCH_0s        0x80 // reset interrupt controller, latch time 0s
-#define LATCH_025s      0x90 // reset interrupt controller, latch time 0.25s
-#define LATCH_05s       0xA0 // reset interrupt controller, latch time 0.5s
-#define LATCH_1s        0xB0 // reset interrupt controller, latch time 1s
-#define LATCH_2s        0xC0 // reset interrupt controller, latch time 2s
-#define LATCH_4s        0xD0 // reset interrupt controller, latch time 4s
-#define LATCH_8s        0xE0 // reset interrupt controller, latch time 8s
-#define LATCH_PERM      0xF0 // reset interrupt controller, latch permanently
-
-#define SLEEP_2ms       0b01000000 // sleep enabled, 2 ms
-#define SLEEP_10ms      0b01001000 // sleep enabled, 10 ms
-#define SLEEP_25ms      0b01010000 // sleep enabled, 25 ms
-#define SLEEP_50ms      0b01011000 // sleep enabled, 50 ms
-#define SLEEP_100ms     0b01100000 // sleep enabled, 100 ms
-#define SLEEP_500ms     0b01101000 // sleep enabled, 500 ms
-#define SLEEP_1s        0b01110000 // sleep enabled, 1 s
-#define SLEEP_2s        0b01111000 // sleep enabled, 2 s
-
-#define SLOPEDETECT     0x38 // select envelope slope detection
-#define SLOPEPAR1       0x0D // slope detection parameter: threshold: 0011, duration: 01
-
-#define ONEBYTE         0x01 // read one byte
-
-#define PI 3.1415926535897932384626433832795
-
-template<typename T>
-std::vector<std::vector<float> > MatrixMult(const std::vector<std::vector<T> > Matrix1, const std::vector<std::vector<T> > Matrix2){
-
-    if((int)Matrix1[0].size()!=(int)Matrix2.size()){
-        //pc.printf("Wrong Matrix Size\n");
-        return Matrix1;
-    }
-
-    std::vector<std::vector<float> > result;
-    result.resize(Matrix1.size(), std::vector<float>(Matrix2[0].size()));
-
-    for (int i=0; i<(int)Matrix1.size(); i++){
-        for (int j = 0; j<(int)Matrix2[0].size(); j++){
-            result[i][j]=0;
-            for (int k = 0; k<(int)Matrix1[0].size(); k++){
-                result[i][j]+=((float)Matrix1[i][k])*((float)Matrix2[k][j]);
-            }
-        }
-    }
-    return result;
-}
-
-/*template<typename T>
-void display(std::vector<std::vector<T> > A){
-    for (int i=0; i<(int)A.size(); i++){
-        for (int j=0; j<(int)A[0].size(); j++){
-            pc.printf("%i", A[i][j]);
-            pc.printf("\t");
-        }
-        pc.printf("\n");
-    }
-}*/
-
-
-struct Acceleration{
-    float Ax, Ay, Az;
-};
-
-struct Quanterion{
-    float q0, q1, q2, q3, angle;
-
-    /*void display(){
-        Angle();
-        pc.printf("%f+ %fi+ %fj+ %fk \t ANgle is: %f",q0, q1, q2, q3, angle);
-    }*/
-
-    void Angle(){
-        angle=2*atan(sqrt(pow(q1, 2) + pow(q2, 2)+ pow(q3, 2))/q0)*180/PI;
-    }
-};
-
-class BMA220{
-    private:
-        float Ax, Ay, Az, pitch, roll, theta;
-        uint8_t resetvalue;
-
-        Acceleration G[2];
-        Quanterion Q[3];
-
-        void Calculate_Q1();
-        void Calculate_Q2();
-        void Calculate_Q3();
-
-        std::vector<std::vector<float> > Quanterion_2_Matrix(const Quanterion &Q);
-
-    public:
-        BMA220();
-        bool begin(void);
-
-        bool set(uint8_t reg, uint8_t value);
-        bool read(uint8_t reg, uint8_t *pvalue);
-        
-        void setRegister(uint8_t reg, uint8_t value);
-        int8_t readRegister(uint8_t reg);
-
-        void readAcceleration(int sensitivity);
-        float getAcceleration_X() const;
-        float getAcceleration_Y() const;
-        float getAcceleration_Z() const;
-        float getPitch() const;
-        float getRoll() const;
-        float getTheta() const;
-
-        float getMag() const;
-
-        void reset(void);
-
-        void FallDetection();
-        Quanterion Q_result1, Q_result2;
-
-        /*uint8_t reset(void);
-        uint8_t chipID(void);
-        uint8_t revisionID(void);*/
-};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bma220/bma220.cpp	Wed Sep 11 12:15:15 2019 +0000
@@ -0,0 +1,231 @@
+#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);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bma220/bma220.h	Wed Sep 11 12:15:15 2019 +0000
@@ -0,0 +1,156 @@
+#include "mbed.h"
+
+#include <vector>
+#include <math.h>
+
+#define BMA220_ADDR     0x14 // I2C Address of the sensor          
+
+#define XAXIS           0x04 // x-axis acceleration value register
+#define YAXIS           0x06 // y-axis acceleration value register
+#define ZAXIS           0x08 // z-axis acceleration value register
+
+#define SLOPE_REG       0x12 // slope detection parameter
+
+#define INTRP_MODE_REG  0x1A // interrupt selection
+#define SLEEP_REG       0x1E // sleep enable and duration
+#define SOFTRESET_REG   0x32 // softreset (triggered by reading)
+#define SENSITIVITY_REG 0x22 // sensitivity selection
+#define FILTER_REG      0x20 // filter selection
+#define CHIPID_REG      0x00 // chip id
+#define REVISIONID_REG  0x02 // revision id
+#define INTRP_RES_REG   0x1C // interrupt reset, latch mode
+
+#define SENS_2g         0x00 // Empfindlichkeit: +- 2g
+#define SENS_4g         0x01 // Empfindlichkeit: +- 4g
+#define SENS_8g         0x02 // Empfindlichkeit: +- 8g
+#define SENS_16g        0x03 // Empfindlichkeit: +- 16g
+
+#define FILTER_32Hz     0x05 // cutoff frequency: 32 Hz
+#define FILTER_64Hz     0x04 // cutoff frequency: 64 Hz
+#define FILTER_125Hz    0x03 // cutoff frequency: 125 Hz
+#define FILTER_250Hz    0x02 // cutoff frequency: 250 Hz
+#define FILTER_500Hz    0x01 // cutoff frequency: 500 Hz
+#define FILTER_1kHz     0x00 // cutoff frequency: 1 kHz
+
+#define LATCH_0s        0x80 // reset interrupt controller, latch time 0s
+#define LATCH_025s      0x90 // reset interrupt controller, latch time 0.25s
+#define LATCH_05s       0xA0 // reset interrupt controller, latch time 0.5s
+#define LATCH_1s        0xB0 // reset interrupt controller, latch time 1s
+#define LATCH_2s        0xC0 // reset interrupt controller, latch time 2s
+#define LATCH_4s        0xD0 // reset interrupt controller, latch time 4s
+#define LATCH_8s        0xE0 // reset interrupt controller, latch time 8s
+#define LATCH_PERM      0xF0 // reset interrupt controller, latch permanently
+
+#define SLEEP_2ms       0b01000000 // sleep enabled, 2 ms
+#define SLEEP_10ms      0b01001000 // sleep enabled, 10 ms
+#define SLEEP_25ms      0b01010000 // sleep enabled, 25 ms
+#define SLEEP_50ms      0b01011000 // sleep enabled, 50 ms
+#define SLEEP_100ms     0b01100000 // sleep enabled, 100 ms
+#define SLEEP_500ms     0b01101000 // sleep enabled, 500 ms
+#define SLEEP_1s        0b01110000 // sleep enabled, 1 s
+#define SLEEP_2s        0b01111000 // sleep enabled, 2 s
+
+#define SLOPEDETECT     0x38 // select envelope slope detection
+#define SLOPEPAR1       0x0D // slope detection parameter: threshold: 0011, duration: 01
+
+#define ONEBYTE         0x01 // read one byte
+
+#define PI 3.1415926535897932384626433832795
+
+template<typename T>
+std::vector<std::vector<float> > MatrixMult(const std::vector<std::vector<T> > Matrix1, const std::vector<std::vector<T> > Matrix2){
+
+    if((int)Matrix1[0].size()!=(int)Matrix2.size()){
+        //pc.printf("Wrong Matrix Size\n");
+        return Matrix1;
+    }
+
+    std::vector<std::vector<float> > result;
+    result.resize(Matrix1.size(), std::vector<float>(Matrix2[0].size()));
+
+    for (int i=0; i<(int)Matrix1.size(); i++){
+        for (int j = 0; j<(int)Matrix2[0].size(); j++){
+            result[i][j]=0;
+            for (int k = 0; k<(int)Matrix1[0].size(); k++){
+                result[i][j]+=((float)Matrix1[i][k])*((float)Matrix2[k][j]);
+            }
+        }
+    }
+    return result;
+}
+
+/*template<typename T>
+void display(std::vector<std::vector<T> > A){
+    for (int i=0; i<(int)A.size(); i++){
+        for (int j=0; j<(int)A[0].size(); j++){
+            pc.printf("%i", A[i][j]);
+            pc.printf("\t");
+        }
+        pc.printf("\n");
+    }
+}*/
+
+
+struct Acceleration{
+    float Ax, Ay, Az;
+};
+
+struct Quanterion{
+    float q0, q1, q2, q3, angle;
+
+    /*void display(){
+        Angle();
+        pc.printf("%f+ %fi+ %fj+ %fk \t Angle is: %f",q0, q1, q2, q3, angle);
+    }*/
+
+    void Angle(){
+        angle = 2 * atan(sqrt(pow(q1, 2) + pow(q2, 2)+ pow(q3, 2)) / q0) * 180 / PI;
+    }
+};
+
+class BMA220{
+    private:
+        I2C *i2c;
+    
+        float Ax, Ay, Az, pitch, roll, theta;
+        uint8_t resetvalue;
+
+        Acceleration G[2];
+        Quanterion Q[3];
+
+        void Calculate_Q1();
+        void Calculate_Q2();
+        void Calculate_Q3();
+
+        std::vector<std::vector<float> > Quanterion_2_Matrix(const Quanterion &Q);
+
+    public:
+        BMA220();
+        BMA220(I2C *_i2c);
+        bool begin(void);
+
+        bool set(uint8_t reg, uint8_t value);
+        bool read(uint8_t reg, uint8_t *pvalue);
+        
+        void setRegister(uint8_t reg, uint8_t value);
+        uint8_t readRegister(uint8_t reg);
+
+        void readAcceleration(int sensitivity);
+        float getAcceleration_X() const;
+        float getAcceleration_Y() const;
+        float getAcceleration_Z() const;
+        float getPitch() const;
+        float getRoll() const;
+        float getTheta() const;
+
+        float getMag() const;
+
+        void reset(void);
+
+        void FallDetection();
+        Quanterion Q_result1, Q_result2;
+
+        /*uint8_t reset(void);
+        uint8_t chipID(void);
+        uint8_t revisionID(void);*/
+};
\ No newline at end of file

--- a/main.cpp	Sun Aug 25 08:58:34 2019 +0000
+++ b/main.cpp	Wed Sep 11 12:15:15 2019 +0000
@@ -1,20 +1,25 @@
-#include <bma220.h>
+#include "mbed.h"
+#include "math.h"
+#include "bma220.h"
+#include "thermistor.h"
 
-BMA220 Sensor;
 Serial pc(USBTX, USBRX);
+AnalogIn ain(A0);
 
 void display(Quanterion Q){
-    pc.printf("%f+ %fi+ %fj+ %fk \t ANgle is: %f\n",Q.q0, Q.q1, Q.q2, Q.q3, Q.angle);
+    pc.printf("%f+ %fi+ %fj+ %fk \t Angle is: %f\n",Q.q0, Q.q1, Q.q2, Q.q3, Q.angle);
 }
 
 int main() {
     pc.baud(115200);
+    
+    I2C i2c(I2C_SDA, I2C_SCL);
+    i2c.frequency(400000);
+    BMA220 Sensor(&i2c);
+    Thermistor thermistor(&ain);
+    
     if (!Sensor.begin()) {
         pc.printf("No valid BMA220 sensor found, check wiring");
-        while (true){  // stop here, no reason to go on...
-            pc.printf("Nothing...\n");
-            wait(5);
-        }
     }
     // Set sensor sensitivity to 4g
     Sensor.setRegister(SENSITIVITY_REG, SENS_2g);
@@ -44,9 +49,6 @@
         display(Sensor.Q_result1);
         display(Sensor.Q_result2);
         
-    //    pc.printf(Sensor.getRoll());
-    //    pc.printf("/");
-    //    pc.printf(Sensor.getPitch());
         wait(1);
-    }
+    }   
 }
\ No newline at end of file

--- a/mbed-os.lib	Sun Aug 25 08:58:34 2019 +0000
+++ b/mbed-os.lib	Wed Sep 11 12:15:15 2019 +0000
@@ -1,1 +1,1 @@
-https://github.com/ARMmbed/mbed-os/#c966348d3f9ca80843be7cdc9b748f06ea73ced0
+https://github.com/ARMmbed/mbed-os/#1bf6b20df9d3cd5f29f001ffc6f0d0fcbbb96118

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/thermistor/thermistor.cpp	Wed Sep 11 12:15:15 2019 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+#include "thermistor.h"
+
+Thermistor::Thermistor(AnalogIn *aIn)
+{
+    _aIn = aIn;
+}
+
+double Thermistor::getTemperature()
+{
+    this->getResistance();
+    if(this->_resistance > RES_25C){
+        this->_temperature = (25.0 - ((this->_resistance - RES_25C) / RES_DIFF_PER_C));
+    } else if(this->_resistance < RES_25C){
+        this->_temperature = (25.0 + ((RES_25C - this->_resistance) / RES_DIFF_PER_C));
+    } else {
+        this->_temperature = 25.0;
+    }
+    printf("resistance : %f\n", _resistance);
+    return this->_temperature;
+}
+
+void Thermistor::getResistance()
+{
+    double sum = 0.0;
+    uint16_t adc_data[SAMPLE_COUNT], average;
+    int i = 0;
+    while(i < SAMPLE_COUNT) {
+        adc_data[i] = ((_aIn->read_u16() >> 4) & 0x0FFF);
+        sum += adc_data[i++];
+    }
+    average = sum / SAMPLE_COUNT;
+    this->_resistance = (((BALANCE_RES_S * 0x0FFF) / average)) - BALANCE_RES_S;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/thermistor/thermistor.h	Wed Sep 11 12:15:15 2019 +0000
@@ -0,0 +1,28 @@
+#define BALANCE_RES_S 1500.0
+#define SAMPLE_COUNT 100
+
+ /* Depends on the thermistor and the circuit
+  *
+  *                                   [V(in)]
+  *                                      |
+  *                       4.7 K(ohm)     |   1.5 K(ohm)
+  * [+5V]-----------+-----/\/\/\/\/\-----+---/\/\/\/\/\-----------[0V]
+  *                 |                    |
+  *                 +-----{/\/\/\/\}-----+
+  *                  THERMISTOR 4.7 K(ohm)
+  *
+  */
+#define RES_25C       2000.0
+#define RES_36C       1400.0
+#define RES_DIFF_PER_C  ((RES_25C - RES_36C)/11.0)
+
+class Thermistor{
+    public:
+        Thermistor(AnalogIn *aIn);
+        double getTemperature();
+    private:
+        void getResistance();
+        
+        double _resistance, _temperature;
+        AnalogIn *_aIn;
+};
\ No newline at end of file