Diff: MPU9250.cpp

Revision:

2:c7897a3f5f11

Parent:

1:b36bbc1c6d27

diff -r b36bbc1c6d27 -r c7897a3f5f11 MPU9250.cpp
--- a/MPU9250.cpp	Sat Apr 11 08:15:48 2020 +0000
+++ b/MPU9250.cpp	Wed Apr 22 11:50:00 2020 +0000
@@ -1,7 +1,6 @@
 #include "MPU9250.h"
 
 
-
 uint8_t Ascale = AFS_2G;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
 uint8_t Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
 uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
@@ -47,18 +46,28 @@
 float a_old[3] = {0.00f, 0.00f, 0.00f};
 float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
 
-
 accData_t myData;
-MPU9250 mpu9250;
 Timer t;
-Serial pc(USBTX, USBRX); // tx, rx
 
 #define SAMPLE_TIME 100
 
-
+#define STEP_NUMBER 5
 float sum = 0;
 uint32_t sumCount = 0;
 char buffer[14];
+float ax_sum = 0;
+static float vx_buffer[STEP_NUMBER];
+static int8_t stepCounter = 0;
+
+
+// MPU9250-Constructor
+MPU9250::MPU9250(Serial* serialPtr)
+{
+    pc = serialPtr;
+    imuSetup();
+    vx_old = 0;
+}
+
 
 
 //===================================================================================================================
@@ -611,192 +620,89 @@
 }
 
 
-
-// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
-// measured ones.
-void MPU9250::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
-{
-    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
-    float norm;
-    float hx, hy, bx, bz;
-    float vx, vy, vz, wx, wy, wz;
-    float ex, ey, ez;
-    float pa, pb, pc;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    float q1q1 = q1 * q1;
-    float q1q2 = q1 * q2;
-    float q1q3 = q1 * q3;
-    float q1q4 = q1 * q4;
-    float q2q2 = q2 * q2;
-    float q2q3 = q2 * q3;
-    float q2q4 = q2 * q4;
-    float q3q3 = q3 * q3;
-    float q3q4 = q3 * q4;
-    float q4q4 = q4 * q4;
-
-    // Normalise accelerometer measurement
-    norm = sqrt(ax * ax + ay * ay + az * az);
-    if (norm == 0.0f) return; // handle NaN
-    norm = 1.0f / norm;        // use reciprocal for division
-    ax *= norm;
-    ay *= norm;
-    az *= norm;
-
-    // Normalise magnetometer measurement
-    norm = sqrt(mx * mx + my * my + mz * mz);
-    if (norm == 0.0f) return; // handle NaN
-    norm = 1.0f / norm;        // use reciprocal for division
-    mx *= norm;
-    my *= norm;
-    mz *= norm;
-
-    // Reference direction of Earth's magnetic field
-    hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
-    hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
-    bx = sqrt((hx * hx) + (hy * hy));
-    bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
-
-    // Estimated direction of gravity and magnetic field
-    vx = 2.0f * (q2q4 - q1q3);
-    vy = 2.0f * (q1q2 + q3q4);
-    vz = q1q1 - q2q2 - q3q3 + q4q4;
-    wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
-    wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
-    wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
-
-    // Error is cross product between estimated direction and measured direction of gravity
-    ex = (ay * vz - az * vy) + (my * wz - mz * wy);
-    ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
-    ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
-    if (Ki > 0.0f) {
-        eInt[0] += ex;      // accumulate integral error
-        eInt[1] += ey;
-        eInt[2] += ez;
-    } else {
-        eInt[0] = 0.0f;     // prevent integral wind up
-        eInt[1] = 0.0f;
-        eInt[2] = 0.0f;
-    }
-
-    // Apply feedback terms
-    gx = gx + Kp * ex + Ki * eInt[0];
-    gy = gy + Kp * ey + Ki * eInt[1];
-    gz = gz + Kp * ez + Ki * eInt[2];
-
-    // Integrate rate of change of quaternion
-    pa = q2;
-    pb = q3;
-    pc = q4;
-    q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
-    q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
-    q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
-    q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
-
-    // Normalise quaternion
-    norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-    norm = 1.0f / norm;
-    q[0] = q1 * norm;
-    q[1] = q2 * norm;
-    q[2] = q3 * norm;
-    q[3] = q4 * norm;
-
-}
-
-// Integrates the acceleration to get the boards translative velocity
-void MPU9250::velocityUpdate(float ax, float ay, float az)
-{
-    // short name local variable for readability
-
-    v_trans[0] = deltat*0.5*(a_old[0] + ax*GRAVITATION) + v_trans[0];
-    v_trans[1] = deltat*0.5*(a_old[1] + ay*GRAVITATION) + v_trans[1];
-    v_trans[2] = deltat*0.5*(a_old[2] + az*GRAVITATION + GRAVITATION) + v_trans[2];
-
-    a_old[0] = ax*GRAVITATION;
-    a_old[1] = ay*GRAVITATION;
-    a_old[2] = az*GRAVITATION;
-}
-
 void MPU9250::readIMU()
 {
     // If intPin goes high, all data registers have new data
-    if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
+    if(readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
 
-        mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
+        readAccelData(accelCount);  // Read the x/y/z adc values
         // Now we'll calculate the accleration value into actual g's
         ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
         ay = (float)accelCount[1]*aRes - accelBias[1];
         az = (float)accelCount[2]*aRes - accelBias[2];
 
-        mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
+        readGyroData(gyroCount);  // Read the x/y/z adc values
         // Calculate the gyro value into actual degrees per second
         gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
         gy = (float)gyroCount[1]*gRes - gyroBias[1];
         gz = (float)gyroCount[2]*gRes - gyroBias[2];
 
-        mpu9250.readMagData(magCount);  // Read the x/y/z adc values
+        readMagData(magCount);  // Read the x/y/z adc values
         // Calculate the magnetometer values in milliGauss
         // Include factory calibration per data sheet and user environmental corrections
         mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
         my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
         mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
     }
+    
 
-    pc.printf("ax, ay, az, delta_t;%f;%f;%f;%f\n\r", ax, ay, az*GRAVITATION + GRAVITATION, deltat);
+
+    //pc.printf("ax, ay, az, delta_t;%f;%f;%f;%f\n\r", ax, ay, az*GRAVITATION + GRAVITATION, deltat);
 
     Now = t.read_us();
 
     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
     lastUpdate = Now;
+    
+    //pc.printf("%f,%f,%f,%f\n\r",ax, ay, az, deltat);
+
 
     sum += deltat;
     sumCount++;
 
-
-    mpu9250.velocityUpdate(ax, ay, az);
-
     // Pass gyro rate as rad/s
-    mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+    MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
     //mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - _count;
+    //pc.printf("Zeit intern: %d\n\r", t.read_ms());
+}
 
