dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add SITL tests for control allocation

This commit is contained in:
Alessandro Simovic 2021-11-26 16:52:14 +01:00 committed by Beat Küng
parent 0f860045f7
commit 06f69cd469
9 changed files with 597 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post
View File

@ -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

3
test/mavsdk_tests/CMakeLists.txt
View File

@ -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

71
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -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<void> {};
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<float, 3> 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<float, 3> AutopilotTester::get_current_position_ned()
{
mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
std::array<float, 3> 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<void> {};
auto fut = prom.get_future();
_telemetry->subscribe_position_velocity_ned([&prom, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) {
std::array<float, 3> 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<void> {};

17
test/mavsdk_tests/autopilot_tester.h
View File

@ -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<float, 3> get_current_position_ned();
protected:
mavsdk::Param *getParams() const { return _param.get();}
mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
private:
mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();

80
test/mavsdk_tests/autopilot_tester_failure.cpp Normal file
View File

@ -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 <iostream>
#include <future>
#include <thread>
#include <unistd.h>
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);
}

61
test/mavsdk_tests/autopilot_tester_failure.h Normal file
View File

@ -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 <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/failure/failure.h>
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<mavsdk::Failure> _failure{};
};

6
test/mavsdk_tests/configs/sitl.json
View File

@ -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
}
]
}

300
test/mavsdk_tests/test_multicopter_control_allocation.cpp Normal file
View File

@ -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 <thread>
#include <chrono>
#include <math.h>
#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

61
test/mavsdk_tests/test_multicopter_failure_injection.cpp Normal file
View File

@ -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 <thread>
#include <chrono>
#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);
}