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
- Committer:
- Leesangwoon
- Date:
- 2017-09-21
- Revision:
- 0:bc5b57f59735
File content as of revision 0:bc5b57f59735:
#include "mbed.h" #include "MPU9250.h" #include "TextLCD.h" struct UART_buf { uint8_t STA; uint8_t MODE; uint8_t CMD; uint8_t LEN; uint8_t DATA[32]; uint8_t END; }; union Data_DB{ int16_t data16; uint8_t data8[2]; }Data_Tr; MPU9250 mpu9250; Ticker Sensor_Timer; Serial SerialUART(PA_2, PA_3); // tx, rx // rs, rw, e, d0-d3 TextLCD lcd(PB_12, PB_13, PB_14, PB_15, PA_9, PA_10, PA_11); uint8_t Buffer[37]; volatile uint8_t Sensor_flag = 0; UART_buf RX_BUF; void SerialUARTRX_ISR(void); void Timer_setting(uint8_t cmd, uint8_t value); void Sensor_Read(void); int main() { SerialUART.baud(115200); //Set up I2C i2c.frequency(400000); // use fast (400 kHz) I2C // 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 //SerialUART.printf("I AM 0x%x\n\r", whoami); SerialUART.printf("I SHOULD BE 0x71\n\r"); if (whoami == 0x71) // WHO_AM_I should always be 0x68 { /*SerialUART.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami); SerialUART.printf("MPU9250 is online...\n\r");*/ lcd.printf("MPU9250 is 0x%x\n",whoami); lcd.printf(" Connected "); wait(1); mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values /*SerialUART.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]); SerialUART.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]); SerialUART.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]); SerialUART.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]); SerialUART.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]); SerialUART.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); */ mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers /*SerialUART.printf("x gyro bias = %f\n\r", gyroBias[0]); SerialUART.printf("y gyro bias = %f\n\r", gyroBias[1]); SerialUART.printf("z gyro bias = %f\n\r", gyroBias[2]); SerialUART.printf("x accel bias = %f\n\r", accelBias[0]); SerialUART.printf("y accel bias = %f\n\r", accelBias[1]); SerialUART.printf("z accel bias = %f\n\r", accelBias[2]);*/ wait(2); mpu9250.initMPU9250(); //SerialUART.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature mpu9250.initAK8963(magCalibration); /*SerialUART.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer SerialUART.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); pSerialUARTc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); if(Mscale == 0) SerialUART.printf("Magnetometer resolution = 14 bits\n\r"); if(Mscale == 1) SerialUART.printf("Magnetometer resolution = 16 bits\n\r"); if(Mmode == 2) SerialUART.printf("Magnetometer ODR = 8 Hz\n\r"); if(Mmode == 6) SerialUART.printf("Magnetometer ODR = 100 Hz\n\r");*/ wait(1); } else { //SerialUART.printf("Could not connect to MPU9250: \n\r"); //SerialUART.printf("%#x \n", whoami); lcd.printf("MPU9250 is 0x%x\n",whoami); lcd.printf(" No connection "); while(1) ; // Loop forever if communication doesn't happen } mpu9250.getAres(); // Get accelerometer sensitivity mpu9250.getGres(); // Get gyro sensitivity mpu9250.getMres(); // Get magnetometer sensitivity /* pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 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 SerialUART.attach(&SerialUARTRX_ISR); //Timer_setting(0x06, 200); Sensor_Timer.attach(&Sensor_Read, 0.05); while(1) { 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 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values mpu9250.readMagData(magCount); // Read the x/y/z adc values // Now we'll calculate the accleration value into actual g's if(Sensor_flag) { Sensor_flag = 0; 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]; // 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]; // 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]; Buffer[0] = 0x76; Buffer[1] = 0x02; Buffer[2] = 0x01; Buffer[3] = 18; Data_Tr.data16 = (int16_t)gx; Buffer[4] = Data_Tr.data8[1]; Buffer[5] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)gy; Buffer[6] = Data_Tr.data8[1]; Buffer[7] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)gz; Buffer[8] = Data_Tr.data8[1]; Buffer[9] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)(ax * 1000); Buffer[10] = Data_Tr.data8[1]; Buffer[11] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)(ay * 1000); Buffer[12] = Data_Tr.data8[1]; Buffer[13] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)(az * 1000); Buffer[14] = Data_Tr.data8[1]; Buffer[15] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)mx; Buffer[16] = Data_Tr.data8[1]; Buffer[17] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)my; Buffer[18] = Data_Tr.data8[1]; Buffer[19] = Data_Tr.data8[0]; Data_Tr.data16 = (int16_t)mz; Buffer[20] = Data_Tr.data8[1]; Buffer[21] = Data_Tr.data8[0]; Buffer[22] = 0x3E; for(int i=0; i<23; i++) SerialUART.putc(Buffer[i]); } } } } void SerialUARTRX_ISR(void) { static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0; uint8_t rx_da = SerialUART.getc(); switch(RX_Status) { case 0: if(rx_da == 0x76) { RX_BUF.STA = rx_da; RX_Status++; } break; case 1: RX_BUF.MODE = rx_da; RX_Status++; break; case 2: RX_BUF.CMD = rx_da; RX_Status++; break; case 3: RX_BUF.LEN = rx_da; RX_Len = RX_BUF.LEN; RX_Status++; if(RX_Len == 0) RX_Status++; break; case 4: RX_BUF.DATA[RX_count] = rx_da; RX_count++; if(RX_count == RX_Len) { RX_Status++; RX_count = 0; RX_Len = 32; } break; case 5: if(rx_da == 0x3E) { RX_BUF.END = rx_da; RX_Status = 0; switch(RX_BUF.MODE) { case 0x04: Timer_setting(RX_BUF.CMD, RX_BUF.DATA[0]); break; } } break; } } void Timer_setting(uint8_t cmd, uint8_t value) { double Time_value = 0; switch(cmd) { case 0x01: Time_value = 30; break; case 0x02: Time_value = 60; break; case 0x03: Time_value = 120; break; case 0x04: Time_value = 300; break; case 0x05: Time_value = 600; break; case 0x06: Time_value = value; Time_value = 1.0/Time_value; break; } Sensor_Timer.attach(&Sensor_Read, Time_value); } void Sensor_Read(void) { Sensor_flag = 1; }