Get altitude from Pixhawk + update README

Retrieve altitude from Pixhawk using the global frame. The altitude is
relative to the mean sea level.

README updated with new message

ArduPilotDriver.py

@@ -14,6 +14,7 @@
     setattr(collections, "MutableMapping", collections.abc.MutableMapping)
 import dronekit
 
+
 def debug_GPS_vars():
     args = get_args()
     print("Waiting for connection to {}...".format(args.port))
@@ -30,6 +31,7 @@ def debug_GPS_vars():
         print("lat: {}, lon: {}, roll: {}, pitch: {}, yaw: {}"
               .format(lat, lon, roll, pitch, yaw))
 
+
 def get_args():
     parser = argparse.ArgumentParser()
     parser.add_argument("-p", "--port", default="/dev/ttyACM10", help="Serial port connected to ardupilot")
@@ -38,6 +40,7 @@ def get_args():
     parser.add_argument("--debug", action="store_true", help="Prints debug info to stdout")
     return parser.parse_args()
 
+
 async def send_gps_message(websocket, lat, lon):
     msg = {
         "type": "gps",
@@ -46,6 +49,7 @@ async def send_gps_message(websocket, lat, lon):
     }
     await websocket.send(json.dumps(msg))
 
+
 async def send_orientation_message(websocket, roll, pitch, yaw):
     msg = {
         "type": "orientation",
@@ -55,13 +59,23 @@ async def send_orientation_message(websocket, roll, pitch, yaw):
     }
     await websocket.send(json.dumps(msg))
 
+
 async def send_heading_message(websocket, heading):
     msg = {
         "type": "heading",
         "heading": heading
     }
     await websocket.send(json.dumps(msg))
 
+
+async def send_altitude_message(websocket, altitude):
+    msg = {
+        "type": "altitude",
+        "altitude": altitude
+    }
+    await websocket.send(json.dumps(msg))
+
+
 async def async_main():
     args = get_args()
     print("Waiting for ardupilot connection on port {}...".format(args.port))
@@ -77,7 +91,8 @@ def cleanup():
     last_data_send_times = {
         "gps": curr_time,
         "orientation": curr_time,
-        "heading": curr_time
+        "heading": curr_time,
+        "altitude": curr_time
     }
 
     # From M8N documentation:
@@ -88,6 +103,7 @@ def cleanup():
     print("Connecting to rover server...")
     async for websocket in websockets.connect("ws://localhost:3001/ardupilot"):
         print("Done!")
+
         def gps_callback(self, _, frame):
             curr_time = time.perf_counter()
             if curr_time >= last_data_send_times["gps"] + 1./args.frequency:
@@ -96,7 +112,7 @@ def gps_callback(self, _, frame):
                 if ardupilot.gps_0.fix_type > 1:
                     last_data_send_times["gps"] = curr_time
                     asyncio.run(send_gps_message(websocket, frame.lat, frame.lon))
-        
+
         def orientation_callback(self, _, attitude):
             curr_time = time.perf_counter()
             if curr_time >= last_data_send_times["orientation"] + 1./args.frequency:
@@ -108,18 +124,27 @@ def orientation_callback(self, _, attitude):
                     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))
-        
+
         def heading_callback(self, _, heading):
             curr_time = time.perf_counter()
             if curr_time >= last_data_send_times["heading"] + 1./args.frequency:
                 if args.debug:
                     print("Heading", heading)
                 last_data_send_times["heading"] = curr_time
                 asyncio.run(send_heading_message(websocket, heading))
-
+
+        def altitude_callback(self, _, altitude):
+            curr_time = time.perf_counter()
+            if curr_time >= last_data_send_times["altitude"] + 1./args.frequency:
+                if args.debug:
+                    print("Altitude", altitude)
+                last_data_send_times["altitude"] = curr_time
+                asyncio.run(send_altitude_message(websocket, altitude))
+
         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("location.global_frame.alt", altitude_callback)
 
         try:
             while True:
@@ -129,11 +154,14 @@ def heading_callback(self, _, heading):
             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("location.global_frame.alt", altitude_callback)
             continue
 
+
 def main():
     debug_GPS_vars()
 
+
 if __name__ == "__main__":
     asyncio.run(async_main())
     # main()

README.md

@@ -60,3 +60,11 @@ Make sure `tmux` is installed, if not then `sudo apt install tmux`. Then:
     "heading": number
 }
 ```
+
+### Altitude
+```
+{
+    "type": "altitude",
+    "altitude": number
+}
+```