From 43ef7062bd0f35909acd4d416007ff82cf4967ba Mon Sep 17 00:00:00 2001 From: Arthus Golubtsov Date: Mon, 8 Apr 2019 13:00:41 +0100 Subject: [PATCH] Client: Add set_z_to_ground and reset_z_offset command handlers --- Drone/client_config.ini | 2 ++ Drone/copter_client.py | 17 +++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/Drone/client_config.ini b/Drone/client_config.ini index f554f64..923be6d 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -29,6 +29,7 @@ reach_first_point_time = 8.0 land_time = 3.0 x0_common = 0 y0_common = 0 +z0_common = 0 [PRIVATE] id = /hostname @@ -36,4 +37,5 @@ use_leds = True led_pin = 18 x0 = 0 y0 = 0 +z0 = 0 diff --git a/Drone/copter_client.py b/Drone/copter_client.py index ad85d80..4f9424c 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -45,11 +45,13 @@ class CopterClient(client.Client): 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):