![](/media/cache/group/20170824_anyThing_Logo_blue_500_square.jpg.50x50_q85.jpg)
Working sensor, Tap detection, No quaternion
Dependencies: SEGGER_RTT mbed
Fork of MPU9250_simple by
Diff: main.cpp
- Revision:
- 4:9e5dfe44cf2b
- Parent:
- 3:d53674889db3
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;