zinnet yazıcı / Mbed OS BMA220

j

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