mavsdk_tests: add two new tests for VTOLs

- add functionality to specify world name for simulation in case name
- add test for triggering an airspeed invalidation in case of pitot blockage
- add test for high wind (ramped up wind over short period) to NOT invalidate airspeed in this case

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -328,12 +328,14 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 	// differential pressure
 	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_disconnected) {
 
+		const float blockage_fraction = 0.7; // defines max blockage (fully ramped)
+		const float airspeed_blockage_rampup_time = 1_s; // time it takes to go max blockage, linear ramp
+
 		float airspeed_blockage_scale = 1.f;
-		const float airspeed_blockage_rampup_time = 60_s; // time it takes to block the airspeed sensor completely, linear ramp
 
 		if (_airspeed_blocked_timestamp > 0) {
 			airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - _airspeed_blocked_timestamp) /
-						  airspeed_blockage_rampup_time, 0.5f, 1.f);
+						  airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f);
 		}
 
 		differential_pressure_s report{};
@@ -1443,7 +1445,7 @@ void SimulatorMavlink::check_failure_injections()
 				_airspeed_disconnected = true;
 
 			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
-				PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate icing)");
+				PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)");
 				supported = true;
 				_airspeed_blocked_timestamp = hrt_absolute_time();
 

src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake

@@ -112,6 +112,7 @@ if(gazebo_FOUND)
 		empty
 		ksql_airport
 		mcmillan_airfield
+		ramped_up_wind
 		sonoma_raceway
 		warehouse
 		windy

test/mavsdk_tests/CMakeLists.txt

@@ -31,6 +31,8 @@ if(MAVSDK_FOUND)
         test_vtol_mission.cpp
         test_vtol_figure_eight.cpp
         test_vtol_rtl.cpp
+        test_vtol_mission_wind.cpp
+        test_vtol_loiter_airspeed_failure_blockage.cpp
         # test_multicopter_follow_me.cpp
     )
 

test/mavsdk_tests/autopilot_tester.cpp

@@ -164,6 +164,11 @@ void AutopilotTester::set_rc_loss_exception(AutopilotTester::RcLossException mas
 	}
 }
 
+void AutopilotTester::set_param_vt_fwd_thrust_en(int value)
+{
+	CHECK(_param->set_param_int("VT_FWD_THRUST_EN", value) == Param::Result::Success);
+}
+
 void AutopilotTester::arm()
 {
 	const auto result = _action->arm();
@@ -947,3 +952,25 @@ void AutopilotTester::report_speed_factor()
 		std::this_thread::sleep_for(std::chrono::milliseconds(100));
 	}
 }
+
+void AutopilotTester::enable_fixedwing_mectrics()
+{
+	CHECK(getTelemetry()->set_rate_fixedwing_metrics(10.f) == Telemetry::Result::Success);
+}
+
+void AutopilotTester::check_airspeed_is_valid()
+{
+	// If the airspeed was invalidated during the flight, the airspeed is sent in the
+	// telemetry is NAN and stays so with the default parameter settings.
+	const Telemetry::FixedwingMetrics &metrics = getTelemetry()->fixedwing_metrics();
+	REQUIRE(std::isfinite(metrics.airspeed_m_s));
+}
+
+void AutopilotTester::check_airspeed_is_invalid()
+{
+	// If the airspeed was invalidated during the flight, the airspeed is sent in the
+	// telemetry is NAN and stays so with the default parameter settings.
+	const Telemetry::FixedwingMetrics &metrics = getTelemetry()->fixedwing_metrics();
+	std::cout << "Reported airspeed after failure: " << metrics.airspeed_m_s ;
+	REQUIRE(!std::isfinite(metrics.airspeed_m_s));
+}

test/mavsdk_tests/autopilot_tester.h

@@ -110,6 +110,7 @@ public:
 	void set_rtl_altitude(const float altitude_m);
 	void set_height_source(HeightSource height_source);
 	void set_rc_loss_exception(RcLossException mask);
+	void set_param_vt_fwd_thrust_en(int value);
 	void arm();
 	void takeoff();
 	void land();
@@ -151,6 +152,10 @@ public:
 	void send_custom_mavlink_message(mavlink_message_t &message);
 	void add_mavlink_message_callback(uint16_t message_id, std::function< void(const mavlink_message_t &)> callback);
 
+	void enable_fixedwing_mectrics();
+	void check_airspeed_is_valid();
+	void check_airspeed_is_invalid();
+
 	// Blocking call to get the drone's current position in NED frame
 	std::array<float, 3> get_current_position_ned();
 

test/mavsdk_tests/configs/sitl.json

