zinnet yazıcı / Mbed OS BMA220

j

bma220.cpp@0:d861e7a56281, 2019-08-25 (annotated)

Committer:
zinnetyazicii53
Date:
Sun Aug 25 08:58:34 2019 +0000
Revision:
0:d861e7a56281
cmm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zinnetyazicii53 0:d861e7a56281 1 #include "bma220.h"
zinnetyazicii53 0:d861e7a56281 2
zinnetyazicii53 0:d861e7a56281 3 I2C i2c(I2C_SDA, I2C_SCL);
zinnetyazicii53 0:d861e7a56281 4 Serial dbug(USBTX, USBRX);
zinnetyazicii53 0:d861e7a56281 5
zinnetyazicii53 0:d861e7a56281 6 BMA220::BMA220(){
zinnetyazicii53 0:d861e7a56281 7 G[0].Ax=0; G[0].Ay=0; G[0].Az=0;
zinnetyazicii53 0:d861e7a56281 8 }
zinnetyazicii53 0:d861e7a56281 9
zinnetyazicii53 0:d861e7a56281 10 bool BMA220::begin(void){
zinnetyazicii53 0:d861e7a56281 11 resetvalue=readRegister(SOFTRESET_REG); // change mode
zinnetyazicii53 0:d861e7a56281 12 if (resetvalue == 0xFF) { // was in reset mode, is in operation mode now
zinnetyazicii53 0:d861e7a56281 13 resetvalue = readRegister(SOFTRESET_REG); // sensor should be in reset mode now
zinnetyazicii53 0:d861e7a56281 14 }
zinnetyazicii53 0:d861e7a56281 15
zinnetyazicii53 0:d861e7a56281 16 if (resetvalue != 0x00) {
zinnetyazicii53 0:d861e7a56281 17 return false;
zinnetyazicii53 0:d861e7a56281 18 }
zinnetyazicii53 0:d861e7a56281 19 // sensor should be in reset mode
zinnetyazicii53 0:d861e7a56281 20 resetvalue = readRegister(SOFTRESET_REG);
zinnetyazicii53 0:d861e7a56281 21 if (resetvalue != 0xFF) { // sensor was not in reset mode
zinnetyazicii53 0:d861e7a56281 22 return false;
zinnetyazicii53 0:d861e7a56281 23 }
zinnetyazicii53 0:d861e7a56281 24 return true;
zinnetyazicii53 0:d861e7a56281 25 }
zinnetyazicii53 0:d861e7a56281 26
zinnetyazicii53 0:d861e7a56281 27 bool BMA220::set(uint8_t reg, uint8_t value){
zinnetyazicii53 0:d861e7a56281 28 char ach_i2c_data[2];
zinnetyazicii53 0:d861e7a56281 29 ach_i2c_data[0]=reg;
zinnetyazicii53 0:d861e7a56281 30 ach_i2c_data[1]=value;
zinnetyazicii53 0:d861e7a56281 31
zinnetyazicii53 0:d861e7a56281 32 if(i2c.write(BMA220_ADDR, ach_i2c_data, 2, false)==0)
zinnetyazicii53 0:d861e7a56281 33 return true;
zinnetyazicii53 0:d861e7a56281 34 else
zinnetyazicii53 0:d861e7a56281 35 return false;
zinnetyazicii53 0:d861e7a56281 36 }
zinnetyazicii53 0:d861e7a56281 37
zinnetyazicii53 0:d861e7a56281 38 bool BMA220::read(uint8_t reg, uint8_t *pvalue){
zinnetyazicii53 0:d861e7a56281 39 char data=reg;
zinnetyazicii53 0:d861e7a56281 40
zinnetyazicii53 0:d861e7a56281 41 if(i2c.write(BMA220_ADDR, &data, 1, true)!=0)
zinnetyazicii53 0:d861e7a56281 42 return false;
zinnetyazicii53 0:d861e7a56281 43 if(i2c.read(BMA220_ADDR, &data, 1, false)==0)
zinnetyazicii53 0:d861e7a56281 44 {
zinnetyazicii53 0:d861e7a56281 45 *pvalue=(uint8_t) data;
zinnetyazicii53 0:d861e7a56281 46 return true;
zinnetyazicii53 0:d861e7a56281 47 }
zinnetyazicii53 0:d861e7a56281 48 else return false;
zinnetyazicii53 0:d861e7a56281 49 }
zinnetyazicii53 0:d861e7a56281 50
zinnetyazicii53 0:d861e7a56281 51 void BMA220::setRegister(uint8_t reg, uint8_t value){
zinnetyazicii53 0:d861e7a56281 52 if(set(reg, value)){return;}
zinnetyazicii53 0:d861e7a56281 53 dbug.printf("Error: Register Not Set\n");
zinnetyazicii53 0:d861e7a56281 54 }
zinnetyazicii53 0:d861e7a56281 55
zinnetyazicii53 0:d861e7a56281 56 int8_t BMA220::readRegister(uint8_t reg){
zinnetyazicii53 0:d861e7a56281 57 uint8_t pvalue;
zinnetyazicii53 0:d861e7a56281 58 char data=reg;
zinnetyazicii53 0:d861e7a56281 59 i2c.read(BMA220_ADDR, &data, 1);
zinnetyazicii53 0:d861e7a56281 60 pvalue=(uint8_t) data;
zinnetyazicii53 0:d861e7a56281 61 if(read(reg, &pvalue)){return pvalue;}
zinnetyazicii53 0:d861e7a56281 62 dbug.printf("Error: Register Not read\n");
zinnetyazicii53 0:d861e7a56281 63 return pvalue;
zinnetyazicii53 0:d861e7a56281 64 }
zinnetyazicii53 0:d861e7a56281 65
zinnetyazicii53 0:d861e7a56281 66 void BMA220::readAcceleration(int sensitivity){
zinnetyazicii53 0:d861e7a56281 67 G[1].Ax=G[0].Ax; G[1].Ay=G[0].Ay; G[1].Az=G[0].Az; //Acc After= Acc Before
zinnetyazicii53 0:d861e7a56281 68
zinnetyazicii53 0:d861e7a56281 69 this->Ax=((float)readRegister(XAXIS))/sensitivity;
zinnetyazicii53 0:d861e7a56281 70 this->Ay=((float)readRegister(YAXIS))/sensitivity;
zinnetyazicii53 0:d861e7a56281 71 this->Az=((float)readRegister(ZAXIS))/sensitivity;
zinnetyazicii53 0:d861e7a56281 72
zinnetyazicii53 0:d861e7a56281 73 // Calculate Roll and Pitch (rotation around X-axis, rotation around Y-axis)
zinnetyazicii53 0:d861e7a56281 74 // 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.”
zinnetyazicii53 0:d861e7a56281 75 int r= atan(Ay / sqrt(pow(Ax, 2) + pow(Az, 2)))*180/PI;
zinnetyazicii53 0:d861e7a56281 76 int p= atan(Ax / sqrt(pow(Ay, 2) + pow(Az, 2)))*180/PI;
zinnetyazicii53 0:d861e7a56281 77 int t=atan(sqrt(pow(Ax, 2) + pow(Ay, 2))/Az)*180/PI;
zinnetyazicii53 0:d861e7a56281 78
zinnetyazicii53 0:d861e7a56281 79 // Low-pass filter
zinnetyazicii53 0:d861e7a56281 80 this->roll=r;//(0.94*this->roll)+0.06*r;
zinnetyazicii53 0:d861e7a56281 81 this->pitch=p;//(0.94*this->pitch)+0.06*p;
zinnetyazicii53 0:d861e7a56281 82 this->theta=t;//(0.94*this->theta)+0.06*t;
zinnetyazicii53 0:d861e7a56281 83
zinnetyazicii53 0:d861e7a56281 84 G[0].Ax=Ax; G[0].Ay=Ay; G[0].Az=Az; //Acc Before=Present Acc
zinnetyazicii53 0:d861e7a56281 85
zinnetyazicii53 0:d861e7a56281 86 FallDetection();
zinnetyazicii53 0:d861e7a56281 87 }
zinnetyazicii53 0:d861e7a56281 88
zinnetyazicii53 0:d861e7a56281 89 void BMA220::FallDetection(){
zinnetyazicii53 0:d861e7a56281 90 Calculate_Q1();
zinnetyazicii53 0:d861e7a56281 91 Calculate_Q2();
zinnetyazicii53 0:d861e7a56281 92 Calculate_Q3();
zinnetyazicii53 0:d861e7a56281 93
zinnetyazicii53 0:d861e7a56281 94 /*Serial.println("Q0: ");
zinnetyazicii53 0:d861e7a56281 95 display(Quanterion_2_Matrix(this->Q[0]));
zinnetyazicii53 0:d861e7a56281 96 Serial.println("Q1: ");
zinnetyazicii53 0:d861e7a56281 97 display(Quanterion_2_Matrix(this->Q[1]));
zinnetyazicii53 0:d861e7a56281 98 Serial.println("Q2: ");
zinnetyazicii53 0:d861e7a56281 99 display(Quanterion_2_Matrix(this->Q[2]));*/
zinnetyazicii53 0:d861e7a56281 100
zinnetyazicii53 0:d861e7a56281 101
zinnetyazicii53 0:d861e7a56281 102 std::vector<std::vector<float> > Q;
zinnetyazicii53 0:d861e7a56281 103 Q.resize(4, std::vector<float>(4));
zinnetyazicii53 0:d861e7a56281 104
zinnetyazicii53 0:d861e7a56281 105 Q=MatrixMult(MatrixMult(Quanterion_2_Matrix(this->Q[0]), Quanterion_2_Matrix(this->Q[1])), Quanterion_2_Matrix(this->Q[2]));
zinnetyazicii53 0:d861e7a56281 106
zinnetyazicii53 0:d861e7a56281 107 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];
zinnetyazicii53 0:d861e7a56281 108 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];
zinnetyazicii53 0:d861e7a56281 109
zinnetyazicii53 0:d861e7a56281 110 //result1.display();
zinnetyazicii53 0:d861e7a56281 111 //result2.display();
zinnetyazicii53 0:d861e7a56281 112 }
zinnetyazicii53 0:d861e7a56281 113
zinnetyazicii53 0:d861e7a56281 114 void BMA220::Calculate_Q1(){
zinnetyazicii53 0:d861e7a56281 115 float theta1=atan(G[0].Az/ sqrt(pow(G[0].Ax, 2) + pow(G[0].Ay, 2)))*180/PI;
zinnetyazicii53 0:d861e7a56281 116 float sin_alpha=(-1*(G[0].Ay/ sqrt(pow(G[0].Ax, 2) + pow(G[0].Ay, 2))))*180/PI;
zinnetyazicii53 0:d861e7a56281 117 float cos_alpha=(G[0].Ax/ sqrt(pow(G[0].Ax, 2) + pow(G[0].Ay, 2)))*180/PI;
zinnetyazicii53 0:d861e7a56281 118
zinnetyazicii53 0:d861e7a56281 119 this->Q[0].q0=cos(theta1/2);
zinnetyazicii53 0:d861e7a56281 120 this->Q[0].q1=sin(theta1/2)*sin_alpha;
zinnetyazicii53 0:d861e7a56281 121 this->Q[0].q2=sin(theta1/2)*cos_alpha;
zinnetyazicii53 0:d861e7a56281 122 this->Q[0].q3=0;
zinnetyazicii53 0:d861e7a56281 123 }
zinnetyazicii53 0:d861e7a56281 124
zinnetyazicii53 0:d861e7a56281 125 void BMA220::Calculate_Q2(){
zinnetyazicii53 0:d861e7a56281 126 float theta2=(atan((2*G[1].Ay)/G[1].Ax)-atan((2*G[0].Ay)/G[0].Ax))*180/PI;
zinnetyazicii53 0:d861e7a56281 127
zinnetyazicii53 0:d861e7a56281 128 this->Q[1].q0=cos(theta2/2);
zinnetyazicii53 0:d861e7a56281 129 this->Q[1].q1=0;
zinnetyazicii53 0:d861e7a56281 130 this->Q[1].q2=0;
zinnetyazicii53 0:d861e7a56281 131 this->Q[1].q3=sin(theta2/2);
zinnetyazicii53 0:d861e7a56281 132 }
zinnetyazicii53 0:d861e7a56281 133
zinnetyazicii53 0:d861e7a56281 134 void BMA220::Calculate_Q3(){
zinnetyazicii53 0:d861e7a56281 135 float theta3=-1*atan(G[1].Az/ sqrt(pow(G[1].Ax, 2) + pow(G[1].Ay, 2)))*180/PI;
zinnetyazicii53 0:d861e7a56281 136 float sin_beta=(-1*(G[1].Ay/ sqrt(pow(G[1].Ax, 2) + pow(G[1].Ay, 2))))*180/PI;
zinnetyazicii53 0:d861e7a56281 137 float cos_beta=(G[1].Ax/ sqrt(pow(G[1].Ax, 2) + pow(G[1].Ay, 2)))*180/PI;
zinnetyazicii53 0:d861e7a56281 138
zinnetyazicii53 0:d861e7a56281 139 this->Q[2].q0=cos(theta3/2);
zinnetyazicii53 0:d861e7a56281 140 this->Q[2].q1=sin(theta3/2)*sin_beta;
zinnetyazicii53 0:d861e7a56281 141 this->Q[2].q2=sin(theta3/2)*cos_beta;
zinnetyazicii53 0:d861e7a56281 142 this->Q[2].q3=0;
zinnetyazicii53 0:d861e7a56281 143 }
zinnetyazicii53 0:d861e7a56281 144
zinnetyazicii53 0:d861e7a56281 145 std::vector<std::vector<float> > BMA220::Quanterion_2_Matrix(const Quanterion &Q){
zinnetyazicii53 0:d861e7a56281 146 std::vector<std::vector<float> > result;
zinnetyazicii53 0:d861e7a56281 147 result.resize(4, std::vector<float>(4));
zinnetyazicii53 0:d861e7a56281 148
zinnetyazicii53 0:d861e7a56281 149 result[0][0]=Q.q0; result[0][1]=-1*Q.q1; result[0][2]=-1*Q.q2; result[0][3]=-1*Q.q3;
zinnetyazicii53 0:d861e7a56281 150 result[1][0]=Q.q1; result[1][1]=Q.q0; result[1][2]=-1*Q.q3; result[1][3]=-1*Q.q2;
zinnetyazicii53 0:d861e7a56281 151 result[2][0]=Q.q2; result[2][1]=Q.q3; result[2][2]=Q.q0; result[2][3]=-1*Q.q1;
zinnetyazicii53 0:d861e7a56281 152 result[3][0]=Q.q3; result[3][1]=-1*Q.q2; result[3][2]=Q.q1; result[3][3]=Q.q0;
zinnetyazicii53 0:d861e7a56281 153 return result;
zinnetyazicii53 0:d861e7a56281 154 }
zinnetyazicii53 0:d861e7a56281 155
zinnetyazicii53 0:d861e7a56281 156
zinnetyazicii53 0:d861e7a56281 157 float BMA220::getAcceleration_X() const{
zinnetyazicii53 0:d861e7a56281 158 return Ax;
zinnetyazicii53 0:d861e7a56281 159 }
zinnetyazicii53 0:d861e7a56281 160
zinnetyazicii53 0:d861e7a56281 161 float BMA220::getAcceleration_Y() const{
zinnetyazicii53 0:d861e7a56281 162 return Ay;
zinnetyazicii53 0:d861e7a56281 163 }
zinnetyazicii53 0:d861e7a56281 164
zinnetyazicii53 0:d861e7a56281 165 float BMA220::getAcceleration_Z() const{
zinnetyazicii53 0:d861e7a56281 166 return Az;
zinnetyazicii53 0:d861e7a56281 167 }
zinnetyazicii53 0:d861e7a56281 168
zinnetyazicii53 0:d861e7a56281 169 float BMA220::getPitch() const{
zinnetyazicii53 0:d861e7a56281 170 return pitch;
zinnetyazicii53 0:d861e7a56281 171 }
zinnetyazicii53 0:d861e7a56281 172
zinnetyazicii53 0:d861e7a56281 173 float BMA220::getRoll() const{
zinnetyazicii53 0:d861e7a56281 174 return roll;
zinnetyazicii53 0:d861e7a56281 175 }
zinnetyazicii53 0:d861e7a56281 176
zinnetyazicii53 0:d861e7a56281 177 float BMA220::getTheta() const{
zinnetyazicii53 0:d861e7a56281 178 return theta;
zinnetyazicii53 0:d861e7a56281 179 }
zinnetyazicii53 0:d861e7a56281 180
zinnetyazicii53 0:d861e7a56281 181 float BMA220::getMag() const{
zinnetyazicii53 0:d861e7a56281 182 return sqrt(pow(Ax, 2) + pow(Ay, 2)+ pow(Az, 2));
zinnetyazicii53 0:d861e7a56281 183 }
zinnetyazicii53 0:d861e7a56281 184
zinnetyazicii53 0:d861e7a56281 185 void BMA220::reset(void) {;
zinnetyazicii53 0:d861e7a56281 186 resetvalue = readRegister(SOFTRESET_REG);
zinnetyazicii53 0:d861e7a56281 187 if (resetvalue == 0x00){
zinnetyazicii53 0:d861e7a56281 188 // BMA220 is in reset mode now. Reading soft reset register
zinnetyazicii53 0:d861e7a56281 189 // again, brings the sensor back to operation mode.
zinnetyazicii53 0:d861e7a56281 190 resetvalue = readRegister(SOFTRESET_REG);
zinnetyazicii53 0:d861e7a56281 191 }
zinnetyazicii53 0:d861e7a56281 192 }