Diff: main.cpp

Revision:

0:645f2b28239f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 17 08:42:47 2017 +0000
@@ -0,0 +1,83 @@
+/*CODED by Qiyong Mu on 21/06/2014
+kylongmu@msn.com
+*/
+#include "mbed.h"
+#include "MPU9250.h"        //Include library
+
+Serial pc(USBTX, USBRX);
+Serial xbee(p9,p10); 
+DigitalOut myled(LED1);
+//Serial pc(USBTX, USBRX);
+SPI spi(p5, p6, p7);
+mpu9250_spi imu(spi,p8);   //define the mpu9250 object
+int main(){
+    //float mx,my,mz,F;
+    //float ax,ay,az,acc_roll,acc_pitch,calib_az,roll_deg;
+    //float mag_x,mag_y,Orient;
+    //int i;
+    pc.baud(115200);
+    //xbee.baud(115200);
+    if(imu.init(1,BITS_DLPF_CFG_188HZ)){  //INIT the mpu9250
+        printf("\nCouldn't initialize MPU9250 via SPI!");
+    }    
+    printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
+    wait(1);    
+    printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
+    wait(1);  
+    printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
+    wait(1);
+    printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
+    wait(0.1);  
+    imu.AK8963_calib_Magnetometer();
+    imu.calib_acc();
+    pc.printf("good luck");
+    while(1) {
+     
+        wait(0.1);
+        
+        //imu.read_temp();
+        //imu.read_acc();
+        //imu.read_rot();
+        //imu.AK8963_read_Magnetometer();
+        
+        imu.read_all();
+    xbee.printf("%f,%f,%f\r\n", 
+            //imu.accelerometer_data[0],
+            //imu.accelerometer_data[1],
+            //imu.accelerometer_data[2],
+            imu.Magnetometer[0],
+            imu.Magnetometer[1],
+            imu.Magnetometer[2]
+            );
+           /* ax=ay=az=0;
+            for(i=0;i<100;i++){
+                ax = ax + imu.accelerometer_data[0];
+                ay = ay + imu.accelerometer_data[1];
+                az = az + imu.accelerometer_data[2];
+                }
+                wait(1.0);
+            ax=ax/100;
+            ay=ay/100;
+            az=az/100;
+           acc_roll = atan2(ay,az);
+           //roll_deg = acc_roll*180/3.1415;
+           calib_az = ay*sin(acc_roll)+az*cos(acc_roll);
+           acc_pitch = atan2(-ax,calib_az);
+           mag_x = (imu.Magnetometer[2]+13.755)*sin(acc_roll)-(imu.Magnetometer[1] - 20.469)*cos(acc_roll);
+           mag_y = (imu.Magnetometer[0] - 12.983)*cos(acc_pitch)
+                   +(imu.Magnetometer[1] - 20.469)*sin(acc_pitch)*sin(acc_roll)+(imu.Magnetometer[2]+13.755)*sin(acc_pitch)*cos(acc_roll);
+           Orient = atan2(mag_x,mag_y)*180/3.1415; 
+           printf("Orientation=%10.5f\n",Orient);*/       
+           //printf("roll=%10.5f,pitch=%10.5f\n",roll_deg,acc_pitch); 
+           
+          // mx = (imu.Magnetometer[0]- 12.983)*cos(7*3.1415/180);
+           //my = (imu.Magnetometer[1]- 20.469)*cos(7*3.1415/180);
+           //mz = imu.Magnetometer[2];
+         
+           // F = atan2(my,mx)*180/3.1415;
+          //printf("degree=%10.5f\n",F);
+        //myled = 0;*/
+        wait(0.5);
+    }
+}
+