Diff: main.cpp

Revision:

0:6b20025ef151

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 30 21:59:28 2020 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "LSM9DS1.h"
+
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);
+LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
+Timer t;
+
+float PI = 3.14159265358979323846f;
+
+void mag_correction(float mx, float my, float mz, float mag_c[3])
+{
+    float bias[3]= {-0.0259,0.2015,0.1879};
+    float scale[3][3]= {{1.0269,0.0512,0.0242}, {0.0512,1.0074,0.0203}, {0.0242,0.0203,1.0419}};
+    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(5);
+    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);
+        gyro[0]=lol.calcGyro(lol.gx);
+        gyro[1]=lol.calcGyro(lol.gy);
+        gyro[2]=-lol.calcGyro(lol.gz);
+
+        mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag);
+        mag[2]=-mag[2];
+
+        roll=atan2(accel[1], accel[2]/abs(accel[2])*(sqrt((accel[0]*accel[0])+(accel[2]*accel[2]))));
+        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