Controller Update 7

src/autonomous_controller.py

@@ -450,11 +450,30 @@ class AutonomousController:
         print(f"\n[CTRL] Takeoff ended at {self.altitude:.1f}m")
         return self.altitude > 1.0
     
-    def fly_to(self, target_x, target_y, altitude, tolerance=1.0, timeout=30):
+    def fly_to(self, target_x, target_y, altitude, tolerance=1.0, timeout=60):
-        """Fly to position using velocity commands."""
+        """Fly to position using attitude+thrust commands (works in GUIDED_NOGPS).
+        
+        Uses pitch for forward/backward movement, roll for left/right.
+        """
         print(f"[CTRL] Flying to ({target_x:.1f}, {target_y:.1f}, {altitude:.1f}m)")
         
-        for i in range(int(timeout * 10)):
+        hover_thrust = 0.6
+        max_tilt = 0.15  # ~8.5 degrees max tilt angle in radians
+        
+        start_time = time.time()
+        
+        while time.time() - start_time < timeout:
+            # Actively poll for position
+            for _ in range(3):
+                msg = self.mav.recv_match(type=['LOCAL_POSITION_NED', 'GLOBAL_POSITION_INT'], 
+                                          blocking=True, timeout=0.03)
+                if msg:
+                    if msg.get_type() == 'LOCAL_POSITION_NED':
+                        self.altitude = -msg.z
+                        self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
+                    elif msg.get_type() == 'GLOBAL_POSITION_INT':
+                        self.altitude = msg.relative_alt / 1000.0
+            
             self.update_state()
             
             dx = target_x - self.position["x"]
@@ -463,33 +482,50 @@ class AutonomousController:
             
             dist = math.sqrt(dx*dx + dy*dy)
             
+            # Check if reached target
             if dist < tolerance and abs(dz) < tolerance:
-                self.send_velocity_ned(0, 0, 0)
+                self.send_attitude_thrust(0, 0, 0, hover_thrust)
                 print(f"\n[CTRL] Reached ({self.position['x']:.1f}, {self.position['y']:.1f})")
                 return True
             
-            speed = min(2.0, max(0.5, dist * 0.5))
+            # Calculate tilt angles for movement
+            # In NED frame: +X is forward (North), +Y is right (East)
+            # Pitch: negative = nose down = fly forward (+X)
+            # Roll: positive = right wing down = fly right (+Y)
             
             if dist > 0.1:
-                vx = (dx / dist) * speed
+                # Calculate direction to target
-                vy = (dy / dist) * speed
+                # Scale tilt based on distance (more tilt = faster movement)
+                tilt_scale = min(1.0, dist / 5.0)  # Full tilt at 5+ meters away
+                
+                # Pitch for forward/back movement (negative pitch = forward)
+                pitch = -max_tilt * tilt_scale * (dx / dist)
+                
+                # Roll for left/right movement (positive roll = right)
+                roll = max_tilt * tilt_scale * (dy / dist)
             else:
-                vx, vy = 0, 0
+                pitch = 0
+                roll = 0
             
+            # Thrust for altitude
             if dz > 0.5:
-                vz = -1.0
+                thrust = hover_thrust + 0.1  # Climb
             elif dz < -0.5:
-                vz = 0.5
+                thrust = hover_thrust - 0.1  # Descend
             else:
-                vz = -dz
+                thrust = hover_thrust + dz * 0.1  # Small adjustment
             
-            self.send_velocity_ned(vx, vy, vz)
+            # Clamp thrust
+            thrust = max(0.4, min(0.8, thrust))
             
-            if i % 10 == 0:
+            self.send_attitude_thrust(roll, pitch, 0, thrust)
-                print(f"\r[CTRL] Pos: ({self.position['x']:.1f}, {self.position['y']:.1f}) Alt: {self.altitude:.1f}m Dist: {dist:.1f}m", end="")
+            
-            time.sleep(0.1)
+            elapsed = time.time() - start_time
+            if int(elapsed * 10) % 10 == 0:  # Print every second
+                print(f"\r[CTRL] Pos: ({self.position['x']:.1f}, {self.position['y']:.1f}) -> ({target_x:.1f}, {target_y:.1f}) Dist: {dist:.1f}m Alt: {self.altitude:.1f}m", end="", flush=True)
+            time.sleep(0.05)  # 20Hz control rate
         
-        self.send_velocity_ned(0, 0, 0)
+        self.send_attitude_thrust(0, 0, 0, hover_thrust)
         print(f"\n[CTRL] Waypoint timeout")
         return False
     
@@ -534,7 +570,7 @@ class AutonomousController:
             self.update_state()
             self.send_attitude_thrust(0, 0, 0, 0.6)
             remaining = duration - (time.time() - start)
-            print(f"\r[CTRL] Hovering... {remaining:.0f}s remaining, Alt: {self.altitude:.1f}m", end="")
+            print(f"\r[CTRL] Hovering... {remaining:.0f}s remaining, Alt: {self.altitude:.1f}m   ", end="", flush=True)
             time.sleep(0.1)
         
         print()