Working sensor, Tap detection, No quaternion

Dependencies:   SEGGER_RTT mbed

Fork of MPU9250_simple by anyThing Connected Team

Files at this revision

API Documentation at this revision

Comitter:
MarijnJ
Date:
Thu Mar 01 21:34:57 2018 +0000
Parent:
3:d53674889db3
Commit message:
no quaternions, triple tap detection

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r d53674889db3 -r 9e5dfe44cf2b main.cpp
--- a/main.cpp	Mon Feb 19 12:43:16 2018 +0000
+++ b/main.cpp	Thu Mar 01 21:34:57 2018 +0000
@@ -21,29 +21,102 @@
 #include "SEGGER_RTT.c"
 #include "SEGGER_RTT_printf.c"
 
+//Initialize variables
+char buffer[14];
 float sum = 0, shift = 0;
 uint32_t sumCount = 0;
-int cycle = 0;
-char buffer[14];
 
-MPU9250 mpu9250;
-   
+//Initialize tap variables
+bool xTap = 0, xTapStarted = 0;
+int xDiff = 0, xTapLimit = 0, xTapDelay = 0;
+int tapDiff = 0, tapCount = 0, lastTapFrame = 0, tapCycleStart = 0 , tapCycleDuration = 0, now = 0, timerCycle = 0;
+float xMin = 0, xMax = 0, xTapStartValue = 0;
+//Initialize classes
+MPU9250 mpu9250;   
 Timer t;
 
-short toInt(float x)
+// Tap Settings
+int tapXThresh = 2000 ; //x and y axis acceleration threshold to trigger tap in mg
+int tapPulseLimit = 50 ; //Not a tap if acceleration duration over this limit
+int tapDelayDuration = 500; //Minimum time between taps in ms
+int tapCycleLimit = 3000; //Maximum cycle duration to detect multitap from first tap in ms
+int tapAction = 3; //Number of taps needed to trigger multitap action
+
+//Custom functions
+short toInt(float x) //Convert float to integer
 {
     return (x >= 0) ? (int)(x + 0.5) : (int)(x - 0.5);
 }
-int getTime(int counter)
+
+int getTime(int timer) //Get time in minutes since sensor was turned on
+{
+    return (int)((timer/60000.0f)+shift);
+}
+
+void resetTap()
+{
+    xTapLimit = now + tapPulseLimit;
+    xMax = ax;
+    xMin = ax;
+    xDiff = 0;
+    xTap = 0;
+    xTapStarted = 0;
+    xTapStartValue = 0; 
+}
+
+void detectTap()
 {
-    return (int)((counter/60000.0f)+shift);
+    now = t.read_ms();
+    if (now < xTapLimit)
+    {
+        if (ax < xMin) xMin = ax;
+        if (ax > xMax)  xMax = ax;
+        tapDiff = toInt(1000*(xMax - xMin));
+        if (tapDiff > xDiff) xDiff = tapDiff;
+    }
+    else  //Reset tap difference values every 50ms max tap detection duration time
+    {
+        resetTap();
+    }
+    
+    if ((xDiff > tapXThresh) //if acceleration is above threshold
+        && (now > xTapDelay)  //only register tap again after tap delay
+        && (xTapStarted == 0)) //wait for acceleration to end
+    {
+        resetTap();
+        xTapStarted = 1;
+        xTapStartValue = ax;
+    }
+    if (now < xTapLimit && xTapStarted == 1) //Check if acceleration stops within pulse window
+    {
+        if (ax < xTapStartValue) //if acceleration falls again within limit, tap detected
+        {
+            resetTap();
+            xTap = 1;
+            xTapDelay = now + tapDelayDuration;
+            SEGGER_RTT_WriteString(0, "---------------------------------Tap detected\n"); 
+        }
+    }
+    else
+    {
+        resetTap();
+    }
+    // Detects taps on the x axis and resets some of the flags
+    if (xTap)    // check for tap
+    {
+        now = t.read_ms();
+        if (tapCount == 0) tapCycleStart = now;  // if first tap reset tap cycle
+        tapCount++;                            // increment tap counter
+        if ((now - tapCycleStart) > tapCycleLimit || tapCount >= tapAction) tapCount = 0;    //Reset tap count after cycle ends
+        if (tapCount == tapAction) SEGGER_RTT_WriteString(0, "---------------------------------Triple tap\n");                 // do we have 3 taps during cycle?
+    }
 }
