Controller Update 6

src/autonomous_controller.py

@@ -69,15 +69,33 @@ class AutonomousController:
             
             print(f"[CTRL] Connected! System {self.mav.target_system}, Component {self.mav.target_component}")
             
-            # Request data streams for telemetry
+            # Request ALL data streams at higher rate
             self.mav.mav.request_data_stream_send(
                 self.mav.target_system,
                 self.mav.target_component,
                 mavutil.mavlink.MAV_DATA_STREAM_ALL,
-                10,  # 10 Hz
+                50,  # 50 Hz for everything
                 1    # Start
             )
             
+            # Specifically request position stream at high rate
+            self.mav.mav.request_data_stream_send(
+                self.mav.target_system,
+                self.mav.target_component,
+                mavutil.mavlink.MAV_DATA_STREAM_POSITION,
+                50,  # 50 Hz
+                1
+            )
+            
+            # Request extended status for altitude
+            self.mav.mav.request_data_stream_send(
+                self.mav.target_system,
+                self.mav.target_component,
+                mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS,
+                10,
+                1
+            )
+            
             return True
         except Exception as e:
             print(f"[ERROR] Connection failed: {e}")
@@ -103,6 +121,16 @@ class AutonomousController:
                 self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
                 self.altitude = -msg.z  # NED: down is positive
             
+            elif msg_type == "GLOBAL_POSITION_INT":
+                # Backup altitude source (relative altitude in mm)
+                if self.altitude == 0:
+                    self.altitude = msg.relative_alt / 1000.0
+            
+            elif msg_type == "VFR_HUD":
+                # Another backup altitude source
+                if self.altitude == 0:
+                    self.altitude = msg.alt
+                
             elif msg_type == "STATUSTEXT":
                 text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
                 text = text.strip()
@@ -360,8 +388,27 @@ class AutonomousController:
         timeout = 30
         last_alt = 0
         stuck_count = 0
+        got_alt_reading = False
         
         while time.time() - start < timeout:
+            # Actively poll for position messages
+            for _ in range(5):
+                msg = self.mav.recv_match(type=['LOCAL_POSITION_NED', 'GLOBAL_POSITION_INT', 'VFR_HUD'], 
+                                          blocking=True, timeout=0.02)
+                if msg:
+                    msg_type = msg.get_type()
+                    if msg_type == 'LOCAL_POSITION_NED':
+                        self.altitude = -msg.z
+                        self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
+                        got_alt_reading = True
+                    elif msg_type == 'GLOBAL_POSITION_INT':
+                        self.altitude = msg.relative_alt / 1000.0
+                        got_alt_reading = True
+                    elif msg_type == 'VFR_HUD':
+                        # VFR_HUD.alt is MSL, but climb rate is useful
+                        if msg.climb > 0.1:
+                            got_alt_reading = True
+            
             self.update_state()
             
             if self.altitude >= altitude * 0.90:
@@ -370,25 +417,33 @@ class AutonomousController:
                 time.sleep(0.5)
                 return True
             
-            # Ramp thrust
-            if self.altitude < 0.5 and thrust < max_thrust:
-                thrust = min(thrust + 0.01, max_thrust)
+            # Ramp thrust more aggressively
+            elapsed = time.time() - start
+            if elapsed < 2.0:
+                # Initial ramp
+                thrust = 0.5 + (elapsed / 2.0) * 0.35  # 0.5 -> 0.85 in 2 seconds
             elif self.altitude > 0.5:
+                # Flying - reduce to climb thrust
                 thrust = 0.75
+            else:
+                # Max thrust if not climbing after 2s
+                thrust = max_thrust
             
-            # Check if stuck
-            if thrust >= max_thrust - 0.05:
+            # Only check for stuck if we have altitude readings
+            if got_alt_reading and elapsed > 5.0:
                 if abs(self.altitude - last_alt) < 0.01:
                     stuck_count += 1
-                    if stuck_count > 30:
-                        print(f"\n[WARN] Not climbing at thrust={thrust:.2f}")
+                    if stuck_count > 50:  # 5 seconds stuck
+                        print(f"\n[WARN] Not climbing at thrust={thrust:.2f}, alt={self.altitude:.2f}m")
                         break
                 else:
                     stuck_count = 0
             last_alt = self.altitude
             
             self.send_attitude_thrust(0, 0, 0, thrust)
-            print(f"\r[CTRL] Climbing: {self.altitude:.1f}m / {altitude:.1f}m (thrust={thrust:.2f})", end="")
+            
+            alt_str = f"{self.altitude:.1f}" if got_alt_reading else "?"
+            print(f"\r[CTRL] Climbing: {alt_str}m / {altitude:.1f}m (thrust={thrust:.2f}, t={elapsed:.1f}s)", end="")
             time.sleep(0.1)
         
         self.send_attitude_thrust(0, 0, 0, hover_thrust)