Fixed betaflight sitl state port bug

utiasDSL · Oct 12, 2023 · 0da365f · 0da365f
1 parent a8f3c2c
commit 0da365f
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 4 deletions.
diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh
@@ -24,8 +24,10 @@ pattern2="#define PORT_PWM        9002    // Out"
 pattern3="#define PORT_STATE      9003    // In"
 pattern4="#define PORT_RC         9004    // In"
 pattern5="#define BASE_PORT 5760"
+pattern6="ret = udpInit(&stateLink, NULL, 9003, true);"
 
 replacement0="// delayMicroseconds_real(50); // max rate 20kHz"
+replacement6="ret = udpInit(&stateLink, NULL, PORT_STATE, true);"
 
 for ((i = 1; i <= num_iterations; i++)); do
 
@@ -51,6 +53,8 @@ for ((i = 1; i <= num_iterations; i++)); do
     replacement5="#define BASE_PORT 57${i}0"
     sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c
 
+    sed -i "s/$pattern6/$replacement6/g" ./src/main/target/SITL/sitl.c
+
     # Build
     make arm_sdk_install 
     make TARGET=SITL 

diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py
@@ -95,7 +95,7 @@ def run(
     #### Run the simulation ####################################
     with open("../assets/beta.csv", mode='r') as csv_file:
         csv_reader = csv.DictReader(csv_file)
-        trajectory = iter([{
+        trajectory1 = iter([{
             "pos": np.array([
                 float(row["p_x"]),
                 float(row["p_y"]),
@@ -107,6 +107,20 @@ def run(
                 float(row["v_z"]),
             ]),
         } for row in csv_reader])
+    with open("../assets/beta.csv", mode='r') as csv_file:
+        csv_reader = csv.DictReader(csv_file)
+        trajectory2 = iter(reversed([{
+            "pos": np.array([
+                float(row["p_x"]),
+                float(row["p_y"]),
+                float(row["p_z"]),
+            ]),
+            "vel": np.array([
+                float(row["v_x"]),
+                float(row["v_y"]),
+                float(row["v_z"]),
+            ]),
+        } for row in csv_reader]))
     action = np.zeros((NUM_DRONES,4))
     ARM_TIME = 1.
     TRAJ_TIME = 1.5
@@ -119,9 +133,7 @@ def run(
         if t > env.TRAJ_TIME:
             for j in range(NUM_DRONES):
                 try:
-                    target = next(trajectory)
-                    print(target['pos'])
-                    print(target['vel'])
+                    target = next(trajectory1) if j%2 == 0 else next(trajectory2)
                     action[j,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP,
                                                     state=obs[j],
                                                     target_pos=target["pos"]+[INIT_XYZ[j][0], INIT_XYZ[j][1], 0],