Improve offboard failsafe (#18160)

* 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>

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)
 

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,

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
+				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;
 		}

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();

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();

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