From eec82af58f9ea06f2d8eb9f36310a045d0e1edd9 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Sun, 24 Nov 2019 07:55:58 +0000
Subject: [PATCH] Client: Add landing if position delta is bigger than value
 from client_config

---
 Drone/FlightLib/FlightLib.py | 14 +++++++++++++-
 Drone/client_config.ini      |  7 ++++---
 Drone/copter_client.py       |  6 ++++++
 3 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index abf8e59..44f9094 100644
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -44,6 +44,7 @@ FLIP_MIN_Z = 2.0
 
 checklist = []
 get_telemetry_lock = threading.Lock()
+delta = 0.0
 
 def get_telemetry_locked(*args, **kwargs):
     with get_telemetry_lock:
@@ -174,12 +175,22 @@ def selfcheck():
 
     return checks
 
+def get_delta():
+    global delta
+    return delta
+
+def reset_delta():
+    global delta
+    delta = 0
 
 def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs):
+    global delta
     set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, auto_arm=auto_arm)
-    #telemetry = get_telemetry_locked(frame_id=frame_id)
+    telemetry = get_telemetry_locked(frame_id=frame_id)
+    delta = get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z)
 
     logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
+    #logger.info('Delta: {}'.format(delta))
     #print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
     ##logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))
     #print('Telemetry now: | z: {:.3f}'.format(telemetry.z))
@@ -272,6 +283,7 @@ def stop(frame_id='body', hold_speed=SPEED):
 
 def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRAME_ID,
          timeout_descend=TIMEOUT_DESCEND, timeout_land=TIMEOUT_LAND, freq=FREQUENCY, interrupter=INTERRUPTER):
+    reset_delta()
     if descend:
         logger.info("Descending to: | z: {:.3f}".format(z))
         #print("Descending to: | z: {:.3f}".format(z))
diff --git a/Drone/client_config.ini b/Drone/client_config.ini
index e2e8249..f95abda 100644
--- a/Drone/client_config.ini
+++ b/Drone/client_config.ini
@@ -24,6 +24,7 @@ timeout_to_disarm_after_watchdog_action = 10.0
 frequency = 1
 transmit = True
 clear_tasks_when_emergency = True
+land_if_pos_delta_bigger_than = 3.0
 log_cpu_and_memory = True
 
 [ANIMATION]
@@ -40,18 +41,18 @@ takeoff_height = 1.0
 takeoff_time = 5.0
 safe_takeoff = False
 reach_first_point_time = 5.0
-land_time = 3.0
+land_time = 1.0
 x0_common = 0
 y0_common = 0
 z0_common = 0
 yaw = 180
-land_timeout = 6.0
+land_timeout = 10.0
 
 [FLOOR FRAME]
 parent = aruco_map
 x = 2.4
 y = 12.4
-z = 6.27
+z = 6.4
 roll = 180
 pitch = 0
 yaw = -90
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 093ecdd..49422d0 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -80,6 +80,7 @@ class CopterClient(client.Client):
         self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit')
         self.CLEAR_TASKS_WHEN_EMERGENCY = self.config.getboolean('TELEMETRY', 'clear_tasks_when_emergency')
         self.LOG_CPU_AND_MEMORY = self.config.getboolean('TELEMETRY', 'log_cpu_and_memory')
+        self.LAND_POS_DELTA = self.config.getfloat('TELEMETRY', 'land_if_pos_delta_bigger_than')
         self.FRAME_ID = self.config.get('COPTERS', 'frame_id')
         self.FRAME_FLIPPED_HEIGHT = 0.
         self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height')
@@ -759,6 +760,7 @@ def telemetry_loop():
                     logger.info("Clear task manager because of {}".format(log_msg))
                     logger.info("Mode: {} | armed: {} | last task: {} ".format(mode, armed, last_task))
                     task_manager.reset()
+                    FlightLib.reset_delta()
                     tasks_cleared = True
                     equal_state_counter = 0
             else:
@@ -780,6 +782,10 @@ def telemetry_loop():
             else:
                 cpu_temp_state = 'normal'
             logger.info("CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format(cpu_usage, mem_usage, cpu_temp, cpu_temp_state, power_state))
+        delta = FlightLib.get_delta()
+        logger.info("Delta: {}".format(delta))
+        if delta > client.active_client.LAND_POS_DELTA:
+            _command_land()
 
         rate.sleep()