Diff: main.cpp

Revision:

20:c27bce3cf63b

Parent:

19:428ad790bdb3

Child:

21:c62412a679b0

--- a/main.cpp	Mon May 13 11:59:00 2019 +0000
+++ b/main.cpp	Mon May 20 08:22:32 2019 +0000
@@ -39,8 +39,8 @@
 /* Includes */
 #include "mbed.h"
 #include "XNucleoIKS01A2.h"
-#include "MagdwickAHRS.h"
-
+#include "MadgwickAHRS.h"
+#define sampleFreqDef   1500.0f
 /* Instantiate the expansion board */
 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
 
@@ -96,7 +96,9 @@
   int32_t mag[3];
   int32_t gyr[3];
   int32_t ac[3];
+  float offset[9] = {0};
   float pitch, roll, yaw;
+  int offset_cnt = 0;
   
   
   /* Enable all sensors */
@@ -117,14 +119,14 @@
  // printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
  
  
- Magdwick magdwick_filter;
- magdwick_filter::begin(sampleFreqDef); 
+ Madgwick madgwick_filter;
+ madgwick_filter.begin(sampleFreqDef); 
  
  
   while(1) {
     printf("\r\n");
     
-    magnetometer->get_m_axes(axes);-
+    magnetometer->get_m_axes(axes);
     mag[0] = axes[0];
     mag[1] = axes[1];
     mag[2] = axes[2];
@@ -136,7 +138,7 @@
     ac[0] = axes[0];
     ac[1] = axes[1];
     ac[2] = axes[2];
-    printf("LSM303AGR [acc/mg]:  %6ld, %6ld, %6ld\r\n", ac[0], ac[1], ac[2]);
+    printf("LSM303AGR [acc/mg]:  %6ld, %6ld, %6ld\r\n", ac[0] - offset[3]*1000, ac[1] - offset[4]*1000, ac[2] - offset[5]*1000);
     
    //acc_gyro->get_x_axes(axes);
    //printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", ac[0], ac[1], ac[2]);    
@@ -145,16 +147,41 @@
     gyr[0] = axes[0];
     gyr[1] = axes[1];
     gyr[2] = axes[2];
-    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", gyr[0], gyr[1], gyr[2]);
-
-    magdwick_filter::update(gyr[0], gyr[1], gyr[2], ac[0], ac[1], ac[2], mag[0], mag[1], mag[2]);
-    magdwick_filter::computeAngles();
+    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", gyr[0]/1000 - offset[0], gyr[1]/1000 - offset[1], gyr[2]/1000 - offset[2]);
+    if (offset_cnt > 10)
+    {
+    madgwick_filter.update(gyr[0]/1000 - offset[6], gyr[1]/1000 - offset[7], gyr[2]/1000 - offset[8], ac[0]/1000 - offset[3], ac[1]/1000 - offset[4], ac[2]/1000- offset[5], mag[0]/1000, mag[1]/1000, mag[2]/1000);
+    //madgwick_filter.update(0,0,0,0,0,0,0,0,0);
+    madgwick_filter.computeAngles();
+    }
+    
+    pitch = madgwick_filter.getPitch();
+    roll = madgwick_filter.getRoll();
+    yaw = madgwick_filter.getYaw();
     
-    pitch = magdwick_filter::getPitch();
-    roll = magdwick_filter::getRoll();
-    yaw = magdwick_filter::getYaw();
-    
-    printf(" Pitch: %.2f\n Roll: %.2f\n, Yaw: %.2f\n", pitch, roll, yaw);
+    if (offset_cnt < 10)
+    {
+    offset[0] += mag[0]/1000;
+    offset[1] += mag[1]/1000;
+    offset[2] += mag[2]/1000;
+    offset[3] += ac[0]/1000;
+    offset[4] += ac[1]/1000;
+    offset[5] += ac[2]/1000;
+    offset[6] += gyr[0]/1000;
+    offset[7] += gyr[1]/1000;
+    offset[8] += gyr[2]/1000;
+    }
+    if(offset_cnt == 10)
+    {
+        for(int i = 0; i<10;i++)    
+        {
+            offset[i] = offset[i]/10;
+            printf("Offset[%d]: %.2f/n",i,offset[i]);
+        }
+    }
+      
+    offset_cnt++;
+    printf(" Roll: %.2f\n Pitch: %.2f\n , Yaw: %.2f\n", roll, pitch, yaw);
     printf("Hehehehe");
     wait(0.5);
   }