Added Config.py

drone_controller.py

@@ -13,23 +13,39 @@ from rclpy.node import Node
 from geometry_msgs.msg import Twist
 from std_msgs.msg import String
 
+# Load configuration
+try:
+    from config import CONTROLLER, DRONE, LANDING
+    CONFIG_LOADED = True
+except ImportError:
+    CONFIG_LOADED = False
+    CONTROLLER = {"Kp_z": 0.5, "Kd_z": 0.3, "Kp_xy": 0.3, "Kd_xy": 0.2, "rate": 50}
+    DRONE = {"max_thrust": 1.0, "max_pitch": 0.5, "max_roll": 0.5}
+    LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
+
 
 class DroneController(Node):
     """Drone controller for GPS-denied landing."""
-    
-    CONTROL_RATE = 50.0
-    LANDING_HEIGHT_THRESHOLD = 0.1
-    LANDING_VELOCITY_THRESHOLD = 0.1
-    MAX_THRUST = 1.0
-    MAX_PITCH = 0.5
-    MAX_ROLL = 0.5
-    MAX_YAW = 0.5
 
     def __init__(self):
         super().__init__('drone_controller')
         
+        # Load from config
+        self._control_rate = CONTROLLER.get("rate", 50.0)
+        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)
+        self._max_thrust = DRONE.get("max_thrust", 1.0)
+        self._max_pitch = DRONE.get("max_pitch", 0.5)
+        self._max_roll = DRONE.get("max_roll", 0.5)
+        self._landing_height = LANDING.get("height_threshold", 0.1)
+        self._landing_velocity = LANDING.get("success_velocity", 0.1)
+        
         self.get_logger().info('=' * 50)
         self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
+        if CONFIG_LOADED:
+            self.get_logger().info('  Configuration loaded from config.py')
         self.get_logger().info('=' * 50)
         
         self._latest_telemetry: Optional[Dict[str, Any]] = None
@@ -50,9 +66,9 @@ class DroneController(Node):
         self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
         self.get_logger().info('  Publishing to: /cmd_vel')
         
-        control_period = 1.0 / self.CONTROL_RATE
+        control_period = 1.0 / self._control_rate
         self._control_timer = self.create_timer(control_period, self._control_loop)
-        self.get_logger().info(f'  Control loop: {self.CONTROL_RATE} Hz')
+        self.get_logger().info(f'  Control loop: {self._control_rate} Hz')
         
         self.get_logger().info('Drone Controller Ready!')
         self.get_logger().info('Sensors: IMU, Altimeter, Camera, Landing Pad Detection')
@@ -92,10 +108,10 @@ class DroneController(Node):
             self._rover_telemetry
         )
         
-        thrust = max(min(thrust, self.MAX_THRUST), -self.MAX_THRUST)
-        pitch = max(min(pitch, self.MAX_PITCH), -self.MAX_PITCH)
-        roll = max(min(roll, self.MAX_ROLL), -self.MAX_ROLL)
-        yaw = max(min(yaw, self.MAX_YAW), -self.MAX_YAW)
+        thrust = max(min(thrust, self._max_thrust), -self._max_thrust)
+        pitch = max(min(pitch, self._max_pitch), -self._max_pitch)
+        roll = max(min(roll, self._max_roll), -self._max_roll)
+        yaw = max(min(yaw, 0.5), -0.5)
         
         self._publish_command(thrust, pitch, roll, yaw)
 
@@ -106,7 +122,7 @@ class DroneController(Node):
             altimeter = self._latest_telemetry.get('altimeter', {})
             altitude = altimeter.get('altitude', float('inf'))
             vertical_velocity = abs(altimeter.get('vertical_velocity', float('inf')))
-            return altitude < self.LANDING_HEIGHT_THRESHOLD and vertical_velocity < self.LANDING_VELOCITY_THRESHOLD
+            return altitude < self._landing_height and vertical_velocity < self._landing_velocity
         except (KeyError, TypeError):
             return False