CONFIGURACION
Revision 0:d173af3ca217, committed 2020-02-06
- Comitter:
- flozada
- Date:
- Thu Feb 06 03:28:07 2020 +0000
- Commit message:
- AVANCE CONFIG ACELEROMETRO 1_ FLOZADA
Changed in this revision
myMPU9250.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r d173af3ca217 myMPU9250.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/myMPU9250.h Thu Feb 06 03:28:07 2020 +0000 @@ -0,0 +1,151 @@ +#ifndef MYMPU9250_H +#define MYMPU9250_H + +#include "mpu9250Define.h" + +class MyMPU9250 +{ +public: + MyMPU9250() + : pc_(USBTX, USBRX), i2c_(p28, p27) { + accelLSB2MG_ = 10.0; + gyroLSB2DegPerSec_ = 0.0; + }; + + bool initialize() { + i2c_.frequency(400000); + i2c_.start(); + + wait(0.5); + + char cmd[2] = { 0x00 }; + cmd[0] = PWR_MGMT_1; // 0x6B + cmd[1] = (PWR_MGMT_1_CLKSEL_0); // 0x00 + this->write(cmd, BIT_2); + + wait(0.5); + + char i_am[1] = { 0x00 }; + if(! this->read(WHO_AM_I, i_am, BIT_1)) { + pc_.printf("Fail to read! I am %x\n", i_am[0]); + return false; + } else if(!(i_am[0] == 0x71)) { + pc_.printf("Fail to identify! I am %x\n", i_am[0]); + return false; + } + return true; + }; + + void setup(AccelSensitivity accel_mode=AFS_SEL_2, + GyroSensitivity gyro_mode=FS_SEL_250) { + char cmd[2] = { 0x00 }; + + cmd[0] = ACCEL_CONFIG; + cmd[1] = ACCEL_CONFIG_AFS_SEL_2;//(0x18); + this->write(cmd, BIT_2); + accelLSB2MG_ = getAccelLSB2MG(accel_mode); + pc_.printf("accelLSB2MG: %f, %x", accelLSB2MG_, cmd[1]); + + cmd[0] = GYRO_CONFIG; + cmd[1] = getGyroSensitivity(gyro_mode);//(0x18); + this->write(cmd, BIT_2); + gyroLSB2DegPerSec_ = getGyroLSB2DegPerSec(gyro_mode); + + cmd[0] = INT_PIN_CFG; // 0x37 + cmd[1] = (INT_PIN_DFG_I2C_BYPASS_EN); // 0x02 + this->write(cmd, BIT_2); + }; + + bool updateAccelAllAxes() { + return this->read(ACCEL_XOUT_H, accel_all_, BIT_6); + }; + + int accelXRaw() { + return (int16_t)(accel_all_[0]*0x100 + accel_all_[1]) ; + }; + + double accelX() { + return accelLSB2MG_ * accelXRaw() ; + }; + + int accelYRaw() { + return (int16_t)(accel_all_[2]*0x100 + accel_all_[3]) ; + }; + + double accelY() { + return accelLSB2MG_ * accelYRaw() ; + }; + + int accelZRaw() { + return (int16_t)(accel_all_[4]*0x100 + accel_all_[5]) ; + }; + + double accelZ() { + return accelLSB2MG_ * accelZRaw() ; + }; + + + bool updateGyroAllAxes() { + return this->read(GYRO_XOUT_H, gyro_all_, BIT_6); + }; + + int gyroXRaw() { + return (int16_t)(gyro_all_[0]*0x100 + gyro_all_[1]) ; + }; + + double gyroX() { + return accelLSB2MG_ * gyroXRaw() ; + }; + + int gyroYRaw() { + return (int16_t)(gyro_all_[2]*0x100 + gyro_all_[3]) ; + }; + + double gyroY() { + return accelLSB2MG_ * gyroYRaw() ; + }; + + int gyroZRaw() { + return (int16_t)(gyro_all_[4]*0x100 + gyro_all_[5]) ; + }; + + double gyroZ() { + return accelLSB2MG_ * gyroZRaw() ; + }; + + +private: + Serial pc_; + I2C i2c_; + float accelLSB2MG_; + float gyroLSB2DegPerSec_; + char accel_all_[6]; + char gyro_all_[6]; + + bool write(char* _cmd, int _length) { + int w_status = i2c_.write(MPU9250_ADDRESS, _cmd, _length, false); + if(w_status == 0) { + return true; + } else { + pc_.printf("write register: %x \n", _cmd[0]); + pc_.printf(" write: %d \n", w_status); + return false; + } + } + + bool read(char _register, char* _data, int _length) { + char cmd[1] = { _register }; + int w_status = i2c_.write(MPU9250_ADDRESS, cmd, BIT_1, false); + int r_status = i2c_.read(MPU9250_ADDRESS, _data, _length, false); + if(w_status + r_status == 0) { + return true; + } else { + pc_.printf("write register: %x \n", cmd[0]); + pc_.printf("read register: %x \n", _register); + pc_.printf(" write: %d | read: %d \n", w_status, r_status); + return false; + } + }; +}; + +#endif /* MYMPU9250_H */ \ No newline at end of file