FRANCISCO LOZADA / myMPU9250

CONFIGURACION

myMPU9250.h@0:d173af3ca217, 2020-02-06 (annotated)

Committer:
flozada
Date:
Thu Feb 06 03:28:07 2020 +0000
Revision:
0:d173af3ca217
AVANCE CONFIG ACELEROMETRO 1_ FLOZADA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
flozada 0:d173af3ca217 1 #ifndef MYMPU9250_H
flozada 0:d173af3ca217 2 #define MYMPU9250_H
flozada 0:d173af3ca217 3
flozada 0:d173af3ca217 4 #include "mpu9250Define.h"
flozada 0:d173af3ca217 5
flozada 0:d173af3ca217 6 class MyMPU9250
flozada 0:d173af3ca217 7 {
flozada 0:d173af3ca217 8 public:
flozada 0:d173af3ca217 9 MyMPU9250()
flozada 0:d173af3ca217 10 : pc_(USBTX, USBRX), i2c_(p28, p27) {
flozada 0:d173af3ca217 11 accelLSB2MG_ = 10.0;
flozada 0:d173af3ca217 12 gyroLSB2DegPerSec_ = 0.0;
flozada 0:d173af3ca217 13 };
flozada 0:d173af3ca217 14
flozada 0:d173af3ca217 15 bool initialize() {
flozada 0:d173af3ca217 16 i2c_.frequency(400000);
flozada 0:d173af3ca217 17 i2c_.start();
flozada 0:d173af3ca217 18
flozada 0:d173af3ca217 19 wait(0.5);
flozada 0:d173af3ca217 20
flozada 0:d173af3ca217 21 char cmd[2] = { 0x00 };
flozada 0:d173af3ca217 22 cmd[0] = PWR_MGMT_1; // 0x6B
flozada 0:d173af3ca217 23 cmd[1] = (PWR_MGMT_1_CLKSEL_0); // 0x00
flozada 0:d173af3ca217 24 this->write(cmd, BIT_2);
flozada 0:d173af3ca217 25
flozada 0:d173af3ca217 26 wait(0.5);
flozada 0:d173af3ca217 27
flozada 0:d173af3ca217 28 char i_am[1] = { 0x00 };
flozada 0:d173af3ca217 29 if(! this->read(WHO_AM_I, i_am, BIT_1)) {
flozada 0:d173af3ca217 30 pc_.printf("Fail to read! I am %x\n", i_am[0]);
flozada 0:d173af3ca217 31 return false;
flozada 0:d173af3ca217 32 } else if(!(i_am[0] == 0x71)) {
flozada 0:d173af3ca217 33 pc_.printf("Fail to identify! I am %x\n", i_am[0]);
flozada 0:d173af3ca217 34 return false;
flozada 0:d173af3ca217 35 }
flozada 0:d173af3ca217 36 return true;
flozada 0:d173af3ca217 37 };
flozada 0:d173af3ca217 38
flozada 0:d173af3ca217 39 void setup(AccelSensitivity accel_mode=AFS_SEL_2,
flozada 0:d173af3ca217 40 GyroSensitivity gyro_mode=FS_SEL_250) {
flozada 0:d173af3ca217 41 char cmd[2] = { 0x00 };
flozada 0:d173af3ca217 42
flozada 0:d173af3ca217 43 cmd[0] = ACCEL_CONFIG;
flozada 0:d173af3ca217 44 cmd[1] = ACCEL_CONFIG_AFS_SEL_2;//(0x18);
flozada 0:d173af3ca217 45 this->write(cmd, BIT_2);
flozada 0:d173af3ca217 46 accelLSB2MG_ = getAccelLSB2MG(accel_mode);
flozada 0:d173af3ca217 47 pc_.printf("accelLSB2MG: %f, %x", accelLSB2MG_, cmd[1]);
flozada 0:d173af3ca217 48
flozada 0:d173af3ca217 49 cmd[0] = GYRO_CONFIG;
flozada 0:d173af3ca217 50 cmd[1] = getGyroSensitivity(gyro_mode);//(0x18);
flozada 0:d173af3ca217 51 this->write(cmd, BIT_2);
flozada 0:d173af3ca217 52 gyroLSB2DegPerSec_ = getGyroLSB2DegPerSec(gyro_mode);
flozada 0:d173af3ca217 53
flozada 0:d173af3ca217 54 cmd[0] = INT_PIN_CFG; // 0x37
flozada 0:d173af3ca217 55 cmd[1] = (INT_PIN_DFG_I2C_BYPASS_EN); // 0x02
flozada 0:d173af3ca217 56 this->write(cmd, BIT_2);
flozada 0:d173af3ca217 57 };
flozada 0:d173af3ca217 58
flozada 0:d173af3ca217 59 bool updateAccelAllAxes() {
flozada 0:d173af3ca217 60 return this->read(ACCEL_XOUT_H, accel_all_, BIT_6);
flozada 0:d173af3ca217 61 };
flozada 0:d173af3ca217 62
flozada 0:d173af3ca217 63 int accelXRaw() {
flozada 0:d173af3ca217 64 return (int16_t)(accel_all_[0]*0x100 + accel_all_[1]) ;
flozada 0:d173af3ca217 65 };
flozada 0:d173af3ca217 66
flozada 0:d173af3ca217 67 double accelX() {
flozada 0:d173af3ca217 68 return accelLSB2MG_ * accelXRaw() ;
flozada 0:d173af3ca217 69 };
flozada 0:d173af3ca217 70
flozada 0:d173af3ca217 71 int accelYRaw() {
flozada 0:d173af3ca217 72 return (int16_t)(accel_all_[2]*0x100 + accel_all_[3]) ;
flozada 0:d173af3ca217 73 };
flozada 0:d173af3ca217 74
flozada 0:d173af3ca217 75 double accelY() {
flozada 0:d173af3ca217 76 return accelLSB2MG_ * accelYRaw() ;
flozada 0:d173af3ca217 77 };
flozada 0:d173af3ca217 78
flozada 0:d173af3ca217 79 int accelZRaw() {
flozada 0:d173af3ca217 80 return (int16_t)(accel_all_[4]*0x100 + accel_all_[5]) ;
flozada 0:d173af3ca217 81 };
flozada 0:d173af3ca217 82
flozada 0:d173af3ca217 83 double accelZ() {
flozada 0:d173af3ca217 84 return accelLSB2MG_ * accelZRaw() ;
flozada 0:d173af3ca217 85 };
flozada 0:d173af3ca217 86
flozada 0:d173af3ca217 87
flozada 0:d173af3ca217 88 bool updateGyroAllAxes() {
flozada 0:d173af3ca217 89 return this->read(GYRO_XOUT_H, gyro_all_, BIT_6);
flozada 0:d173af3ca217 90 };
flozada 0:d173af3ca217 91
flozada 0:d173af3ca217 92 int gyroXRaw() {
flozada 0:d173af3ca217 93 return (int16_t)(gyro_all_[0]*0x100 + gyro_all_[1]) ;
flozada 0:d173af3ca217 94 };
flozada 0:d173af3ca217 95
flozada 0:d173af3ca217 96 double gyroX() {
flozada 0:d173af3ca217 97 return accelLSB2MG_ * gyroXRaw() ;
flozada 0:d173af3ca217 98 };
flozada 0:d173af3ca217 99
flozada 0:d173af3ca217 100 int gyroYRaw() {
flozada 0:d173af3ca217 101 return (int16_t)(gyro_all_[2]*0x100 + gyro_all_[3]) ;
flozada 0:d173af3ca217 102 };
flozada 0:d173af3ca217 103
flozada 0:d173af3ca217 104 double gyroY() {
flozada 0:d173af3ca217 105 return accelLSB2MG_ * gyroYRaw() ;
flozada 0:d173af3ca217 106 };
flozada 0:d173af3ca217 107
flozada 0:d173af3ca217 108 int gyroZRaw() {
flozada 0:d173af3ca217 109 return (int16_t)(gyro_all_[4]*0x100 + gyro_all_[5]) ;
flozada 0:d173af3ca217 110 };
flozada 0:d173af3ca217 111
flozada 0:d173af3ca217 112 double gyroZ() {
flozada 0:d173af3ca217 113 return accelLSB2MG_ * gyroZRaw() ;
flozada 0:d173af3ca217 114 };
flozada 0:d173af3ca217 115
flozada 0:d173af3ca217 116
flozada 0:d173af3ca217 117 private:
flozada 0:d173af3ca217 118 Serial pc_;
flozada 0:d173af3ca217 119 I2C i2c_;
flozada 0:d173af3ca217 120 float accelLSB2MG_;
flozada 0:d173af3ca217 121 float gyroLSB2DegPerSec_;
flozada 0:d173af3ca217 122 char accel_all_[6];
flozada 0:d173af3ca217 123 char gyro_all_[6];
flozada 0:d173af3ca217 124
flozada 0:d173af3ca217 125 bool write(char* _cmd, int _length) {
flozada 0:d173af3ca217 126 int w_status = i2c_.write(MPU9250_ADDRESS, _cmd, _length, false);
flozada 0:d173af3ca217 127 if(w_status == 0) {
flozada 0:d173af3ca217 128 return true;
flozada 0:d173af3ca217 129 } else {
flozada 0:d173af3ca217 130 pc_.printf("write register: %x \n", _cmd[0]);
flozada 0:d173af3ca217 131 pc_.printf(" write: %d \n", w_status);
flozada 0:d173af3ca217 132 return false;
flozada 0:d173af3ca217 133 }
flozada 0:d173af3ca217 134 }
flozada 0:d173af3ca217 135
flozada 0:d173af3ca217 136 bool read(char _register, char* _data, int _length) {
flozada 0:d173af3ca217 137 char cmd[1] = { _register };
flozada 0:d173af3ca217 138 int w_status = i2c_.write(MPU9250_ADDRESS, cmd, BIT_1, false);
flozada 0:d173af3ca217 139 int r_status = i2c_.read(MPU9250_ADDRESS, _data, _length, false);
flozada 0:d173af3ca217 140 if(w_status + r_status == 0) {
flozada 0:d173af3ca217 141 return true;
flozada 0:d173af3ca217 142 } else {
flozada 0:d173af3ca217 143 pc_.printf("write register: %x \n", cmd[0]);
flozada 0:d173af3ca217 144 pc_.printf("read register: %x \n", _register);
flozada 0:d173af3ca217 145 pc_.printf(" write: %d | read: %d \n", w_status, r_status);
flozada 0:d173af3ca217 146 return false;
flozada 0:d173af3ca217 147 }
flozada 0:d173af3ca217 148 };
flozada 0:d173af3ca217 149 };
flozada 0:d173af3ca217 150
flozada 0:d173af3ca217 151 #endif /* MYMPU9250_H */