anyThing Connected Team / Mbed 2 deprecated MPU9250_tap_better

Working sensor, Tap detection, No quaternion

Dependencies:   SEGGER_RTT mbed

Fork of MPU9250_simple by anyThing Connected Team

main.cpp@4:9e5dfe44cf2b, 2018-03-01 (annotated)

Committer:
MarijnJ
Date:
Thu Mar 01 21:34:57 2018 +0000
Revision:
4:9e5dfe44cf2b
Parent:
3:d53674889db3 
no quaternions, triple tap detection

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onehorse 0:2e5e65a6fb30 1 /* MPU9250 Basic Example Code
MarijnJ 3:d53674889db3 2 by: Kris Winer
MarijnJ 3:d53674889db3 3 date: April 1, 2014
MarijnJ 3:d53674889db3 4 license: Beerware - Use this code however you'd like. If you
MarijnJ 3:d53674889db3 5 find it useful you can buy me a beer some time.
MarijnJ 3:d53674889db3 6
MarijnJ 3:d53674889db3 7 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
MarijnJ 3:d53674889db3 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
MarijnJ 3:d53674889db3 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
MarijnJ 3:d53674889db3 10 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
MarijnJ 3:d53674889db3 11
MarijnJ 3:d53674889db3 12 -----------------------------------------------------------------------
MarijnJ 3:d53674889db3 13
MarijnJ 3:d53674889db3 14 Adapted by Marijn Jeurissen for the anyThing Connected Sensor Sticker based on Nordic nRF51822
MarijnJ 3:d53674889db3 15 date: February 16, 2018
MarijnJ 3:d53674889db3 16 */
MarijnJ 3:d53674889db3 17
onehorse 0:2e5e65a6fb30 18 #include "mbed.h"
onehorse 0:2e5e65a6fb30 19 #include "MPU9250.h"
MarijnJ 3:d53674889db3 20 #include "SEGGER_RTT.h"
MarijnJ 3:d53674889db3 21 #include "SEGGER_RTT.c"
MarijnJ 3:d53674889db3 22 #include "SEGGER_RTT_printf.c"
onehorse 0:2e5e65a6fb30 23
MarijnJ 4:9e5dfe44cf2b 24 //Initialize variables
MarijnJ 4:9e5dfe44cf2b 25 char buffer[14];
MarijnJ 3:d53674889db3 26 float sum = 0, shift = 0;
onehorse 0:2e5e65a6fb30 27 uint32_t sumCount = 0;
onehorse 0:2e5e65a6fb30 28
MarijnJ 4:9e5dfe44cf2b 29 //Initialize tap variables
MarijnJ 4:9e5dfe44cf2b 30 bool xTap = 0, xTapStarted = 0;
MarijnJ 4:9e5dfe44cf2b 31 int xDiff = 0, xTapLimit = 0, xTapDelay = 0;
MarijnJ 4:9e5dfe44cf2b 32 int tapDiff = 0, tapCount = 0, lastTapFrame = 0, tapCycleStart = 0 , tapCycleDuration = 0, now = 0, timerCycle = 0;
MarijnJ 4:9e5dfe44cf2b 33 float xMin = 0, xMax = 0, xTapStartValue = 0;
MarijnJ 4:9e5dfe44cf2b 34 //Initialize classes
MarijnJ 4:9e5dfe44cf2b 35 MPU9250 mpu9250;
MarijnJ 3:d53674889db3 36 Timer t;
onehorse 0:2e5e65a6fb30 37
MarijnJ 4:9e5dfe44cf2b 38 // Tap Settings
MarijnJ 4:9e5dfe44cf2b 39 int tapXThresh = 2000 ; //x and y axis acceleration threshold to trigger tap in mg
MarijnJ 4:9e5dfe44cf2b 40 int tapPulseLimit = 50 ; //Not a tap if acceleration duration over this limit
MarijnJ 4:9e5dfe44cf2b 41 int tapDelayDuration = 500; //Minimum time between taps in ms
MarijnJ 4:9e5dfe44cf2b 42 int tapCycleLimit = 3000; //Maximum cycle duration to detect multitap from first tap in ms
MarijnJ 4:9e5dfe44cf2b 43 int tapAction = 3; //Number of taps needed to trigger multitap action
MarijnJ 4:9e5dfe44cf2b 44
MarijnJ 4:9e5dfe44cf2b 45 //Custom functions
MarijnJ 4:9e5dfe44cf2b 46 short toInt(float x) //Convert float to integer
MarijnJ 3:d53674889db3 47 {
MarijnJ 3:d53674889db3 48 return (x >= 0) ? (int)(x + 0.5) : (int)(x - 0.5);
MarijnJ 3:d53674889db3 49 }
MarijnJ 4:9e5dfe44cf2b 50
MarijnJ 4:9e5dfe44cf2b 51 int getTime(int timer) //Get time in minutes since sensor was turned on
MarijnJ 4:9e5dfe44cf2b 52 {
MarijnJ 4:9e5dfe44cf2b 53 return (int)((timer/60000.0f)+shift);
MarijnJ 4:9e5dfe44cf2b 54 }
MarijnJ 4:9e5dfe44cf2b 55
MarijnJ 4:9e5dfe44cf2b 56 void resetTap()
MarijnJ 4:9e5dfe44cf2b 57 {
MarijnJ 4:9e5dfe44cf2b 58 xTapLimit = now + tapPulseLimit;
MarijnJ 4:9e5dfe44cf2b 59 xMax = ax;
MarijnJ 4:9e5dfe44cf2b 60 xMin = ax;
MarijnJ 4:9e5dfe44cf2b 61 xDiff = 0;
MarijnJ 4:9e5dfe44cf2b 62 xTap = 0;
MarijnJ 4:9e5dfe44cf2b 63 xTapStarted = 0;
MarijnJ 4:9e5dfe44cf2b 64 xTapStartValue = 0;
MarijnJ 4:9e5dfe44cf2b 65 }
MarijnJ 4:9e5dfe44cf2b 66
MarijnJ 4:9e5dfe44cf2b 67 void detectTap()
MarijnJ 3:d53674889db3 68 {
MarijnJ 4:9e5dfe44cf2b 69 now = t.read_ms();
MarijnJ 4:9e5dfe44cf2b 70 if (now < xTapLimit)
MarijnJ 4:9e5dfe44cf2b 71 {
MarijnJ 4:9e5dfe44cf2b 72 if (ax < xMin) xMin = ax;
MarijnJ 4:9e5dfe44cf2b 73 if (ax > xMax) xMax = ax;
MarijnJ 4:9e5dfe44cf2b 74 tapDiff = toInt(1000*(xMax - xMin));
MarijnJ 4:9e5dfe44cf2b 75 if (tapDiff > xDiff) xDiff = tapDiff;
MarijnJ 4:9e5dfe44cf2b 76 }
MarijnJ 4:9e5dfe44cf2b 77 else //Reset tap difference values every 50ms max tap detection duration time
MarijnJ 4:9e5dfe44cf2b 78 {
MarijnJ 4:9e5dfe44cf2b 79 resetTap();
MarijnJ 4:9e5dfe44cf2b 80 }
MarijnJ 4:9e5dfe44cf2b 81
MarijnJ 4:9e5dfe44cf2b 82 if ((xDiff > tapXThresh) //if acceleration is above threshold
MarijnJ 4:9e5dfe44cf2b 83 && (now > xTapDelay) //only register tap again after tap delay
MarijnJ 4:9e5dfe44cf2b 84 && (xTapStarted == 0)) //wait for acceleration to end
MarijnJ 4:9e5dfe44cf2b 85 {
MarijnJ 4:9e5dfe44cf2b 86 resetTap();
MarijnJ 4:9e5dfe44cf2b 87 xTapStarted = 1;
MarijnJ 4:9e5dfe44cf2b 88 xTapStartValue = ax;
MarijnJ 4:9e5dfe44cf2b 89 }
MarijnJ 4:9e5dfe44cf2b 90 if (now < xTapLimit && xTapStarted == 1) //Check if acceleration stops within pulse window
MarijnJ 4:9e5dfe44cf2b 91 {
MarijnJ 4:9e5dfe44cf2b 92 if (ax < xTapStartValue) //if acceleration falls again within limit, tap detected
MarijnJ 4:9e5dfe44cf2b 93 {
MarijnJ 4:9e5dfe44cf2b 94 resetTap();
MarijnJ 4:9e5dfe44cf2b 95 xTap = 1;
MarijnJ 4:9e5dfe44cf2b 96 xTapDelay = now + tapDelayDuration;
MarijnJ 4:9e5dfe44cf2b 97 SEGGER_RTT_WriteString(0, "---------------------------------Tap detected\n");
MarijnJ 4:9e5dfe44cf2b 98 }
MarijnJ 4:9e5dfe44cf2b 99 }
MarijnJ 4:9e5dfe44cf2b 100 else
MarijnJ 4:9e5dfe44cf2b 101 {
MarijnJ 4:9e5dfe44cf2b 102 resetTap();
MarijnJ 4:9e5dfe44cf2b 103 }
MarijnJ 4:9e5dfe44cf2b 104 // Detects taps on the x axis and resets some of the flags
MarijnJ 4:9e5dfe44cf2b 105 if (xTap) // check for tap
MarijnJ 4:9e5dfe44cf2b 106 {
MarijnJ 4:9e5dfe44cf2b 107 now = t.read_ms();
MarijnJ 4:9e5dfe44cf2b 108 if (tapCount == 0) tapCycleStart = now; // if first tap reset tap cycle
MarijnJ 4:9e5dfe44cf2b 109 tapCount++; // increment tap counter
MarijnJ 4:9e5dfe44cf2b 110 if ((now - tapCycleStart) > tapCycleLimit || tapCount >= tapAction) tapCount = 0; //Reset tap count after cycle ends
MarijnJ 4:9e5dfe44cf2b 111 if (tapCount == tapAction) SEGGER_RTT_WriteString(0, "---------------------------------Triple tap\n"); // do we have 3 taps during cycle?
MarijnJ 4:9e5dfe44cf2b 112 }
MarijnJ 3:d53674889db3 113 }
MarijnJ 4:9e5dfe44cf2b 114
onehorse 0:2e5e65a6fb30 115 int main()
onehorse 0:2e5e65a6fb30 116 {
MarijnJ 3:d53674889db3 117 //Set up I2C
MarijnJ 3:d53674889db3 118 i2c.frequency(400000); // use fast (400 kHz) I2C
MarijnJ 4:9e5dfe44cf2b 119 SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\n", SystemCoreClock);
MarijnJ 3:d53674889db3 120 t.start();
onehorse 0:2e5e65a6fb30 121
MarijnJ 3:d53674889db3 122 // Read the WHO_AM_I register, this is a good test of communication
MarijnJ 3:d53674889db3 123 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
onehorse 0:2e5e65a6fb30 124
MarijnJ 3:d53674889db3 125 if (whoami == 0x71) // WHO_AM_I should always be 0x68
MarijnJ 3:d53674889db3 126 {
MarijnJ 3:d53674889db3 127 SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n");
MarijnJ 4:9e5dfe44cf2b 128 wait(2);
MarijnJ 3:d53674889db3 129
MarijnJ 3:d53674889db3 130 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
MarijnJ 3:d53674889db3 131 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
MarijnJ 3:d53674889db3 132 SEGGER_RTT_printf(0, "x-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[0]));
MarijnJ 3:d53674889db3 133 SEGGER_RTT_printf(0, "y-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[1]));
MarijnJ 3:d53674889db3 134 SEGGER_RTT_printf(0, "z-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[2]));
MarijnJ 3:d53674889db3 135 SEGGER_RTT_printf(0, "x-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[3]));
MarijnJ 3:d53674889db3 136 SEGGER_RTT_printf(0, "y-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[4]));
MarijnJ 3:d53674889db3 137 SEGGER_RTT_printf(0, "z-axis self test: gyration trim within : %d % of factory value\n\n", toInt(SelfTest[5]));
MarijnJ 3:d53674889db3 138
MarijnJ 3:d53674889db3 139 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
MarijnJ 3:d53674889db3 140 SEGGER_RTT_printf(0, "x gyro bias = %d\n", toInt(gyroBias[0]));
MarijnJ 3:d53674889db3 141 SEGGER_RTT_printf(0, "y gyro bias = %d\n", toInt(gyroBias[1]));
MarijnJ 3:d53674889db3 142 SEGGER_RTT_printf(0, "z gyro bias = %d\n", toInt(gyroBias[2]));
MarijnJ 3:d53674889db3 143 SEGGER_RTT_printf(0, "x accel bias = %d\n", toInt(accelBias[0]));
MarijnJ 3:d53674889db3 144 SEGGER_RTT_printf(0, "y accel bias = %d\n", toInt(accelBias[1]));
MarijnJ 3:d53674889db3 145 SEGGER_RTT_printf(0, "z accel bias = %d\n\n", toInt(accelBias[2]));
MarijnJ 3:d53674889db3 146 wait(2);
MarijnJ 3:d53674889db3 147
MarijnJ 3:d53674889db3 148 mpu9250.initMPU9250();
MarijnJ 3:d53674889db3 149 SEGGER_RTT_WriteString(0, "MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
MarijnJ 3:d53674889db3 150 mpu9250.initAK8963(magCalibration);
MarijnJ 3:d53674889db3 151 SEGGER_RTT_WriteString(0, "AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer
MarijnJ 3:d53674889db3 152 SEGGER_RTT_printf(0, "Accelerometer full-scale range = %d g\n", toInt(2.0f*(float)(1<<Ascale)));
MarijnJ 3:d53674889db3 153 SEGGER_RTT_printf(0, "Gyroscope full-scale range = %d deg/s\n", toInt(250.0f*(float)(1<<Gscale)));
MarijnJ 3:d53674889db3 154 if(Mscale == 0) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 14 bits\n");
MarijnJ 3:d53674889db3 155 if(Mscale == 1) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 16 bits\n");
MarijnJ 3:d53674889db3 156 if(Mmode == 2) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 8 Hz\n");
MarijnJ 3:d53674889db3 157 if(Mmode == 6) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 100 Hz\n");
MarijnJ 3:d53674889db3 158 SEGGER_RTT_WriteString(0, "\n");
MarijnJ 3:d53674889db3 159 wait(1);
MarijnJ 3:d53674889db3 160 }
MarijnJ 4:9e5dfe44cf2b 161 else
MarijnJ 4:9e5dfe44cf2b 162 {
MarijnJ 3:d53674889db3 163 SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n", whoami);
MarijnJ 3:d53674889db3 164 while(1); // Loop forever if communication doesn't happen
onehorse 0:2e5e65a6fb30 165 }
onehorse 0:2e5e65a6fb30 166
onehorse 0:2e5e65a6fb30 167 mpu9250.getAres(); // Get accelerometer sensitivity
onehorse 0:2e5e65a6fb30 168 mpu9250.getGres(); // Get gyro sensitivity
onehorse 0:2e5e65a6fb30 169 mpu9250.getMres(); // Get magnetometer sensitivity
MarijnJ 3:d53674889db3 170 SEGGER_RTT_printf(0, "Accelerometer sensitivity is %d LSB/g \n", toInt(1.0f/aRes));
MarijnJ 3:d53674889db3 171 SEGGER_RTT_printf(0, "Gyroscope sensitivity is %d LSB/deg/s \n", toInt(1.0f/gRes));
MarijnJ 3:d53674889db3 172 SEGGER_RTT_printf(0, "Magnetometer sensitivity is %d LSB/G \n", toInt(1.0f/mRes));
onehorse 0:2e5e65a6fb30 173 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
onehorse 0:2e5e65a6fb30 174 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
onehorse 0:2e5e65a6fb30 175 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
MarijnJ 4:9e5dfe44cf2b 176 resetTap();
MarijnJ 4:9e5dfe44cf2b 177
MarijnJ 3:d53674889db3 178 while(1)
MarijnJ 3:d53674889db3 179 {
MarijnJ 3:d53674889db3 180 // If intPin goes high, all data registers have new data
MarijnJ 3:d53674889db3 181 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) // On interrupt, check if data ready interrupt
MarijnJ 3:d53674889db3 182 {
MarijnJ 3:d53674889db3 183 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
MarijnJ 3:d53674889db3 184 // Now we'll calculate the accleration value into actual g's
MarijnJ 3:d53674889db3 185 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
MarijnJ 3:d53674889db3 186 ay = (float)accelCount[1]*aRes - accelBias[1];
MarijnJ 3:d53674889db3 187 az = (float)accelCount[2]*aRes - accelBias[2];
MarijnJ 3:d53674889db3 188
MarijnJ 3:d53674889db3 189 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
MarijnJ 3:d53674889db3 190 // Calculate the gyro value into actual degrees per second
MarijnJ 3:d53674889db3 191 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
MarijnJ 3:d53674889db3 192 gy = (float)gyroCount[1]*gRes - gyroBias[1];
MarijnJ 3:d53674889db3 193 gz = (float)gyroCount[2]*gRes - gyroBias[2];
MarijnJ 3:d53674889db3 194
MarijnJ 3:d53674889db3 195 mpu9250.readMagData(magCount); // Read the x/y/z adc values
MarijnJ 3:d53674889db3 196 // Calculate the magnetometer values in milliGauss
MarijnJ 3:d53674889db3 197 // Include factory calibration per data sheet and user environmental corrections
MarijnJ 3:d53674889db3 198 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
MarijnJ 3:d53674889db3 199 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
MarijnJ 4:9e5dfe44cf2b 200 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
MarijnJ 4:9e5dfe44cf2b 201
MarijnJ 4:9e5dfe44cf2b 202 // Update Tap detection values
MarijnJ 4:9e5dfe44cf2b 203 detectTap();
MarijnJ 3:d53674889db3 204 }
onehorse 0:2e5e65a6fb30 205
MarijnJ 4:9e5dfe44cf2b 206 now = t.read_us();
MarijnJ 4:9e5dfe44cf2b 207 deltat = (float)((now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
MarijnJ 4:9e5dfe44cf2b 208 lastUpdate = now;
MarijnJ 3:d53674889db3 209
MarijnJ 3:d53674889db3 210 sum += deltat;
MarijnJ 3:d53674889db3 211 sumCount++;
MarijnJ 3:d53674889db3 212
MarijnJ 4:9e5dfe44cf2b 213 //if(lastUpdate - firstUpdate > 10000000.0f)
MarijnJ 4:9e5dfe44cf2b 214 //{
MarijnJ 4:9e5dfe44cf2b 215 // beta = 0.04; // decrease filter gain after stabilized
MarijnJ 4:9e5dfe44cf2b 216 // zeta = 0.015; // increasey bias drift gain after stabilized
MarijnJ 4:9e5dfe44cf2b 217 //}
MarijnJ 3:d53674889db3 218
MarijnJ 3:d53674889db3 219 // Pass gyro rate as rad/s
MarijnJ 3:d53674889db3 220 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
MarijnJ 4:9e5dfe44cf2b 221 //mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
onehorse 0:2e5e65a6fb30 222
MarijnJ 3:d53674889db3 223 // Serial print 1 s rate independent of data rates
MarijnJ 3:d53674889db3 224 delt_t = t.read_ms() - count;
MarijnJ 4:9e5dfe44cf2b 225 if (delt_t > 200) // update print once per second independent of read rate
MarijnJ 3:d53674889db3 226 {
MarijnJ 4:9e5dfe44cf2b 227 SEGGER_RTT_printf(0, "\nax = %d", toInt(1000*ax));
MarijnJ 3:d53674889db3 228 SEGGER_RTT_printf(0, " ay = %d", toInt(1000*ay));
MarijnJ 3:d53674889db3 229 SEGGER_RTT_printf(0, " az = %d mg\n", toInt(1000*az));
MarijnJ 3:d53674889db3 230
MarijnJ 3:d53674889db3 231 SEGGER_RTT_printf(0, "gx = %d", toInt(gx));
MarijnJ 3:d53674889db3 232 SEGGER_RTT_printf(0, " gy = %d", toInt(gy));
MarijnJ 3:d53674889db3 233 SEGGER_RTT_printf(0, " gz = %d deg/s\n", toInt(gz));
MarijnJ 3:d53674889db3 234
MarijnJ 3:d53674889db3 235 SEGGER_RTT_printf(0, "mx = %d", toInt(mx));
MarijnJ 3:d53674889db3 236 SEGGER_RTT_printf(0, " my = %d", toInt(my));
MarijnJ 3:d53674889db3 237 SEGGER_RTT_printf(0, " mz = %d mG\n", toInt(mz));
MarijnJ 3:d53674889db3 238
MarijnJ 3:d53674889db3 239 tempCount = mpu9250.readTempData(); // Read the adc values
MarijnJ 3:d53674889db3 240 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Celsius
MarijnJ 3:d53674889db3 241 SEGGER_RTT_printf(0, "Temperature = %d C\n\n", toInt(temperature));
MarijnJ 3:d53674889db3 242
MarijnJ 3:d53674889db3 243 //SEGGER_RTT_printf(0, "q0 = %d\n", toInt(q[0]*100));
MarijnJ 3:d53674889db3 244 //SEGGER_RTT_printf(0, "q1 = %d\n", toInt(q[1]*100));
MarijnJ 3:d53674889db3 245 //SEGGER_RTT_printf(0, "q2 = %d\n", toInt(q[2]*100));
MarijnJ 3:d53674889db3 246 //SEGGER_RTT_printf(0, "q3 = %d\n", toInt(q[3]*100));
MarijnJ 3:d53674889db3 247
MarijnJ 3:d53674889db3 248 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
MarijnJ 3:d53674889db3 249 // In this coordinate system, the positive z-axis is down toward Earth.
MarijnJ 3:d53674889db3 250 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
MarijnJ 3:d53674889db3 251 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
MarijnJ 3:d53674889db3 252 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
MarijnJ 3:d53674889db3 253 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
MarijnJ 3:d53674889db3 254 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
MarijnJ 3:d53674889db3 255 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
MarijnJ 3:d53674889db3 256 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
MarijnJ 4:9e5dfe44cf2b 257 /*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]);
MarijnJ 3:d53674889db3 258 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
MarijnJ 3:d53674889db3 259 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]);
MarijnJ 3:d53674889db3 260 pitch *= 180.0f / PI;
MarijnJ 3:d53674889db3 261 yaw *= 180.0f / PI;
MarijnJ 3:d53674889db3 262 yaw -= 1.05f; // Declination at Delft is 1 degrees 3 minutes on 2018-02-16
MarijnJ 3:d53674889db3 263 roll *= 180.0f / PI;
MarijnJ 3:d53674889db3 264
MarijnJ 4:9e5dfe44cf2b 265 SEGGER_RTT_printf(0, "Yaw: %d Pitch: %d Roll: %d\n\n", toInt(yaw), toInt(pitch), toInt(roll));*/
MarijnJ 4:9e5dfe44cf2b 266 SEGGER_RTT_printf(0, "average rate = %d\n\n\n", toInt((float) sumCount/sum));
MarijnJ 3:d53674889db3 267
MarijnJ 3:d53674889db3 268 count = t.read_ms();
MarijnJ 4:9e5dfe44cf2b 269 SEGGER_RTT_printf(0, "Time active: %d minutes\n---------------------------------\n\n", getTime(count));
MarijnJ 3:d53674889db3 270
MarijnJ 3:d53674889db3 271 if(count > 1<<21)
MarijnJ 3:d53674889db3 272 {
MarijnJ 3:d53674889db3 273 t.start(); // start the timer over again if ~30 minutes has passed
MarijnJ 3:d53674889db3 274 count = 0;
MarijnJ 3:d53674889db3 275 deltat= 0;
MarijnJ 3:d53674889db3 276 lastUpdate = t.read_us();
MarijnJ 4:9e5dfe44cf2b 277 shift = (++timerCycle * 34.9525f);
MarijnJ 3:d53674889db3 278 }
MarijnJ 3:d53674889db3 279 sum = 0;
MarijnJ 3:d53674889db3 280 sumCount = 0;
MarijnJ 3:d53674889db3 281 }
MarijnJ 3:d53674889db3 282 }
MarijnJ 3:d53674889db3 283 }