Diff: main.cpp

Revision:

5:b7855a02ddf6

Parent:

3:64a8188c5a44

--- a/main.cpp	Mon Mar 02 10:08:57 2020 +0000
+++ b/main.cpp	Thu Jul 02 08:25:06 2020 +0000
@@ -2,11 +2,16 @@
 
 //DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
-#define PI 3,14 
+#define PI 3.14 
 #define printff_CALCULATED
 #define printff_SPEED 250 
 #define DECLINATION -8.58
 
+
+#define dt 1/975  
+#define gey_sensitivity 8.75 
+
+
 void printfGyro();
 void printfAccel();
 void printfMag();
@@ -22,6 +27,7 @@
         while(1) ; 
     }
     lol.calibrate();
+    lol.calibrateMag() ;
     while(1) {
         
         //lol.readTemp();
@@ -45,11 +51,11 @@
         pc.printf("\n") ;
         printfMag();   // printfff "M: mx, my, mz"
         pc.printf("\n") ;
-        // printff the heading and orientation for fun!
+        // printff the heading and orientation fofun!
         // Call printfff attitude. The LSM9DS1's mag x and y
         // axes are opposite to the accelerometer, so my, mx are
         // substituted for each other.
-        printfAttitude(lol.ax, lol.ay, lol.az,-lol.my, -lol.mx, lol.mz);
+        printfAttitude(lol.ax, lol.ay, lol.az,lol.gx, lol.gy, lol.gz);
         pc.printf("\n") ;
         wait(1);
    } 
@@ -131,44 +137,42 @@
     #endif
 }
 
-void printfAttitude(float ax, float ay, float az, float mx, float my, float mz)
+void printfAttitude(float ax, float ay, float az, float gx, float gy, float gz)
 {
-  float roll = atan2(ay, az);
-  float pitch = atan2(-ax, sqrt(ay * ay + az * az));
+  //double  roll = atan2(ay, az);
+  //double  pitch = atan2(-ax, sqrt(ay * ay + az * az));
+    
+    
+    double *pitch , *roll ;
+    
+     *pitch +=((float)gx /gey_sensitivity)*dt ; 
+     *roll  -= ((float)gy /gey_sensitivity)*dt ; 
 
-  float heading;
+    double pitchacc = atan2(ay,az)*180/PI ;
+    *pitch = *pitch *0,98+pitchacc*0.02 ; 
+    
+    double rollacc =atan2(ax,az) *180/PI;
+    *roll = *roll *0,98+rollacc*0.02 ; 
+     
+/*  double heading;
   if (my == 0)
     heading = (mx < 0) ? PI : 0;
   else
-    heading = atan2(mx, my);
+    heading = atan2(-my, mx);
 
   heading -= DECLINATION * PI / 180;
 
-  if (heading > PI) heading -= (2 * PI);
-  else if (heading < -PI) heading += (2 * PI);
+  if (heading > 3.14) heading -= (2 * 3.14);
+  else if (heading < -3.14) heading += (2 * 3.14);
 
   // Convert everything from radians to degrees:
-  heading *= 180.0 / PI;
-  pitch *= 180.0 / PI;
-  roll  *= 180.0 / PI;
+  heading *= 180.0 / PI;*/
+  *pitch *= 180.0 / PI;
+  *roll  *= 180.0 / PI;
 
   pc.printf("Pitch, Roll: ");
-  pc.printf("%f" , pitch, 2);
+  pc.printf("%f" , *pitch, 2);
   pc.printf(", ");
-  pc.printf("%f" , roll, 2);
-  pc.printf("\n") ;
-  pc.printf("Heading: "); pc.printf("%f" , heading, 2);
+  pc.printf("%f" , *roll, 2);
   
-}
-        
-        //pc.printffff("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
-        //pc.printffff("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        //pc.printffff("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        //pc.printffff("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        //pc.printffff("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        //myled = 1;
-        //wait_ms(500);
-        //myled = 0;
-        //wait_ms(500);
-    
-
+  }
\ No newline at end of file