Revision 17:eb0a0e587903, committed 2019-12-16

Comitter:

DGonz

Date:

Mon Dec 16 20:50:02 2019 +0000

Parent:

16:dd12119017a4

Child:

18:74ebec0bbca9

Commit message:

Working!

Changed in this revision

--- a/UM7_USMA.lib	Mon Dec 16 19:33:21 2019 +0000
+++ b/UM7_USMA.lib	Mon Dec 16 20:50:02 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/agentarod23/code/UM7_USMA/#272459c68a53
+https://os.mbed.com/users/agentarod23/code/UM7_USMA/#1f4ce60da5a8

--- a/tests/IMUTest.cpp	Mon Dec 16 19:33:21 2019 +0000
+++ b/tests/IMUTest.cpp	Mon Dec 16 20:50:02 2019 +0000
@@ -1,16 +1,26 @@
 #include "mbed.h"
 #include "IMUTest.h"
 
+#define RAW2ANG (1.0/91.02222)
+#define RAW2ANGDOT (1.0/16.0)
+#define NELEMS(x)  (sizeof(x) / sizeof((x)[0]))
+
 Serial pc(USBTX, USBRX);
 Serial um7serial(A0, A1);
 DigitalOut myled(LED1);
 
-void IMUTest()
-{
-    pc.baud(115200);
-    um7serial.baud(115200);
-    
-    
+int16_t pitchRaw = 0;//divide by 91.02222
+int16_t rollRaw = 0;
+int16_t yawRaw = 0;
+int16_t pitchDotRaw = 0;//divide by 16
+int16_t rollDotRaw = 0;
+int16_t yawDotRaw = 0;
+float eulerTime = 0;
+double pitch = 0.0;
+double roll = 0.0;
+double yaw = 0.0;
+
+void readIMU(){
     uint8_t rx_data[78];
     //total message length for Euler angles = 78
     
@@ -23,29 +33,50 @@
             i++;
         }
     }
+    pc.printf("\n");
     
     int rx_data_length = 78;
 
     UM7_packet new_packet;
-    char FW_revision[5];
     // Call the parse_serial_data function to handle the incoming serial data. The serial data should
     // be placed in ’rx_data’ and the length in ‘rx_data_length’ before this function is called.
     int datastatus = parse_serial_data( rx_data, rx_data_length, &new_packet );
-    pc.printf("%i", datastatus);
+//    pc.printf("%i\n", datastatus);
     if( !datastatus){
-    // We got a good packet! Check to see if it is the firmware revision
-   
+    // We got a good packet!
+//        pc.printf("Is it 0x70?: %i\n", new_packet.Address==0x70);
+//        pc.printf("Is it 0x71?: %i\n", new_packet.Address==0x71);
+//        pc.printf("Is it 0x72?: %i\n", new_packet.Address==0x72);
+//        pc.printf("Is it 0x73?: %i\n", new_packet.Address==0x73);
+//        pc.printf("Is it 0x74?: %i\n", new_packet.Address==0x74);
+//        pc.printf("Size of data packet: %i\n\n", NELEMS(new_packet.data));
+//        pc.printf("data_length: %i\n\n", new_packet.data_length);
+//        pc.printf("Address: %hhx\n\n", new_packet.Address);
+
         if( new_packet.Address == 0x70 ){
-            // Extract the firmware revision
-            FW_revision[0] = new_packet.data[0];
-            FW_revision[1] = new_packet.data[1];
-            FW_revision[2] = new_packet.data[2];
-            FW_revision[3] = new_packet.data[3];
-            FW_revision[4] = '\0'; // Null-terminate the FW revision so we can use it like a string
-            // Print the firmware revision to the terminal (or do whatever else you want…)
-            pc.printf("Got the firmware revision: %s\r\n", FW_revision);
+            rollRaw = 256*new_packet.data[0] + new_packet.data[1];
+            pitchRaw = 256*new_packet.data[2] + new_packet.data[3];
+            pitch = pitchRaw*RAW2ANG;
+            roll = rollRaw*RAW2ANG;
+            
+            yawRaw = 256*new_packet.data[4] + new_packet.data[5];
+            yaw = yawRaw*RAW2ANG;
         }
-    // TODO: Check to see if any of the other packets that we care about have been found.
-    // If so, do stuff with them.
     }
 }
+
+void IMUTest()
+{
+    pc.baud(115200);
+    um7serial.baud(115200);
+    
+    pc.printf("IMU Test\n");
+    
+    while(1){
+        readIMU();
+        pc.printf("pitch: %f\r\n", pitch);
+        pc.printf("roll : %f\r\n", roll);
+        pc.printf("yaw  : %f\r\n", yaw);
+        pc.printf("\n");
+    }
+}