Revision 4:9e5dfe44cf2b, committed 2018-03-01

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

--- 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; 

--- 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