Diff: main.cpp

Revision:

1:3eec9883598a

Parent:

0:30a995e45e2a

Child:

2:02175845b24c

diff -r 30a995e45e2a -r 3eec9883598a main.cpp
--- a/main.cpp	Thu Oct 12 18:06:49 2017 +0000
+++ b/main.cpp	Thu Oct 12 18:47:26 2017 +0000
@@ -8,27 +8,57 @@
 MPU9250 mpu = MPU9250(p26, p27);
 
 // Configuration
-bool test_comms = true;
-bool do_sensor_init = false;
-bool do_sensor_self_test = true;
-bool print_accel = true;
-bool print_gyro = true;
+bool testMPUConnection = true;
+bool doSensorInitialization = false;
+bool printAccelerometer = true;
+bool printGyroscope = true;
 
 int main () {
-    float test_result[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
-    int16_t accel[3] = {0,0,0};
-    int16_t gyro[3] = {0,0,0};
-    int16_t temp = 0;
-    
-    // Test MPU connection
-    if (test_comms) {
+    int16_t accelerometer[3] = {0,0,0};
+    int16_t gyroscope[3] = {0,0,0};    
+        
+    if (testMPUConnection) {
         uint8_t whoami = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
         pc.printf("I AM 0x%x\n\r", whoami); 
         pc.printf("I SHOULD BE 0x71\n\r");
         wait(1);   
     }  
     
+    if (doSensorInitialization) {
+        // Initialise sensor board    
+        pc.printf("Initializing sensor\n\r");
+        mpu.initMPU9250();
+        pc.printf("Initialization finished\n\r");
+        wait(1);
+    }
+        
     while(1) {
-        pc.printf("Hi hi");                    
+        pc.printf("Starting to stream data");                    
+        if(printAccelerometer){
+            mpu.readAccelData(accelerometer);
+            float ax = accelerometer[0] * 2.0 / 32768.0;
+            float ay = accelerometer[1] * 2.0 / 32768.0;
+            float az = accelerometer[2] * 2.0 / 32768.0;
+            
+            pc.printf("Acelerometer information, AX, AY, AZ \n");
+            pc.printf("(%f, %f, %f)\n", ax,ay,az);
+            
+            float roll = float(atan2(ay, az) * 180/3.1416f);
+            float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f);
+            float yaw = atan(ax/-ay);
+            
+            pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw);
+        }
+        
+        if(printGyroscope){
+            mpu.readGyroData(gyroscope);
+            float gx = gyroscope[0] * 250.0 / 32768.0;
+            float gy = gyroscope[1] * 250.0 / 32768.0;
+            float gz = gyroscope[2] * 250.0 / 32768.0;
+            pc.printf("Gyroscope information, GX, GY, GZ \n");
+            pc.printf("(:%f, %f, %f)\n", gx,gy,gz);
+        }
+        
+        wait(0.3);
     } 
 }
\ No newline at end of file