Diff: main.cpp

Revision:

7:d27f97ac2bea

Parent:

0:da114b98e013

Child:

8:005b0a85be70

--- a/main.cpp	Mon Nov 14 20:23:43 2022 +0000
+++ b/main.cpp	Tue Nov 15 04:20:38 2022 +0000
@@ -1,12 +1,71 @@
+// uLCD-144-G2 demo program for uLCD-4GL LCD driver library
+//
+#include <cmath>
 #include "mbed.h"
+#include "uLCD_4DGL.h"
+#include "LSM9DS1.h"
+#define PI 3.14159
+
+uLCD_4DGL uLCD(p9,p10,p30);
+LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
+Serial pc(USBTX, USBRX);
 
-DigitalOut myled(LED1);
+void getAttitude(LSM9DS1& imu, float& roll, float& pitch, float& yaw)
+{
+    float ax = imu.calcAccel(imu.ax),
+          ay = imu.calcAccel(imu.ay),
+          az = imu.calcAccel(imu.az),
+          mx = -imu.calcMag(imu.mx),
+          my = imu.calcMag(imu.my),
+          mz = imu.calcMag(imu.mz);
+
+    pitch = atan2(-ax, sqrt(ay * ay + az * az));
+
+    if (az == 0.0)  roll = (ay < 0.0) ? 180 : 0.0;
+    else            roll = atan2(ay, az);
+        
+    if (my == 0.0)  yaw = (mx < 0.0) ? 180.0 : 0.0;
+    else            yaw = atan2(mx, my);
+    
+    if (yaw > 180.0)        yaw -=PI;
+    else if (yaw < -180.0)  yaw += PI;
+
+}
 
-int main() {
-    while(1) {
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
+int main()
+{
+    imu.begin();
+    if (!imu.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    imu.calibrate(1);
+
+
+    while (true) {
+        // Read IMU
+        while(!imu.accelAvailable());
+        imu.readAccel();
+
+        while (!imu.magAvailable());
+        imu.readMag();
+
+
+
+        float roll, pitch, yaw;
+        getAttitude(imu, roll, pitch, yaw);
+
+        pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);
+
+
+        uLCD.cls();
+
+        float x = 64 + roll * 60;
+        float y = 64 + pitch * 60;
+        float dx = 12 * sin(yaw);
+        float dy = 12 * cos(yaw);
+
+        uLCD.circle(x, y, 2, GREEN);
+        uLCD.line(x-dx, y-dy, x+dx, y+dy, GREEN);
+        wait(.05);
     }
 }