@@ -22,7 +22,7 @@
         {
             "model": "standard_vtol",
             "vehicle": "standard_vtol",
-            "test_filter": "[vtol]",
+            "test_filter": "[vtol], [vtol_wind], [vtol_airspeed_fail]",
             "timeout_min": 10
         },
         {

test/mavsdk_tests/mavsdk_test_runner.py

@@ -7,6 +7,7 @@ import json
 import math
 import os
 import psutil  # type: ignore
+import re
 import signal
 import subprocess
 import sys
@@ -402,6 +403,13 @@ class Tester:
 
         if self.config['mode'] == 'sitl':
             if self.config['simulator'] == 'gazebo':
+                # Use RegEx to extract worldname.world from case name
+                match = re.search(r'\((.*?\.world)\)', case)
+                if match:
+                    world_name = match.group(1)
+                else:
+                    world_name = 'empty.world'
+
                 gzserver_runner = ph.GzserverRunner(
                     os.getcwd(),
                     log_dir,
@@ -409,7 +417,8 @@ class Tester:
                     case,
                     self.get_max_speed_factor(test),
                     self.verbose,
-                    self.build_dir)
+                    self.build_dir,
+                    world_name)
                 self.active_runners.append(gzserver_runner)
 
                 gzmodelspawn_runner = ph.GzmodelspawnRunner(

test/mavsdk_tests/process_helper.py

@@ -221,7 +221,8 @@ class GzserverRunner(Runner):
                  case: str,
                  speed_factor: float,
                  verbose: bool,
-                 build_dir: str):
+                 build_dir: str,
+                 world_name: str):
         super().__init__(log_dir, model, case, verbose)
         self.name = "gzserver"
         self.cwd = workspace_dir
@@ -234,7 +235,7 @@ class GzserverRunner(Runner):
         self.args = ["-o0", "-e0", "gzserver", "--verbose",
                      os.path.join(workspace_dir,
                                   PX4_GAZEBO_WORLDS,
-                                  "empty.world")]
+                                  world_name)]
 
     def has_started_ok(self) -> bool:
         # Wait until gzerver has started and connected to gazebo master.

test/mavsdk_tests/test_vtol_loiter_airspeed_failure_blockage.cpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "autopilot_tester.h"
+#include "autopilot_tester_failure.h"
+
+
+TEST_CASE("Fly VTOL Loiter with airspeed failure", "[vtol_airspeed_fail]")
+{
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+
+	tester.enable_fixedwing_mectrics();
+
+	// configuration
+	const float takeoff_altitude = 10.f;
+	tester.set_takeoff_altitude(takeoff_altitude);
+
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_straight_south.plan");
+	tester.arm();
+
+	tester.takeoff();
+	tester.wait_until_altitude(takeoff_altitude, std::chrono::seconds(30));
+	tester.transition_to_fixedwing();
+
+
+	// tester.wait_until_altitude(50.f, std::chrono::seconds(30));
+	std::this_thread::sleep_for(std::chrono::seconds(10));
+	tester.inject_failure(mavsdk::Failure::FailureUnit::SensorAirspeed, mavsdk::Failure::FailureType::Wrong, 0,
+			      mavsdk::Failure::Result::Success);
+
+
+	std::this_thread::sleep_for(std::chrono::seconds(10));
+
+	tester.check_airspeed_is_invalid(); // it's enough to check once after landing, as invalidation is permanent
+}

test/mavsdk_tests/test_vtol_mission_wind.cpp

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "autopilot_tester.h"
+
+
+TEST_CASE("Fly VTOL mission with wind change (ramped_up_wind.world)", "[vtol_wind]")
+{
+	AutopilotTester tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+
+	tester.set_param_vt_fwd_thrust_en(1); // disable in land to be more robust in wind (less lift)
+	tester.enable_fixedwing_mectrics();
+
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_straight_south.plan");
+	tester.arm();
+	tester.execute_mission_raw();
+	tester.wait_until_disarmed();
+
+	tester.check_airspeed_is_valid(); // it's enough to check once after landing, as invalidation is permanent
+}

test/mavsdk_tests/vtol_mission_straight_south.plan

@@ -0,0 +1,73 @@
+{
+    "UUID": "6c95e3a0dedfb83ac6a4c3711fa182224c31dba9",
+    "fileType": "Plan",
+    "geoFence": {
+        "circles": [
+        ],
+        "polygons": [
+        ],
+        "version": 2
+    },
+    "groundStation": "QGroundControl",
+    "mission": {
+        "cruiseSpeed": 15,
+        "firmwareType": 12,
+        "globalPlanAltitudeMode": 1,
+        "hoverSpeed": 5,
+        "items": [
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "MISSION_ITEM_ID": "1",
+                "autoContinue": true,
+                "command": 84,
+                "doJumpId": 1,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.39712597967924,
+                    8.545720879548579,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "MISSION_ITEM_ID": "2",
+                "autoContinue": true,
+                "command": 21,
+                "doJumpId": 2,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.38497919751799,
+                    8.54578066404548,
+                    30
+                ],
+                "type": "SimpleItem"
+            }
+        ],
+        "plannedHomePosition": [
+            47.3977508,
+            8.5456074,
+            488.145
+        ],
+        "vehicleType": 20,
+        "version": 2
+    },
+    "rallyPoints": {
+        "points": [
+        ],
+        "version": 2
+    },
+    "version": 1
+}