Client: Add set_z_to_ground and reset_z_offset command handlers

CopterExpress · Apr 8, 2019 · 43ef706 · 43ef706
1 parent 5f0c78f
commit 43ef706
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 0 deletions.
diff --git a/Drone/client_config.ini b/Drone/client_config.ini
@@ -29,11 +29,13 @@ reach_first_point_time = 8.0
 land_time = 3.0
 x0_common = 0
 y0_common = 0
+z0_common = 0
 
 [PRIVATE]
 id = /hostname
 use_leds = True
 led_pin = 18
 x0 = 0
 y0 = 0
+z0 = 0
 
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
@@ -45,11 +45,13 @@ def load_config(self):
         self.LAND_TIME = self.config.getfloat('COPTERS', 'land_time')
         self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common')
         self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common')
+        self.Z0_COMMON = self.config.getfloat('COPTERS', 'z0_common')
         self.TAKEOFF_CHECK = self.config.getboolean('ANIMATION', 'takeoff_animation_check')
         self.LAND_CHECK = self.config.getboolean('ANIMATION', 'land_animation_check')
         self.FRAME_DELAY = self.config.getfloat('ANIMATION', 'frame_delay')
         self.X0 = self.config.getfloat('PRIVATE', 'x0')
         self.Y0 = self.config.getfloat('PRIVATE', 'y0')
+        self.Z0 = self.config.getfloat('PRIVATE', 'z0')
         self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds')
         self.LED_PIN = self.config.getint('PRIVATE', 'led_pin')
 
@@ -225,6 +227,21 @@ def _command_reset_start(**kwargs):
     client.active_client.load_config()
     print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
 
+@messaging.message_callback("set_z_to_ground")
+def _command_set_z(**kwargs):
+    telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+    client.active_client.config.set('PRIVATE', 'z0', telem.z)
+    client.active_client.rewrite_config()
+    client.active_client.load_config()
+    print ("Set z offset to {:.2f}".format(client.active_client.Z0))
+
+@messaging.message_callback("reset_z_offset")
+def _command_reset_z(**kwargs):
+    client.active_client.config.set('PRIVATE', 'z0', 0)
+    client.active_client.rewrite_config()
+    client.active_client.load_config()
+    print ("Reset z offset to {:.2f}".format(client.active_client.Z0))
+
 
 @messaging.message_callback("update_repo")
 def _command_update_repo(**kwargs):