mavsdk_tests: Add integration tests for figure of 8

This commit is contained in:
Konrad 2023-10-20 13:37:54 +02:00 committed by Daniel Agar
parent 8edd7ce2c1
commit e3473a0f90
6 changed files with 329 additions and 0 deletions

View File

@ -18,6 +18,7 @@ if(MAVSDK_FOUND)
autopilot_tester.cpp
autopilot_tester_failure.cpp
autopilot_tester_rtl.cpp
autopilot_tester_figure_eight.cpp
# follow-me needs a MAVSDK update:
# https://github.com/mavlink/MAVSDK/pull/1770
# autopilot_tester_follow_me.cpp
@ -28,6 +29,7 @@ if(MAVSDK_FOUND)
test_multicopter_offboard.cpp
test_multicopter_manual.cpp
test_vtol_mission.cpp
test_vtol_figure_eight.cpp
test_vtol_rtl.cpp
# test_multicopter_follow_me.cpp
)

View File

@ -220,6 +220,12 @@ void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::sec
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}
void AutopilotTester::wait_until_fixedwing(std::chrono::seconds timeout)
{
REQUIRE(poll_condition_with_timeout(
[this]() { return _telemetry->vtol_state() == Telemetry::VtolState::Fw; }, timeout));
}
void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
{
const auto ct = get_coordinate_transformation();

View File

@ -118,6 +118,7 @@ public:
void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(60));
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_fixedwing(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);

View File

