From 0da40e19e7ee75bfb9b84983a278d28e14328d0c Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 17 Feb 2020 17:12:04 +0300 Subject: [PATCH 1/5] Server: Update config specs --- Server/config/spec/configspec_server.ini | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Server/config/spec/configspec_server.ini b/Server/config/spec/configspec_server.ini index 8c8e96a..99c1bc8 100644 --- a/Server/config/spec/configspec_server.ini +++ b/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,18 +12,18 @@ 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 From d87ea4c3899a3d5097d46212e36ede43a54df3bd Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 17 Feb 2020 17:40:30 +0300 Subject: [PATCH 2/5] Server: Add check_current_position parameter to config --- Server/config/spec/configspec_server.ini | 4 +++- Server/copter_table_models.py | 3 +++ Server/server_qt.py | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/Server/config/spec/configspec_server.ini b/Server/config/spec/configspec_server.ini index 99c1bc8..8798f79 100644 --- a/Server/config/spec/configspec_server.ini +++ b/Server/config/spec/configspec_server.ini @@ -29,10 +29,12 @@ config_version = float(default='1.0') __many__ = preset_param [CHECKS] + # in meters battery_min = float(default=50.0, min=0, max=100) + check_current_position = boolean(default=True) # in meters start_pos_delta_max = float(default=1.0, min=0) - # in meters + # in seconds time_delta_max = float(default=1.0, min=0) [BROADCAST] diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index f643e32..90fdf3b 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -45,6 +45,7 @@ class ModelChecks: battery_min = 50.0 start_pos_delta_max = 1.0 time_delta_max = 1.0 + check_current_pos = True @classmethod def column_check(cls, column, pass_context=False): @@ -111,6 +112,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]) diff --git a/Server/server_qt.py b/Server/server_qt.py index 2cb251b..b013b33 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -91,6 +91,7 @@ class ServerQt(Server): def load_config(self): super().load_config() table.ModelChecks.battery_min = self.config.checks_battery_min + table.ModelChecks.check_current_pos = self.config.checks_check_current_position table.ModelChecks.start_pos_delta_max = self.config.checks_start_pos_delta_max table.ModelChecks.time_delta_max = self.config.checks_time_delta_max From 07456bd4148905afb2a3a2f06dcfcfa9921a906b Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 17 Feb 2020 17:52:52 +0300 Subject: [PATCH 3/5] Server: Add check_git_version parameter --- Server/config/spec/configspec_server.ini | 3 ++- Server/copter_table_models.py | 3 +++ Server/server_qt.py | 3 ++- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/Server/config/spec/configspec_server.ini b/Server/config/spec/configspec_server.ini index 8798f79..185126d 100644 --- a/Server/config/spec/configspec_server.ini +++ b/Server/config/spec/configspec_server.ini @@ -29,9 +29,10 @@ config_version = float(default='1.0') __many__ = preset_param [CHECKS] + check_git_version = boolean(default=True) + check_current_position = boolean(default=True) # in meters battery_min = float(default=50.0, min=0, max=100) - check_current_position = boolean(default=True) # in meters start_pos_delta_max = float(default=1.0, min=0) # in seconds diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 90fdf3b..c063ed6 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -46,6 +46,7 @@ class ModelChecks: 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): @@ -75,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 diff --git a/Server/server_qt.py b/Server/server_qt.py index b013b33..3c30fb9 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -90,8 +90,9 @@ class ExitMsgbox(logging.Handler): class ServerQt(Server): def load_config(self): super().load_config() - table.ModelChecks.battery_min = self.config.checks_battery_min + 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 From 3f518dd6bd2de0d0ea45b192653d8e990fe1918c Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 17 Feb 2020 18:24:40 +0300 Subject: [PATCH 4/5] Server: Add comments to configspec --- Server/config/spec/configspec_server.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Server/config/spec/configspec_server.ini b/Server/config/spec/configspec_server.ini index 185126d..8d1a21a 100644 --- a/Server/config/spec/configspec_server.ini +++ b/Server/config/spec/configspec_server.ini @@ -32,9 +32,9 @@ config_version = float(default='1.0') check_git_version = boolean(default=True) check_current_position = boolean(default=True) # in meters - battery_min = float(default=50.0, min=0, max=100) + 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) + 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) From c114197761712060eac2274e1020b760e27d161c Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 17 Feb 2020 16:41:07 +0000 Subject: [PATCH 5/5] Client: Rework position watchdog parameters --- Drone/config/spec/configspec_client.ini | 16 ++++++----- Drone/visual_pose_watchdog.py | 35 +++++++++++++------------ 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/Drone/config/spec/configspec_client.ini b/Drone/config/spec/configspec_client.ini index 0a6f063..8b595ab 100644 --- a/Drone/config/spec/configspec_client.ini +++ b/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) diff --git a/Drone/visual_pose_watchdog.py b/Drone/visual_pose_watchdog.py index db206a0..ebb9336 100644 --- a/Drone/visual_pose_watchdog.py +++ b/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)