Revision 2:d068248795d9, committed 2020-12-26

Comitter:

aktk

Date:

Sat Dec 26 13:55:46 2020 +0000

Parent:

1:7ca0d7773e29

Child:

3:1cda7adb6a2e

Commit message:

Fixed a bug when using MAG data;

Changed in this revision

--- a/MadgwickFilter.lib	Fri Aug 21 03:17:49 2020 +0000
+++ b/MadgwickFilter.lib	Sat Dec 26 13:55:46 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/aktk/code/MadgwickFilter/#72c013425ecc
+https://os.mbed.com/users/aktk/code/MadgwickFilter/#c20656d96585

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SphereFitting.lib	Sat Dec 26 13:55:46 2020 +0000
@@ -0,0 +1,1 @@
+SphereFitting#bdae9d79b923

--- a/main.cpp	Fri Aug 21 03:17:49 2020 +0000
+++ b/main.cpp	Sat Dec 26 13:55:46 2020 +0000
@@ -3,6 +3,7 @@
 #include "MadgwickFilter.hpp"
 #define M_PI 3.14159265358979323846
 #include "Quaternion.hpp"
+#include "SphereFitting.hpp"
 
 Serial pc(USBTX, USBRX);
 DigitalOut led(LED1);
@@ -20,8 +21,9 @@
     char bytes[4];
 };
 
-static const int NUMOFCH = 1; //9;
-BMX055 imu;
+static const int NUMOFCH = 1;//4;// 1; //9;
+//BMX055 imu(0x19, 0x69, 0x13, p9,  p10);
+BMX055 imu(0x19, 0x69, 0x13, p28,  p27);
 
 DigitalOut imu_sel[9] = {
     DigitalOut(p11), DigitalOut(p12), DigitalOut(p13),
@@ -29,19 +31,18 @@
     DigitalOut(p17), DigitalOut(p18), DigitalOut(p19)
 };
 
-short offset_gyr[9][3] = {
-    {0,0,0},{0,0,0},{0,0,0},
-    {0,0,0},{0,0,0},{0,0,0},
-    {0,0,0},{0,0,0},{0,0,0}
-};
+short offset_gyr[NUMOFCH][3];
+int offset_mag[NUMOFCH][3];
 