@ -0,0 +1,175 @@
/****************************************************************************
*
* 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_figure_eight.h"
#include <cmath>
#include <float.h>
#include <future>
#include <vector>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavlink/development/mavlink_msg_figure_eight_execution_status.h>
using namespace mavsdk::geometry;
void AutopilotTesterFigureEight::execute_figure_eight()
{
MavlinkPassthrough::CommandInt figure_eight_command;
auto ct = get_coordinate_transformation();
const auto global_center = ct.global_from_local(_figure_eight.center);
figure_eight_command.target_sysid = getMavlinkPassthrough()->get_target_sysid();
figure_eight_command.target_compid = getMavlinkPassthrough()->get_target_compid();
figure_eight_command.command = 35; // Figure eight command
figure_eight_command.frame = MAV_FRAME_GLOBAL_INT;
figure_eight_command.param1 = _figure_eight.major_axis;
figure_eight_command.param2 = _figure_eight.minor_axis;
figure_eight_command.param3 = NAN;
figure_eight_command.param4 = _figure_eight.orientation;
figure_eight_command.x = static_cast<int32_t>(global_center.latitude_deg * 1E7);
figure_eight_command.y = static_cast<int32_t>(global_center.longitude_deg * 1E7);
figure_eight_command.z = _figure_eight.alt;
send_custom_mavlink_command(figure_eight_command);
}
void AutopilotTesterFigureEight::set_figure_eight(const double major_axis, const double minor_axis,
const double orientation, const double home_offset_N, const double home_offset_E, const double rel_alt)
{
_figure_eight.major_axis = major_axis;
_figure_eight.minor_axis = minor_axis;
_figure_eight.orientation = orientation;
_figure_eight.alt = getHome().absolute_altitude_m + rel_alt;
_figure_eight.center = {home_offset_N, home_offset_E};
}
void AutopilotTesterFigureEight::check_tracks_figure_eight(std::chrono::seconds timeout, double corridor_radius_m)
{
auto prom = std::promise<bool> {};
auto fut = prom.get_future();
const double cos_or = cos(_figure_eight.orientation);
const double sin_or = sin(_figure_eight.orientation);
std::vector<CoordinateTransformation::LocalCoordinate> figure_eight_point_of_interest;
figure_eight_point_of_interest.push_back(_figure_eight.center);
figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) - sin_or * (- _figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) + cos_or * (- _figure_eight.minor_axis)});
figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (abs(_figure_eight.major_axis)) - sin_or * 0., _figure_eight.center.east_m + sin_or * (abs(_figure_eight.major_axis)) + cos_or * 0.});
figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) - sin_or * (_figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) + cos_or * (_figure_eight.minor_axis)});
figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) - sin_or * (- _figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) + cos_or * (- _figure_eight.minor_axis)});
figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (-abs(_figure_eight.major_axis)) - sin_or * 0., _figure_eight.center.east_m + sin_or * (-abs(_figure_eight.major_axis)) + cos_or * (0.)});
figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) - sin_or * (+ _figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) + cos_or * (_figure_eight.minor_axis)});
std::vector<int32_t> order_to_fly;
if (_figure_eight.major_axis > 0) {
order_to_fly = std::vector<int32_t> {0, 1, 2, 3, 0, 4, 5, 6, 0};
} else {
order_to_fly = std::vector<int32_t> {0, 3, 2, 1, 0, 6, 5, 4, 0};
}
getTelemetry()->subscribe_position_velocity_ned([&figure_eight_point_of_interest, &prom, corridor_radius_m,
&order_to_fly, this](Telemetry::PositionVelocityNed position_velocity_ned) {
static size_t index{0};
int32_t close_index{-1};
for (size_t interest_point_index{0}; interest_point_index < figure_eight_point_of_interest.size();
interest_point_index++) {
if ((abs(position_velocity_ned.position.north_m - figure_eight_point_of_interest[interest_point_index].north_m) <
corridor_radius_m) &&
(abs(position_velocity_ned.position.east_m - figure_eight_point_of_interest[interest_point_index].east_m) <
corridor_radius_m)) {
close_index = static_cast<int32_t>(interest_point_index);
break;
}
}
if (close_index >= 0) {
if (close_index == order_to_fly[index]) { // Still at the same point already found
// Do nothing
} else if (close_index == order_to_fly[index + 1]) { // reached the next expected point
index++;
} else { // reached an out of order point
if (index > 0U) { // only set to false if we already hve passed the first center point
getTelemetry()->subscribe_position_velocity_ned(nullptr);
prom.set_value(false);
}
}
}
if (index + 1 == order_to_fly.size()) {
getTelemetry()->subscribe_position_velocity_ned(nullptr);
prom.set_value(true);
}
});
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
CHECK(fut.get() == true);
}
void AutopilotTesterFigureEight::check_receive_execution_status(std::chrono::seconds timeout)
{
auto prom = std::promise<void> {};
auto fut = prom.get_future();
auto ct = get_coordinate_transformation();
const auto global_center = ct.global_from_local(_figure_eight.center);
add_mavlink_message_callback(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS, [&prom, global_center,
this](const mavlink_message_t &message) {
add_mavlink_message_callback(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS, nullptr);
mavlink_figure_eight_execution_status_t status_message;
mavlink_msg_figure_eight_execution_status_decode(&message, &status_message);
CHECK(abs(status_message.major_radius - _figure_eight.major_axis) < 1E-4);
CHECK(abs(status_message.minor_radius - _figure_eight.minor_axis) < 1E-4);
CHECK(abs(status_message.orientation - _figure_eight.orientation) < 1E-7);
CHECK(status_message.x == static_cast<int32_t>(global_center.latitude_deg * 1E7));
CHECK(status_message.y == static_cast<int32_t>(global_center.longitude_deg * 1E7));
CHECK(abs(status_message.z - _figure_eight.alt) < 1E-4);
prom.set_value();
return true;
});
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}

View File

@ -0,0 +1,64 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#include "autopilot_tester.h"
#include <mavsdk/geometry.h>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
class AutopilotTesterFigureEight : public AutopilotTester
{
public:
AutopilotTesterFigureEight() = default;
~AutopilotTesterFigureEight() = default;
void set_figure_eight(double major_axis, double minor_axis, double orientation, double home_offset_N,
double home_offset_E, double rel_alt);
void execute_figure_eight();
void check_tracks_figure_eight(std::chrono::seconds timeout, double corridor_radius_m = 5.f);
void check_receive_execution_status(std::chrono::seconds timeout);
private:
struct {
double major_axis;
double minor_axis;
double orientation;
CoordinateTransformation::LocalCoordinate center;
double alt;
} _figure_eight;
};

View File

@ -0,0 +1,81 @@
/****************************************************************************
*
* 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_figure_eight.h"
#include <chrono>
#include <thread>
TEST_CASE("Figure eight execution clockwise", "[vtol]")
{
AutopilotTesterFigureEight tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
const float takeoff_altitude = 20.f;
tester.set_takeoff_altitude(takeoff_altitude);
tester.arm();
tester.takeoff();
tester.wait_until_hovering();
tester.wait_until_altitude(takeoff_altitude - 1.f, std::chrono::seconds(30));
tester.transition_to_fixedwing();
tester.wait_until_fixedwing(std::chrono::seconds(5));
std::this_thread::sleep_for(std::chrono::seconds(1));
tester.set_figure_eight(150., 50., 0., 200., 0., 20.);
tester.execute_figure_eight();
tester.check_tracks_figure_eight(std::chrono::seconds(60), 10.);
// tester.check_receive_execution_status(std::chrono::seconds(
// 5)); //TODO With mavsdk we can't subscribe to custom messages. Need to wait until messages are recognised by mavsdk
}
TEST_CASE("Figure eight execution counterclockwise", "[vtol]")
{
AutopilotTesterFigureEight tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
const float takeoff_altitude = 20.f;
tester.set_takeoff_altitude(takeoff_altitude);
tester.arm();
tester.takeoff();
tester.wait_until_hovering();
tester.wait_until_altitude(takeoff_altitude - 1.f, std::chrono::seconds(30));
tester.transition_to_fixedwing();
tester.wait_until_fixedwing(std::chrono::seconds(5));
std::this_thread::sleep_for(std::chrono::seconds(1));
tester.set_figure_eight(-150., 50., 30.*M_PI / 180., 200., 0., 20.);
tester.execute_figure_eight();
tester.check_tracks_figure_eight(std::chrono::seconds(60), 10.);
// tester.check_receive_execution_status(std::chrono::seconds(
// 5)); //TODO With mavsdk we can't subscribe to custom messages. Need to wait until messages are recognised by mavsdk.
}