Mavlink ARM Command Fixes

src/drone_controller.py

@@ -177,7 +177,8 @@ class DroneController:
             float(value),
             mavutil.mavlink.MAV_PARAM_TYPE_REAL32
         )
-        time.sleep(0.3)
+        # Wait for parameter to be acknowledged
+        time.sleep(0.5)
     
     def setup_gps_denied(self):
         """Configure for GPS-denied operation."""
@@ -186,44 +187,100 @@ class DroneController:
         # Disable all arming checks for testing
         self.set_param("ARMING_CHECK", 0)
         
+        # Configure EKF3 for non-GPS operation
+        # These parameters tell the EKF to not require GPS
+        self.set_param("EK3_SRC1_POSXY", 0)  # 0 = None (no GPS for horizontal position)
+        self.set_param("EK3_SRC1_VELXY", 0)  # 0 = None (no GPS for horizontal velocity)
+        self.set_param("EK3_SRC1_POSZ", 1)   # 1 = Baro (use barometer for altitude)
+        
         print("[OK] GPS-denied parameters set")
-        time.sleep(0.5)
+        # Wait longer for parameters to take effect
+        time.sleep(1.0)
     
     def arm(self):
         """Force arm the drone (bypasses pre-arm checks)."""
         print("[INFO] Arming...")
         
+        # Result code meanings for COMMAND_ACK
+        result_names = {
+            0: "ACCEPTED",
+            1: "TEMPORARILY_REJECTED",
+            2: "DENIED",
+            3: "UNSUPPORTED",
+            4: "FAILED",
+            5: "IN_PROGRESS",
+            6: "CANCELLED",
+        }
+        
         for attempt in range(5):
-            # Method 1: Use arducopter_arm with force
-            try:
-                self.mav.arducopter_arm()
-            except:
+            # Clear any pending messages
+            while self.mav.recv_match(blocking=False):
                 pass
             
-            time.sleep(0.5)
-            
-            # Method 2: Force arm command
+            # Send force arm command with magic number 21196
+            print(f"[INFO] Sending arm command (attempt {attempt + 1}/5)...")
             self.mav.mav.command_long_send(
                 self.mav.target_system,
                 self.mav.target_component,
                 mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-                0,
-                1,      # 1 = arm
-                21196,  # Force arm magic number (2989 also works)
-                0, 0, 0, 0, 0
+                0,          # confirmation
+                1,          # param1: 1 = arm
+                21196,      # param2: force arm magic number
+                0, 0, 0, 0, 0  # params 3-7 unused
             )
             
-            time.sleep(1.5)
-            self.update_state()
+            # Wait for COMMAND_ACK with timeout
+            ack_received = False
+            start_time = time.time()
+            while time.time() - start_time < 3.0:  # 3 second timeout
+                msg = self.mav.recv_match(type=['COMMAND_ACK', 'HEARTBEAT'], blocking=True, timeout=0.5)
+                if msg is None:
+                    continue
                     
+                if msg.get_type() == 'HEARTBEAT':
+                    self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
                     if self.armed:
-                print("[OK] Armed")
+                        print("[OK] Armed successfully!")
                         return True
                         
-            print(f"[WARN] Arm attempt {attempt + 1}/5...")
+                elif msg.get_type() == 'COMMAND_ACK':
+                    if msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
+                        ack_received = True
+                        result_name = result_names.get(msg.result, f"UNKNOWN({msg.result})")
                         
-        print("[ERROR] Failed to arm")
+                        if msg.result == 0:  # ACCEPTED
+                            print(f"[OK] Arm command accepted")
+                            # Give it a moment to actually arm
+                            time.sleep(0.5)
+                            self.update_state()
+                            if self.armed:
+                                print("[OK] Armed successfully!")
+                                return True
+                        else:
+                            print(f"[WARN] Arm command result: {result_name}")
+                            # Try to get more info from statustext
+                            break
+            
+            if not ack_received:
+                print("[WARN] No ACK received for arm command")
+            
+            # Check for any STATUSTEXT messages that might explain the failure
+            for _ in range(10):
+                msg = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=0.1)
+                if msg:
+                    print(f"[SITL] {msg.text}")
+            
+            # Update state and check if we're armed
+            self.update_state()
+            if self.armed:
+                print("[OK] Armed!")
+                return True
+                
+            time.sleep(0.5)
+        
+        print("[ERROR] Failed to arm after 5 attempts")
         print("[TIP] In MAVProxy, try: arm throttle force")
+        print("[TIP] Check MAVProxy console for pre-arm failure messages")
         return False
     
     def takeoff(self, altitude=5.0):