han back / Mbed OS CLEO_UART_ACCEL

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:f96b9b35ac4c, 2017-09-28 (annotated)

Committer:
SMART_CLEO
Date:
Thu Sep 28 03:29:42 2017 +0000
Revision:
0:f96b9b35ac4c
SMART_CLEO

Who changed what in which revision?

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