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) diff --git a/Server/config/spec/configspec_server.ini b/Server/config/spec/configspec_server.ini index 8c8e96a..8d1a21a 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,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] diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index f643e32..c063ed6 100644 --- a/Server/copter_table_models.py +++ b/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]) diff --git a/Server/server_qt.py b/Server/server_qt.py index 2cb251b..3c30fb9 100644 --- a/Server/server_qt.py +++ b/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