Revision 11:36209d0b82ea, committed 2022-09-16

Comitter:

mus3

Date:

Fri Sep 16 20:27:40 2022 +0000

Parent:

10:22f453bd8038

Commit message:

sort of working code

Changed in this revision

--- a/main.cpp	Fri Sep 16 19:31:38 2022 +0000
+++ b/main.cpp	Fri Sep 16 20:27:40 2022 +0000
@@ -8,6 +8,9 @@
 
 uLCD_4DGL uLCD(p28,p27,p30); // serial tx, serial rx, reset pin;
 
+int xc = 64;
+int yc = 64;
+
 DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
 // Calculate pitch, roll, and heading.
@@ -15,10 +18,10 @@
 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
 // Heading calculations taken from this app note:
 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
-float[] printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
 {
-    roll = atan2(ay, az);
-    pitch = atan2(-ax, sqrt(ay * ay + az * az));
+    float roll = atan2(ay, az);
+    float pitch = atan2(-ax, sqrt(ay * ay + az * az));
 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
     mx = -mx;
     float heading;
@@ -40,6 +43,23 @@
 
     pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
     pc.printf("Magnetic Heading: %f degress\n\r",heading);
+    
+    uLCD.filled_circle(xc, yc, 10, BLACK);
+    xc = (int) ((roll + 180.0) * 16.0 / 45.0);
+    if (xc > 110) {
+        xc = 110;
+    } else if (xc < 18) {
+        xc = 18;
+    }
+    yc = (int) ((pitch + 180.0) * 16.0 / 45.0);
+    if (yc > 110) {
+        yc = 110;
+    } else if (yc < 18) {
+        yc = 18;
+    }
+    uLCD.filled_circle(xc, yc, 10, RED);
+    
+    pc.printf("xc: %d,    yc: %d degress\n\r",xc,yc);
 }
 
 
@@ -73,11 +93,6 @@
         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
-        uLCD.cls();
-        int xc = (int) ((roll + 180.0) / 128.0);
-        int yc = (int) ((pitch + 180.0) / 128.0);
-        uLCD.circle(64, 64, 60, WHITE);
-        uLCD.filled_circle(xc, yc, 10, RED);
         myled = 1;
         wait(0.1);
         myled = 0;