SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

drone controller update 4

Browse Source
This commit is contained in:
default
2026-01-09 20:29:31 +00:00
parent 66d9424bf0
commit 389eed9341
1 changed files with 24 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/drone_controller.py
View File

@@ -311,14 +311,16 @@ class DroneController:
print(f"[INFO] Taking off to {altitude}m...")
# In GUIDED_NOGPS, we need to use attitude+thrust commands
# Hover thrust is approximately 0.5-0.6 for a balanced quad
hover_thrust = 0.5
climb_thrust = 0.65 # Slightly above hover to climb
# Thrust values: 0=min, 1=max. Hover is typically 0.5-0.7 depending on weight
# For Gazebo iris model, we may need higher thrust
hover_thrust = 0.6
max_climb_thrust = 0.85 # High thrust for takeoff
start_time = time.time()
timeout = 30 # seconds
last_alt = 0
stuck_count = 0
thrust = 0.5 # Start low and ramp up
while time.time() - start_time < timeout:
self.update_state()
@@ -331,20 +333,29 @@ class DroneController:
time.sleep(0.5) # Stabilize
return True
# Check if making progress
# Ramp up thrust if not climbing
if self.altitude < 0.5 and thrust < max_climb_thrust:
thrust = min(thrust + 0.01, max_climb_thrust) # Gradually increase
elif self.altitude > 0.5:
# We're flying! Maintain good climb thrust
thrust = 0.75
# Check if making progress (after ramping)
if thrust >= max_climb_thrust - 0.05:
if abs(self.altitude - last_alt) < 0.01:
stuck_count += 1
if stuck_count > 50: # 5 seconds stuck
print(f"\n[WARN] Drone not climbing - check motor output")
if stuck_count > 30: # 3 seconds stuck at max thrust
print(f"\n[WARN] Drone not climbing at thrust={thrust:.2f}")
print("[TIP] Check Gazebo physics or motor configuration")
break
else:
stuck_count = 0
last_alt = self.altitude
# Send climb thrust command (level attitude, thrust up)
self._send_attitude_thrust(0, 0, 0, climb_thrust)
self._send_attitude_thrust(0, 0, 0, thrust)
print(f"\r Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={climb_thrust:.2f})", end="")
print(f"\r Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={thrust:.2f})", end="")
time.sleep(0.1)
# Stop - reduce to hover thrust