diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py index c99a649d90..836b0da3b6 100755 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py @@ -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) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py index ef334e1f38..642d669335 100644 --- a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py @@ -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, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6cadb0d1b0..0c56e73c92 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -749,17 +749,30 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ case commander_state_s::MAIN_STATE_OFFBOARD: if (status_flags.offboard_control_signal_lost) { - if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) { + if (status.rc_signal_lost) { // Offboard and RC are lost enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard); set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); } else { // Offboard is lost, RC is ok - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); - set_offboard_loss_rc_nav_state(status, armed, status_flags, offb_loss_rc_act); + if (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); + set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); + + } else { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); + set_offboard_loss_rc_nav_state(status, armed, status_flags, offb_loss_rc_act); + + } + } + } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) { + // Only RC is lost + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); + } else { status.nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index a80f4d9334..8ec38394b6 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -143,6 +143,22 @@ void AutopilotTester::set_height_source(AutopilotTester::HeightSource height_sou } } +void AutopilotTester::set_rc_loss_exception(AutopilotTester::RcLossException mask) +{ + switch (mask) { + case RcLossException::Mission: + CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 0) == Param::Result::Success); + break; + + case RcLossException::Hold: + CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 1) == Param::Result::Success); + break; + + case RcLossException::Offboard: + CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 2) == Param::Result::Success); + } +} + void AutopilotTester::arm() { const auto result = _action->arm(); diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 6da29a3944..680fd42aca 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -85,6 +85,12 @@ public: Gps }; + enum class RcLossException { + Mission = 0, + Hold = 1, + Offboard = 2 + }; + AutopilotTester(); ~AutopilotTester(); @@ -96,6 +102,7 @@ public: void check_home_not_within(float min_distance_m); void set_takeoff_altitude(const float altitude_m); void set_height_source(HeightSource height_source); + void set_rc_loss_exception(RcLossException mask); void arm(); void takeoff(); void land(); diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index 0900bffab2..b8984d4207 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -42,6 +42,7 @@ TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]") tester.connect(connection_url); tester.wait_until_ready(); tester.store_home(); + tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard); tester.arm(); std::chrono::seconds goto_timeout = std::chrono::seconds(90); tester.offboard_goto(takeoff_position, 0.1f, goto_timeout); @@ -60,6 +61,7 @@ TEST_CASE("Offboard position control", "[multicopter][offboard]") tester.connect(connection_url); tester.wait_until_ready(); tester.store_home(); + tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard); tester.arm(); std::chrono::seconds goto_timeout = std::chrono::seconds(120); tester.offboard_goto(takeoff_position, 0.1f, goto_timeout);