Merge branch 'qt-gui-update' of https://github.com/CopterExpress/clever-show into qt-gui-update

Drone/config/spec/configspec_client.ini

@@ -1,5 +1,5 @@
-config_name = string(default='Copter config')
-config_version = float(default=0.0)
+config_name = string(default='client')
+config_version = float(default=1.0)
 
 [SERVER]
 port = integer(default=25000, min=1)
@@ -15,16 +15,20 @@ transmit = boolean(default=True)
 frequency = float(default=1.0, min=0)
 log_resources = boolean(default=True)
 
-[VISUAL POSE WATCHDOG]
-timeout = float(default=1.0, min=0)
-pos_delta_max = float(default=3.0, min=0)
+[POSITION WATCHDOG]
+enabled = boolean(default=True)
 # Available options: emergency_land, land, disarm
 action = string(default=emergency_land)
+# Timeout for the last vision pose in /mavros/vision_pose/pose
+vision_pose_timeout = float(default=1.0, min=0) # Set 0 to disable this check
+# Max delta between current position and setpoint
+position_delta_max = float(default=3.0, min=0) # Set 0 to disable this check
+# Time to disarm after action is triggered
+disarm_timeout = float(default=10.0, min=0)
 
 [EMERGENCY LAND]
 thrust = float(default=0.45, min=0, max=1)
 decrease_thrust_after = float(default=5.0, min=0)
-disarm_timeout = float(default=10.0, min=0)
 
 [COPTER]
 frame_id = string(default=map)

Drone/visual_pose_watchdog.py

@@ -23,12 +23,13 @@ from config import ConfigManager
 config = ConfigManager()
 config.load_config_and_spec("config/client.ini")
 
-visual_pose_timeout = config.visual_pose_watchdog_timeout
-pos_delta_max = config.visual_pose_watchdog_pos_delta_max
-timeout_action = config.visual_pose_watchdog_action
+watchdog_is_enabled = config.position_watchdog_enabled
+visual_pose_timeout = config.position_watchdog_vision_pose_timeout
+pos_delta_max = config.position_watchdog_position_delta_max
+watchdog_action = config.position_watchdog_action
+timeout_to_disarm = config.position_watchdog_disarm_timeout
 emergency_land_thrust = config.emergency_land_thrust
 emergency_land_decrease_thrust_after = config.emergency_land_decrease_thrust_after
-timeout_to_disarm = config.emergency_land_disarm_timeout
 
 logging.basicConfig(  # TODO all prints as logs
    level=logging.DEBUG, # INFO
@@ -69,9 +70,9 @@ emergency_land_called = False
 
 rospy.init_node('visual_pose_watchdog')
 logger.info('visual_pose_watchdog inited')
-logger.info('timeout = {} | timeout_action = {}'.format(visual_pose_timeout, timeout_action))
+logger.info('visual_pose_timeout = {} | position_delta_max = {} | watchdog_action = {}'.format(visual_pose_timeout, pos_delta_max, watchdog_action))
 logger.info('timeout_to_disarm = {}'.format(timeout_to_disarm))
-if timeout_action == 'emergency_land':
+if watchdog_action == 'emergency_land':
     logger.info('emergency_land_thrust: {}'.format(emergency_land_thrust))
 
 rate = rospy.Rate(10)
@@ -174,21 +175,21 @@ def emergency_land_service(request):
     return responce  
 
 def watchdog_callback(event):
-    global visual_pose_last_timestamp, armed, mode, timeout_action, laser_range, emergency, local_pose, setpoint_pose, offboard_start_time, emergency_land_called
+    global visual_pose_last_timestamp, armed, mode, watchdog_action, laser_range, emergency, local_pose, setpoint_pose, offboard_start_time, emergency_land_called
     pos_delta = get_pos_delta(local_pose, setpoint_pose)
     pos_dt = get_time_delta(local_pose, setpoint_pose)
-    logger.debug("armed: {} | mode: {} | viz_dt: {:.2f} | pos_delta: {:.2f} | pos_dt: {:.2f} | action: {} | range: {:.2f}".format(
-                    armed, mode, abs(time.time() - visual_pose_last_timestamp), pos_delta, pos_dt, timeout_action, laser_range))
-    if mode == 'OFFBOARD':
+    logger.debug("armed: {} | mode: {} | viz_dt: {:.2f} | pos_delta: {:.2f} | pos_dt: {:.2f} | range: {:.2f} | watchdog_action: {}".format(
+                    armed, mode, abs(time.time() - visual_pose_last_timestamp), pos_delta, pos_dt, laser_range, watchdog_action))
+    if mode == 'OFFBOARD' and watchdog_is_enabled:
         if offboard_start_time is None:
             offboard_start_time = time.time()
-        if armed:
+        if armed and time.time() - offboard_start_time > visual_pose_timeout:
             visual_pose_dt = abs(time.time() - visual_pose_last_timestamp)
-            if visual_pose_dt > visual_pose_timeout or pos_delta > pos_delta_max:
+            if (visual_pose_dt > visual_pose_timeout and visual_pose_timeout != 0.) or (pos_delta > pos_delta_max and pos_delta_max != 0.):
                 action_timestamp = time.time()
-                if timeout_action in ['land', 'emergency_land', 'disarm']:
+                if watchdog_action in ['land', 'emergency_land', 'disarm']:
                     emergency = True
-                if timeout_action == 'land':
+                if watchdog_action == 'land':
                     logger.info('Visual pose data is too old, copter is armed, landing...')
                     while mode != "AUTO.LAND":
                         try:
@@ -207,7 +208,7 @@ def watchdog_callback(event):
                             except rospy.ServiceException as e:
                                 logger.info(e)   
                         rate.sleep()
-                elif timeout_action == 'disarm':
+                elif watchdog_action == 'disarm':
                     logger.info('Visual pose data is too old, copter is armed, disarming...')
                     while armed:
                         try:
@@ -215,7 +216,7 @@ def watchdog_callback(event):
                         except rospy.ServiceException as e:
                             logger.info(e)   
                         rate.sleep() 
-                elif timeout_action == 'emergency_land':
+                elif watchdog_action == 'emergency_land':
                     if visual_pose_dt > visual_pose_timeout:
                         logger.info('Visual pose data is too old, copter is armed, emergency landing...')
                     if pos_delta > pos_delta_max:
@@ -238,7 +239,7 @@ def watchdog_callback(event):
                     logger.info(e)
     else:
         offboard_start_time = None
-        if abs(time.time() - visual_pose_last_timestamp) > visual_pose_timeout:
+        if (abs(time.time() - visual_pose_last_timestamp) > visual_pose_timeout and visual_pose_timeout != 0.0):
             logger.info('Visual pose data is too old')
 
 rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)

