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@0:ba048a293a0d, 2017-04-20 (annotated)

Committer:
ma123r
Date:
Thu Apr 20 14:59:27 2017 +0000
Revision:
0:ba048a293a0d
Child:
1:c80b6c45e943
removed the ms5611's object and its functionality

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ma123r 0:ba048a293a0d 1 /* MPU9250 Basic Example Code
ma123r 0:ba048a293a0d 2 by: Kris Winer
ma123r 0:ba048a293a0d 3 date: April 1, 2014
ma123r 0:ba048a293a0d 4 license: Beerware - Use this code however you'd like. If you
ma123r 0:ba048a293a0d 5 find it useful you can buy me a beer some time.
ma123r 0:ba048a293a0d 6
ma123r 0:ba048a293a0d 7 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
ma123r 0:ba048a293a0d 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
ma123r 0:ba048a293a0d 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
ma123r 0:ba048a293a0d 10 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
ma123r 0:ba048a293a0d 11
ma123r 0:ba048a293a0d 12 SDA and SCL should have external pull-up resistors (to 3.3V).
ma123r 0:ba048a293a0d 13 10k resistors are on the EMSENSR-9250 breakout board.
ma123r 0:ba048a293a0d 14
ma123r 0:ba048a293a0d 15 Hardware setup:
ma123r 0:ba048a293a0d 16 MPU9250 Breakout --------- Arduino
ma123r 0:ba048a293a0d 17 VDD ---------------------- 3.3V
ma123r 0:ba048a293a0d 18 VDDI --------------------- 3.3V
ma123r 0:ba048a293a0d 19 SDA ----------------------- A4
ma123r 0:ba048a293a0d 20 SCL ----------------------- A5
ma123r 0:ba048a293a0d 21 GND ---------------------- GND
ma123r 0:ba048a293a0d 22
ma123r 0:ba048a293a0d 23 Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
ma123r 0:ba048a293a0d 24 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
ma123r 0:ba048a293a0d 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
ma123r 0:ba048a293a0d 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
ma123r 0:ba048a293a0d 27 */
ma123r 0:ba048a293a0d 28
ma123r 0:ba048a293a0d 29 //#include "ST_F401_84MHZ.h"
ma123r 0:ba048a293a0d 30 //F401_init84 myinit(0);
ma123r 0:ba048a293a0d 31 #include "mbed.h"
ma123r 0:ba048a293a0d 32 #include "MPU9250.h"
ma123r 0:ba048a293a0d 33 #include "MS5611I2C.h"
ma123r 0:ba048a293a0d 34
ma123r 0:ba048a293a0d 35 float sum = 0;
ma123r 0:ba048a293a0d 36 uint32_t sumCount = 0;
ma123r 0:ba048a293a0d 37 char buffer[14];
ma123r 0:ba048a293a0d 38
ma123r 0:ba048a293a0d 39 MPU9250 mpu9250;
ma123r 0:ba048a293a0d 40
ma123r 0:ba048a293a0d 41 //MS5611I2C ms5611(I2C_SDA, I2C_SCL, false);
ma123r 0:ba048a293a0d 42
ma123r 0:ba048a293a0d 43 Timer t;
ma123r 0:ba048a293a0d 44
ma123r 0:ba048a293a0d 45 Serial pc(USBTX, USBRX); // tx, rx
ma123r 0:ba048a293a0d 46
ma123r 0:ba048a293a0d 47
ma123r 0:ba048a293a0d 48 /* MAIN */
ma123r 0:ba048a293a0d 49 int main()
ma123r 0:ba048a293a0d 50 {
ma123r 0:ba048a293a0d 51 wait(3);
ma123r 0:ba048a293a0d 52
ma123r 0:ba048a293a0d 53
ma123r 0:ba048a293a0d 54 pc.baud(115200);
ma123r 0:ba048a293a0d 55
ma123r 0:ba048a293a0d 56 //Set up I2C
ma123r 0:ba048a293a0d 57 i2c.frequency(400000); // use fast (400 kHz) I2C
ma123r 0:ba048a293a0d 58
ma123r 0:ba048a293a0d 59
ma123r 0:ba048a293a0d 60 //pc.printf("CPU SystemCoreClock is %d Hz\n", SystemCoreClock);
ma123r 0:ba048a293a0d 61
ma123r 0:ba048a293a0d 62
ma123r 0:ba048a293a0d 63 //Print the Coefficients from the
ma123r 0:ba048a293a0d 64 //ms5611.printCoefficients();
ma123r 0:ba048a293a0d 65
ma123r 0:ba048a293a0d 66 t.start();
ma123r 0:ba048a293a0d 67
ma123r 0:ba048a293a0d 68
ma123r 0:ba048a293a0d 69 // Read the WHO_AM_I register, this is a good test of communication
ma123r 0:ba048a293a0d 70 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
ma123r 0:ba048a293a0d 71 //pc.printf("I AM 0x%x\n", whoami); //pc.printf("I SHOULD BE 0x71\n");
ma123r 0:ba048a293a0d 72
ma123r 0:ba048a293a0d 73 if (whoami == 0x71) // WHO_AM_I should always be 0x68
ma123r 0:ba048a293a0d 74 {
ma123r 0:ba048a293a0d 75 wait(1);
ma123r 0:ba048a293a0d 76
ma123r 0:ba048a293a0d 77 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
ma123r 0:ba048a293a0d 78 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
ma123r 0:ba048a293a0d 79 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
ma123r 0:ba048a293a0d 80
ma123r 0:ba048a293a0d 81 wait(2);
ma123r 0:ba048a293a0d 82 mpu9250.initMPU9250();
ma123r 0:ba048a293a0d 83 //pc.printf("MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
ma123r 0:ba048a293a0d 84 mpu9250.initAK8963(magCalibration);
ma123r 0:ba048a293a0d 85 //pc.printf("AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer
ma123r 0:ba048a293a0d 86 //pc.printf("Accelerometer full-scale range = %f g\n", 2.0f*(float)(1<<Ascale));
ma123r 0:ba048a293a0d 87 //pc.printf("Gyroscope full-scale range = %f deg/s\n", 250.0f*(float)(1<<Gscale));
ma123r 0:ba048a293a0d 88 if(Mscale == 0) //pc.printf("Magnetometer resolution = 14 bits\n");
ma123r 0:ba048a293a0d 89 if(Mscale == 1) //pc.printf("Magnetometer resolution = 16 bits\n");
ma123r 0:ba048a293a0d 90 if(Mmode == 2) //pc.printf("Magnetometer ODR = 8 Hz\n");
ma123r 0:ba048a293a0d 91 if(Mmode == 6) //pc.printf("Magnetometer ODR = 100 Hz\n");
ma123r 0:ba048a293a0d 92 wait(1);
ma123r 0:ba048a293a0d 93 }
ma123r 0:ba048a293a0d 94 else
ma123r 0:ba048a293a0d 95 {
ma123r 0:ba048a293a0d 96 //pc.printf("Could not connect to MPU9250: \n");
ma123r 0:ba048a293a0d 97 //pc.printf("%#x \n", whoami);
ma123r 0:ba048a293a0d 98
ma123r 0:ba048a293a0d 99 while(1) ; // Loop forever if communication doesn't happen
ma123r 0:ba048a293a0d 100 }
ma123r 0:ba048a293a0d 101
ma123r 0:ba048a293a0d 102 mpu9250.getAres(); // Get accelerometer sensitivity
ma123r 0:ba048a293a0d 103 mpu9250.getGres(); // Get gyro sensitivity
ma123r 0:ba048a293a0d 104 mpu9250.getMres(); // Get magnetometer sensitivity
ma123r 0:ba048a293a0d 105 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
ma123r 0:ba048a293a0d 106 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
ma123r 0:ba048a293a0d 107 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
ma123r 0:ba048a293a0d 108
ma123r 0:ba048a293a0d 109
ma123r 0:ba048a293a0d 110
ma123r 0:ba048a293a0d 111 // main loop
ma123r 0:ba048a293a0d 112 while(1) {
ma123r 0:ba048a293a0d 113 // If intPin goes high, all data registers have new data
ma123r 0:ba048a293a0d 114 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
ma123r 0:ba048a293a0d 115
ma123r 0:ba048a293a0d 116 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
ma123r 0:ba048a293a0d 117 // Now we'll calculate the accleration value into actual g's
ma123r 0:ba048a293a0d 118 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ma123r 0:ba048a293a0d 119 ay = (float)accelCount[1]*aRes - accelBias[1];
ma123r 0:ba048a293a0d 120 az = (float)accelCount[2]*aRes - accelBias[2];
ma123r 0:ba048a293a0d 121
ma123r 0:ba048a293a0d 122 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
ma123r 0:ba048a293a0d 123 // Calculate the gyro value into actual degrees per second
ma123r 0:ba048a293a0d 124 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
ma123r 0:ba048a293a0d 125 gy = (float)gyroCount[1]*gRes - gyroBias[1];
ma123r 0:ba048a293a0d 126 gz = (float)gyroCount[2]*gRes - gyroBias[2];
ma123r 0:ba048a293a0d 127
ma123r 0:ba048a293a0d 128 mpu9250.readMagData(magCount); // Read the x/y/z adc values
ma123r 0:ba048a293a0d 129 // Calculate the magnetometer values in milliGauss
ma123r 0:ba048a293a0d 130 // Include factory calibration per data sheet and user environmental corrections
ma123r 0:ba048a293a0d 131 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
ma123r 0:ba048a293a0d 132 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
ma123r 0:ba048a293a0d 133 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
ma123r 0:ba048a293a0d 134 }
ma123r 0:ba048a293a0d 135
ma123r 0:ba048a293a0d 136 Now = t.read_us();
ma123r 0:ba048a293a0d 137 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
ma123r 0:ba048a293a0d 138 lastUpdate = Now;
ma123r 0:ba048a293a0d 139
ma123r 0:ba048a293a0d 140 sum += deltat;
ma123r 0:ba048a293a0d 141 sumCount++;
ma123r 0:ba048a293a0d 142
ma123r 0:ba048a293a0d 143
ma123r 0:ba048a293a0d 144 // Pass gyro rate as rad/s
ma123r 0:ba048a293a0d 145 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ma123r 0:ba048a293a0d 146 //mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ma123r 0:ba048a293a0d 147
ma123r 0:ba048a293a0d 148 delt_t = t.read_ms() - count;
ma123r 0:ba048a293a0d 149
ma123r 0:ba048a293a0d 150 if (delt_t > 20) {
ma123r 0:ba048a293a0d 151 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 0:ba048a293a0d 152 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
ma123r 0:ba048a293a0d 153 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 0:ba048a293a0d 154 pitch *= 180.0f / PI;
ma123r 0:ba048a293a0d 155 yaw *= 180.0f / PI;
ma123r 0:ba048a293a0d 156 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
ma123r 0:ba048a293a0d 157 yaw -= 3.8f; // Declination in Cork, Ireland in Oct 2016
ma123r 0:ba048a293a0d 158 roll *= 180.0f / PI;
ma123r 0:ba048a293a0d 159
ma123r 0:ba048a293a0d 160 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n", yaw, pitch, roll);
ma123r 0:ba048a293a0d 161 //pc.printf("%f\t%f\t%f\n", yaw, pitch, roll);
ma123r 0:ba048a293a0d 162 //pc.printf("average rate = %f\n", (float) sumCount/sum);
ma123r 0:ba048a293a0d 163
ma123r 0:ba048a293a0d 164 //pc.printf("%.3f\n", (36.0 + ms5611.getAltitude()));
ma123r 0:ba048a293a0d 165
ma123r 0:ba048a293a0d 166 pc.printf("%.2f %.2f %.2f %.2f %.2f %.2f\n",
ma123r 0:ba048a293a0d 167 yaw,
ma123r 0:ba048a293a0d 168 pitch,
ma123r 0:ba048a293a0d 169 roll,
ma123r 0:ba048a293a0d 170 ax,
ma123r 0:ba048a293a0d 171 ay,
ma123r 0:ba048a293a0d 172 az,
ma123r 0:ba048a293a0d 173 //ms5611.getTemperature(),
ma123r 0:ba048a293a0d 174 //5611.getAltitude()
ma123r 0:ba048a293a0d 175 );
ma123r 0:ba048a293a0d 176
ma123r 0:ba048a293a0d 177
ma123r 0:ba048a293a0d 178 myled= !myled;
ma123r 0:ba048a293a0d 179 wait(0.01);
ma123r 0:ba048a293a0d 180 count = t.read_ms();
ma123r 0:ba048a293a0d 181
ma123r 0:ba048a293a0d 182 if(count > 1<<21) {
ma123r 0:ba048a293a0d 183 t.start(); // start the timer over again if ~30 minutes has passed
ma123r 0:ba048a293a0d 184 count = 0;
ma123r 0:ba048a293a0d 185 deltat= 0;
ma123r 0:ba048a293a0d 186 lastUpdate = t.read_us();
ma123r 0:ba048a293a0d 187 }
ma123r 0:ba048a293a0d 188 sum = 0;
ma123r 0:ba048a293a0d 189 sumCount = 0;
ma123r 0:ba048a293a0d 190 }
ma123r 0:ba048a293a0d 191 }
ma123r 0:ba048a293a0d 192
ma123r 0:ba048a293a0d 193 }