Client: Add moving and resetting start commands

CopterExpress · Sep 19, 2019 · 2c7f28d · 2c7f28d
1 parent 424f1cb
commit 2c7f28d
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 2 deletions.
diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py
@@ -19,7 +19,6 @@
 # TODO refactor as class
 # TODO separate code for frames transformations (e.g. for gps)
 
-
 def get_id(filepath="animation.csv"):
     global anim_id
     try:
@@ -41,7 +40,6 @@ def get_id(filepath="animation.csv"):
                 print("No animation id in file")
     return anim_id
 
-
 def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
     imported_frames = []
     global anim_id

diff --git a/Drone/copter_client.py b/Drone/copter_client.py
@@ -196,6 +196,36 @@ def _command_test(**kwargs):
     logger.info("logging info test")
     print("stdout test")
 
+@messaging.message_callback("move_start")
+def _command_move_start_to_current_position(**kwargs):
+    # Load animation
+    frames = animation.load_animation(os.path.abspath("animation.csv"),
+                                        x0=client.active_client.X0_COMMON,
+                                        y0=client.active_client.Y0_COMMON,
+                                        )
+    # Correct start and land frames in animation
+    corrected_frames, start_action, start_delay = animation.correct_animation(frames,
+                                        check_takeoff=client.active_client.TAKEOFF_CHECK,
+                                        check_land=client.active_client.LAND_CHECK,
+                                        )
+    x_start = corrected_frames[0]['x']
+    y_start = corrected_frames[0]['y']
+    telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+    client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
+    client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
+    client.active_client.rewrite_config()
+    client.active_client.load_config()
+    print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+
+@messaging.message_callback("reset_start")
+def _command_reset_start(**kwargs):
+    client.active_client.config.set('PRIVATE', 'x0', 0)
+    client.active_client.config.set('PRIVATE', 'y0', 0)
+    client.active_client.rewrite_config()
+    client.active_client.load_config()
+    print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+
+
 @messaging.message_callback("update_repo")
 def _command_update_repo(**kwargs):
     os.system("git reset --hard origin/master")