-    //-----------------------------------------
-    // Update displayed value
-    if (delt_t > SAMPLE_TIME) {
+
+
+/*//-----------------------------------------
+// Update displayed value
+if (delt_t > SAMPLE_TIME) {
 
 
 
-        //pc.printf("vx, vy, vz: %f %f %f\n\r", v_trans[0], v_trans[1], v_trans[2]);
+    //pc.printf("vx, vy, vz: %f %f %f\n\r", v_trans[0], v_trans[1], v_trans[2]);
+
+
+    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+    pitch *= 180.0f / PI;
+    yaw   *= 180.0f / PI;
+    yaw   -= 2.93f; // Declination at 8572 Berg TG: +2° 56'
+    roll  *= 180.0f / PI;
+
 
 
-        yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-        roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-        pitch *= 180.0f / PI;
-        yaw   *= 180.0f / PI;
-        yaw   -= 2.93f; // Declination at 8572 Berg TG: +2° 56'
-        roll  *= 180.0f / PI;
-        
-        myData.ax = yaw;
-        myData.ay = pitch;
-        myData.az = roll;
-
+    myled= !myled;
+    _count = t.read_ms();
 
-        myled= !myled;
-        _count = t.read_ms();
+    if(_count > 1<<21) {
+        t.start(); // start the timer over again if ~30 minutes has passed
+        _count = 0;
+        deltat= 0;
+        lastUpdate = t.read_us();
+    }
+    sum = 0;
+    sumCount = 0;
+}*/
 
-        if(_count > 1<<21) {
-            t.start(); // start the timer over again if ~30 minutes has passed
-            _count = 0;
-            deltat= 0;
-            lastUpdate = t.read_us();
-        }
-        sum = 0;
-        sumCount = 0;
-    }
-}
 
 
 void MPU9250::imuSetup()
@@ -804,74 +710,122 @@
     //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C
 
-    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+    pc->printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
 
     t.start();
 //  lcd.setBrightness(0.05);
 
 
     // Read the WHO_AM_I register, this is a good test of communication
