Mariusz Wilk / Mbed 2 deprecated HC-06_MPU9250_F303K_Stream

This program streams sensor readings from the MPU950 sensor via HC-06 Bluetooth Module. It uses the Nucleo-32 board (STM32F303K8). It's a messy program but it works.

Dependencies:   MS5611 mbed

main.cpp@3:2e25f16c9fc3, 2017-08-10 (annotated)

Committer:
ma123r
Date:
Thu Aug 10 11:13:37 2017 +0000
Revision:
3:2e25f16c9fc3
Parent:
2:3946867c4748 
Just Testing Something

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ma123r 0:ba048a293a0d 1 #include "mbed.h"
ma123r 0:ba048a293a0d 2 #include "MPU9250.h"
ma123r 0:ba048a293a0d 3 #include "MS5611I2C.h"
ma123r 0:ba048a293a0d 4
ma123r 0:ba048a293a0d 5 float sum = 0;
ma123r 0:ba048a293a0d 6 uint32_t sumCount = 0;
ma123r 0:ba048a293a0d 7 char buffer[14];
ma123r 3:2e25f16c9fc3 8 const uint16_t dly_ms = 33; // delay for o/p computing and printing
ma123r 0:ba048a293a0d 9
ma123r 0:ba048a293a0d 10 MPU9250 mpu9250;
ma123r 0:ba048a293a0d 11
ma123r 0:ba048a293a0d 12 //MS5611I2C ms5611(I2C_SDA, I2C_SCL, false);
ma123r 0:ba048a293a0d 13
ma123r 3:2e25f16c9fc3 14 Timer t;
ma123r 0:ba048a293a0d 15 Serial pc(USBTX, USBRX); // tx, rx
ma123r 3:2e25f16c9fc3 16
ma123r 3:2e25f16c9fc3 17 //HC-05
ma123r 3:2e25f16c9fc3 18 Serial bt(D1,D0);
ma123r 0:ba048a293a0d 19
ma123r 0:ba048a293a0d 20 /* MAIN */
ma123r 0:ba048a293a0d 21 int main()
ma123r 3:2e25f16c9fc3 22 {
ma123r 3:2e25f16c9fc3 23 //HC-05
ma123r 3:2e25f16c9fc3 24 bt.baud(9600);
ma123r 3:2e25f16c9fc3 25 //prints data on mobile
ma123r 3:2e25f16c9fc3 26 //bt.printf("Connection Established\r\n");
ma123r 3:2e25f16c9fc3 27 //print data on pc terminal
ma123r 3:2e25f16c9fc3 28 pc.printf("Connection Established\r\n");
ma123r 0:ba048a293a0d 29
ma123r 2:3946867c4748 30 //pc.baud(115200);
ma123r 3:2e25f16c9fc3 31 //pc.baud(256000);
ma123r 0:ba048a293a0d 32
ma123r 0:ba048a293a0d 33 //Set up I2C
ma123r 0:ba048a293a0d 34 i2c.frequency(400000); // use fast (400 kHz) I2C
ma123r 3:2e25f16c9fc3 35
ma123r 0:ba048a293a0d 36 //pc.printf("CPU SystemCoreClock is %d Hz\n", SystemCoreClock);
ma123r 3:2e25f16c9fc3 37
ma123r 0:ba048a293a0d 38 //Print the Coefficients from the
ma123r 0:ba048a293a0d 39 //ms5611.printCoefficients();
ma123r 0:ba048a293a0d 40
ma123r 0:ba048a293a0d 41 t.start();
ma123r 3:2e25f16c9fc3 42
ma123r 0:ba048a293a0d 43 // Read the WHO_AM_I register, this is a good test of communication
ma123r 0:ba048a293a0d 44 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
ma123r 0:ba048a293a0d 45 //pc.printf("I AM 0x%x\n", whoami); //pc.printf("I SHOULD BE 0x71\n");
ma123r 0:ba048a293a0d 46
ma123r 0:ba048a293a0d 47 if (whoami == 0x71) // WHO_AM_I should always be 0x68
ma123r 0:ba048a293a0d 48 {
ma123r 0:ba048a293a0d 49 wait(1);
ma123r 0:ba048a293a0d 50
ma123r 0:ba048a293a0d 51 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
ma123r 0:ba048a293a0d 52 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
ma123r 0:ba048a293a0d 53 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
ma123r 0:ba048a293a0d 54
ma123r 0:ba048a293a0d 55 wait(2);
ma123r 0:ba048a293a0d 56 mpu9250.initMPU9250();
ma123r 0:ba048a293a0d 57 //pc.printf("MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
ma123r 0:ba048a293a0d 58 mpu9250.initAK8963(magCalibration);
ma123r 0:ba048a293a0d 59 //pc.printf("AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer
ma123r 0:ba048a293a0d 60 //pc.printf("Accelerometer full-scale range = %f g\n", 2.0f*(float)(1<<Ascale));
ma123r 0:ba048a293a0d 61 //pc.printf("Gyroscope full-scale range = %f deg/s\n", 250.0f*(float)(1<<Gscale));
ma123r 0:ba048a293a0d 62 if(Mscale == 0) //pc.printf("Magnetometer resolution = 14 bits\n");
ma123r 0:ba048a293a0d 63 if(Mscale == 1) //pc.printf("Magnetometer resolution = 16 bits\n");
ma123r 0:ba048a293a0d 64 if(Mmode == 2) //pc.printf("Magnetometer ODR = 8 Hz\n");
ma123r 0:ba048a293a0d 65 if(Mmode == 6) //pc.printf("Magnetometer ODR = 100 Hz\n");
ma123r 0:ba048a293a0d 66 wait(1);
ma123r 0:ba048a293a0d 67 }
ma123r 0:ba048a293a0d 68 else
ma123r 0:ba048a293a0d 69 {
ma123r 3:2e25f16c9fc3 70 pc.printf("Could not connect to MPU9250: \n");
ma123r 3:2e25f16c9fc3 71 pc.printf("%#x \n", whoami);
ma123r 0:ba048a293a0d 72 while(1) ; // Loop forever if communication doesn't happen
ma123r 0:ba048a293a0d 73 }
ma123r 0:ba048a293a0d 74
ma123r 0:ba048a293a0d 75 mpu9250.getAres(); // Get accelerometer sensitivity
ma123r 0:ba048a293a0d 76 mpu9250.getGres(); // Get gyro sensitivity
ma123r 0:ba048a293a0d 77 mpu9250.getMres(); // Get magnetometer sensitivity
ma123r 0:ba048a293a0d 78 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
ma123r 0:ba048a293a0d 79 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
ma123r 0:ba048a293a0d 80 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
ma123r 0:ba048a293a0d 81
ma123r 0:ba048a293a0d 82
ma123r 0:ba048a293a0d 83 // main loop
ma123r 0:ba048a293a0d 84 while(1) {
ma123r 0:ba048a293a0d 85 // If intPin goes high, all data registers have new data
ma123r 0:ba048a293a0d 86 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
ma123r 0:ba048a293a0d 87 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
ma123r 0:ba048a293a0d 88 // Now we'll calculate the accleration value into actual g's
ma123r 0:ba048a293a0d 89 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ma123r 0:ba048a293a0d 90 ay = (float)accelCount[1]*aRes - accelBias[1];
ma123r 0:ba048a293a0d 91 az = (float)accelCount[2]*aRes - accelBias[2];
ma123r 0:ba048a293a0d 92
ma123r 0:ba048a293a0d 93 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
ma123r 0:ba048a293a0d 94 // Calculate the gyro value into actual degrees per second
ma123r 0:ba048a293a0d 95 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
ma123r 0:ba048a293a0d 96 gy = (float)gyroCount[1]*gRes - gyroBias[1];
ma123r 0:ba048a293a0d 97 gz = (float)gyroCount[2]*gRes - gyroBias[2];
ma123r 0:ba048a293a0d 98
ma123r 0:ba048a293a0d 99 mpu9250.readMagData(magCount); // Read the x/y/z adc values
ma123r 0:ba048a293a0d 100 // Calculate the magnetometer values in milliGauss
ma123r 0:ba048a293a0d 101 // Include factory calibration per data sheet and user environmental corrections
ma123r 0:ba048a293a0d 102 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
ma123r 0:ba048a293a0d 103 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
ma123r 0:ba048a293a0d 104 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
ma123r 0:ba048a293a0d 105 }
ma123r 3:2e25f16c9fc3 106
ma123r 0:ba048a293a0d 107 Now = t.read_us();
ma123r 0:ba048a293a0d 108 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
ma123r 0:ba048a293a0d 109 lastUpdate = Now;
ma123r 0:ba048a293a0d 110
ma123r 0:ba048a293a0d 111 sum += deltat;
ma123r 0:ba048a293a0d 112 sumCount++;
ma123r 0:ba048a293a0d 113
ma123r 0:ba048a293a0d 114 // Pass gyro rate as rad/s
ma123r 2:3946867c4748 115 //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ma123r 2:3946867c4748 116 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ma123r 0:ba048a293a0d 117
ma123r 0:ba048a293a0d 118 delt_t = t.read_ms() - count;
ma123r 0:ba048a293a0d 119
ma123r 2:3946867c4748 120 if (delt_t > dly_ms) {
ma123r 2:3946867c4748 121 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]);
ma123r 2:3946867c4748 122 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
ma123r 2:3946867c4748 123 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]);
ma123r 2:3946867c4748 124 pitch *= 180.0f / PI;
ma123r 2:3946867c4748 125 yaw *= 180.0f / PI;
ma123r 2:3946867c4748 126 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
ma123r 2:3946867c4748 127 yaw -= 3.8f; // Declination in Cork, Ireland in Oct 2016
ma123r 2:3946867c4748 128 roll *= 180.0f / PI;
ma123r 0:ba048a293a0d 129
ma123r 3:2e25f16c9fc3 130 pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",
ma123r 2:3946867c4748 131 yaw,
ma123r 2:3946867c4748 132 pitch,
ma123r 3:2e25f16c9fc3 133 roll,
ma123r 2:3946867c4748 134 ax,
ma123r 2:3946867c4748 135 ay,
ma123r 3:2e25f16c9fc3 136 az
ma123r 2:3946867c4748 137 );
ma123r 3:2e25f16c9fc3 138
ma123r 3:2e25f16c9fc3 139 // Send it to HC-06
ma123r 3:2e25f16c9fc3 140 bt.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
ma123r 3:2e25f16c9fc3 141 yaw,
ma123r 3:2e25f16c9fc3 142 pitch,
ma123r 3:2e25f16c9fc3 143 roll,
ma123r 3:2e25f16c9fc3 144 ax,
ma123r 3:2e25f16c9fc3 145 ay,
ma123r 3:2e25f16c9fc3 146 az
ma123r 3:2e25f16c9fc3 147 );
ma123r 3:2e25f16c9fc3 148
ma123r 2:3946867c4748 149 myled= !myled;
ma123r 2:3946867c4748 150 count = t.read_ms();
ma123r 0:ba048a293a0d 151
ma123r 2:3946867c4748 152 if(count > 1<<21) {
ma123r 2:3946867c4748 153 t.start(); // start the timer over again if ~30 minutes has passed
ma123r 2:3946867c4748 154 count = 0;
ma123r 2:3946867c4748 155 deltat= 0;
ma123r 2:3946867c4748 156 lastUpdate = t.read_us();
ma123r 2:3946867c4748 157 }
ma123r 2:3946867c4748 158 sum = 0;
ma123r 2:3946867c4748 159 sumCount = 0;
ma123r 0:ba048a293a0d 160 }
ma123r 2:3946867c4748 161 }
ma123r 2:3946867c4748 162 }