-       
+
 int main()
 {
     //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C    
-    SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+    SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\n", SystemCoreClock);
     t.start();
     
     // Read the WHO_AM_I register, this is a good test of communication
@@ -52,7 +125,7 @@
     if (whoami == 0x71) // WHO_AM_I should always be 0x68
     { 
         SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n");
-        wait(1);
+        wait(2);
         
         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
         mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
@@ -85,7 +158,8 @@
         SEGGER_RTT_WriteString(0, "\n");
         wait(1);
     }
-    else {
+    else
+    {
         SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n",  whoami); 
         while(1); // Loop forever if communication doesn't happen
     }
@@ -99,7 +173,8 @@
     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
-
+    resetTap();
+    
     while(1)
     {
         // If intPin goes high, all data registers have new data
@@ -122,31 +197,34 @@
             // 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];   
+            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+            
+            // Update Tap detection values
+            detectTap();
         }
    
-        Now = t.read_us();
-        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-        lastUpdate = Now;
+        now = t.read_us();
+        deltat = (float)((now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        lastUpdate = now;
         
         sum += deltat;
         sumCount++;
         
-        if(lastUpdate - firstUpdate > 10000000.0f)
-        {
-            beta = 0.04;  // decrease filter gain after stabilized
-            zeta = 0.015; // increasey bias drift gain after stabilized
-        }
+        //if(lastUpdate - firstUpdate > 10000000.0f)
+        //{
+        //    beta = 0.04;  // decrease filter gain after stabilized
+        //    zeta = 0.015; // increasey bias drift gain after stabilized
+        //}
         
        // 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);
-        mpu9250.MahonyQuaternionUpdate(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 1 s rate independent of data rates
         delt_t = t.read_ms() - count;
-        if (delt_t > 1000) // update print once per second independent of read rate
+        if (delt_t > 200) // update print once per second independent of read rate
         {
-            SEGGER_RTT_printf(0, "\n\nax = %d", toInt(1000*ax)); 
+            SEGGER_RTT_printf(0, "\nax = %d", toInt(1000*ax)); 
             SEGGER_RTT_printf(0, " ay = %d", toInt(1000*ay)); 
             SEGGER_RTT_printf(0, " az = %d  mg\n", toInt(1000*az)); 
         
@@ -176,7 +254,7 @@
             // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
             // applied in the correct order which for this configuration is yaw, pitch, and then roll.
             // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
-            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]);   
+            /*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;
@@ -184,11 +262,11 @@
             yaw   -= 1.05f; // Declination at Delft is 1 degrees 3 minutes on 2018-02-16
             roll  *= 180.0f / PI;
         
-            SEGGER_RTT_printf(0, "Yaw: %d   Pitch: %d   Roll: %d\n\n", toInt(yaw), toInt(pitch), toInt(roll));
-            //SEGGER_RTT_printf(0, "average rate = %d\n\n\n", toInt((float) sumCount/sum));
+            SEGGER_RTT_printf(0, "Yaw: %d   Pitch: %d   Roll: %d\n\n", toInt(yaw), toInt(pitch), toInt(roll));*/
+            SEGGER_RTT_printf(0, "average rate = %d\n\n\n", toInt((float) sumCount/sum));
         
             count = t.read_ms();
-            SEGGER_RTT_printf(0, "Time active: %d minutes\n---------------------------------", getTime(count));
+            SEGGER_RTT_printf(0, "Time active: %d minutes\n---------------------------------\n\n", getTime(count));
         
             if(count > 1<<21)
             {
@@ -196,7 +274,7 @@
                 count = 0;
                 deltat= 0;
                 lastUpdate = t.read_us();
-                shift = (++cycle * 34.9525f);
+                shift = (++timerCycle * 34.9525f);
             }
             sum = 0;
             sumCount = 0; 
diff -r d53674889db3 -r 9e5dfe44cf2b mbed.bld
--- a/mbed.bld	Mon Feb 19 12:43:16 2018 +0000
+++ b/mbed.bld	Thu Mar 01 21:34:57 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/7130f322cb7e
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5571c4ff569f
\ No newline at end of file