From 06f69cd469695bd46967d7338818570ab15fe010 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Fri, 26 Nov 2021 16:52:14 +0100 Subject: [PATCH] Add SITL tests for control allocation --- .../6012_typhoon_h480_ctrlalloc.post | 2 - test/mavsdk_tests/CMakeLists.txt | 3 + test/mavsdk_tests/autopilot_tester.cpp | 71 +++++ test/mavsdk_tests/autopilot_tester.h | 17 +- .../mavsdk_tests/autopilot_tester_failure.cpp | 80 +++++ test/mavsdk_tests/autopilot_tester_failure.h | 61 ++++ test/mavsdk_tests/configs/sitl.json | 6 + .../test_multicopter_control_allocation.cpp | 300 ++++++++++++++++++ .../test_multicopter_failure_injection.cpp | 61 ++++ 9 files changed, 597 insertions(+), 4 deletions(-) create mode 100644 test/mavsdk_tests/autopilot_tester_failure.cpp create mode 100644 test/mavsdk_tests/autopilot_tester_failure.h create mode 100644 test/mavsdk_tests/test_multicopter_control_allocation.cpp create mode 100644 test/mavsdk_tests/test_multicopter_failure_injection.cpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post index 0af01438b7..13788654eb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post @@ -1,6 +1,4 @@ -mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix - mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p # shellcheck disable=SC2154 diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 8f96ab644c..01bdf58727 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -16,6 +16,9 @@ if(MAVSDK_FOUND) add_executable(mavsdk_tests test_main.cpp autopilot_tester.cpp + autopilot_tester_failure.cpp + test_multicopter_control_allocation.cpp + test_multicopter_failure_injection.cpp test_multicopter_failsafe.cpp test_multicopter_mission.cpp test_multicopter_offboard.cpp diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 670ef26268..8f977451d2 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -131,6 +131,14 @@ void AutopilotTester::set_takeoff_altitude(const float altitude_m) CHECK(result.second == Approx(altitude_m)); } +void AutopilotTester::set_rtl_altitude(const float altitude_m) +{ + CHECK(Action::Result::Success == _action->set_return_to_launch_altitude(altitude_m)); + const auto result = _action->get_return_to_launch_altitude(); + CHECK(result.first == Action::Result::Success); + CHECK(result.second == Approx(altitude_m)); +} + void AutopilotTester::set_height_source(AutopilotTester::HeightSource height_source) { switch (height_source) { @@ -200,6 +208,21 @@ void AutopilotTester::wait_until_hovering() wait_for_landed_state(Telemetry::LandedState::InAir, std::chrono::seconds(30)); } +void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout) +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _telemetry->subscribe_position([&prom, rel_altitude_m, this](Telemetry::Position new_position) { + if (fabs(rel_altitude_m - new_position.relative_altitude_m) <= 0.5) { + _telemetry->subscribe_position(nullptr); + prom.set_value(); + } + }); + + REQUIRE(fut.wait_for(timeout) == std::future_status::ready); +} + void AutopilotTester::prepare_square_mission(MissionOptions mission_options) { const auto ct = get_coordinate_transformation(); @@ -367,6 +390,11 @@ void AutopilotTester::execute_rtl() REQUIRE(Action::Result::Success == _action->return_to_launch()); } +void AutopilotTester::execute_land() +{ + REQUIRE(Action::Result::Success == _action->land()); +} + void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m, std::chrono::seconds timeout_duration) { @@ -461,6 +489,22 @@ void AutopilotTester::fly_forward_in_altctl() } } +void AutopilotTester::start_checking_altitude(const float max_deviation_m) +{ + std::array initial_position = get_current_position_ned(); + float target_altitude = initial_position[2]; + + _telemetry->subscribe_position([target_altitude, max_deviation_m, this](Telemetry::Position new_position) { + const float current_deviation = fabs((-target_altitude) - new_position.relative_altitude_m); + CHECK(current_deviation <= max_deviation_m); + }); +} + +void AutopilotTester::stop_checking_altitude() +{ + _telemetry->subscribe_position(nullptr); +} + void AutopilotTester::check_tracks_mission(float corridor_radius_m) { auto mission = _mission->download_mission(); @@ -490,6 +534,12 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m) }); } +std::array AutopilotTester::get_current_position_ned() +{ + mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned(); + std::array position_ned{position_velocity_ned.position.north_m, position_velocity_ned.position.east_m, position_velocity_ned.position.down_m}; + return position_ned; +} void AutopilotTester::offboard_land() { @@ -640,6 +690,27 @@ void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state, REQUIRE(fut.wait_for(timeout) == std::future_status::ready); } +void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seconds timeout) +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _telemetry->subscribe_position_velocity_ned([&prom, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) { + std::array current_velocity; + current_velocity[0] = position_velocity_ned.velocity.north_m_s; + current_velocity[1] = position_velocity_ned.velocity.east_m_s; + current_velocity[2] = position_velocity_ned.velocity.down_m_s; + const float current_speed = norm(current_velocity); + + if (current_speed <= speed) { + _telemetry->subscribe_position_velocity_ned(nullptr); + prom.set_value(); + } + }); + + REQUIRE(fut.wait_for(timeout) == std::future_status::ready); +} + void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout) { auto prom = std::promise {}; diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 680fd42aca..42102c4c30 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2021 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 @@ -101,6 +101,7 @@ public: void check_home_within(float acceptance_radius_m); void check_home_not_within(float min_distance_m); void set_takeoff_altitude(const float altitude_m); + void set_rtl_altitude(const float altitude_m); void set_height_source(HeightSource height_source); void set_rc_loss_exception(RcLossException mask); void arm(); @@ -109,7 +110,9 @@ public: void transition_to_fixedwing(); void transition_to_multicopter(); void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(60)); - void wait_until_hovering(); + void wait_until_hovering(); // TODO: name suggests, that function waits for drone velocity to be zero and not just drone in the air + void wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout); + void wait_until_speed_lower_than(float speed, std::chrono::seconds timeout); void prepare_square_mission(MissionOptions mission_options); void prepare_straight_mission(MissionOptions mission_options); void execute_mission(); @@ -121,6 +124,7 @@ public: void load_qgc_mission_raw_and_move_here(const std::string &plan_file); void execute_mission_raw(); void execute_rtl(); + void execute_land(); void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f, std::chrono::seconds timeout_duration = std::chrono::seconds(60)); void offboard_land(); @@ -129,7 +133,16 @@ public: 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); + void start_checking_altitude(const float max_deviation_m); + void stop_checking_altitude(); + // Blocking call to get the drone's current position in NED frame + std::array get_current_position_ned(); + +protected: + mavsdk::Param *getParams() const { return _param.get();} + mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();} + std::shared_ptr get_system() { return _mavsdk.systems().at(0);} private: mavsdk::geometry::CoordinateTransformation get_coordinate_transformation(); diff --git a/test/mavsdk_tests/autopilot_tester_failure.cpp b/test/mavsdk_tests/autopilot_tester_failure.cpp new file mode 100644 index 0000000000..0737dea951 --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_failure.cpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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_failure.h" + +#include "math_helpers.h" +#include +#include +#include +#include + + +void AutopilotTesterFailure::connect(const std::string uri) +{ + AutopilotTester::connect(uri); + + auto system = get_system(); + _failure.reset(new Failure(system)); +} + +void AutopilotTesterFailure::set_param_sys_failure_en(bool value) +{ + CHECK(getParams()->set_param_int("SYS_FAILURE_EN", value) == Param::Result::Success); +} + +void AutopilotTesterFailure::set_param_fd_act_en(bool value) +{ + CHECK(getParams()->set_param_int("FD_ACT_EN", value) == Param::Result::Success); +} + +void AutopilotTesterFailure::set_param_mc_airmode(int value) +{ + CHECK(getParams()->set_param_int("MC_AIRMODE", value) == Param::Result::Success); +} + +void AutopilotTesterFailure::set_param_ca_failure_mode(int value) +{ + CHECK(getParams()->set_param_int("CA_FAILURE_MODE", value) == Param::Result::Success); +} + +void AutopilotTesterFailure::set_param_com_act_fail_act(int value) +{ + CHECK(getParams()->set_param_int("COM_ACT_FAIL_ACT", value) == Param::Result::Success); +} + +void AutopilotTesterFailure::inject_failure(mavsdk::Failure::FailureUnit failure_unit, + mavsdk::Failure::FailureType failure_type, int instance, mavsdk::Failure::Result expected_result) +{ + CHECK(_failure->inject(failure_unit, failure_type, instance) == expected_result); +} diff --git a/test/mavsdk_tests/autopilot_tester_failure.h b/test/mavsdk_tests/autopilot_tester_failure.h new file mode 100644 index 0000000000..f0c4f3df6c --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_failure.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#pragma once + +#include "autopilot_tester.h" + +#include +#include +#include + + +class AutopilotTesterFailure : public AutopilotTester +{ +public: + AutopilotTesterFailure() = default; + ~AutopilotTesterFailure() = default; + + void set_param_sys_failure_en(bool value); + void set_param_fd_act_en(bool value); + void set_param_mc_airmode(int value); + void set_param_ca_failure_mode(int value); + void set_param_com_act_fail_act(int value); + + void inject_failure(mavsdk::Failure::FailureUnit failure_unit, mavsdk::Failure::FailureType failure_type, int instance, + mavsdk::Failure::Result expected_result); + void connect(const std::string uri); + +private: + std::unique_ptr _failure{}; +}; diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index 50b7eeadbc..50e835a5f5 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -21,6 +21,12 @@ "vehicle": "tailsitter", "test_filter": "[vtol]", "timeout_min": 10 + }, + { + "model": "typhoon_h480_ctrlalloc", + "vehicle": "typhoon_h480_ctrlalloc", + "test_filter": "[controlallocation]", + "timeout_min": 10 } ] } diff --git a/test/mavsdk_tests/test_multicopter_control_allocation.cpp b/test/mavsdk_tests/test_multicopter_control_allocation.cpp new file mode 100644 index 0000000000..e23439536f --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_control_allocation.cpp @@ -0,0 +1,300 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 +#include +#include + +#include "autopilot_tester_failure.h" + +TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]") +{ + const float flight_altitude = 10.0f; + const float altitude_tolerance = 4.0f; + const float hover_speed_tolerance = 1.0f; + + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = false; + mission_options.relative_altitude_m = flight_altitude; + + AutopilotTesterFailure tester; + tester.connect(connection_url); + tester.wait_until_ready(); + + // Configuration + tester.set_param_sys_failure_en(true); // Enable failure injection + tester.set_param_fd_act_en(true); // Enable motor failure detection + tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure + tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor + tester.prepare_square_mission(mission_options); + tester.set_takeoff_altitude(flight_altitude); + tester.set_rtl_altitude(flight_altitude); + tester.check_tracks_mission(5.f); + tester.store_home(); + std::this_thread::sleep_for(std::chrono::seconds( + 1)); // This is necessary for the takeoff altitude to be applied properly + + // Takeoff + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30)); + tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30)); + + // Motor failure mid-air + tester.start_checking_altitude(altitude_tolerance); + const int motor_instance = 1; + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance, + mavsdk::Failure::Result::Success); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + tester.execute_mission(); + tester.stop_checking_altitude(); + + // RTL + tester.execute_rtl(); + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); + tester.wait_until_disarmed(until_disarmed_timeout); + tester.check_home_within(5.f); +} + +TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]") +{ + const float flight_altitude = 10.0f; + const float altitude_tolerance = 4.0f; + const float hover_speed_tolerance = 1.0f; + + AutopilotTesterFailure tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.set_param_sys_failure_en(true); // Enable failure injection + tester.set_param_fd_act_en(true); // Enable motor failure detection + tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure + tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor + + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = false; + mission_options.relative_altitude_m = flight_altitude; + + tester.prepare_square_mission(mission_options); + tester.set_takeoff_altitude(flight_altitude); + tester.set_rtl_altitude(flight_altitude); + tester.check_tracks_mission(5.f); + tester.store_home(); + std::this_thread::sleep_for(std::chrono::seconds( + 1)); // This is necessary for the takeoff altitude to be applied properly + + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30)); + tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30)); + + // Remove two motors opposite of one another on the hexa airframe + const int first_motor_instance = 1; + const int second_motor_instance = 2; + tester.start_checking_altitude(altitude_tolerance); + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, + first_motor_instance, + mavsdk::Failure::Result::Success); + std::this_thread::sleep_for(std::chrono::seconds(1)); + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, + second_motor_instance, + mavsdk::Failure::Result::Success); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + tester.execute_mission(); + tester.stop_checking_altitude(); + + // RTL with two motors out won't work because navigator will wait forever until + // the yaw setpoint is reached during RTL, and it won't land. + tester.land(); + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); + tester.wait_until_disarmed(until_disarmed_timeout); +} + +TEST_CASE("Control Allocation - Remove and restore every motor once", "[controlallocation]") +{ + const float flight_altitude = 10.0f; + const float altitude_tolerance = 4.0f; + const float hover_speed_tolerance = 1.0f; + + AutopilotTesterFailure tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.set_param_sys_failure_en(true); // Enable failure injection + tester.set_param_fd_act_en(true); // Enable motor failure detection + tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure + tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor + + AutopilotTester::MissionOptions mission_options; + mission_options.rtl_at_end = false; + mission_options.relative_altitude_m = flight_altitude; + + tester.prepare_square_mission(mission_options); + tester.set_takeoff_altitude(flight_altitude); + tester.set_rtl_altitude(flight_altitude); + tester.check_tracks_mission(5.f); + tester.store_home(); + std::this_thread::sleep_for(std::chrono::seconds( + 1)); // This is necessary for the takeoff altitude to be applied properly + + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30)); + tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30)); + + tester.start_checking_altitude(altitude_tolerance); + + for (int m = 1; m <= 6; m++) { + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, m, + mavsdk::Failure::Result::Success); + std::this_thread::sleep_for(std::chrono::seconds(1)); + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Ok, m, + mavsdk::Failure::Result::Success); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + tester.execute_mission(); + tester.stop_checking_altitude(); + + tester.execute_rtl(); + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); + tester.wait_until_disarmed(until_disarmed_timeout); + tester.check_home_within(5.f); +} + +TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocation]") +{ + const float flight_altitude = 10.0f; + const float hover_speed_tolerance = 1.0f; + + AutopilotTesterFailure tester; + tester.connect(connection_url); + tester.wait_until_ready(); + + // Configuration + tester.set_param_sys_failure_en(true); // Enable failure injection + tester.set_param_fd_act_en(true); // Enable motor failure detection + tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure + tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor + tester.set_param_com_act_fail_act(3); // RTL on motor failure + tester.set_takeoff_altitude(flight_altitude); + tester.store_home(); + std::this_thread::sleep_for(std::chrono::seconds( + 1)); // This is necessary for the takeoff altitude to be applied properly + + // Takeoff + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30)); + tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30)); + + // TODO: Minor improvement, fly forward for a little bit before triggering motor failure to distinguish "RTL" and "Land only" + + // Motor failure mid-air + const int motor_instance = 1; + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance, + mavsdk::Failure::Result::Success); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Wait for RTL to trigger automatically + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); + tester.wait_until_disarmed(until_disarmed_timeout); + tester.check_home_within(5.f); +} + +TEST_CASE("Control Allocation - Terminate on motor failure", "[controlallocation]") +{ + const float flight_altitude = 100.0f; + const float hover_speed_tolerance = 1.0f; + + AutopilotTesterFailure tester; + tester.connect(connection_url); + tester.wait_until_ready(); + + // Configuration + tester.set_param_sys_failure_en(true); // Enable failure injection + tester.set_param_fd_act_en(true); // Enable motor failure detection + tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure + tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor + tester.set_param_com_act_fail_act(4); // Terminate on motor failure + tester.set_takeoff_altitude(flight_altitude); + std::this_thread::sleep_for(std::chrono::seconds( + 1)); // This is necessary for the takeoff altitude to be applied properly + + // Takeoff + tester.arm(); + tester.takeoff(); + tester.wait_until_altitude(flight_altitude, std::chrono::seconds(60)); + tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(60)); + + // Motor failure mid-air + const int motor_instance = 1; + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance, + mavsdk::Failure::Result::Success); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Wait for disarm with a low enough timeout such that it's necessary for the + // drone to freefall (terminate) in order to disarm quickly enough: + // h = g/2 * t^2 -> solve for t + const int seconds_to_touchdown = 2 + sqrt(flight_altitude * 2 / 10.0); + std::cout << "seconds_to_touchdown: " << seconds_to_touchdown << std::endl; + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(seconds_to_touchdown); + tester.wait_until_disarmed(until_disarmed_timeout); +} + +#if 0 +// This is for checking that the SITL test is actually capable of detecting the drone crash +// when not reallocating the control allocation on a motor failure +TEST_CASE("Control Allocation - Remove two motors and expect crash", "[controlallocation]") +{ + // TODO +} +#endif + +#if 0 +TEST_CASE("Control Allocation with multiple sequential motor failures", "[controlallocation]") +{ + // TODO +} +#endif + +#if 0 +TEST_CASE("Control Allocation with multiple simultaneous motor failures", "[controlallocation]") +{ + // TODO +} +#endif diff --git a/test/mavsdk_tests/test_multicopter_failure_injection.cpp b/test/mavsdk_tests/test_multicopter_failure_injection.cpp new file mode 100644 index 0000000000..f2337ceee2 --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_failure_injection.cpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 +#include + +#include "autopilot_tester_failure.h" + +TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopter]") +{ + AutopilotTesterFailure tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, 1, + mavsdk::Failure::Result::Disabled); + tester.execute_rtl(); + std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); + tester.wait_until_disarmed(until_disarmed_timeout); +} + +TEST_CASE("Failure Injection - Reject before arming", "[multicopter]") +{ + AutopilotTesterFailure tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, 1, + mavsdk::Failure::Result::Disabled); +}