diff --git a/scripts/run_ardupilot.py b/scripts/run_ardupilot.py index 8147af7..65bc169 100644 --- a/scripts/run_ardupilot.py +++ b/scripts/run_ardupilot.py @@ -199,6 +199,52 @@ class ArduPilotInterface: 0, yaw_rate # yaw, yaw_rate ) +class SimpleController: + """Simple landing controller that doesn't require ROS 2.""" + + def __init__(self): + self.Kp_z = CONTROLLER.get("Kp_z", 0.5) + self.Kd_z = CONTROLLER.get("Kd_z", 0.3) + self.Kp_xy = CONTROLLER.get("Kp_xy", 0.3) + self.Kd_xy = CONTROLLER.get("Kd_xy", 0.2) + + def calculate_landing_maneuver(self, telemetry, rover_telemetry=None): + """Calculate control outputs for landing.""" + altimeter = telemetry.get('altimeter', {}) + altitude = altimeter.get('altitude', 5.0) + vertical_vel = altimeter.get('vertical_velocity', 0.0) + + velocity = telemetry.get('velocity', {'x': 0, 'y': 0, 'z': 0}) + vel_x = velocity.get('x', 0.0) + vel_y = velocity.get('y', 0.0) + + landing_pad = telemetry.get('landing_pad', None) + + # Descent rate control + if altitude > 1.0: + target_descent_rate = -0.5 + else: + target_descent_rate = -0.2 + + descent_error = target_descent_rate - vertical_vel + thrust = self.Kp_z * descent_error + + # Horizontal control + if landing_pad is not None: + target_x = landing_pad.get('relative_x', 0.0) + target_y = landing_pad.get('relative_y', 0.0) + + pitch = self.Kp_xy * target_x - self.Kd_xy * vel_x + roll = self.Kp_xy * target_y - self.Kd_xy * vel_y + else: + # Just dampen velocity + pitch = -self.Kd_xy * vel_x + roll = -self.Kd_xy * vel_y + + yaw = 0.0 + + return (thrust, pitch, roll, yaw) + def main(): parser = argparse.ArgumentParser(description="ArduPilot Drone Controller") @@ -219,8 +265,8 @@ def main(): print("Make sure sim_vehicle.py is running") sys.exit(1) - # Create drone controller - controller = DroneController() + # Create simple controller (no ROS 2 required) + controller = SimpleController() print("\n" + "=" * 50) print(" ArduPilot GPS-Denied Controller")