Print orientation matrix

DocGarbanzo · Sep 28, 2024 · 67facca · 67facca
1 parent 0f850e2
commit 67facca
Showing 1 changed file with 6 additions and 5 deletions.
diff --git a/donkeycar/parts/imu.py b/donkeycar/parts/imu.py
@@ -183,7 +183,7 @@ def poll(self):
 
     def run(self):
         self.poll()
-        # return self.euler
+        return self.euler, self.matrix
 
     def run_threaded(self):
         return self.accel, self.gyro
@@ -197,15 +197,16 @@ def shutdown(self):
 if __name__ == "__main__":
     import sys
     from sys import stdout
+    np.set_printoptions(precision=3, sign='+', floatmode='fixed', suppress=True)
     count = 0
     p = Mpu6050Ada()
     while True:
         try:
-            matrix = p.run()
+            euler, matrix = p.run()
             #out_str = f"\reuler: " + f",".join(f"{x:+5.3f}" for x in matrix)
-            #out_str = f"\rm = {matrix}"
-            #stdout.write(out_str)
-            # stdout.flush()
+            out_str = f"\rm = {matrix}"
+            stdout.write(out_str)
+            stdout.flush()
             time.sleep(0.01)
             count += 1
         except KeyboardInterrupt: