diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 12dc4a9ff4..20eed499b5 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/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(); diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 37e4014d20..b67fe546cc 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/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 diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 502e5e4741..2049f0d0a6 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/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 ) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index ebaf861dfc..0c8e774c41 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/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)); +} diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 9e3c7dd0f7..2586fd213e 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/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 get_current_position_ned(); diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index 791f3d6766..f2bf6e74b1 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/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 }, { diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 5b4ce39b5e..785806d308 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/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( diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index fa6bc95b4e..5c8701e39d 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/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. diff --git a/test/mavsdk_tests/test_vtol_loiter_airspeed_failure_blockage.cpp b/test/mavsdk_tests/test_vtol_loiter_airspeed_failure_blockage.cpp new file mode 100644 index 0000000000..1b8266d72b --- /dev/null +++ b/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 +} diff --git a/test/mavsdk_tests/test_vtol_mission_wind.cpp b/test/mavsdk_tests/test_vtol_mission_wind.cpp new file mode 100644 index 0000000000..f7df5035b6 --- /dev/null +++ b/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 +} diff --git a/test/mavsdk_tests/vtol_mission_straight_south.plan b/test/mavsdk_tests/vtol_mission_straight_south.plan new file mode 100644 index 0000000000..38e4edcc51 --- /dev/null +++ b/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 +}