starting to work on supporting px4

.github/workflows/unittests.yml

@@ -1,6 +1,6 @@
 name: Run Unittests
 
-on: [push, pull_request]
+on: [pull_request]
 
 jobs:
   test:

get_autopilot_info.py

@@ -0,0 +1,36 @@
+from pymavlink import mavutil
+import time
+import argparse
+
+def get_autopilot_info(connection, sysid=1):
+    autopilot_info = {"autopilot": "", "type": "", "version": ""}
+    heartbeat_sysid = 0
+    while heartbeat_sysid != sysid:
+        msg = connection.recv_match(type='HEARTBEAT', blocking=True, timeout=3)
+        heartbeat_sysid = msg.get_srcSystem()
+    print(msg)
+    autopilot = mavutil.mavlink.enums['MAV_AUTOPILOT'][msg.autopilot].name.replace("MAV_AUTOPILOT_", "").lower()
+    autopilot_info["autopilot"] = autopilot
+    autopilot_info["type"] = mavutil.mavlink.enums['MAV_TYPE'][msg.type].name.replace("MAV_TYPE_", "").lower()
+
+    if autopilot_info["autopilot"] == "ardupilotmega":
+        connection.mav.send(mavutil.mavlink.MAVLink_autopilot_version_request_message(
+            connection.target_system, connection.target_component))
+        msg = connection.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=3)
+        if msg:
+            major = str(msg.flight_sw_version >> 24)
+            sub = str(msg.flight_sw_version >> 16 & 0xFF)
+            rev = str(msg.flight_sw_version >> 8 & 0xFF)
+            fc_version = f"{major}.{sub}.{rev}"
+            git_hash = ''.join(chr(i) for i in msg.flight_custom_version)
+            autopilot_info["version"] = fc_version
+    return autopilot_info
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Get autopilot info')
+    parser.add_argument('--sysid', type=int, default=1, help='System ID to search for (default: 1)')
+    args = parser.parse_args()
+
+    connection = mavutil.mavlink_connection('udpin:localhost:14551')
+    autopilot_info = get_autopilot_info(connection, args.sysid)
+    print(autopilot_info)

takeoff.py

@@ -41,26 +41,31 @@ def takeoff(mav_connection, takeoff_altitude):
     print("Heartbeat from system (system %u component %u)" %
           (mav_connection.target_system, mav_connection.target_component))
 
-    mode_id = mav_connection.mode_mapping()["GUIDED"]
+    mode_id = mav_connection.mode_mapping()["TAKEOFF"]
 
 
-    mav_connection.mav.command_long_send(mav_connection.target_system, mav_connection.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+    mav_connection.mav.command_long_send(1, 1, mavutil.mavlink.MAV_CMD_DO_SET_MODE,
                                 0, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode_id, 0, 0, 0, 0, 0)
     ack_msg = mav_connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
     print(f"Change Mode ACK:  {ack_msg}")
 
-    mav_connection.mav.command_long_send(mav_connection.target_system, mav_connection.target_component,
+    mav_connection.mav.command_long_send(1, 1,
                                          mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0)
 
     arm_msg = mav_connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
     print(f"Arm ACK:  {arm_msg}")
 
-    mav_connection.mav.command_long_send(mav_connection.target_system, mav_connection.target_component,
-                                         mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, takeoff_altitude)
+    mav_connection.mav.command_long_send(1, 1,
+                                         mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, -1.0, 0, 0, 0, float("NAN"), float("NAN"), 600)
 
     takeoff_msg = mav_connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
     print(f"Takeoff ACK:  {takeoff_msg}")
 
+    mav_connection.mav.command_long_send(1, 1,
+                                         mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0)
+
+    arm_msg = mav_connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    print(f"Arm ACK:  {arm_msg}")
     return takeoff_msg.result
 
 def main():