mavsdk_tests: add offboard attitude test & run with Q estimator

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -187,6 +187,11 @@ param set-default SYS_FAILURE_EN 1
 # does not go below 50% by default, but failure injection can trigger failsafes.
 param set-default COM_LOW_BAT_ACT 2
 
+# Allow to override SYS_MC_EST_GROUP via env
+if [ -n "$SYS_MC_EST_GROUP" ]; then
+	param set SYS_MC_EST_GROUP $SYS_MC_EST_GROUP
+fi
+
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
 	COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)

test/mavsdk_tests/autopilot_tester.cpp

@@ -484,6 +484,65 @@ void AutopilotTester::fly_forward_in_altctl()
 		}
 	}
 }
+void AutopilotTester::fly_forward_in_offboard_attitude()
+{
+	// This test does not depend on valid position estimate.
+	// Wait for raw gps & stable attitude estimate
+	CHECK(poll_condition_with_timeout(
+	[this]() {
+		auto attitude = _telemetry->attitude_euler();
+		return _telemetry->raw_gps().altitude_ellipsoid_m > 0.f && fabsf(attitude.roll_deg) < 5.f
+		       && fabsf(attitude.pitch_deg) < 5.f;
+	}, std::chrono::seconds(20)));
+
+	const float start_altitude_ellipsoid_m = _telemetry->raw_gps().altitude_ellipsoid_m;
+
+	Offboard::Attitude attitude{};
+	_offboard->set_attitude(attitude);
+	REQUIRE(_offboard->start() == Offboard::Result::Success);
+
+	// Wait until we can arm
+	CHECK(poll_condition_with_timeout(
+	[this]() {	return _telemetry->health().is_armable;	}, std::chrono::seconds(20)));
+	arm();
+
+	const unsigned offboard_rate_hz = 50;
+
+	// Climb
+	const float climb_altitude_m = 10.f;
+	attitude.thrust_value = 0.8f;
+
+	while (_telemetry->raw_gps().altitude_ellipsoid_m - start_altitude_ellipsoid_m < climb_altitude_m) {
+		CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success);
+		sleep_for(std::chrono::milliseconds(1000 / offboard_rate_hz));
+	}
+
+	// Fly forward for 3s
+	attitude.thrust_value = 0.8f;
+	attitude.pitch_deg = -20.f;
+
+	for (unsigned i = 0; i < 3 * offboard_rate_hz; ++i) {
+		CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success);
+		sleep_for(std::chrono::milliseconds(1000 / offboard_rate_hz));
+	}
+
+	// Check attitude
+	auto attitude_estimate = _telemetry->attitude_euler();
+	CHECK(fabsf(attitude.roll_deg - attitude_estimate.roll_deg) < 5.f);
+	CHECK(fabsf(attitude.pitch_deg - attitude_estimate.pitch_deg) < 5.f);
+
+	// Descend
+	attitude.thrust_value = 0.2f;
+	attitude.pitch_deg = 0.f;
+
+	for (unsigned i = 0; i < 3 * offboard_rate_hz; ++i) {
+		CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success);
+		sleep_for(std::chrono::milliseconds(1000 / offboard_rate_hz));
+	}
+
+	attitude.thrust_value = 0.0f;
+	CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success);
+}
 
 void AutopilotTester::start_checking_altitude(const float max_deviation_m)
 {

test/mavsdk_tests/autopilot_tester.h

@@ -134,6 +134,7 @@ public:
 	void offboard_land();
 	void fly_forward_in_posctl();
 	void fly_forward_in_altctl();
+	void fly_forward_in_offboard_attitude();
 	void request_ground_truth();
 	void check_mission_item_speed_above(int item_index, float min_speed_m_s);
 	void check_tracks_mission(float corridor_radius_m = 1.5f);

test/mavsdk_tests/configs/sitl.json

@@ -7,9 +7,18 @@
         {
             "model": "iris",
             "vehicle": "iris",
-            "test_filter": "[multicopter],[offboard]",
+            "test_filter": "[multicopter],[offboard][offboard_attitude]",
             "timeout_min": 10
         },
+        {
+            "model": "iris",
+            "vehicle": "iris",
+            "test_filter": "[offboard_attitude]",
+            "timeout_min": 10,
+            "env": {
+                "SYS_MC_EST_GROUP": 3
+            }
+        },
         {
             "model": "standard_vtol",
             "vehicle": "standard_vtol",

test/mavsdk_tests/mavsdk_test_runner.py

@@ -444,6 +444,8 @@ class Tester:
                     self.debugger,
                     self.verbose,
                     self.build_dir)
+                for env_key in test.get('env', []):
+                    px4_runner.env[env_key] = str(test['env'][env_key])
                 self.active_runners.append(px4_runner)
 
         mavsdk_tests_runner = ph.TestRunner(

test/mavsdk_tests/test_multicopter_offboard.cpp

@@ -73,3 +73,12 @@ TEST_CASE("Offboard position control", "[multicopter][offboard]")
 	tester.wait_until_disarmed(std::chrono::seconds(120));
 	tester.check_home_within(1.0f);
 }
+
+TEST_CASE("Offboard attitude control", "[multicopter][offboard_attitude]")
+{
+	AutopilotTester tester;
+	tester.connect(connection_url);
+	tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard);
+	tester.fly_forward_in_offboard_attitude();
+	tester.wait_until_disarmed(std::chrono::seconds(120));
+}