Server/config/spec/configspec_server.ini

@@ -1,5 +1,5 @@
-config_name = string(default='Copter config')
-config_version = float(default='0.0')
+config_name = string(default='server')
+config_version = float(default='1.0')
 
 [SERVER]
     port = integer(default=25000)
@@ -12,27 +12,30 @@ config_version = float(default='0.0')
     [[PRESETS]]
         current = string(default="DEFAULT")
         [[[DEFAULT]]]
-            copter_id = preset_param(default=list(True, 150))
-            git_version = preset_param(default=list(True, 100))
-            config_version = preset_param(default=list(True, 100))
+            copter_id = preset_param(default=list(True, 100))
+            git_version = preset_param(default=list(True, 75))
+            config_version = preset_param(default=list(True, 140))
             animation_id = preset_param(default=list(True, 100))
             battery = preset_param(default=list(True, 100))
             fcu_status = preset_param(default=list(True, 100))
-            calibration_status = preset_param(default=list(True, 100))
+            calibration_status = preset_param(default=list(True, 65))
             mode = preset_param(default=list(True, 100))
-            selfcheck = preset_param(default=list(True, 100))
-            current_position = preset_param(default=list(True, 100))
-            start_position = preset_param(default=list(True, 100))
-            last_task = preset_param(default=list(True, 100))
+            selfcheck = preset_param(default=list(True, 65))
+            current_position = preset_param(default=list(True, 250))
+            start_position = preset_param(default=list(True, 150))
+            last_task = preset_param(default=list(True, 250))
             time_delta = preset_param(default=list(True, 100))
         [[[__many__]]]
             __many__ = preset_param
 
 [CHECKS]
-    battery_min = float(default=50.0, min=0, max=100)
+    check_git_version = boolean(default=True)
+    check_current_position = boolean(default=True)
     # in meters
-    start_pos_delta_max = float(default=1.0, min=0)
+    battery_min = float(default=50.0, min=0, max=100) # Set 0 to disable this check
     # in meters
+    start_pos_delta_max = float(default=1.0, min=0) # Set 0 to disable this check
+    # in seconds
     time_delta_max = float(default=1.0, min=0)
 
 [BROADCAST]

Server/copter_table_models.py

@@ -45,6 +45,8 @@ class ModelChecks:
     battery_min = 50.0
     start_pos_delta_max = 1.0
     time_delta_max = 1.0
+    check_current_pos = True
+    check_git = True
 
     @classmethod
     def column_check(cls, column, pass_context=False):
@@ -74,6 +76,8 @@ class ModelChecks:
 
 @ModelChecks.column_check("git_version")
 def check_ver(item):
+    if not ModelChecks.check_git:
+        return True
     return get_git_version() == item
 
 
@@ -111,6 +115,8 @@ def check_selfcheck(item):
 
 @ModelChecks.column_check("current_position")
 def check_pos(item):
+    if not ModelChecks.check_current_pos:
+        return True
     if item == 'NO_POS':
         return False
     return not math.isnan(item[0])

Server/server_qt.py

@@ -90,6 +90,8 @@ class ExitMsgbox(logging.Handler):
 class ServerQt(Server):
     def load_config(self):
         super().load_config()
+        table.ModelChecks.check_git = self.config.checks_check_git_version
+        table.ModelChecks.check_current_pos = self.config.checks_check_current_position
         table.ModelChecks.battery_min = self.config.checks_battery_min
         table.ModelChecks.start_pos_delta_max = self.config.checks_start_pos_delta_max
         table.ModelChecks.time_delta_max = self.config.checks_time_delta_max