Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-10 06:27:12 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

Improve offboard failsafe (#18160)

Browse Source 
* state_machine_helper: improve offboard failsafe

* state_machine_helper: add missing parameter to set_link_loss_nav_state

* state_machine_helper: fix no rc and offboard reason

* Fix offboard test by enabling rcl_except

* mavros_test fix offboard_posctl_test with rcl_except

* autopilot_tester make RcLossException bits explicit

Co-authored-by: Julian Oes <julian@oes.ch>

* autopilot_tester change rcl_except to rc_loss_exception

Co-authored-by: Julian Oes <julian@oes.ch>

* autopilot_tester fix rc_loss_exception renaming errors

Co-authored-by: Julian Oes <julian@oes.ch>
This commit is contained in:
benjinne
2021-09-08 12:19:53 -04:00
committed by GitHub
parent 686bcff8a7
commit 06a91ec752
6 changed files with 79 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py
+4
View File
@@ -45,6 +45,7 @@ import rospy
import math
import numpy as np
from geometry_msgs.msg import PoseStamped, Quaternion
from mavros_msgs.msg import ParamValue
from mavros_test_common import MavrosTestCommon
from pymavlink import mavutil
from six.moves import xrange
@@ -163,6 +164,9 @@ class MavrosOffboardPosctlTest(MavrosTestCommon):
10, -1)
self.log_topic_vars()
# exempting failsafe from lost RC to allow offboard
rcl_except = ParamValue(1<<2, 0.0)
self.set_param("COM_RCL_EXCEPT", rcl_except, 5)
self.set_mode("OFFBOARD", 5)
self.set_arm(True, 5)
integrationtests/python_src/px4_it/mavros/mavros_test_common.py
+34 -2
View File
@@ -5,9 +5,9 @@ import unittest
import rospy
import math
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, State, \
from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, ParamValue, State, \
WaypointList
from mavros_msgs.srv import CommandBool, ParamGet, SetMode, WaypointClear, \
from mavros_msgs.srv import CommandBool, ParamGet, ParamSet, SetMode, SetModeRequest, WaypointClear, \
WaypointPush
from pymavlink import mavutil
from sensor_msgs.msg import NavSatFix, Imu
@@ -42,6 +42,7 @@ class MavrosTestCommon(unittest.TestCase):
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/param/set', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
@@ -50,6 +51,7 @@ class MavrosTestCommon(unittest.TestCase):
except rospy.ROSException:
self.fail("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_param_srv = rospy.ServiceProxy('mavros/param/set', ParamSet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
@@ -234,6 +236,36 @@ class MavrosTestCommon(unittest.TestCase):
"failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}".
format(mode, old_mode, timeout)))
def set_param(self, param_id, param_value, timeout):
"""param: PX4 param string, ParamValue, timeout(int): seconds"""
if param_value.integer != 0:
value = param_value.integer
else:
value = param_value.real
rospy.loginfo("setting PX4 parameter: {0} with value {1}".
format(param_id, value))
loop_freq = 1 # Hz
rate = rospy.Rate(loop_freq)
param_set = False
for i in xrange(timeout * loop_freq):
try:
res = self.set_param_srv(param_id, param_value)
if res.success:
rospy.loginfo("param {0} set to {1} | seconds: {2} of {3}".
format(param_id, value, i / loop_freq, timeout))
break
except rospy.ServiceException as e:
rospy.logerr(e)
try:
rate.sleep()
except rospy.ROSException as e:
self.fail(e)
self.assertTrue(res.success, (
"failed to set param | param_id: {0}, param_value: {1} | timeout(seconds): {2}".
format(param_id, value, timeout)))
def wait_for_topics(self, timeout):
"""wait for simulation to be ready, make sure we're getting topic info
from all topics by checking dictionary of flag values set in callbacks,