more code
Dependencies: 4DGL-uLCD-SE LSM9DS1_Library_cal mbed
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
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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;