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

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