-    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-    pc.printf("I AM 0x%x\n\r", whoami);
-    pc.printf("I SHOULD BE 0x71\n\r");
+    uint8_t whoami = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
+    pc->printf("I AM 0x%x\n\r", whoami);
+    pc->printf("I SHOULD BE 0x71\n\r");
 
     if (whoami == 0x71) { // WHO_AM_I should always be 0x68
-        pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
-        pc.printf("MPU9250 is online...\n\r");
+        pc->printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
+        pc->printf("MPU9250 is online...\n\r");
         sprintf(buffer, "0x%x", whoami);
         wait(1);
 
-        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
-        mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
-        pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
-        pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
-        pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
-        pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
-        pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
-        pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
-        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-        pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
-        pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
-        pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
-        pc.printf("x accel bias = %f\n\r", accelBias[0]);
-        pc.printf("y accel bias = %f\n\r", accelBias[1]);
-        pc.printf("z accel bias = %f\n\r", accelBias[2]);
+        resetMPU9250(); // Reset registers to default in preparation for device calibration
+        MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
+        pc->printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
+        pc->printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
+        pc->printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
+        pc->printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
+        pc->printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
+        pc->printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
+        calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+        pc->printf("x gyro bias = %f\n\r", gyroBias[0]);
+        pc->printf("y gyro bias = %f\n\r", gyroBias[1]);
+        pc->printf("z gyro bias = %f\n\r", gyroBias[2]);
+        pc->printf("x accel bias = %f\n\r", accelBias[0]);
+        pc->printf("y accel bias = %f\n\r", accelBias[1]);
+        pc->printf("z accel bias = %f\n\r", accelBias[2]);
         wait(2);
-        mpu9250.initMPU9250();
-        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
-        mpu9250.initAK8963(magCalibration);
-        pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
-        pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
-        pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
-        if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
-        if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
-        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
-        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+        initMPU9250();
+        pc->printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+        initAK8963(magCalibration);
+        pc->printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+        pc->printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
+        pc->printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
+        if(Mscale == 0) pc->printf("Magnetometer resolution = 14  bits\n\r");
+        if(Mscale == 1) pc->printf("Magnetometer resolution = 16  bits\n\r");
+        if(Mmode == 2) pc->printf("Magnetometer ODR = 8 Hz\n\r");
+        if(Mmode == 6) pc->printf("Magnetometer ODR = 100 Hz\n\r");
         wait(1);
     } else {
-        pc.printf("Could not connect to MPU9250: \n\r");
-        pc.printf("%#x \n",  whoami);
+        pc->printf("Could not connect to MPU9250: \n\r");
+        pc->printf("%#x \n",  whoami);
         sprintf(buffer, "WHO_AM_I 0x%x", whoami);
 
         while(1) {
             // Loop forever if communication doesn't happen
-            pc.printf("no IMU detected (verify if it's plugged in)\n\r");
+            pc->printf("no IMU detected (verify if it's plugged in)\n\r");
         }
     }
 
-    mpu9250.getAres(); // Get accelerometer sensitivity
-    mpu9250.getGres(); // Get gyro sensitivity
-    mpu9250.getMres(); // Get magnetometer sensitivity
-    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
-    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
-    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+    getAres(); // Get accelerometer sensitivity
+    getGres(); // Get gyro sensitivity
+    getMres(); // Get magnetometer sensitivity
+    pc->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+    pc->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+    pc->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
 }
 
-accData_t MPU9250::getVelocityFromIMU(){
-    readIMU();
+
+// N Werte sammeln, Integrale aufsummieren
+/*float MPU9250::getMedianAcc(){
+    int8_t i;
+    ax_sum = 0;
+    for (i=0; i<STEP_NUMBER; i++) {
+        readIMU();
+        //pc.printf("Zeit extern: %d\n\n\r", t.read_ms());
+        pc.printf("i=%d: ax=%f\n\r", i, ax);
+        ax_sum += ax;
+    }
+    pc.printf("Summe: %f\n\n\r", ax_sum/steps);
+    return ax_sum/steps;
+}*/
+
+
+// BufferArray-Variante
+float MPU9250::getVBuffer()
+{
+    // Save the last ax value before readIMU() is executed
+    float ax_old = ax;
     
-    return myData;
-}
\ No newline at end of file
+    // Save the last vx value
+    if (stepCounter == 0) {
+        vx_old = vx_buffer[STEP_NUMBER-1];
+    }
+    else{
+        vx_old = vx_buffer[stepCounter - 1];
+    }
+    
+    
+    // Sets new ax value
+    readIMU();
+
+
+    // Calculate Integration Step of new and old value
+    vx_buffer[stepCounter] = vx_old + deltat*0.5f*(ax + ax_old)/GRAVITATION;
+
+    int i;
+    float vx_median = 0;
+    for (i=0; i<STEP_NUMBER; i++) {
+        vx_median += vx_buffer[i];
+        //pc.printf("%d: %f\n\r", i, vx_buffer[i]);
+    }
+    //pc.printf("\n\r");
+    //pc.printf("%f,%f,%f,%f\n\r", ax, vx_median/STEP_NUMBER, vx_buffer[stepCounter], deltat);
+
+    stepCounter++;
+    if (stepCounter >= STEP_NUMBER) {
+        stepCounter = 0;
+    }
+    return vx_median/STEP_NUMBER;
+}