diff --git a/Drone/config/spec/configspec_client.ini b/Drone/config/spec/configspec_client.ini index 8b595ab..b3c1575 100644 --- a/Drone/config/spec/configspec_client.ini +++ b/Drone/config/spec/configspec_client.ini @@ -17,12 +17,19 @@ log_resources = boolean(default=True) [POSITION WATCHDOG] enabled = boolean(default=True) +log_state = boolean(default=True) # Available options: emergency_land, land, disarm action = string(default=emergency_land) +# Time to get vision position after arm +# No visual position will be checked +# during this time after arming +vision_pose_delay_after_arm = float(default=3.0, min=0) # 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 +# Set 0 to disable vision pose check +vision_pose_timeout = float(default=0.0, min=0) # Max delta between current position and setpoint -position_delta_max = float(default=3.0, min=0) # Set 0 to disable this check +# Set 0 to disable position delta check +position_delta_max = float(default=3.0, min=0) # Time to disarm after action is triggered disarm_timeout = float(default=10.0, min=0) diff --git a/Drone/visual_pose_watchdog.py b/Drone/visual_pose_watchdog.py index ebb9336..4ca07cb 100644 --- a/Drone/visual_pose_watchdog.py +++ b/Drone/visual_pose_watchdog.py @@ -24,6 +24,8 @@ config = ConfigManager() config.load_config_and_spec("config/client.ini") watchdog_is_enabled = config.position_watchdog_enabled +log_state = config.position_watchdog_log_state +vision_pose_delay_after_arm = config.position_watchdog_vision_pose_delay_after_arm visual_pose_timeout = config.position_watchdog_vision_pose_timeout pos_delta_max = config.position_watchdog_position_delta_max watchdog_action = config.position_watchdog_action @@ -63,6 +65,7 @@ setpoint_raw = None setpoint_position = None setpoint_pose = None +arm_start_time = None offboard_start_time = None offboard_disarmed_timeout = 3. @@ -175,56 +178,65 @@ def emergency_land_service(request): return responce def watchdog_callback(event): - global visual_pose_last_timestamp, armed, mode, watchdog_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 + global emergency, local_pose, setpoint_pose, emergency_land_called, log_state + global offboard_start_time, arm_start_time, vision_pose_delay_after_arm 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} | 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: + visual_pose_dt = abs(time.time() - visual_pose_last_timestamp) + if log_state: + logger.info("armed: {} | mode: {} | vis_dt: {:.2f} | pos_delta: {:.2f} | pos_dt: {:.2f} | range: {:.2f} | watchdog_action: {}".format( + armed, mode, visual_pose_dt, pos_delta, pos_dt, laser_range, watchdog_action)) + if mode == 'OFFBOARD': if offboard_start_time is None: offboard_start_time = time.time() - 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 and visual_pose_timeout != 0.) or (pos_delta > pos_delta_max and pos_delta_max != 0.): - action_timestamp = time.time() - if watchdog_action in ['land', 'emergency_land', 'disarm']: + if armed: + if arm_start_time is None: + arm_start_time = time.time() + arm_time = time.time() - arm_start_time + logger.debug('arm time: {}'.format(arm_time)) + if arm_time > vision_pose_delay_after_arm and watchdog_is_enabled: + 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() emergency = True - if watchdog_action == 'land': - logger.info('Visual pose data is too old, copter is armed, landing...') - while mode != "AUTO.LAND": - try: - set_mode(custom_mode='AUTO.LAND') - except rospy.ServiceException as e: - logger.info(e) - if time.time() - action_timestamp > timeout_to_disarm: - break - rate.sleep() - else: - logger.info('Land mode is set') - while armed: - if time.time() - action_timestamp > timeout_to_disarm: + if watchdog_action not in ['land', 'emergency_land', 'disarm']: + watchdog_action = 'land' + if watchdog_action == 'land': + logger.info('Visual pose data is too old, copter is armed, landing...') + while mode != "AUTO.LAND": + try: + set_mode(custom_mode='AUTO.LAND') + except rospy.ServiceException as e: + logger.info(e) + if time.time() - action_timestamp > timeout_to_disarm: + break + rate.sleep() + else: + logger.info('Land mode is set') + while armed: + if time.time() - action_timestamp > timeout_to_disarm: + try: + arming(False) + except rospy.ServiceException as e: + logger.info(e) + rate.sleep() + elif watchdog_action == 'disarm': + logger.info('Visual pose data is too old, copter is armed, disarming...') + while armed: try: arming(False) except rospy.ServiceException as e: logger.info(e) - rate.sleep() - elif watchdog_action == 'disarm': - logger.info('Visual pose data is too old, copter is armed, disarming...') - while armed: - try: - arming(False) - except rospy.ServiceException as e: - logger.info(e) - rate.sleep() - 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: - logger.info('Position delta is {} m, copter is armed, emergency landing...'.format(pos_delta)) - emergency_land() - logger.info('Disarmed') - emergency = False - elif emergency_land_called: + rate.sleep() + 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: + logger.info('Position delta is {} m, copter is armed, emergency landing...'.format(pos_delta)) + emergency_land() + logger.info('Disarmed') + emergency = False + if emergency_land_called: emergency = True logger.info('/emergency_land service was called, start emergency landing...') emergency_land() @@ -232,6 +244,7 @@ def watchdog_callback(event): emergency = False emergency_land_called = False else: + arm_start_time = None if time.time() - offboard_start_time > offboard_disarmed_timeout: try: set_mode(custom_mode='AUTO.LAND')