zinnet yazıcı / Mbed OS BMA220

j

bma220/bma220.cpp@1:dc9389ccc09d, 2019-09-11 (annotated)

Committer:
zinnetyazicii53
Date:
Wed Sep 11 12:15:15 2019 +0000
Revision:
1:dc9389ccc09d
commit to pass repo another account

Who changed what in which revision?

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