Diff: main.cpp

Revision:

0:042921e9375d

Child:

1:9331c419c9b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 28 16:42:41 2020 +0000
@@ -0,0 +1,71 @@
+#include "LSM9DS1.h"
+
+DigitalOut myled(LED1);
+Serial pc(USBTX,USBRX);
+LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
+Timer t;
+
+float PI=3.1415926535897;
+
+void mag_correction(float mx, float my, float mz, float mag_c[3])
+{
+    float bias[3] = {-0.1102,0.3965,-0.0683};
+    float scale[3][3] = {{1.0558, 0.0640, -0.0220},
+        {0.0640, 1.0657, 0.0645},
+        {-0.0220, 0.0645, 1.0650}
+    };
+
+    mag_c[0] = (mx-bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
+    mag_c[1] = (mx-bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
+    mag_c[2] = (mx-bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
+}
+int main()
+{
+    float roll, pitch, yaw;
+    float accel[3], mag[3], gyro[3];
+
+    lol.begin();
+    if (!lol.begin()) {
+        pc.printf("failed to communicate with LSM9DS1.\n");
+    }
+    lol.calibrate(true);
+    pc.printf("Gyro bias = %f,%f,%f\r\n", lol.gBias[0], lol.gBias[1], lol.gBias[2]);
+    pc.printf("Accel bias = %f,%f,%f\r\n", lol.aBias[0], lol.aBias[1], lol.aBias[2]);
+    wait(1);
+    t.start();
+
+    while(1) {
+        lol.readMag();
+        lol.readGyro();
+        lol.readAccel();
+        accel[0]= lol.calcAccel(lol.ax);
+        accel[1]= lol.calcAccel(lol.ay);
+        accel[2]= - lol.calcAccel(lol.az); //reverse z
+        gyro[0]= lol.calcGyro(lol.gx);
+        gyro[1]= lol.calcGyro(lol.gy);
+        gyro[2]= - lol.calcGyro(lol.gz); //reverse z
+        mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag);
+        mag[2] = - mag[2]; //reverse z
+        //reg geometry conversion
+        roll = atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0]*accel[0]) +(accel[2] * accel[2]))));
+        //negative is added after testing pose in real situation
+        pitch = - atan2 (-accel[0], (sqrt((accel[1]*accel[1]+(accel[2]*accel[2])))));
+        float Yh = (mag[1] * cos(roll)) - (mag[2] *sin(roll));
+        float Xh = (mag[0] * cos(pitch))+(mag[1] * sin(roll)*sin(pitch)) + (mag[2]*cos(roll) * sin(pitch));
+        yaw = atan2(Yh,Xh);
+        pitch *= 180.0f / PI;
+        yaw *= 180.0f / PI;
+        roll *= 180.0f /PI;
+        if(yaw<=0) {
+            yaw = yaw+360;
+        }
+        if(roll<=0) {
+            roll = roll+360;
+        }
+        if(pitch<=0) {
+            pitch = pitch + 360;
+        }
+
+        pc.printf("$imu,4,4,%f,%3.3f,   %3.3f,  %3.3f;\r\n",t.read(),roll,pitch,yaw);
+    }
+}
\ No newline at end of file