Update

Control/Jetson/main.py

@@ -230,7 +230,7 @@ def queue_executor_thread():
                 executor_logger.warning("DO_DROPOFF_CARGO flag is false, skipping dropping off cargo")
 
             if DO_REVERSE_TURN:
-                reverse_turn = reverse_turn(FORKLIFT)
+                reverse_turn = __reverse_turn(FORKLIFT)
                 if not reverse_turn:
                     executor_logger.critical("Problem with reverse turn, check logs !")
                     FORKLIFT.set_state("OTHER_ERROR")

Control/Pi/dronekit_class.py

@@ -363,12 +363,12 @@ class DroneInterface:
         if self.verify_state_for("NAVIGATING_TO_POINT_GPS"):
             approaching = True
             while approaching:
+                current_heading = self.get_heading()
                 if target_heading - 7 <= current_heading <= target_heading + 7:
                     approaching = False
                     self.log.info(f"Heading {target_heading} reached")
                     self.override_stop()
                     return True
-                current_heading = self.get_heading()
                 error = target_heading - current_heading
                 self.log.error(f"Angle: {error} deg.")
                 if error > 180:

Control/Pi/pallet_proximity_sensor.py

@@ -24,7 +24,7 @@ class PalletProximity:
             self.ser.flushInput()
             # time.sleep(0.05)
             readings = 0
-            for _ in range(2):
+            for _ in range(3):
                 raw = str(self.ser.readline())
                 raw2 = int(raw.split("|")[1])
                 readings += raw2