Merge pull request #599 from IMRCLab/fix-587

fix planner api change sitl
IMRCLab · Nov 27, 2024 · b896e6d · b896e6d
2 parents 5e731c9 + f7aba61
commit b896e6d
Showing 1 changed file with 16 additions and 5 deletions.
diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_sil.py b/crazyflie_sim/crazyflie_sim/crazyflie_sil.py
@@ -132,11 +132,22 @@ def goTo(self, goal, yaw, duration, relative=False, groupMask=0):
                 # We need to update to the latest firmware that has go_to_from.
                 raise ValueError('goTo from low-level modes not yet supported.')
             self.mode = CrazyflieSIL.MODE_HIGH_POLY
-            firm.plan_go_to(
-                self.planner,
-                relative,
-                firm.mkvec(*goal),
-                yaw, duration, self.time_func())
+            try:
+                firm.plan_go_to(
+                    self.planner,
+                    relative,
+                    False,
+                    firm.mkvec(*goal),
+                    yaw, duration, self.time_func())
+            except TypeError:
+                message = ('Warning: Your Crazyflie firmware is outdated. '
+                           ' Please update to the latest version.')
+                print(message)
+                firm.plan_go_to(
+                    self.planner,
+                    relative,
+                    firm.mkvec(*goal),
+                    yaw, duration, self.time_func())
 
     def uploadTrajectory(self,
                          trajectoryId: int,