Hitl json parser

modules/hitl/camera_emulator.py

@@ -91,31 +91,10 @@ def __init__(
         self.__image_index = 0
         self.__next_image_time = time.time() + time_between_images
         self.__time_between_images = time_between_images
-
+        
         self.__get_images()
         self.update_current_image()
 
-    def periodic(self) -> None:
-        """
-        Periodic function.
-        """
-        try:
-            # Send frame and pace to target FPS
-            self.send_frame()
-            self.sleep_until_next_frame()
-
-            now = time.time()
-            if now >= self.__next_image_time:
-                # Cycle image once per second
-                try:
-                    self.next_image()
-                    self.update_current_image()
-                except Exception as exc:  # pylint: disable=broad-except
-                    print(f"HITL camera image update error: {exc}")
-                self.__next_image_time = now + self.__time_between_images
-        except Exception as exc:  # pylint: disable=broad-except
-            print(f"HITL camera periodic error: {exc}")
-
     def send_frame(self) -> None:
         """
         sends a new frame to virtual camera, should be called in a loop

modules/hitl/example_usage.py

@@ -0,0 +1,189 @@
+#!/usr/bin/env python3
+"""
+Example usage of the JSON Position Emulator in HITL mode.
+This demonstrates how the position emulator cycles through JSON coordinates.
+"""
+
+import time
+from modules.hitl.hitl_base import HITL
+from modules.mavlink import dronekit
+
+def example_json_position_emulator():
+    """
+    Example showing how the JSON position emulator works.
+    """
+
+    # Example 1: Basic usage with JSON file
+    print("=== Example 1: Basic JSON Position Emulation ===")
+
+    # Create a mock drone (in real usage, this would be a real dronekit.Vehicle)
+    # For this example, we'll assume you have a real drone connection
+    # drone = dronekit.connect("tcp:127.0.0.1:5760")  # SITL connection
+
+    # Create HITL with JSON coordinates
+    success, hitl = HITL.create(
+        drone=None,  # Replace with actual drone connection
+        hitl_enabled=True,
+        position_module=True,
+        camera_module=False,
+        json_file_path="test_coordinates.json",  # Our test file
+        position_update_interval=1.0  # 1 second intervals
+    )
+
+    if success and hitl:
+        print("✅ HITL created successfully with JSON position emulation")
+        print("📁 JSON file: test_coordinates.json")
+        print("⏱️  Update interval: 1.0 seconds")
+        print("🔄 Will cycle through coordinates every second")
+
+        # Start the HITL system
+        hitl.start()
+        print("🚀 HITL started - position emulation running...")
+
+        # Simulate running for 5 seconds to show coordinate cycling
+        print("\n📊 Coordinate cycling simulation:")
+        for i in range(5):
+            time.sleep(1)
+            print(f"⏰ Time: {i+1}s - Position emulator is running...")
+
+        # Stop the system
+        hitl.shutdown()
+        print("🛑 HITL stopped")
+
+    print("\n" + "="*60)
+
+    # Example 2: Custom update interval
+    print("=== Example 2: Custom Update Interval (2 seconds) ===")
+
+    success, hitl2 = HITL.create(
+        drone=None,  # Replace with actual drone connection
+        hitl_enabled=True,
+        position_module=True,
+        camera_module=False,
+        json_file_path="test_coordinates.json",
+        position_update_interval=2.0  # 2 second intervals
+    )
+
+    if success and hitl2:
+        print("✅ HITL created with 2-second update interval")
+        print("⏱️  Coordinates will change every 2 seconds")
+
+    print("\n" + "="*60)
+
+    # Example 3: No JSON file (uses Ardupilot)
+    print("=== Example 3: No JSON File (Ardupilot Mode) ===")
+
+    success, hitl3 = HITL.create(
+        drone=None,  # Replace with actual drone connection
+        hitl_enabled=True,
+        position_module=True,
+        camera_module=False
+        # No json_file_path - will use Ardupilot pathing
+    )
+
+    if success and hitl3:
+        print("✅ HITL created without JSON file")
+        print("🎯 Will use Ardupilot's internal pathing")
+        print("❌ No 1-second coordinate shifting")
+
+def explain_coordinate_cycling():
+    """
+    Explains how the coordinate cycling works internally.
+    """
+    print("\n" + "="*60)
+    print("🔍 HOW COORDINATE CYCLING WORKS INTERNALLY")
+    print("="*60)
+
+    print("""
+1. 📁 JSON Loading:
+   - Loads coordinates from test_coordinates.json
+   - Validates format: [[lat, lon, alt], [lat, lon, alt], ...]
+   - Stores in self.json_coordinates list
+
+2. ⏰ Timing Control:
+   - Sets next_coordinate_time = current_time + update_interval
+   - Checks every periodic() call if it's time to update
+
+3. 🔄 Coordinate Cycling:
+   - Gets current coordinate: json_coordinates[current_index]
+   - Calls set_target_position(lat, lon, alt)
+   - Increments current_index (cycles back to 0 at end)
+   - Updates next_coordinate_time for next cycle
+
+4. 📡 Position Injection:
+   - inject_position() sends GPS data to flight controller
+   - Uses MAVLink GPS_INPUT message
+   - Simulates GPS coordinates for the drone
+
+5. 🎯 Priority System:
+   - JSON coordinates have priority over Ardupilot
+   - If JSON available: use JSON cycling
+   - If no JSON: fallback to Ardupilot pathing
+""")
+
+def show_json_format():
+    """
+    Shows the expected JSON format and example data.
+    """
+    print("\n" + "="*60)
+    print("📄 JSON FILE FORMAT")
+    print("="*60)
+
+    print("""
+Expected JSON format:
+[
+    [latitude, longitude, altitude],
+    [latitude, longitude, altitude],
+    ...
+]
+
+Example (test_coordinates.json):
+[
+    [43.43405014107003, -80.57898027451816, 373.0],
+    [40.0, -40.0, 200.0],
+    [41.29129039399329, -81.78471782918818, 373.0]
+]
+
+This will cycle through:
+1. First coordinate for 1 second
+2. Second coordinate for 1 second  
+3. Third coordinate for 1 second
+4. Back to first coordinate (loops forever)
+""")
+
+if __name__ == "__main__":
+    print("🚁 HITL JSON Position Emulator Example")
+    print("="*60)
+
+    # Show JSON format
+    show_json_format()
+
+    # Explain how it works
+    explain_coordinate_cycling()
+
+    # Run examples (commented out since we don't have real drone)
+    print("\n" + "="*60)
+    print("💡 TO USE IN REAL CODE:")
+    print("="*60)
+    print("""
+# In your flight_controller.py:
+from modules.hitl.hitl_base import HITL
+
+# Create HITL with JSON coordinates
+success, hitl = HITL.create(
+    drone=your_drone_connection,
+    hitl_enabled=True,
+    position_module=True,
+    camera_module=False,
+    json_file_path="path/to/your/coordinates.json",
+    position_update_interval=1.0  # seconds
+)
+
+if success:
+    hitl.start()  # Start position emulation
+    # Your main flight loop here
+    hitl.shutdown()  # Clean shutdown
+""")
+
+    # Uncomment to run actual examples (requires drone connection):
+    # example_json_position_emulator()

modules/hitl/execution_flow.txt

@@ -0,0 +1,142 @@
+HITL JSON Position Emulator - Execution Flow
+=============================================
+
+INITIALIZATION PHASE:
+---------------------
+1. HITL.create() called with json_file_path parameter
+2. PositionEmulator.create() called with json_file_path and update_interval
+3. PositionEmulator.__init__() runs:
+   - Sets up json_coordinates = []
+   - Sets current_coordinate_index = 0
+   - Sets update_interval (default 1.0 seconds)
+   - Sets next_coordinate_time = current_time + update_interval
+   - Calls _load_json_coordinates() if json_file_path provided
+
+4. _load_json_coordinates() runs:
+   - Opens and parses JSON file
+   - Validates format: list of [lat, lon, alt] lists
+   - Stores in self.json_coordinates
+   - Prints confirmation message
+
+RUNTIME PHASE (periodic() called every loop):
+---------------------------------------------
+1. periodic() called by HITL thread
+2. Check if json_coordinates exist:
+
+   IF JSON COORDINATES EXIST:
+   ---------------------------
+   3a. Check if current_time >= next_coordinate_time:
+       - IF YES: Call _next_json_coordinate()
+         * Get coordinate: json_coordinates[current_coordinate_index]
+         * Extract: latitude, longitude, altitude
+         * Call set_target_position(lat, lon, alt)
+         * Print progress message
+         * Increment current_coordinate_index (cycle at end)
+         * Set next_coordinate_time = current_time + update_interval
+       - IF NO: Do nothing (wait for next cycle)
+
+   ELSE (NO JSON COORDINATES):
+   ---------------------------
+   3b. Call get_target_position() to get Ardupilot position
+       - Try to get POSITION_TARGET_GLOBAL_INT from flight controller
+       - If available: use Ardupilot coordinates
+       - If not available: use fallback target_position
+
+4. ALWAYS: Call inject_position(*target_position)
+   - Create GPS_INPUT MAVLink message
+   - Send to flight controller
+   - Flush connection
+
+EXAMPLE EXECUTION WITH test_coordinates.json:
+=============================================
+
+JSON File Content:
+[
+    [43.43405014107003, -80.57898027451816, 373.0],  // Coordinate 0
+    [40.0, -40.0, 200.0],                            // Coordinate 1  
+    [41.29129039399329, -81.78471782918818, 373.0]   // Coordinate 2
+]
+
+Time 0.0s: INITIALIZATION
+- Loads 3 coordinates from JSON
+- Sets current_coordinate_index = 0
+- Sets next_coordinate_time = 0.0 + 1.0 = 1.0s
+- Prints: "HITL loaded 3 coordinates from test_coordinates.json"
+
+Time 0.1s: periodic() call
+- current_time = 0.1s
+- 0.1s < 1.0s, so no coordinate change
+- Uses current target_position (default: 43.434..., -80.578..., 373.0)
+- Injects position to flight controller
+
+Time 0.2s: periodic() call
+- current_time = 0.2s  
+- 0.2s < 1.0s, so no coordinate change
+- Uses current target_position
+- Injects position to flight controller
+
+... (continues every ~0.1s) ...
+
+Time 1.0s: periodic() call
+- current_time = 1.0s
+- 1.0s >= 1.0s, so TIME TO CHANGE COORDINATE!
+- Calls _next_json_coordinate():
+  * Gets coordinate[0]: [43.434..., -80.578..., 373.0]
+  * Sets target_position = (43.434..., -80.578..., 373.0)
+  * Prints: "HITL set JSON coordinate 1/3: (43.434..., -80.578..., 373.0)"
+  * Increments current_coordinate_index = 1
+  * Sets next_coordinate_time = 1.0 + 1.0 = 2.0s
+- Injects new position to flight controller
+
+Time 1.1s: periodic() call
+- current_time = 1.1s
+- 1.1s < 2.0s, so no coordinate change
+- Uses current target_position (still coordinate 0)
+- Injects position to flight controller
+
+... (continues every ~0.1s) ...
+
+Time 2.0s: periodic() call
+- current_time = 2.0s
+- 2.0s >= 2.0s, so TIME TO CHANGE COORDINATE!
+- Calls _next_json_coordinate():
+  * Gets coordinate[1]: [40.0, -40.0, 200.0]
+  * Sets target_position = (40.0, -40.0, 200.0)
+  * Prints: "HITL set JSON coordinate 2/3: (40.0, -40.0, 200.0)"
+  * Increments current_coordinate_index = 2
+  * Sets next_coordinate_time = 2.0 + 1.0 = 3.0s
+- Injects new position to flight controller
+
+Time 3.0s: periodic() call
+- current_time = 3.0s
+- 3.0s >= 3.0s, so TIME TO CHANGE COORDINATE!
+- Calls _next_json_coordinate():
+  * Gets coordinate[2]: [41.291..., -81.784..., 373.0]
+  * Sets target_position = (41.291..., -81.784..., 373.0)
+  * Prints: "HITL set JSON coordinate 3/3: (41.291..., -81.784..., 373.0)"
+  * Increments current_coordinate_index = 3, but 3 % 3 = 0 (cycles back!)
+  * Sets next_coordinate_time = 3.0 + 1.0 = 4.0s
+- Injects new position to flight controller
+
+Time 4.0s: periodic() call
+- current_time = 4.0s
+- 4.0s >= 4.0s, so TIME TO CHANGE COORDINATE!
+- Calls _next_json_coordinate():
+  * Gets coordinate[0]: [43.434..., -80.578..., 373.0] (back to first!)
+  * Sets target_position = (43.434..., -80.578..., 373.0)
+  * Prints: "HITL set JSON coordinate 1/3: (43.434..., -80.578..., 373.0)"
+  * Increments current_coordinate_index = 1
+  * Sets next_coordinate_time = 4.0 + 1.0 = 5.0s
+- Injects new position to flight controller
+
+... (continues cycling forever) ...
+
+KEY POINTS:
+===========
+- Coordinates change every 1 second (or custom interval)
+- Cycles through all coordinates then repeats
+- Each coordinate is held for the full interval duration
+- Position is injected to flight controller every periodic() call
+- JSON coordinates override Ardupilot when available
+- If no JSON file, falls back to Ardupilot pathing
+- Error handling prevents crashes if JSON loading fails