forked from Archive/PX4-Autopilot
mavsdk_tests: Add integration tests for figure of 8
This commit is contained in:
Konrad
Daniel Agar
parent
8edd7ce2c1
commit
e3473a0f90
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
@ -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.
|
||||
}
|
||||
Loading…
Reference in New Issue