Lee Sang Woon / Mbed OS CLEO_UART_MPU9250

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@0:bc5b57f59735, 2017-09-21 (annotated)

Committer:
Leesangwoon
Date:
Thu Sep 21 14:20:17 2017 +0000
Revision:
0:bc5b57f59735
hanback

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Leesangwoon 0:bc5b57f59735 1 #include "mbed.h"
Leesangwoon 0:bc5b57f59735 2 #include "MPU9250.h"
Leesangwoon 0:bc5b57f59735 3 #include "TextLCD.h"
Leesangwoon 0:bc5b57f59735 4
Leesangwoon 0:bc5b57f59735 5 struct UART_buf
Leesangwoon 0:bc5b57f59735 6 {
Leesangwoon 0:bc5b57f59735 7 uint8_t STA;
Leesangwoon 0:bc5b57f59735 8 uint8_t MODE;
Leesangwoon 0:bc5b57f59735 9 uint8_t CMD;
Leesangwoon 0:bc5b57f59735 10 uint8_t LEN;
Leesangwoon 0:bc5b57f59735 11 uint8_t DATA[32];
Leesangwoon 0:bc5b57f59735 12 uint8_t END;
Leesangwoon 0:bc5b57f59735 13
Leesangwoon 0:bc5b57f59735 14 };
Leesangwoon 0:bc5b57f59735 15
Leesangwoon 0:bc5b57f59735 16 union Data_DB{
Leesangwoon 0:bc5b57f59735 17 int16_t data16;
Leesangwoon 0:bc5b57f59735 18 uint8_t data8[2];
Leesangwoon 0:bc5b57f59735 19 }Data_Tr;
Leesangwoon 0:bc5b57f59735 20
Leesangwoon 0:bc5b57f59735 21 MPU9250 mpu9250;
Leesangwoon 0:bc5b57f59735 22
Leesangwoon 0:bc5b57f59735 23 Ticker Sensor_Timer;
Leesangwoon 0:bc5b57f59735 24
Leesangwoon 0:bc5b57f59735 25 Serial SerialUART(PA_2, PA_3); // tx, rx
Leesangwoon 0:bc5b57f59735 26
Leesangwoon 0:bc5b57f59735 27 // rs, rw, e, d0-d3
Leesangwoon 0:bc5b57f59735 28 TextLCD lcd(PB_12, PB_13, PB_14, PB_15, PA_9, PA_10, PA_11);
Leesangwoon 0:bc5b57f59735 29
Leesangwoon 0:bc5b57f59735 30 uint8_t Buffer[37];
Leesangwoon 0:bc5b57f59735 31 volatile uint8_t Sensor_flag = 0;
Leesangwoon 0:bc5b57f59735 32
Leesangwoon 0:bc5b57f59735 33 UART_buf RX_BUF;
Leesangwoon 0:bc5b57f59735 34
Leesangwoon 0:bc5b57f59735 35 void SerialUARTRX_ISR(void);
Leesangwoon 0:bc5b57f59735 36 void Timer_setting(uint8_t cmd, uint8_t value);
Leesangwoon 0:bc5b57f59735 37 void Sensor_Read(void);
Leesangwoon 0:bc5b57f59735 38
Leesangwoon 0:bc5b57f59735 39 int main()
Leesangwoon 0:bc5b57f59735 40 {
Leesangwoon 0:bc5b57f59735 41 SerialUART.baud(115200);
Leesangwoon 0:bc5b57f59735 42
Leesangwoon 0:bc5b57f59735 43 //Set up I2C
Leesangwoon 0:bc5b57f59735 44 i2c.frequency(400000); // use fast (400 kHz) I2C
Leesangwoon 0:bc5b57f59735 45
Leesangwoon 0:bc5b57f59735 46 // Read the WHO_AM_I register, this is a good test of communication
Leesangwoon 0:bc5b57f59735 47 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
Leesangwoon 0:bc5b57f59735 48 //SerialUART.printf("I AM 0x%x\n\r", whoami); SerialUART.printf("I SHOULD BE 0x71\n\r");
Leesangwoon 0:bc5b57f59735 49
Leesangwoon 0:bc5b57f59735 50 if (whoami == 0x71) // WHO_AM_I should always be 0x68
Leesangwoon 0:bc5b57f59735 51 {
Leesangwoon 0:bc5b57f59735 52 /*SerialUART.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
Leesangwoon 0:bc5b57f59735 53 SerialUART.printf("MPU9250 is online...\n\r");*/
Leesangwoon 0:bc5b57f59735 54 lcd.printf("MPU9250 is 0x%x\n",whoami);
Leesangwoon 0:bc5b57f59735 55 lcd.printf(" Connected ");
Leesangwoon 0:bc5b57f59735 56
Leesangwoon 0:bc5b57f59735 57 wait(1);
Leesangwoon 0:bc5b57f59735 58
Leesangwoon 0:bc5b57f59735 59 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
Leesangwoon 0:bc5b57f59735 60 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
Leesangwoon 0:bc5b57f59735 61 /*SerialUART.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
Leesangwoon 0:bc5b57f59735 62 SerialUART.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
Leesangwoon 0:bc5b57f59735 63 SerialUART.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
Leesangwoon 0:bc5b57f59735 64 SerialUART.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
Leesangwoon 0:bc5b57f59735 65 SerialUART.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
Leesangwoon 0:bc5b57f59735 66 SerialUART.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); */
Leesangwoon 0:bc5b57f59735 67 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
Leesangwoon 0:bc5b57f59735 68 /*SerialUART.printf("x gyro bias = %f\n\r", gyroBias[0]);
Leesangwoon 0:bc5b57f59735 69 SerialUART.printf("y gyro bias = %f\n\r", gyroBias[1]);
Leesangwoon 0:bc5b57f59735 70 SerialUART.printf("z gyro bias = %f\n\r", gyroBias[2]);
Leesangwoon 0:bc5b57f59735 71 SerialUART.printf("x accel bias = %f\n\r", accelBias[0]);
Leesangwoon 0:bc5b57f59735 72 SerialUART.printf("y accel bias = %f\n\r", accelBias[1]);
Leesangwoon 0:bc5b57f59735 73 SerialUART.printf("z accel bias = %f\n\r", accelBias[2]);*/
Leesangwoon 0:bc5b57f59735 74 wait(2);
Leesangwoon 0:bc5b57f59735 75 mpu9250.initMPU9250();
Leesangwoon 0:bc5b57f59735 76 //SerialUART.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
Leesangwoon 0:bc5b57f59735 77 mpu9250.initAK8963(magCalibration);
Leesangwoon 0:bc5b57f59735 78 /*SerialUART.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
Leesangwoon 0:bc5b57f59735 79 SerialUART.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
Leesangwoon 0:bc5b57f59735 80 pSerialUARTc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
Leesangwoon 0:bc5b57f59735 81 if(Mscale == 0) SerialUART.printf("Magnetometer resolution = 14 bits\n\r");
Leesangwoon 0:bc5b57f59735 82 if(Mscale == 1) SerialUART.printf("Magnetometer resolution = 16 bits\n\r");
Leesangwoon 0:bc5b57f59735 83 if(Mmode == 2) SerialUART.printf("Magnetometer ODR = 8 Hz\n\r");
Leesangwoon 0:bc5b57f59735 84 if(Mmode == 6) SerialUART.printf("Magnetometer ODR = 100 Hz\n\r");*/
Leesangwoon 0:bc5b57f59735 85 wait(1);
Leesangwoon 0:bc5b57f59735 86 }
Leesangwoon 0:bc5b57f59735 87 else
Leesangwoon 0:bc5b57f59735 88 {
Leesangwoon 0:bc5b57f59735 89 //SerialUART.printf("Could not connect to MPU9250: \n\r");
Leesangwoon 0:bc5b57f59735 90 //SerialUART.printf("%#x \n", whoami);
Leesangwoon 0:bc5b57f59735 91
Leesangwoon 0:bc5b57f59735 92 lcd.printf("MPU9250 is 0x%x\n",whoami);
Leesangwoon 0:bc5b57f59735 93 lcd.printf(" No connection ");
Leesangwoon 0:bc5b57f59735 94
Leesangwoon 0:bc5b57f59735 95 while(1) ; // Loop forever if communication doesn't happen
Leesangwoon 0:bc5b57f59735 96 }
Leesangwoon 0:bc5b57f59735 97
Leesangwoon 0:bc5b57f59735 98 mpu9250.getAres(); // Get accelerometer sensitivity
Leesangwoon 0:bc5b57f59735 99 mpu9250.getGres(); // Get gyro sensitivity
Leesangwoon 0:bc5b57f59735 100 mpu9250.getMres(); // Get magnetometer sensitivity
Leesangwoon 0:bc5b57f59735 101 /* pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
Leesangwoon 0:bc5b57f59735 102 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
Leesangwoon 0:bc5b57f59735 103 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);*/
Leesangwoon 0:bc5b57f59735 104 // magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
Leesangwoon 0:bc5b57f59735 105 // magbias[1] = +120.; // User environmental x-axis correction in milliGauss
Leesangwoon 0:bc5b57f59735 106 // magbias[2] = +125.; // User environmental x-axis correction in milliGauss
Leesangwoon 0:bc5b57f59735 107
Leesangwoon 0:bc5b57f59735 108 SerialUART.attach(&SerialUARTRX_ISR);
Leesangwoon 0:bc5b57f59735 109
Leesangwoon 0:bc5b57f59735 110 //Timer_setting(0x06, 200);
Leesangwoon 0:bc5b57f59735 111 Sensor_Timer.attach(&Sensor_Read, 0.05);
Leesangwoon 0:bc5b57f59735 112
Leesangwoon 0:bc5b57f59735 113 while(1)
Leesangwoon 0:bc5b57f59735 114 {
Leesangwoon 0:bc5b57f59735 115 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
Leesangwoon 0:bc5b57f59735 116
Leesangwoon 0:bc5b57f59735 117 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
Leesangwoon 0:bc5b57f59735 118 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
Leesangwoon 0:bc5b57f59735 119 mpu9250.readMagData(magCount); // Read the x/y/z adc values
Leesangwoon 0:bc5b57f59735 120 // Now we'll calculate the accleration value into actual g's
Leesangwoon 0:bc5b57f59735 121 if(Sensor_flag)
Leesangwoon 0:bc5b57f59735 122 {
Leesangwoon 0:bc5b57f59735 123 Sensor_flag = 0;
Leesangwoon 0:bc5b57f59735 124
Leesangwoon 0:bc5b57f59735 125 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
Leesangwoon 0:bc5b57f59735 126 ay = (float)accelCount[1]*aRes - accelBias[1];
Leesangwoon 0:bc5b57f59735 127 az = (float)accelCount[2]*aRes - accelBias[2];
Leesangwoon 0:bc5b57f59735 128
Leesangwoon 0:bc5b57f59735 129 // Calculate the gyro value into actual degrees per second
Leesangwoon 0:bc5b57f59735 130 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
Leesangwoon 0:bc5b57f59735 131 gy = (float)gyroCount[1]*gRes - gyroBias[1];
Leesangwoon 0:bc5b57f59735 132 gz = (float)gyroCount[2]*gRes - gyroBias[2];
Leesangwoon 0:bc5b57f59735 133
Leesangwoon 0:bc5b57f59735 134 // Calculate the magnetometer values in milliGauss
Leesangwoon 0:bc5b57f59735 135 // Include factory calibration per data sheet and user environmental corrections
Leesangwoon 0:bc5b57f59735 136 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
Leesangwoon 0:bc5b57f59735 137 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
Leesangwoon 0:bc5b57f59735 138 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
Leesangwoon 0:bc5b57f59735 139
Leesangwoon 0:bc5b57f59735 140 Buffer[0] = 0x76;
Leesangwoon 0:bc5b57f59735 141 Buffer[1] = 0x02;
Leesangwoon 0:bc5b57f59735 142 Buffer[2] = 0x01;
Leesangwoon 0:bc5b57f59735 143 Buffer[3] = 18;
Leesangwoon 0:bc5b57f59735 144 Data_Tr.data16 = (int16_t)gx;
Leesangwoon 0:bc5b57f59735 145 Buffer[4] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 146 Buffer[5] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 147 Data_Tr.data16 = (int16_t)gy;
Leesangwoon 0:bc5b57f59735 148 Buffer[6] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 149 Buffer[7] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 150 Data_Tr.data16 = (int16_t)gz;
Leesangwoon 0:bc5b57f59735 151 Buffer[8] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 152 Buffer[9] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 153 Data_Tr.data16 = (int16_t)(ax * 1000);
Leesangwoon 0:bc5b57f59735 154 Buffer[10] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 155 Buffer[11] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 156 Data_Tr.data16 = (int16_t)(ay * 1000);
Leesangwoon 0:bc5b57f59735 157 Buffer[12] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 158 Buffer[13] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 159 Data_Tr.data16 = (int16_t)(az * 1000);
Leesangwoon 0:bc5b57f59735 160 Buffer[14] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 161 Buffer[15] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 162 Data_Tr.data16 = (int16_t)mx;
Leesangwoon 0:bc5b57f59735 163 Buffer[16] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 164 Buffer[17] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 165 Data_Tr.data16 = (int16_t)my;
Leesangwoon 0:bc5b57f59735 166 Buffer[18] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 167 Buffer[19] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 168 Data_Tr.data16 = (int16_t)mz;
Leesangwoon 0:bc5b57f59735 169 Buffer[20] = Data_Tr.data8[1];
Leesangwoon 0:bc5b57f59735 170 Buffer[21] = Data_Tr.data8[0];
Leesangwoon 0:bc5b57f59735 171 Buffer[22] = 0x3E;
Leesangwoon 0:bc5b57f59735 172
Leesangwoon 0:bc5b57f59735 173 for(int i=0; i<23; i++)
Leesangwoon 0:bc5b57f59735 174 SerialUART.putc(Buffer[i]);
Leesangwoon 0:bc5b57f59735 175 }
Leesangwoon 0:bc5b57f59735 176 }
Leesangwoon 0:bc5b57f59735 177 }
Leesangwoon 0:bc5b57f59735 178 }
Leesangwoon 0:bc5b57f59735 179
Leesangwoon 0:bc5b57f59735 180 void SerialUARTRX_ISR(void)
Leesangwoon 0:bc5b57f59735 181 {
Leesangwoon 0:bc5b57f59735 182 static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
Leesangwoon 0:bc5b57f59735 183 uint8_t rx_da = SerialUART.getc();
Leesangwoon 0:bc5b57f59735 184 switch(RX_Status)
Leesangwoon 0:bc5b57f59735 185 {
Leesangwoon 0:bc5b57f59735 186 case 0:
Leesangwoon 0:bc5b57f59735 187 if(rx_da == 0x76)
Leesangwoon 0:bc5b57f59735 188 {
Leesangwoon 0:bc5b57f59735 189 RX_BUF.STA = rx_da;
Leesangwoon 0:bc5b57f59735 190 RX_Status++;
Leesangwoon 0:bc5b57f59735 191 }
Leesangwoon 0:bc5b57f59735 192 break;
Leesangwoon 0:bc5b57f59735 193 case 1:
Leesangwoon 0:bc5b57f59735 194 RX_BUF.MODE = rx_da;
Leesangwoon 0:bc5b57f59735 195 RX_Status++;
Leesangwoon 0:bc5b57f59735 196 break;
Leesangwoon 0:bc5b57f59735 197 case 2:
Leesangwoon 0:bc5b57f59735 198 RX_BUF.CMD = rx_da;
Leesangwoon 0:bc5b57f59735 199 RX_Status++;
Leesangwoon 0:bc5b57f59735 200 break;
Leesangwoon 0:bc5b57f59735 201 case 3:
Leesangwoon 0:bc5b57f59735 202 RX_BUF.LEN = rx_da;
Leesangwoon 0:bc5b57f59735 203 RX_Len = RX_BUF.LEN;
Leesangwoon 0:bc5b57f59735 204 RX_Status++;
Leesangwoon 0:bc5b57f59735 205 if(RX_Len == 0)
Leesangwoon 0:bc5b57f59735 206 RX_Status++;
Leesangwoon 0:bc5b57f59735 207 break;
Leesangwoon 0:bc5b57f59735 208 case 4:
Leesangwoon 0:bc5b57f59735 209 RX_BUF.DATA[RX_count] = rx_da;
Leesangwoon 0:bc5b57f59735 210 RX_count++;
Leesangwoon 0:bc5b57f59735 211 if(RX_count == RX_Len)
Leesangwoon 0:bc5b57f59735 212 {
Leesangwoon 0:bc5b57f59735 213 RX_Status++;
Leesangwoon 0:bc5b57f59735 214 RX_count = 0;
Leesangwoon 0:bc5b57f59735 215 RX_Len = 32;
Leesangwoon 0:bc5b57f59735 216 }
Leesangwoon 0:bc5b57f59735 217 break;
Leesangwoon 0:bc5b57f59735 218 case 5:
Leesangwoon 0:bc5b57f59735 219 if(rx_da == 0x3E)
Leesangwoon 0:bc5b57f59735 220 {
Leesangwoon 0:bc5b57f59735 221 RX_BUF.END = rx_da;
Leesangwoon 0:bc5b57f59735 222 RX_Status = 0;
Leesangwoon 0:bc5b57f59735 223 switch(RX_BUF.MODE)
Leesangwoon 0:bc5b57f59735 224 {
Leesangwoon 0:bc5b57f59735 225 case 0x04:
Leesangwoon 0:bc5b57f59735 226 Timer_setting(RX_BUF.CMD, RX_BUF.DATA[0]);
Leesangwoon 0:bc5b57f59735 227 break;
Leesangwoon 0:bc5b57f59735 228 }
Leesangwoon 0:bc5b57f59735 229 }
Leesangwoon 0:bc5b57f59735 230 break;
Leesangwoon 0:bc5b57f59735 231 }
Leesangwoon 0:bc5b57f59735 232 }
Leesangwoon 0:bc5b57f59735 233
Leesangwoon 0:bc5b57f59735 234 void Timer_setting(uint8_t cmd, uint8_t value)
Leesangwoon 0:bc5b57f59735 235 {
Leesangwoon 0:bc5b57f59735 236 double Time_value = 0;
Leesangwoon 0:bc5b57f59735 237 switch(cmd)
Leesangwoon 0:bc5b57f59735 238 {
Leesangwoon 0:bc5b57f59735 239 case 0x01:
Leesangwoon 0:bc5b57f59735 240 Time_value = 30;
Leesangwoon 0:bc5b57f59735 241 break;
Leesangwoon 0:bc5b57f59735 242 case 0x02:
Leesangwoon 0:bc5b57f59735 243 Time_value = 60;
Leesangwoon 0:bc5b57f59735 244 break;
Leesangwoon 0:bc5b57f59735 245 case 0x03:
Leesangwoon 0:bc5b57f59735 246 Time_value = 120;
Leesangwoon 0:bc5b57f59735 247 break;
Leesangwoon 0:bc5b57f59735 248 case 0x04:
Leesangwoon 0:bc5b57f59735 249 Time_value = 300;
Leesangwoon 0:bc5b57f59735 250 break;
Leesangwoon 0:bc5b57f59735 251 case 0x05:
Leesangwoon 0:bc5b57f59735 252 Time_value = 600;
Leesangwoon 0:bc5b57f59735 253 break;
Leesangwoon 0:bc5b57f59735 254 case 0x06:
Leesangwoon 0:bc5b57f59735 255 Time_value = value;
Leesangwoon 0:bc5b57f59735 256 Time_value = 1.0/Time_value;
Leesangwoon 0:bc5b57f59735 257 break;
Leesangwoon 0:bc5b57f59735 258 }
Leesangwoon 0:bc5b57f59735 259 Sensor_Timer.attach(&Sensor_Read, Time_value);
Leesangwoon 0:bc5b57f59735 260 }
Leesangwoon 0:bc5b57f59735 261
Leesangwoon 0:bc5b57f59735 262 void Sensor_Read(void)
Leesangwoon 0:bc5b57f59735 263 {
Leesangwoon 0:bc5b57f59735 264 Sensor_flag = 1;
Leesangwoon 0:bc5b57f59735 265 }