Skip to content

Commit 1eeca49

Browse files
Saahib13Saahib13
committed
added JSON parser and manual GPS coordinate option
1 parent a25cbc9 commit 1eeca49

File tree

5 files changed

+421
-21
lines changed

5 files changed

+421
-21
lines changed

‎modules/hitl/example_usage.py‎

Lines changed: 189 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
#!/usr/bin/env python3
2+
"""
3+
Example usage of the JSON Position Emulator in HITL mode.
4+
This demonstrates how the position emulator cycles through JSON coordinates.
5+
"""
6+
7+
import time
8+
from modules.hitl.hitl_base import HITL
9+
from modules.mavlink import dronekit
10+
11+
def example_json_position_emulator():
12+
"""
13+
Example showing how the JSON position emulator works.
14+
"""
15+
16+
# Example 1: Basic usage with JSON file
17+
print("=== Example 1: Basic JSON Position Emulation ===")
18+
19+
# Create a mock drone (in real usage, this would be a real dronekit.Vehicle)
20+
# For this example, we'll assume you have a real drone connection
21+
# drone = dronekit.connect("tcp:127.0.0.1:5760") # SITL connection
22+
23+
# Create HITL with JSON coordinates
24+
success, hitl = HITL.create(
25+
drone=None, # Replace with actual drone connection
26+
hitl_enabled=True,
27+
position_module=True,
28+
camera_module=False,
29+
json_file_path="test_coordinates.json", # Our test file
30+
position_update_interval=1.0 # 1 second intervals
31+
)
32+
33+
if success and hitl:
34+
print("✅ HITL created successfully with JSON position emulation")
35+
print("📁 JSON file: test_coordinates.json")
36+
print("⏱️ Update interval: 1.0 seconds")
37+
print("🔄 Will cycle through coordinates every second")
38+
39+
# Start the HITL system
40+
hitl.start()
41+
print("🚀 HITL started - position emulation running...")
42+
43+
# Simulate running for 5 seconds to show coordinate cycling
44+
print("\n📊 Coordinate cycling simulation:")
45+
for i in range(5):
46+
time.sleep(1)
47+
print(f"⏰ Time: {i+1}s - Position emulator is running...")
48+
49+
# Stop the system
50+
hitl.shutdown()
51+
print("🛑 HITL stopped")
52+
53+
print("\n" + "="*60)
54+
55+
# Example 2: Custom update interval
56+
print("=== Example 2: Custom Update Interval (2 seconds) ===")
57+
58+
success, hitl2 = HITL.create(
59+
drone=None, # Replace with actual drone connection
60+
hitl_enabled=True,
61+
position_module=True,
62+
camera_module=False,
63+
json_file_path="test_coordinates.json",
64+
position_update_interval=2.0 # 2 second intervals
65+
)
66+
67+
if success and hitl2:
68+
print("✅ HITL created with 2-second update interval")
69+
print("⏱️ Coordinates will change every 2 seconds")
70+
71+
print("\n" + "="*60)
72+
73+
# Example 3: No JSON file (uses Ardupilot)
74+
print("=== Example 3: No JSON File (Ardupilot Mode) ===")
75+
76+
success, hitl3 = HITL.create(
77+
drone=None, # Replace with actual drone connection
78+
hitl_enabled=True,
79+
position_module=True,
80+
camera_module=False
81+
# No json_file_path - will use Ardupilot pathing
82+
)
83+
84+
if success and hitl3:
85+
print("✅ HITL created without JSON file")
86+
print("🎯 Will use Ardupilot's internal pathing")
87+
print("❌ No 1-second coordinate shifting")
88+
89+
def explain_coordinate_cycling():
90+
"""
91+
Explains how the coordinate cycling works internally.
92+
"""
93+
print("\n" + "="*60)
94+
print("🔍 HOW COORDINATE CYCLING WORKS INTERNALLY")
95+
print("="*60)
96+
97+
print("""
98+
1. 📁 JSON Loading:
99+
- Loads coordinates from test_coordinates.json
100+
- Validates format: [[lat, lon, alt], [lat, lon, alt], ...]
101+
- Stores in self.json_coordinates list
102+
103+
2. ⏰ Timing Control:
104+
- Sets next_coordinate_time = current_time + update_interval
105+
- Checks every periodic() call if it's time to update
106+
107+
3. 🔄 Coordinate Cycling:
108+
- Gets current coordinate: json_coordinates[current_index]
109+
- Calls set_target_position(lat, lon, alt)
110+
- Increments current_index (cycles back to 0 at end)
111+
- Updates next_coordinate_time for next cycle
112+
113+
4. 📡 Position Injection:
114+
- inject_position() sends GPS data to flight controller
115+
- Uses MAVLink GPS_INPUT message
116+
- Simulates GPS coordinates for the drone
117+
118+
5. 🎯 Priority System:
119+
- JSON coordinates have priority over Ardupilot
120+
- If JSON available: use JSON cycling
121+
- If no JSON: fallback to Ardupilot pathing
122+
""")
123+
124+
def show_json_format():
125+
"""
126+
Shows the expected JSON format and example data.
127+
"""
128+
print("\n" + "="*60)
129+
print("📄 JSON FILE FORMAT")
130+
print("="*60)
131+
132+
print("""
133+
Expected JSON format:
134+
[
135+
[latitude, longitude, altitude],
136+
[latitude, longitude, altitude],
137+
...
138+
]
139+
140+
Example (test_coordinates.json):
141+
[
142+
[43.43405014107003, -80.57898027451816, 373.0],
143+
[40.0, -40.0, 200.0],
144+
[41.29129039399329, -81.78471782918818, 373.0]
145+
]
146+
147+
This will cycle through:
148+
1. First coordinate for 1 second
149+
2. Second coordinate for 1 second
150+
3. Third coordinate for 1 second
151+
4. Back to first coordinate (loops forever)
152+
""")
153+
154+
if __name__ == "__main__":
155+
print("🚁 HITL JSON Position Emulator Example")
156+
print("="*60)
157+
158+
# Show JSON format
159+
show_json_format()
160+
161+
# Explain how it works
162+
explain_coordinate_cycling()
163+
164+
# Run examples (commented out since we don't have real drone)
165+
print("\n" + "="*60)
166+
print("💡 TO USE IN REAL CODE:")
167+
print("="*60)
168+
print("""
169+
# In your flight_controller.py:
170+
from modules.hitl.hitl_base import HITL
171+
172+
# Create HITL with JSON coordinates
173+
success, hitl = HITL.create(
174+
drone=your_drone_connection,
175+
hitl_enabled=True,
176+
position_module=True,
177+
camera_module=False,
178+
json_file_path="path/to/your/coordinates.json",
179+
position_update_interval=1.0 # seconds
180+
)
181+
182+
if success:
183+
hitl.start() # Start position emulation
184+
# Your main flight loop here
185+
hitl.shutdown() # Clean shutdown
186+
""")
187+
188+
# Uncomment to run actual examples (requires drone connection):
189+
# example_json_position_emulator()

