Working sensor, Tap detection, No quaternion
Dependencies: SEGGER_RTT mbed
Fork of MPU9250_simple by
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?
User | Revision | Line number | New 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 | } |