Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

mavsdk_tests: add time in front of debug lines

Browse Source
This commit is contained in:
Julian Oes
2021-02-09 08:45:38 +01:00
committed by Lorenz Meier
parent dec353219d
commit 397fa84cd0
2 changed files with 39 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/autopilot_tester.cpp
+23 -23
View File
@@ -43,7 +43,7 @@ void AutopilotTester::connect(const std::string uri)
ConnectionResult ret = _mavsdk.add_any_connection(uri); ConnectionResult ret = _mavsdk.add_any_connection(uri);
REQUIRE(ret == ConnectionResult::Success); REQUIRE(ret == ConnectionResult::Success);
std::cout << "Waiting for system connect" << std::endl; std::cout << time_str() << "Waiting for system connect" << std::endl;
REQUIRE(poll_condition_with_timeout( REQUIRE(poll_condition_with_timeout(
[this]() { return _mavsdk.is_connected(); }, adjust_to_lockstep_speed(std::chrono::seconds(25)))); [this]() { return _mavsdk.is_connected(); }, adjust_to_lockstep_speed(std::chrono::seconds(25))));
@@ -61,7 +61,7 @@ void AutopilotTester::connect(const std::string uri)
void AutopilotTester::wait_until_ready() void AutopilotTester::wait_until_ready()
{ {
std::cout << "Waiting for system to be ready" << std::endl; std::cout << time_str() << "Waiting for system to be ready" << std::endl;
CHECK(poll_condition_with_timeout( CHECK(poll_condition_with_timeout(
[this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30))); [this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30)));
@@ -73,7 +73,7 @@ void AutopilotTester::wait_until_ready()
void AutopilotTester::wait_until_ready_local_position_only() void AutopilotTester::wait_until_ready_local_position_only()
{ {
std::cout << "Waiting for system to be ready" << std::endl; std::cout << time_str() << "Waiting for system to be ready" << std::endl;
CHECK(poll_condition_with_timeout( CHECK(poll_condition_with_timeout(
[this]() { [this]() {
return return
@@ -88,7 +88,7 @@ void AutopilotTester::wait_until_ready_local_position_only()
void AutopilotTester::store_home() void AutopilotTester::store_home()
{ {
request_ground_truth(); request_ground_truth();
std::cout << "Waiting to get home position" << std::endl; std::cout << time_str() << "Waiting to get home position" << std::endl;
CHECK(poll_condition_with_timeout( CHECK(poll_condition_with_timeout(
[this]() { [this]() {
_home = _telemetry->ground_truth(); _home = _telemetry->ground_truth();
@@ -241,7 +241,7 @@ void AutopilotTester::execute_mission_and_lose_gps()
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
_mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) {
std::cout << "Progress: " << progress.current << "/" << progress.total << std::endl; std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current == 1) { if (progress.current == 1) {
std::thread([this]() { std::thread([this]() {
@@ -272,7 +272,7 @@ void AutopilotTester::execute_mission_and_lose_mag()
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
_mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) {
std::cout << "Progress: " << progress.current << "/" << progress.total << std::endl; std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current == 1) { if (progress.current == 1) {
std::thread([this]() { std::thread([this]() {
@@ -304,7 +304,7 @@ void AutopilotTester::execute_mission_and_lose_baro()
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
_mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) {
std::cout << "Progress: " << progress.current << "/" << progress.total << std::endl; std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current == 1) { if (progress.current == 1) {
std::thread([this]() { std::thread([this]() {
@@ -335,7 +335,7 @@ void AutopilotTester::execute_mission_and_get_baro_stuck()
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
_mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) {
std::cout << "Progress: " << progress.current << "/" << progress.total << std::endl; std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current == 1) { if (progress.current == 1) {
std::thread([this]() { std::thread([this]() {
@@ -366,7 +366,7 @@ void AutopilotTester::execute_mission_and_get_mag_stuck()
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
_mission->subscribe_mission_progress([this](Mission::MissionProgress progress) { _mission->subscribe_mission_progress([this](Mission::MissionProgress progress) {
std::cout << "Progress: " << progress.current << "/" << progress.total << std::endl; std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current == 1) { if (progress.current == 1) {
std::thread([this]() { std::thread([this]() {
@@ -426,7 +426,7 @@ void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, floa
REQUIRE(_offboard->start() == Offboard::Result::Success); REQUIRE(_offboard->start() == Offboard::Result::Success);
CHECK(poll_condition_with_timeout( CHECK(poll_condition_with_timeout(
[ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration)); [ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration));
std::cout << "Target position reached" << std::endl; std::cout << time_str() << "Target position reached" << std::endl;
} }
void AutopilotTester::check_mission_item_speed_above(int item_index, float min_speed_m_s) void AutopilotTester::check_mission_item_speed_above(int item_index, float min_speed_m_s)
@@ -562,7 +562,7 @@ bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNedYaw
const bool pass = distance_m < acceptance_radius_m; const bool pass = distance_m < acceptance_radius_m;
if (!pass) { if (!pass) {
std::cout << "distance: " << distance_m << ", " << "acceptance: " << acceptance_radius_m << std::endl; std::cout << time_str() << "distance: " << distance_m << ", " << "acceptance: " << acceptance_radius_m << std::endl;
} }
return pass; return pass;
@@ -601,12 +601,12 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
const bool pass = distance_m < acceptance_radius_m; const bool pass = distance_m < acceptance_radius_m;
if (!pass) { if (!pass) {
std::cout << "target_pos.lat: " << target_pos.latitude_deg << std::endl; std::cout << time_str() << "target_pos.lat: " << target_pos.latitude_deg << std::endl;
std::cout << "target_pos.lon: " << target_pos.longitude_deg << std::endl; std::cout << time_str() << "target_pos.lon: " << target_pos.longitude_deg << std::endl;
std::cout << "current.lat: " << current_pos.latitude_deg << std::endl; std::cout << time_str() << "current.lat: " << current_pos.latitude_deg << std::endl;
std::cout << "current.lon: " << current_pos.longitude_deg << std::endl; std::cout << time_str() << "current.lon: " << current_pos.longitude_deg << std::endl;
std::cout << "Distance: " << distance_m << std::endl; std::cout << time_str() << "Distance: " << distance_m << std::endl;
std::cout << "Acceptance radius: " << acceptance_radius_m << std::endl; std::cout << time_str() << "Acceptance radius: " << acceptance_radius_m << std::endl;
} }
return pass; return pass;
@@ -632,12 +632,12 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry:
const bool pass = distance_m > min_distance_m; const bool pass = distance_m > min_distance_m;
if (!pass) { if (!pass) {
std::cout << "target_pos.lat: " << target_pos.latitude_deg << std::endl; std::cout << time_str() << "target_pos.lat: " << target_pos.latitude_deg << std::endl;
std::cout << "target_pos.lon: " << target_pos.longitude_deg << std::endl; std::cout << time_str() << "target_pos.lon: " << target_pos.longitude_deg << std::endl;
std::cout << "current.lat: " << current_pos.latitude_deg << std::endl; std::cout << time_str() << "current.lat: " << current_pos.latitude_deg << std::endl;
std::cout << "current.lon: " << current_pos.longitude_deg << std::endl; std::cout << time_str() << "current.lon: " << current_pos.longitude_deg << std::endl;
std::cout << "Distance: " << distance_m << std::endl; std::cout << time_str() << "Distance: " << distance_m << std::endl;
std::cout << "Min distance: " << min_distance_m << std::endl; std::cout << time_str() << "Min distance: " << min_distance_m << std::endl;
} }
return pass; return pass;
test/mavsdk_tests/autopilot_tester.h
+16 -3
View File
@@ -45,6 +45,7 @@
#include <mavsdk/plugins/param/param.h> #include <mavsdk/plugins/param/param.h>
#include "catch2/catch.hpp" #include "catch2/catch.hpp"
#include <chrono> #include <chrono>
#include <ctime>
#include <iostream> #include <iostream>
#include <memory> #include <memory>
#include <thread> #include <thread>
@@ -54,6 +55,17 @@ extern std::string connection_url;
using namespace mavsdk; using namespace mavsdk;
using namespace mavsdk::geometry; using namespace mavsdk::geometry;
inline std::string time_str()
{
time_t rawtime;
time(&rawtime);
struct tm *timeinfo = localtime(&rawtime);
char time_buffer[18];
strftime(time_buffer, 18, "[%I:%M:%S|Info ] ", timeinfo);
return time_buffer;
}
class AutopilotTester class AutopilotTester
{ {
public: public:
@@ -136,8 +148,8 @@ private:
const int64_t elapsed_time_ms = _info->get_flight_information().second.time_boot_ms - start_time; const int64_t elapsed_time_ms = _info->get_flight_information().second.time_boot_ms - start_time;
if (elapsed_time_ms > duration_ms.count()) { if (elapsed_time_ms > duration_ms.count()) {
std::cout << "Timeout, connected to vehicle but waiting for test for " << elapsed_time_ms / 1000.0 << " seconds" << std::cout << time_str() << "Timeout, connected to vehicle but waiting for test for "
std::endl; << elapsed_time_ms / 1000.0 << " seconds\n";
return false; return false;
} }
} }
@@ -152,7 +164,8 @@ private:
start_time).count(); start_time).count();
if (elapsed_time_us > duration_ms.count() * 1000) { if (elapsed_time_us > duration_ms.count() * 1000) {
std::cout << "Timeout, waiting for the vehicle for " << elapsed_time_us / 1000000.0 << " seconds" << std::endl; std::cout << time_str() << "Timeout, waiting for the vehicle for "
<< elapsed_time_us / 1000000.0 << " seconds\n";
return false; return false;
} }
} }