Client: Add log_state and vision_pose_delay_after_arm parameters for position_watchdog

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)
 

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')