diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 804fe05407..7924bac559 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -257,6 +257,7 @@ private: bool _accel_blocked{false}; bool _gyro_blocked{false}; bool _baro_blocked{false}; + bool _baro_stuck{false}; bool _mag_blocked{false}; bool _gps_blocked{false}; bool _airspeed_blocked{false}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 7d959bd974..822803a5f9 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -253,8 +253,14 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor // baro if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) { - _px4_baro_0.update(time, sensors.abs_pressure); - _px4_baro_1.update(time, sensors.abs_pressure); + if (_baro_stuck) { + _px4_baro_0.update(time, _px4_baro_0.get().pressure); + _px4_baro_1.update(time, _px4_baro_1.get().pressure); + + } else { + _px4_baro_0.update(time, sensors.abs_pressure); + _px4_baro_1.update(time, sensors.abs_pressure); + } } // differential pressure @@ -995,6 +1001,11 @@ void Simulator::check_failure_injections() supported = true; _baro_blocked = true; + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + _baro_stuck = true; + _baro_blocked = false; + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { supported = true; _baro_blocked = false; diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index f13effaccd..40949d0d40 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -355,7 +355,35 @@ void AutopilotTester::execute_mission_and_lose_baro() auto progress = _mission->mission_progress(); return progress.current == progress.total; }, std::chrono::seconds(60))); +} +void AutopilotTester::execute_mission_and_get_baro_stuck() +{ + CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); + + _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { + std::cout << "Progress: " << progress.current << "/" << progress.total; + + if (progress.current == 1) { + CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck) + == Failure::Result::Success); + } + }); + + std::promise prom; + auto fut = prom.get_future(); + _mission->start_mission_async([&prom](Mission::Result result) { + REQUIRE(Mission::Result::Success == result); + prom.set_value(); + }); + REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); + + // We except the mission to continue with a stuck baro just fine. + REQUIRE(poll_condition_with_timeout( + [this]() { + auto progress = _mission->mission_progress(); + return progress.current == progress.total; + }, std::chrono::seconds(60))); } CoordinateTransformation AutopilotTester::get_coordinate_transformation() diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 3092512ddb..5b6b476cc0 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -80,6 +80,7 @@ public: void execute_mission_and_lose_gps(); void execute_mission_and_lose_mag(); void execute_mission_and_lose_baro(); + void execute_mission_and_get_baro_stuck(); void execute_rtl(); void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f, std::chrono::seconds timeout_duration = std::chrono::seconds(60)); diff --git a/test/mavsdk_tests/test_multicopter_failsafe.cpp b/test/mavsdk_tests/test_multicopter_failsafe.cpp index 91499e5293..fc2960600a 100644 --- a/test/mavsdk_tests/test_multicopter_failsafe.cpp +++ b/test/mavsdk_tests/test_multicopter_failsafe.cpp @@ -80,3 +80,17 @@ TEST_CASE("Continue on baro lost during mission", "[multicopter][vtol]") tester.execute_mission_and_lose_baro(); tester.wait_until_disarmed(); } + +TEST_CASE("Continue on baro stuck during mission", "[multicopter][vtol]") +{ + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = true; + tester.prepare_square_mission(mission_options); + tester.arm(); + tester.execute_mission_and_get_baro_stuck(); + tester.wait_until_disarmed(); +}