zinnet yazıcı / Mbed OS BMA220

j

bma220/bma220.h@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 "mbed.h"
zinnetyazicii53 1:dc9389ccc09d 2
zinnetyazicii53 1:dc9389ccc09d 3 #include <vector>
zinnetyazicii53 1:dc9389ccc09d 4 #include <math.h>
zinnetyazicii53 1:dc9389ccc09d 5
zinnetyazicii53 1:dc9389ccc09d 6 #define BMA220_ADDR 0x14 // I2C Address of the sensor
zinnetyazicii53 1:dc9389ccc09d 7
zinnetyazicii53 1:dc9389ccc09d 8 #define XAXIS 0x04 // x-axis acceleration value register
zinnetyazicii53 1:dc9389ccc09d 9 #define YAXIS 0x06 // y-axis acceleration value register
zinnetyazicii53 1:dc9389ccc09d 10 #define ZAXIS 0x08 // z-axis acceleration value register
zinnetyazicii53 1:dc9389ccc09d 11
zinnetyazicii53 1:dc9389ccc09d 12 #define SLOPE_REG 0x12 // slope detection parameter
zinnetyazicii53 1:dc9389ccc09d 13
zinnetyazicii53 1:dc9389ccc09d 14 #define INTRP_MODE_REG 0x1A // interrupt selection
zinnetyazicii53 1:dc9389ccc09d 15 #define SLEEP_REG 0x1E // sleep enable and duration
zinnetyazicii53 1:dc9389ccc09d 16 #define SOFTRESET_REG 0x32 // softreset (triggered by reading)
zinnetyazicii53 1:dc9389ccc09d 17 #define SENSITIVITY_REG 0x22 // sensitivity selection
zinnetyazicii53 1:dc9389ccc09d 18 #define FILTER_REG 0x20 // filter selection
zinnetyazicii53 1:dc9389ccc09d 19 #define CHIPID_REG 0x00 // chip id
zinnetyazicii53 1:dc9389ccc09d 20 #define REVISIONID_REG 0x02 // revision id
zinnetyazicii53 1:dc9389ccc09d 21 #define INTRP_RES_REG 0x1C // interrupt reset, latch mode
zinnetyazicii53 1:dc9389ccc09d 22
zinnetyazicii53 1:dc9389ccc09d 23 #define SENS_2g 0x00 // Empfindlichkeit: +- 2g
zinnetyazicii53 1:dc9389ccc09d 24 #define SENS_4g 0x01 // Empfindlichkeit: +- 4g
zinnetyazicii53 1:dc9389ccc09d 25 #define SENS_8g 0x02 // Empfindlichkeit: +- 8g
zinnetyazicii53 1:dc9389ccc09d 26 #define SENS_16g 0x03 // Empfindlichkeit: +- 16g
zinnetyazicii53 1:dc9389ccc09d 27
zinnetyazicii53 1:dc9389ccc09d 28 #define FILTER_32Hz 0x05 // cutoff frequency: 32 Hz
zinnetyazicii53 1:dc9389ccc09d 29 #define FILTER_64Hz 0x04 // cutoff frequency: 64 Hz
zinnetyazicii53 1:dc9389ccc09d 30 #define FILTER_125Hz 0x03 // cutoff frequency: 125 Hz
zinnetyazicii53 1:dc9389ccc09d 31 #define FILTER_250Hz 0x02 // cutoff frequency: 250 Hz
zinnetyazicii53 1:dc9389ccc09d 32 #define FILTER_500Hz 0x01 // cutoff frequency: 500 Hz
zinnetyazicii53 1:dc9389ccc09d 33 #define FILTER_1kHz 0x00 // cutoff frequency: 1 kHz
zinnetyazicii53 1:dc9389ccc09d 34
zinnetyazicii53 1:dc9389ccc09d 35 #define LATCH_0s 0x80 // reset interrupt controller, latch time 0s
zinnetyazicii53 1:dc9389ccc09d 36 #define LATCH_025s 0x90 // reset interrupt controller, latch time 0.25s
zinnetyazicii53 1:dc9389ccc09d 37 #define LATCH_05s 0xA0 // reset interrupt controller, latch time 0.5s
zinnetyazicii53 1:dc9389ccc09d 38 #define LATCH_1s 0xB0 // reset interrupt controller, latch time 1s
zinnetyazicii53 1:dc9389ccc09d 39 #define LATCH_2s 0xC0 // reset interrupt controller, latch time 2s
zinnetyazicii53 1:dc9389ccc09d 40 #define LATCH_4s 0xD0 // reset interrupt controller, latch time 4s
zinnetyazicii53 1:dc9389ccc09d 41 #define LATCH_8s 0xE0 // reset interrupt controller, latch time 8s
zinnetyazicii53 1:dc9389ccc09d 42 #define LATCH_PERM 0xF0 // reset interrupt controller, latch permanently
zinnetyazicii53 1:dc9389ccc09d 43
zinnetyazicii53 1:dc9389ccc09d 44 #define SLEEP_2ms 0b01000000 // sleep enabled, 2 ms
zinnetyazicii53 1:dc9389ccc09d 45 #define SLEEP_10ms 0b01001000 // sleep enabled, 10 ms
zinnetyazicii53 1:dc9389ccc09d 46 #define SLEEP_25ms 0b01010000 // sleep enabled, 25 ms
zinnetyazicii53 1:dc9389ccc09d 47 #define SLEEP_50ms 0b01011000 // sleep enabled, 50 ms
zinnetyazicii53 1:dc9389ccc09d 48 #define SLEEP_100ms 0b01100000 // sleep enabled, 100 ms
zinnetyazicii53 1:dc9389ccc09d 49 #define SLEEP_500ms 0b01101000 // sleep enabled, 500 ms
zinnetyazicii53 1:dc9389ccc09d 50 #define SLEEP_1s 0b01110000 // sleep enabled, 1 s
zinnetyazicii53 1:dc9389ccc09d 51 #define SLEEP_2s 0b01111000 // sleep enabled, 2 s
zinnetyazicii53 1:dc9389ccc09d 52
zinnetyazicii53 1:dc9389ccc09d 53 #define SLOPEDETECT 0x38 // select envelope slope detection
zinnetyazicii53 1:dc9389ccc09d 54 #define SLOPEPAR1 0x0D // slope detection parameter: threshold: 0011, duration: 01
zinnetyazicii53 1:dc9389ccc09d 55
zinnetyazicii53 1:dc9389ccc09d 56 #define ONEBYTE 0x01 // read one byte
zinnetyazicii53 1:dc9389ccc09d 57
zinnetyazicii53 1:dc9389ccc09d 58 #define PI 3.1415926535897932384626433832795
zinnetyazicii53 1:dc9389ccc09d 59
zinnetyazicii53 1:dc9389ccc09d 60 template<typename T>
zinnetyazicii53 1:dc9389ccc09d 61 std::vector<std::vector<float> > MatrixMult(const std::vector<std::vector<T> > Matrix1, const std::vector<std::vector<T> > Matrix2){
zinnetyazicii53 1:dc9389ccc09d 62
zinnetyazicii53 1:dc9389ccc09d 63 if((int)Matrix1[0].size()!=(int)Matrix2.size()){
zinnetyazicii53 1:dc9389ccc09d 64 //pc.printf("Wrong Matrix Size\n");
zinnetyazicii53 1:dc9389ccc09d 65 return Matrix1;
zinnetyazicii53 1:dc9389ccc09d 66 }
zinnetyazicii53 1:dc9389ccc09d 67
zinnetyazicii53 1:dc9389ccc09d 68 std::vector<std::vector<float> > result;
zinnetyazicii53 1:dc9389ccc09d 69 result.resize(Matrix1.size(), std::vector<float>(Matrix2[0].size()));
zinnetyazicii53 1:dc9389ccc09d 70
zinnetyazicii53 1:dc9389ccc09d 71 for (int i=0; i<(int)Matrix1.size(); i++){
zinnetyazicii53 1:dc9389ccc09d 72 for (int j = 0; j<(int)Matrix2[0].size(); j++){
zinnetyazicii53 1:dc9389ccc09d 73 result[i][j]=0;
zinnetyazicii53 1:dc9389ccc09d 74 for (int k = 0; k<(int)Matrix1[0].size(); k++){
zinnetyazicii53 1:dc9389ccc09d 75 result[i][j]+=((float)Matrix1[i][k])*((float)Matrix2[k][j]);
zinnetyazicii53 1:dc9389ccc09d 76 }
zinnetyazicii53 1:dc9389ccc09d 77 }
zinnetyazicii53 1:dc9389ccc09d 78 }
zinnetyazicii53 1:dc9389ccc09d 79 return result;
zinnetyazicii53 1:dc9389ccc09d 80 }
zinnetyazicii53 1:dc9389ccc09d 81
zinnetyazicii53 1:dc9389ccc09d 82 /*template<typename T>
zinnetyazicii53 1:dc9389ccc09d 83 void display(std::vector<std::vector<T> > A){
zinnetyazicii53 1:dc9389ccc09d 84 for (int i=0; i<(int)A.size(); i++){
zinnetyazicii53 1:dc9389ccc09d 85 for (int j=0; j<(int)A[0].size(); j++){
zinnetyazicii53 1:dc9389ccc09d 86 pc.printf("%i", A[i][j]);
zinnetyazicii53 1:dc9389ccc09d 87 pc.printf("\t");
zinnetyazicii53 1:dc9389ccc09d 88 }
zinnetyazicii53 1:dc9389ccc09d 89 pc.printf("\n");
zinnetyazicii53 1:dc9389ccc09d 90 }
zinnetyazicii53 1:dc9389ccc09d 91 }*/
zinnetyazicii53 1:dc9389ccc09d 92
zinnetyazicii53 1:dc9389ccc09d 93
zinnetyazicii53 1:dc9389ccc09d 94 struct Acceleration{
zinnetyazicii53 1:dc9389ccc09d 95 float Ax, Ay, Az;
zinnetyazicii53 1:dc9389ccc09d 96 };
zinnetyazicii53 1:dc9389ccc09d 97
zinnetyazicii53 1:dc9389ccc09d 98 struct Quanterion{
zinnetyazicii53 1:dc9389ccc09d 99 float q0, q1, q2, q3, angle;
zinnetyazicii53 1:dc9389ccc09d 100
zinnetyazicii53 1:dc9389ccc09d 101 /*void display(){
zinnetyazicii53 1:dc9389ccc09d 102 Angle();
zinnetyazicii53 1:dc9389ccc09d 103 pc.printf("%f+ %fi+ %fj+ %fk \t Angle is: %f",q0, q1, q2, q3, angle);
zinnetyazicii53 1:dc9389ccc09d 104 }*/
zinnetyazicii53 1:dc9389ccc09d 105
zinnetyazicii53 1:dc9389ccc09d 106 void Angle(){
zinnetyazicii53 1:dc9389ccc09d 107 angle = 2 * atan(sqrt(pow(q1, 2) + pow(q2, 2)+ pow(q3, 2)) / q0) * 180 / PI;
zinnetyazicii53 1:dc9389ccc09d 108 }
zinnetyazicii53 1:dc9389ccc09d 109 };
zinnetyazicii53 1:dc9389ccc09d 110
zinnetyazicii53 1:dc9389ccc09d 111 class BMA220{
zinnetyazicii53 1:dc9389ccc09d 112 private:
zinnetyazicii53 1:dc9389ccc09d 113 I2C *i2c;
zinnetyazicii53 1:dc9389ccc09d 114
zinnetyazicii53 1:dc9389ccc09d 115 float Ax, Ay, Az, pitch, roll, theta;
zinnetyazicii53 1:dc9389ccc09d 116 uint8_t resetvalue;
zinnetyazicii53 1:dc9389ccc09d 117
zinnetyazicii53 1:dc9389ccc09d 118 Acceleration G[2];
zinnetyazicii53 1:dc9389ccc09d 119 Quanterion Q[3];
zinnetyazicii53 1:dc9389ccc09d 120
zinnetyazicii53 1:dc9389ccc09d 121 void Calculate_Q1();
zinnetyazicii53 1:dc9389ccc09d 122 void Calculate_Q2();
zinnetyazicii53 1:dc9389ccc09d 123 void Calculate_Q3();
zinnetyazicii53 1:dc9389ccc09d 124
zinnetyazicii53 1:dc9389ccc09d 125 std::vector<std::vector<float> > Quanterion_2_Matrix(const Quanterion &Q);
zinnetyazicii53 1:dc9389ccc09d 126
zinnetyazicii53 1:dc9389ccc09d 127 public:
zinnetyazicii53 1:dc9389ccc09d 128 BMA220();
zinnetyazicii53 1:dc9389ccc09d 129 BMA220(I2C *_i2c);
zinnetyazicii53 1:dc9389ccc09d 130 bool begin(void);
zinnetyazicii53 1:dc9389ccc09d 131
zinnetyazicii53 1:dc9389ccc09d 132 bool set(uint8_t reg, uint8_t value);
zinnetyazicii53 1:dc9389ccc09d 133 bool read(uint8_t reg, uint8_t *pvalue);
zinnetyazicii53 1:dc9389ccc09d 134
zinnetyazicii53 1:dc9389ccc09d 135 void setRegister(uint8_t reg, uint8_t value);
zinnetyazicii53 1:dc9389ccc09d 136 uint8_t readRegister(uint8_t reg);
zinnetyazicii53 1:dc9389ccc09d 137
zinnetyazicii53 1:dc9389ccc09d 138 void readAcceleration(int sensitivity);
zinnetyazicii53 1:dc9389ccc09d 139 float getAcceleration_X() const;
zinnetyazicii53 1:dc9389ccc09d 140 float getAcceleration_Y() const;
zinnetyazicii53 1:dc9389ccc09d 141 float getAcceleration_Z() const;
zinnetyazicii53 1:dc9389ccc09d 142 float getPitch() const;
zinnetyazicii53 1:dc9389ccc09d 143 float getRoll() const;
zinnetyazicii53 1:dc9389ccc09d 144 float getTheta() const;
zinnetyazicii53 1:dc9389ccc09d 145
zinnetyazicii53 1:dc9389ccc09d 146 float getMag() const;
zinnetyazicii53 1:dc9389ccc09d 147
zinnetyazicii53 1:dc9389ccc09d 148 void reset(void);
zinnetyazicii53 1:dc9389ccc09d 149
zinnetyazicii53 1:dc9389ccc09d 150 void FallDetection();
zinnetyazicii53 1:dc9389ccc09d 151 Quanterion Q_result1, Q_result2;
zinnetyazicii53 1:dc9389ccc09d 152
zinnetyazicii53 1:dc9389ccc09d 153 /*uint8_t reset(void);
zinnetyazicii53 1:dc9389ccc09d 154 uint8_t chipID(void);
zinnetyazicii53 1:dc9389ccc09d 155 uint8_t revisionID(void);*/
zinnetyazicii53 1:dc9389ccc09d 156 };