‎modules/hitl/execution_flow.txt‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
HITL JSON Position Emulator - Execution Flow
2+
=============================================
3+
4+
INITIALIZATION PHASE:
5+
---------------------
6+
1. HITL.create() called with json_file_path parameter
7+
2. PositionEmulator.create() called with json_file_path and update_interval
8+
3. PositionEmulator.__init__() runs:
9+
- Sets up json_coordinates = []
10+
- Sets current_coordinate_index = 0
11+
- Sets update_interval (default 1.0 seconds)
12+
- Sets next_coordinate_time = current_time + update_interval
13+
- Calls _load_json_coordinates() if json_file_path provided
14+
15+
4. _load_json_coordinates() runs:
16+
- Opens and parses JSON file
17+
- Validates format: list of [lat, lon, alt] lists
18+
- Stores in self.json_coordinates
19+
- Prints confirmation message
20+
21+
RUNTIME PHASE (periodic() called every loop):
22+
---------------------------------------------
23+
1. periodic() called by HITL thread
24+
2. Check if json_coordinates exist:
25+
26+
IF JSON COORDINATES EXIST:
27+
---------------------------
28+
3a. Check if current_time >= next_coordinate_time:
29+
- IF YES: Call _next_json_coordinate()
30+
* Get coordinate: json_coordinates[current_coordinate_index]
31+
* Extract: latitude, longitude, altitude
32+
* Call set_target_position(lat, lon, alt)
33+
* Print progress message
34+
* Increment current_coordinate_index (cycle at end)
35+
* Set next_coordinate_time = current_time + update_interval
36+
- IF NO: Do nothing (wait for next cycle)
37+
38+
ELSE (NO JSON COORDINATES):
39+
---------------------------
40+
3b. Call get_target_position() to get Ardupilot position
41+
- Try to get POSITION_TARGET_GLOBAL_INT from flight controller
42+
- If available: use Ardupilot coordinates
43+
- If not available: use fallback target_position
44+
45+
4. ALWAYS: Call inject_position(*target_position)
46+
- Create GPS_INPUT MAVLink message
47+
- Send to flight controller
48+
- Flush connection
49+
50+
EXAMPLE EXECUTION WITH test_coordinates.json:
51+
=============================================
52+
53+
JSON File Content:
54+
[
55+
[43.43405014107003, -80.57898027451816, 373.0], // Coordinate 0
56+
[40.0, -40.0, 200.0], // Coordinate 1
57+
[41.29129039399329, -81.78471782918818, 373.0] // Coordinate 2
58+
]
59+
60+
Time 0.0s: INITIALIZATION
61+
- Loads 3 coordinates from JSON
62+
- Sets current_coordinate_index = 0
63+
- Sets next_coordinate_time = 0.0 + 1.0 = 1.0s
64+
- Prints: "HITL loaded 3 coordinates from test_coordinates.json"
65+
66+
Time 0.1s: periodic() call
67+
- current_time = 0.1s
68+
- 0.1s < 1.0s, so no coordinate change
69+
- Uses current target_position (default: 43.434..., -80.578..., 373.0)
70+
- Injects position to flight controller
71+
72+
Time 0.2s: periodic() call
73+
- current_time = 0.2s
74+
- 0.2s < 1.0s, so no coordinate change
75+
- Uses current target_position
76+
- Injects position to flight controller
77+
78+
... (continues every ~0.1s) ...
79+
80+
Time 1.0s: periodic() call
81+
- current_time = 1.0s
82+
- 1.0s >= 1.0s, so TIME TO CHANGE COORDINATE!
83+
- Calls _next_json_coordinate():
84+
* Gets coordinate[0]: [43.434..., -80.578..., 373.0]
85+
* Sets target_position = (43.434..., -80.578..., 373.0)
86+
* Prints: "HITL set JSON coordinate 1/3: (43.434..., -80.578..., 373.0)"
87+
* Increments current_coordinate_index = 1
88+
* Sets next_coordinate_time = 1.0 + 1.0 = 2.0s
89+
- Injects new position to flight controller
90+
91+
Time 1.1s: periodic() call
92+
- current_time = 1.1s
93+
- 1.1s < 2.0s, so no coordinate change
94+
- Uses current target_position (still coordinate 0)
95+
- Injects position to flight controller
96+
97+
... (continues every ~0.1s) ...
98+
99+
Time 2.0s: periodic() call
100+
- current_time = 2.0s
101+
- 2.0s >= 2.0s, so TIME TO CHANGE COORDINATE!
102+
- Calls _next_json_coordinate():
103+
* Gets coordinate[1]: [40.0, -40.0, 200.0]
104+
* Sets target_position = (40.0, -40.0, 200.0)
105+
* Prints: "HITL set JSON coordinate 2/3: (40.0, -40.0, 200.0)"
106+
* Increments current_coordinate_index = 2
107+
* Sets next_coordinate_time = 2.0 + 1.0 = 3.0s
108+
- Injects new position to flight controller
109+
110+
Time 3.0s: periodic() call
111+
- current_time = 3.0s
112+
- 3.0s >= 3.0s, so TIME TO CHANGE COORDINATE!
113+
- Calls _next_json_coordinate():
114+
* Gets coordinate[2]: [41.291..., -81.784..., 373.0]
115+
* Sets target_position = (41.291..., -81.784..., 373.0)
116+
* Prints: "HITL set JSON coordinate 3/3: (41.291..., -81.784..., 373.0)"
117+
* Increments current_coordinate_index = 3, but 3 % 3 = 0 (cycles back!)
118+
* Sets next_coordinate_time = 3.0 + 1.0 = 4.0s
119+
- Injects new position to flight controller
120+
121+
Time 4.0s: periodic() call
122+
- current_time = 4.0s
123+
- 4.0s >= 4.0s, so TIME TO CHANGE COORDINATE!
124+
- Calls _next_json_coordinate():
125+
* Gets coordinate[0]: [43.434..., -80.578..., 373.0] (back to first!)
126+
* Sets target_position = (43.434..., -80.578..., 373.0)
127+
* Prints: "HITL set JSON coordinate 1/3: (43.434..., -80.578..., 373.0)"
128+
* Increments current_coordinate_index = 1
129+
* Sets next_coordinate_time = 4.0 + 1.0 = 5.0s
130+
- Injects new position to flight controller
131+
132+
... (continues cycling forever) ...
133+
134+
KEY POINTS:
135+
===========
136+
- Coordinates change every 1 second (or custom interval)
137+
- Cycles through all coordinates then repeats
138+
- Each coordinate is held for the full interval duration
139+
- Position is injected to flight controller every periodic() call
140+
- JSON coordinates override Ardupilot when available
141+
- If no JSON file, falls back to Ardupilot pathing
142+
- Error handling prevents crashes if JSON loading fails

‎modules/hitl/hitl_base.py‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@ def create(
2626
position_module: bool,
2727
camera_module: bool,
2828
images_path: str | None = None,
29+
json_file_path: str | None = None,
30+
position_update_interval: float = 1.0,
2931
) -> "tuple[True, HITL] | tuple[False, None]":
3032
"""
3133
Factory method to create a HITL instance.
@@ -36,6 +38,8 @@ def create(
3638
position_module: Boolean indicating if the position module is enabled.
3739
camera_module: Boolean indicating if the camera module is enabled.
3840
images_path: Optional path to the images directory for the camera emulator.
41+
json_file_path: Optional path to JSON file containing coordinates for position emulator.
42+
position_update_interval: Interval (seconds) between switching JSON coordinates.
3943
4044
Returns:
4145
Success, HITL instance | None.
@@ -47,7 +51,7 @@ def create(
4751
return True, HITL(cls.__create_key, drone, None, None)
4852

4953
if position_module:
50-
result, position_emulator = PositionEmulator.create(drone)
54+
result, position_emulator = PositionEmulator.create(drone, json_file_path, position_update_interval)
5155
if not result:
5256
return False, None
5357

0 commit comments

Comments
 (0)