fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 06:19:05 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -43,7 +43,7 @@ void AutopilotTester::connect(const std::string uri)
ConnectionResult ret = _mavsdk.add_any_connection(uri);
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(
[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()
{
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(
[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()
{
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(
[this]() {
return
@ -88,7 +88,7 @@ void AutopilotTester::wait_until_ready_local_position_only()
void AutopilotTester::store_home()
{
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(
[this]() {
_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);
_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) {
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);
_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) {
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);
_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) {
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);
_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) {
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);
_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) {
std::thread([this]() {
@ -426,7 +426,7 @@ void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, floa
REQUIRE(_offboard->start() == Offboard::Result::Success);
CHECK(poll_condition_with_timeout(
[ = ]() { 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)
@ -562,7 +562,7 @@ bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNedYaw
const bool pass = distance_m < acceptance_radius_m;
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;
@ -601,12 +601,12 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
const bool pass = distance_m < 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_m << std::endl;
std::cout << "Acceptance radius: " << acceptance_radius_m << std::endl;
std::cout << time_str() << "target_pos.lat: " << target_pos.latitude_deg << std::endl;
std::cout << time_str() << "target_pos.lon: " << target_pos.longitude_deg << std::endl;
std::cout << time_str() << "current.lat: " << current_pos.latitude_deg << std::endl;
std::cout << time_str() << "current.lon: " << current_pos.longitude_deg << std::endl;
std::cout << time_str() << "Distance: " << distance_m << std::endl;
std::cout << time_str() << "Acceptance radius: " << acceptance_radius_m << std::endl;
}
return pass;
@ -632,12 +632,12 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry:
const bool pass = distance_m > min_distance_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_m << std::endl;
std::cout << "Min distance: " << min_distance_m << std::endl;
std::cout << time_str() << "target_pos.lat: " << target_pos.latitude_deg << std::endl;
std::cout << time_str() << "target_pos.lon: " << target_pos.longitude_deg << std::endl;
std::cout << time_str() << "current.lat: " << current_pos.latitude_deg << std::endl;
std::cout << time_str() << "current.lon: " << current_pos.longitude_deg << std::endl;
std::cout << time_str() << "Distance: " << distance_m << std::endl;
std::cout << time_str() << "Min distance: " << min_distance_m << std::endl;
}
return pass;

19
test/mavsdk_tests/autopilot_tester.h
View File

@ -45,6 +45,7 @@
#include <mavsdk/plugins/param/param.h>
#include "catch2/catch.hpp"
#include <chrono>
#include <ctime>
#include <iostream>
#include <memory>
#include <thread>
@ -54,6 +55,17 @@ extern std::string connection_url;
using namespace mavsdk;
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
{
public:
@ -136,8 +148,8 @@ private:
const int64_t elapsed_time_ms = _info->get_flight_information().second.time_boot_ms - start_time;
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::endl;
std::cout << time_str() << "Timeout, connected to vehicle but waiting for test for "
<< elapsed_time_ms / 1000.0 << " seconds\n";
return false;
}
}
@ -152,7 +164,8 @@ private:
start_time).count();
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;
}
}