-MadgwickFilter mf[9] = {
+MadgwickFilter mf[9];
+/* = {
     MadgwickFilter(0.1f),MadgwickFilter(0.1f),MadgwickFilter(0.1f),
     MadgwickFilter(0.1f),MadgwickFilter(0.1f),MadgwickFilter(0.1f),
     MadgwickFilter(0.1f),MadgwickFilter(0.1f),MadgwickFilter(0.1f)
 };
+*/
 
-const int bufsizeofLPF = 10;
+const int bufsizeofLPF = 1;
 Quaternion q_buf[NUMOFCH][bufsizeofLPF];  //  Buffers for low pass filter
 Quaternion q_sum[NUMOFCH];            //  Sum of values in the buffers
 
@@ -55,6 +56,7 @@
 Timer timer;
 
 void init();
+void calib();
 void update();
 void senddata();
 void initRawdata();
@@ -82,25 +84,41 @@
 
 void init()
 {
+    led = led2 = led3 = led4 = 0;
+    for (int i = 0; i < NUMOFCH; i++) {
+        mf[i] = MadgwickFilter(0.1f);
+    }
+
     for (int j = 0; j < NUMOFCH; j++) {
         for (int i = 0; i < bufsizeofLPF; i++) {
             q_buf[j][i].Set<float>(0,0,0,0);
         }
+        for (int i = 0; i < 3; i ++) {
+            offset_gyr[j][i] = 0;
+            offset_mag[j][i] = 0;
+        }
         q_sum[j].Set<float>(0,0,0,0);
         imu_sel[j] = 0;
     }
     led4 = 1;
 
+    calib();
+    led3 = 1;
+}
+
+void calib()
+{
+    // Calibration for offset of GYR
     for (int j = 0; j < NUMOFCH; j++) {
         imu_sel[j] = 1;
         imu_sel[j] = 1;
         for (int i = 0; i < 1000; i++) {
             led3 = !led3;
-            imu.UpdateIMU();
+            imu.Update();
             offset_gyr[j][0] += imu.gyr.x;
             offset_gyr[j][1] += imu.gyr.y;
             offset_gyr[j][2] += imu.gyr.z;
-            //wait_ms(1);
+            wait_ms(1);
         }
         imu_sel[j] = 0;
         imu_sel[j] = 0;
@@ -108,10 +126,31 @@
         offset_gyr[j][1] /= 1000;
         offset_gyr[j][2] /= 1000;
     }
-
+    led3 = 1;
+    wait_ms(500);
+    led3 = 0;
+    wait_ms(500);
+    // Calibration for offset of MAG
+    for (int j = 0; j < NUMOFCH; j++) {
+        SphereFitting<int> sphere(offset_mag[j][0], offset_mag[j][1], offset_mag[j][2]);
+        imu_sel[j] = 1;
+        imu_sel[j] = 1;
+        for (int i = 0; i < 10000; i++) {
+            led3 = !led3;
+            imu.Update();
+            sphere.addsample(imu.mag.x, imu.mag.y, imu.mag.z);
+            wait_ms(1);
+        }
+        imu_sel[j] = 0;
+        imu_sel[j] = 0;
+        sphere.solve();
+        sphere.copy_param_to(offset_mag[j], offset_mag[j]+1, offset_mag[j]+2, NULL);
+    }
+    
     led3 = 1;
 }
 
+
 void update()
 {
     static int t = 0;
@@ -122,14 +161,25 @@
     for (int j = 0; j < NUMOFCH; j++) {
         imu_sel[j] = 1;
         imu_sel[j] = 1;
-        imu.UpdateIMU();
+        //imu.UpdateIMU();
+        imu.Update();
         imu_sel[j] = 0;
         imu_sel[j] = 0;
+        /*
         mf[j].MadgwickAHRSupdateIMU(
             (imu.gyr.x - offset_gyr[j][0]) * rpspLSB,
             (imu.gyr.y - offset_gyr[j][1]) * rpspLSB,
             (imu.gyr.z - offset_gyr[j][2]) * rpspLSB,
             imu.acc.x, imu.acc.y, imu.acc.z);
+            */
+        mf[j].MadgwickAHRSupdate(
+            (imu.gyr.x - offset_gyr[j][0]) * rpspLSB,
+            (imu.gyr.y - offset_gyr[j][1]) * rpspLSB,
+            (imu.gyr.z - offset_gyr[j][2]) * rpspLSB,
+            imu.acc.x, imu.acc.y, imu.acc.z,
+            -(imu.mag.y - offset_mag[j][1]), 
+            imu.mag.x - offset_mag[j][0],
+            imu.mag.z - offset_mag[j][2]);
         q_sum[j] -= q_buf[j][t];
         mf[j].getAttitude(&(q_buf[j][t]));
         q_sum[j] += q_buf[j][t];
@@ -164,7 +214,15 @@
     static Bits32 q_bits[4];
     static Bits32 tt;
 
-    if(pc.getc() != 0xff || flag == 0) return;
+    int c = pc.getc();
+
+    if(c == 0xee) {
+        tick.detach();
+        init();
+        tick.attach_us(update,5000);
+    }
+
+    if(c != 0xff || flag == 0) return;
 
     tt.data_ui32 = timer.read_us();
     for(int j = 0; j < 4; j++) {
@@ -206,7 +264,9 @@
 {
     static Bits32 tmp;
 
-    if(pc.getc() != 0xf0) return;
+    int c = pc.getc();
+
+    if(c != 0xf0) return;
 
     for (int k = 0; k < NUMOFCH; k++) {
         imu_sel[k] = 1;