Sensor data verwerking
Dependencies: SEGGER_RTT mbed
Fork of MPU9250AHRS by
main.cpp
- Committer:
- MarijnJ
- Date:
- 2018-02-19
- Revision:
- 3:d53674889db3
- Parent:
- 1:71c319f03fda
File content as of revision 3:d53674889db3:
/* MPU9250 Basic Example Code by: Kris Winer date: April 1, 2014 license: Beerware - Use this code however you'd like. If you find it useful you can buy me a beer some time. Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. ----------------------------------------------------------------------- Adapted by Marijn Jeurissen for the anyThing Connected Sensor Sticker based on Nordic nRF51822 date: February 16, 2018 */ #include "mbed.h" #include "MPU9250.h" #include "SEGGER_RTT.h" #include "SEGGER_RTT.c" #include "SEGGER_RTT_printf.c" float sum = 0, shift = 0; uint32_t sumCount = 0; int cycle = 0; char buffer[14]; MPU9250 mpu9250; Timer t; short toInt(float x) { return (x >= 0) ? (int)(x + 0.5) : (int)(x - 0.5); } int getTime(int counter) { return (int)((counter/60000.0f)+shift); } 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); t.start(); // Read the WHO_AM_I register, this is a good test of communication uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 if (whoami == 0x71) // WHO_AM_I should always be 0x68 { SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n"); wait(1); mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values SEGGER_RTT_printf(0, "x-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[0])); SEGGER_RTT_printf(0, "y-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[1])); SEGGER_RTT_printf(0, "z-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[2])); SEGGER_RTT_printf(0, "x-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[3])); SEGGER_RTT_printf(0, "y-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[4])); SEGGER_RTT_printf(0, "z-axis self test: gyration trim within : %d % of factory value\n\n", toInt(SelfTest[5])); mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers SEGGER_RTT_printf(0, "x gyro bias = %d\n", toInt(gyroBias[0])); SEGGER_RTT_printf(0, "y gyro bias = %d\n", toInt(gyroBias[1])); SEGGER_RTT_printf(0, "z gyro bias = %d\n", toInt(gyroBias[2])); SEGGER_RTT_printf(0, "x accel bias = %d\n", toInt(accelBias[0])); SEGGER_RTT_printf(0, "y accel bias = %d\n", toInt(accelBias[1])); SEGGER_RTT_printf(0, "z accel bias = %d\n\n", toInt(accelBias[2])); wait(2); mpu9250.initMPU9250(); SEGGER_RTT_WriteString(0, "MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature mpu9250.initAK8963(magCalibration); SEGGER_RTT_WriteString(0, "AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer SEGGER_RTT_printf(0, "Accelerometer full-scale range = %d g\n", toInt(2.0f*(float)(1<<Ascale))); SEGGER_RTT_printf(0, "Gyroscope full-scale range = %d deg/s\n", toInt(250.0f*(float)(1<<Gscale))); if(Mscale == 0) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 14 bits\n"); if(Mscale == 1) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 16 bits\n"); if(Mmode == 2) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 8 Hz\n"); if(Mmode == 6) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 100 Hz\n"); SEGGER_RTT_WriteString(0, "\n"); wait(1); } else { SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n", whoami); while(1); // Loop forever if communication doesn't happen } mpu9250.getAres(); // Get accelerometer sensitivity mpu9250.getGres(); // Get gyro sensitivity mpu9250.getMres(); // Get magnetometer sensitivity SEGGER_RTT_printf(0, "Accelerometer sensitivity is %d LSB/g \n", toInt(1.0f/aRes)); SEGGER_RTT_printf(0, "Gyroscope sensitivity is %d LSB/deg/s \n", toInt(1.0f/gRes)); SEGGER_RTT_printf(0, "Magnetometer sensitivity is %d LSB/G \n", toInt(1.0f/mRes)); 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 while(1) { // If intPin goes high, all data registers have new data if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) // On interrupt, check if data ready interrupt { mpu9250.readAccelData(accelCount); // Read the x/y/z adc values // Now we'll calculate the accleration value into actual g's ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set ay = (float)accelCount[1]*aRes - accelBias[1]; az = (float)accelCount[2]*aRes - accelBias[2]; mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values // Calculate the gyro value into actual degrees per second gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set gy = (float)gyroCount[1]*gRes - gyroBias[1]; gz = (float)gyroCount[2]*gRes - gyroBias[2]; mpu9250.readMagData(magCount); // Read the x/y/z adc values // Calculate the magnetometer values in milliGauss // 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]; } 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 } // 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); // 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 { SEGGER_RTT_printf(0, "\n\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)); SEGGER_RTT_printf(0, "gx = %d", toInt(gx)); SEGGER_RTT_printf(0, " gy = %d", toInt(gy)); SEGGER_RTT_printf(0, " gz = %d deg/s\n", toInt(gz)); SEGGER_RTT_printf(0, "mx = %d", toInt(mx)); SEGGER_RTT_printf(0, " my = %d", toInt(my)); SEGGER_RTT_printf(0, " mz = %d mG\n", toInt(mz)); tempCount = mpu9250.readTempData(); // Read the adc values temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Celsius SEGGER_RTT_printf(0, "Temperature = %d C\n\n", toInt(temperature)); //SEGGER_RTT_printf(0, "q0 = %d\n", toInt(q[0]*100)); //SEGGER_RTT_printf(0, "q1 = %d\n", toInt(q[1]*100)); //SEGGER_RTT_printf(0, "q2 = %d\n", toInt(q[2]*100)); //SEGGER_RTT_printf(0, "q3 = %d\n", toInt(q[3]*100)); // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. // In this coordinate system, the positive z-axis is down toward Earth. // 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. // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. // 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]); 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; yaw *= 180.0f / PI; 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)); count = t.read_ms(); SEGGER_RTT_printf(0, "Time active: %d minutes\n---------------------------------", getTime(count)); if(count > 1<<21) { t.start(); // start the timer over again if ~30 minutes has passed count = 0; deltat= 0; lastUpdate = t.read_us(); shift = (++cycle * 34.9525f); } sum = 0; sumCount = 0; } } }