2020-03-17 12:15:34 -03:00
|
|
|
/****************************************************************************
|
|
|
|
*
|
|
|
|
* Copyright (c) 2020 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.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2019-10-02 15:14:18 -03:00
|
|
|
#include "autopilot_tester.h"
|
|
|
|
#include <iostream>
|
|
|
|
#include <future>
|
|
|
|
|
2019-10-03 06:16:03 -03:00
|
|
|
std::string connection_url {"udp://"};
|
|
|
|
|
2019-10-02 15:14:18 -03:00
|
|
|
void AutopilotTester::connect(const std::string uri)
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
ConnectionResult ret = _mavsdk.add_any_connection(uri);
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(ret == ConnectionResult::Success);
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
std::cout << "Waiting for system connect" << std::endl;
|
|
|
|
REQUIRE(poll_condition_with_timeout(
|
|
|
|
[this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25)));
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
auto &system = _mavsdk.system();
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
_telemetry.reset(new Telemetry(system));
|
|
|
|
_action.reset(new Action(system));
|
|
|
|
_mission.reset(new Mission(system));
|
|
|
|
_offboard.reset(new Offboard(system));
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::wait_until_ready()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
std::cout << "Waiting for system to be ready" << std::endl;
|
|
|
|
CHECK(poll_condition_with_timeout(
|
|
|
|
[this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(20)));
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
2020-01-30 06:57:48 -04:00
|
|
|
void AutopilotTester::wait_until_ready_local_position_only()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
std::cout << "Waiting for system to be ready" << std::endl;
|
|
|
|
CHECK(poll_condition_with_timeout(
|
|
|
|
[this]() {
|
|
|
|
return
|
2020-06-10 14:26:11 -03:00
|
|
|
(_telemetry->health().is_gyrometer_calibration_ok &&
|
|
|
|
_telemetry->health().is_accelerometer_calibration_ok &&
|
|
|
|
_telemetry->health().is_magnetometer_calibration_ok &&
|
|
|
|
_telemetry->health().is_level_calibration_ok &&
|
|
|
|
_telemetry->health().is_local_position_ok);
|
2020-01-30 06:57:48 -04:00
|
|
|
}, std::chrono::seconds(20)));
|
|
|
|
}
|
|
|
|
|
2020-03-06 07:41:28 -04:00
|
|
|
void AutopilotTester::store_home()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
request_ground_truth();
|
2020-05-26 06:36:30 -03:00
|
|
|
std::cout << "Waiting to get home position" << std::endl;
|
|
|
|
CHECK(poll_condition_with_timeout(
|
|
|
|
[this]() {
|
|
|
|
_home = _telemetry->ground_truth();
|
|
|
|
return std::isfinite(_home.latitude_deg) && std::isfinite(_home.longitude_deg);
|
|
|
|
}, std::chrono::seconds(10)));
|
2020-03-06 07:41:28 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::check_home_within(float acceptance_radius_m)
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m));
|
2020-03-06 07:41:28 -04:00
|
|
|
}
|
|
|
|
|
2019-10-02 15:14:18 -03:00
|
|
|
void AutopilotTester::set_takeoff_altitude(const float altitude_m)
|
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
CHECK(Action::Result::Success == _action->set_takeoff_altitude(altitude_m));
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto result = _action->get_takeoff_altitude();
|
2020-06-10 14:26:11 -03:00
|
|
|
CHECK(result.first == Action::Result::Success);
|
2020-03-17 12:15:34 -03:00
|
|
|
CHECK(result.second == Approx(altitude_m));
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::arm()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto result = _action->arm();
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(result == Action::Result::Success);
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::takeoff()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto result = _action->takeoff();
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(result == Action::Result::Success);
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::land()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto result = _action->land();
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(result == Action::Result::Success);
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
2020-01-25 11:04:02 -04:00
|
|
|
void AutopilotTester::transition_to_fixedwing()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto result = _action->transition_to_fixedwing();
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(result == Action::Result::Success);
|
2020-01-25 11:04:02 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::transition_to_multicopter()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto result = _action->transition_to_multicopter();
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(result == Action::Result::Success);
|
2020-01-25 11:04:02 -04:00
|
|
|
}
|
|
|
|
|
2020-05-05 06:42:06 -03:00
|
|
|
void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration)
|
2019-10-02 15:14:18 -03:00
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
REQUIRE(poll_condition_with_timeout(
|
2020-05-05 06:42:06 -03:00
|
|
|
[this]() { return !_telemetry->armed(); }, timeout_duration));
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::wait_until_hovering()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
REQUIRE(poll_condition_with_timeout(
|
2020-06-10 14:26:11 -03:00
|
|
|
[this]() { return _telemetry->landed_state() == Telemetry::LandedState::InAir; }, std::chrono::seconds(20)));
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto ct = get_coordinate_transformation();
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-06-10 14:26:11 -03:00
|
|
|
Mission::MissionPlan mission_plan {};
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct));
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m},
|
|
|
|
mission_options, ct));
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct));
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
_mission->set_return_to_launch_after_mission(mission_options.rtl_at_end);
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
std::promise<void> prom;
|
|
|
|
auto fut = prom.get_future();
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-06-10 14:26:11 -03:00
|
|
|
_mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) {
|
|
|
|
REQUIRE(Mission::Result::Success == result);
|
2020-03-17 12:15:34 -03:00
|
|
|
prom.set_value();
|
|
|
|
});
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
2020-07-09 12:36:24 -03:00
|
|
|
void AutopilotTester::prepare_straight_mission(MissionOptions mission_options)
|
|
|
|
{
|
|
|
|
const auto ct = get_coordinate_transformation();
|
|
|
|
|
|
|
|
Mission::MissionPlan mission_plan {};
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({0, 0.}, mission_options, ct));
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0}, mission_options, ct));
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({2 * mission_options.leg_length_m, 0}, mission_options, ct));
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({3 * mission_options.leg_length_m, 0}, mission_options, ct));
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({4 * mission_options.leg_length_m, 0}, mission_options, ct));
|
|
|
|
|
|
|
|
_mission->set_return_to_launch_after_mission(mission_options.rtl_at_end);
|
|
|
|
|
|
|
|
std::promise<void> prom;
|
|
|
|
auto fut = prom.get_future();
|
|
|
|
|
|
|
|
_mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) {
|
|
|
|
REQUIRE(Mission::Result::Success == result);
|
|
|
|
prom.set_value();
|
|
|
|
});
|
|
|
|
|
|
|
|
REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
|
|
|
|
}
|
|
|
|
|
2019-10-02 15:14:18 -03:00
|
|
|
void AutopilotTester::execute_mission()
|
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
std::promise<void> prom;
|
|
|
|
auto fut = prom.get_future();
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
_mission->start_mission_async([&prom](Mission::Result result) {
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(Mission::Result::Success == result);
|
2020-03-17 12:15:34 -03:00
|
|
|
prom.set_value();
|
|
|
|
});
|
2019-10-02 15:14:18 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.
|
2019-10-03 06:20:59 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
REQUIRE(poll_condition_with_timeout(
|
2020-06-10 14:26:11 -03:00
|
|
|
[this]() {
|
|
|
|
auto result = _mission->is_mission_finished();
|
|
|
|
return result.first == Mission::Result::Success && result.second;
|
|
|
|
}, std::chrono::seconds(60)));
|
2019-10-03 06:20:59 -03:00
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready);
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
2020-03-06 07:41:28 -04:00
|
|
|
CoordinateTransformation AutopilotTester::get_coordinate_transformation()
|
2019-10-02 15:14:18 -03:00
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
const auto home = _telemetry->home();
|
2020-03-17 12:15:34 -03:00
|
|
|
CHECK(std::isfinite(home.latitude_deg));
|
|
|
|
CHECK(std::isfinite(home.longitude_deg));
|
|
|
|
return CoordinateTransformation({home.latitude_deg, home.longitude_deg});
|
2019-10-02 15:14:18 -03:00
|
|
|
}
|
|
|
|
|
2020-06-10 14:26:11 -03:00
|
|
|
Mission::MissionItem AutopilotTester::create_mission_item(
|
2020-03-17 12:15:34 -03:00
|
|
|
const CoordinateTransformation::LocalCoordinate &local_coordinate,
|
|
|
|
const MissionOptions &mission_options,
|
|
|
|
const CoordinateTransformation &ct)
|
2019-10-02 15:14:18 -03:00
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
auto mission_item = Mission::MissionItem{};
|
2020-03-17 12:15:34 -03:00
|
|
|
const auto pos_north = ct.global_from_local(local_coordinate);
|
2020-06-10 14:26:11 -03:00
|
|
|
mission_item.latitude_deg = pos_north.latitude_deg;
|
|
|
|
mission_item.longitude_deg = pos_north.longitude_deg;
|
|
|
|
mission_item.relative_altitude_m = mission_options.relative_altitude_m;
|
2020-07-09 12:36:24 -03:00
|
|
|
mission_item.is_fly_through = mission_options.fly_through;
|
2020-03-17 12:15:34 -03:00
|
|
|
return mission_item;
|
2019-10-03 06:29:22 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::execute_rtl()
|
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(Action::Result::Success == _action->return_to_launch());
|
2019-10-07 10:04:30 -03:00
|
|
|
}
|
2020-01-30 06:57:48 -04:00
|
|
|
|
2020-06-10 14:26:11 -03:00
|
|
|
void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m,
|
2020-03-17 12:15:34 -03:00
|
|
|
std::chrono::seconds timeout_duration)
|
2020-01-30 06:57:48 -04:00
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
_offboard->set_position_ned(target);
|
2020-06-10 14:26:11 -03:00
|
|
|
REQUIRE(_offboard->start() == Offboard::Result::Success);
|
2020-03-17 12:15:34 -03:00
|
|
|
CHECK(poll_condition_with_timeout(
|
|
|
|
[ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration));
|
|
|
|
std::cout << "Target position reached" << std::endl;
|
2020-01-30 06:57:48 -04:00
|
|
|
}
|
|
|
|
|
2020-07-10 07:57:31 -03:00
|
|
|
void AutopilotTester::check_mission_item_speed_above(int item_index, float min_speed_m_s)
|
2020-07-09 12:36:24 -03:00
|
|
|
{
|
2020-07-10 07:57:31 -03:00
|
|
|
|
|
|
|
_telemetry->set_rate_velocity_ned(10);
|
|
|
|
_telemetry->subscribe_velocity_ned([item_index, min_speed_m_s, this](Telemetry::VelocityNed velocity) {
|
2020-07-09 12:36:24 -03:00
|
|
|
float horizontal = std::hypot(velocity.north_m_s, velocity.east_m_s);
|
|
|
|
auto progress = _mission->mission_progress();
|
|
|
|
|
|
|
|
if (progress.current == item_index) {
|
2020-07-10 07:57:31 -03:00
|
|
|
CHECK(horizontal > min_speed_m_s);
|
2020-07-09 12:36:24 -03:00
|
|
|
}
|
|
|
|
});
|
|
|
|
}
|
|
|
|
|
2020-07-09 12:39:13 -03:00
|
|
|
void AutopilotTester::check_tracks_mission(float corridor_radius_m)
|
|
|
|
{
|
|
|
|
auto mission = _mission->download_mission();
|
|
|
|
CHECK(mission.first == Mission::Result::Success);
|
|
|
|
|
|
|
|
std::vector<Mission::MissionItem> mission_items = mission.second.mission_items;
|
|
|
|
auto ct = get_coordinate_transformation();
|
|
|
|
|
|
|
|
auto get_local_mission_item = [mission_items, ct](int item_index) -> std::array<float, 3> {
|
|
|
|
using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate;
|
|
|
|
auto item = mission_items[item_index];
|
|
|
|
GlobalCoordinate global;
|
|
|
|
global.latitude_deg = item.latitude_deg;
|
|
|
|
global.longitude_deg = item.longitude_deg;
|
|
|
|
auto local = ct.local_from_global(global);
|
|
|
|
std::array<float, 3> res = {static_cast<float>(local.north_m), static_cast<float>(local.east_m), -item.relative_altitude_m};
|
|
|
|
|
|
|
|
return res;
|
|
|
|
};
|
|
|
|
|
|
|
|
_telemetry->set_rate_position_velocity_ned(5);
|
|
|
|
_telemetry->subscribe_position_velocity_ned([get_local_mission_item, corridor_radius_m,
|
|
|
|
this](Telemetry::PositionVelocityNed position_velocity_ned) {
|
|
|
|
auto progress = _mission->mission_progress();
|
|
|
|
|
|
|
|
if (progress.current > 0 && progress.current < progress.total) {
|
|
|
|
// Get shortest distance of current position to 3D line between previous and next waypoint
|
|
|
|
std::array<float, 3> current { position_velocity_ned.position.north_m,
|
|
|
|
position_velocity_ned.position.east_m,
|
|
|
|
position_velocity_ned.position.down_m };
|
|
|
|
std::array<float, 3> wp_prev = get_local_mission_item(progress.current - 1);
|
|
|
|
std::array<float, 3> wp_next = get_local_mission_item(progress.current);
|
|
|
|
|
|
|
|
// wp_dir_norm = (wp_next - wp_prev).normalize();
|
|
|
|
std::array<float, 3> wp_dir { wp_next[0] - wp_prev[0], wp_next[1] - wp_prev[1], wp_next[2] - wp_prev[2]};
|
|
|
|
float norm = std::sqrt(wp_dir[0] * wp_dir[0] + wp_dir[1] * wp_dir[1] + wp_dir[2] * wp_dir[2]);
|
|
|
|
std::array<float, 3> wp_dir_norm {wp_dir[0] / norm, wp_dir[1] / norm, wp_dir[2] / norm};
|
|
|
|
|
|
|
|
// travelled = current - wp_prev
|
|
|
|
std::array<float, 3> travelled {current[0] - wp_prev[0], current[1] - wp_prev[1], current[2] - wp_prev[2]};
|
|
|
|
|
|
|
|
// t = wp_dir_norm.dot(travelled);
|
|
|
|
float t = wp_dir_norm[0] * travelled[0] + wp_dir_norm[1] * travelled[1] + wp_dir_norm[2] * travelled[2];
|
|
|
|
|
|
|
|
// closest_on_line = wp_prev + t * wp_dir_norm;
|
|
|
|
std::array<float, 3> closest_on_line { wp_prev[0] + t *wp_dir_norm[0], wp_prev[1] + t *wp_dir_norm[1], wp_prev[2] + t *wp_dir_norm[2]};
|
|
|
|
|
|
|
|
// distance = (closest_on_line - current).norm();
|
|
|
|
std::array<float, 3> vec_to_line {closest_on_line[0] - current[0], closest_on_line[1] - current[1], closest_on_line[2] - current[2]};
|
|
|
|
float distance_to_trajectory = std::sqrt(vec_to_line[0] * vec_to_line[0] * vec_to_line[1] * vec_to_line[1] *
|
|
|
|
vec_to_line[2] * vec_to_line[2]);
|
|
|
|
|
|
|
|
CHECK(distance_to_trajectory < corridor_radius_m);
|
|
|
|
}
|
|
|
|
});
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2020-01-30 06:57:48 -04:00
|
|
|
void AutopilotTester::offboard_land()
|
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
Offboard::VelocityNedYaw land_velocity;
|
2020-03-17 12:15:34 -03:00
|
|
|
land_velocity.north_m_s = 0.0f;
|
|
|
|
land_velocity.east_m_s = 0.0f;
|
|
|
|
land_velocity.down_m_s = 1.0f;
|
|
|
|
land_velocity.yaw_deg = 0.0f;
|
|
|
|
_offboard->set_velocity_ned(land_velocity);
|
2020-01-30 06:57:48 -04:00
|
|
|
}
|
|
|
|
|
2020-06-10 14:26:11 -03:00
|
|
|
bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m)
|
2020-01-30 06:57:48 -04:00
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
Telemetry::PositionNed est_pos = _telemetry->position_velocity_ned().position;
|
2020-03-17 12:15:34 -03:00
|
|
|
return sq(est_pos.north_m - target_pos.north_m) +
|
|
|
|
sq(est_pos.east_m - target_pos.east_m) +
|
|
|
|
sq(est_pos.down_m - target_pos.down_m) < sq(acceptance_radius_m);
|
2020-01-30 06:57:48 -04:00
|
|
|
}
|
|
|
|
|
2020-06-10 14:26:11 -03:00
|
|
|
bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos,
|
2020-03-17 12:15:34 -03:00
|
|
|
float acceptance_radius_m)
|
2020-01-30 06:57:48 -04:00
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
Telemetry::PositionNed est_pos = _telemetry->position_velocity_ned().position;
|
2020-03-17 12:15:34 -03:00
|
|
|
return sq(est_pos.north_m - target_pos.north_m) +
|
|
|
|
sq(est_pos.east_m - target_pos.east_m) < sq(acceptance_radius_m);
|
2020-01-30 06:57:48 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void AutopilotTester::request_ground_truth()
|
|
|
|
{
|
2020-06-10 14:26:11 -03:00
|
|
|
CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::Success);
|
2020-01-30 06:57:48 -04:00
|
|
|
}
|
|
|
|
|
2020-03-17 12:15:34 -03:00
|
|
|
bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos,
|
|
|
|
float acceptance_radius_m)
|
2020-01-30 06:57:48 -04:00
|
|
|
{
|
2020-03-17 12:15:34 -03:00
|
|
|
CHECK(std::isfinite(target_pos.latitude_deg));
|
|
|
|
CHECK(std::isfinite(target_pos.longitude_deg));
|
|
|
|
using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate;
|
|
|
|
using LocalCoordinate = CoordinateTransformation::LocalCoordinate;
|
|
|
|
CoordinateTransformation ct(GlobalCoordinate{target_pos.latitude_deg, target_pos.longitude_deg});
|
|
|
|
|
|
|
|
Telemetry::GroundTruth current_pos = _telemetry->ground_truth();
|
|
|
|
CHECK(std::isfinite(current_pos.latitude_deg));
|
|
|
|
CHECK(std::isfinite(current_pos.longitude_deg));
|
2020-06-10 14:26:11 -03:00
|
|
|
GlobalCoordinate global_current;
|
|
|
|
global_current.latitude_deg = current_pos.latitude_deg;
|
|
|
|
global_current.longitude_deg = current_pos.longitude_deg;
|
|
|
|
LocalCoordinate local_pos = ct.local_from_global(global_current);
|
2020-05-26 05:38:14 -03:00
|
|
|
const double distance = sqrt(sq(local_pos.north_m) + sq(local_pos.east_m));
|
2020-05-26 05:41:40 -03:00
|
|
|
const bool pass = distance < acceptance_radius_m;
|
|
|
|
|
|
|
|
if (!pass) {
|
|
|
|
std::cout << "target_pos.lat: " << target_pos.latitude_deg << std::endl;
|
|
|
|
std::cout << "target_pos.lon: " << target_pos.longitude_deg << std::endl;
|
|
|
|
std::cout << "current.lat: " << current_pos.latitude_deg << std::endl;
|
|
|
|
std::cout << "current.lon: " << current_pos.longitude_deg << std::endl;
|
|
|
|
std::cout << "Distance: " << distance << std::endl;
|
|
|
|
std::cout << "Acceptance radius: " << acceptance_radius_m << std::endl;
|
|
|
|
}
|
|
|
|
|
|
|
|
return pass;
|
2020-01-30 06:57:48 -04:00
|
|
|
}
|