Print attitude and disable heading message (#1)

* Disable heading message

* print rpy

* Minor fixes

* Remove unused line

ArduPilotDriver.py

@@ -6,6 +6,7 @@
 import websockets
 import json
 import argparse
+import math
 
 # This if is required because dronekit is not up to date for python 3.10
 if sys.version_info.major == 3 and sys.version_info.minor >= 10:
@@ -101,6 +102,10 @@ def orientation_callback(self, _, attitude):
             if curr_time >= last_data_send_times["orientation"] + 1./args.frequency:
                 if args.debug:
                     print(attitude)
+                else:
+                    pitch_deg = attitude.pitch * 180 / math.pi
+                    roll_deg = attitude.roll * 180 / math.pi
+                    print(f"\33[2K\rroll={roll_deg: 4.0f}, pitch={pitch_deg: 4.0f}", end="")
                 last_data_send_times["orientation"] = curr_time
                 asyncio.run(send_orientation_message(websocket, attitude.roll, attitude.pitch, attitude.yaw))
 
@@ -114,7 +119,7 @@ def heading_callback(self, _, heading):
 
         ardupilot.add_attribute_listener("location.global_frame", gps_callback)
         ardupilot.add_attribute_listener("attitude", orientation_callback)
-        ardupilot.add_attribute_listener("heading", heading_callback)
+        # ardupilot.add_attribute_listener("heading", heading_callback)
 
         try:
             while True:
@@ -123,7 +128,7 @@ def heading_callback(self, _, heading):
             print(e)
             ardupilot.remove_attribute_listener("location.global_frame", gps_callback)
             ardupilot.remove_attribute_listener("attitude", orientation_callback)
-            ardupilot.remove_attribute_listener("heading", heading_callback)
+            # ardupilot.remove_attribute_listener("heading", heading_callback)
             continue
 
 def main():