dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavsdk_tests: wait until home is set properly 

Otherwise we might have NaN stored as home.
This commit is contained in:
Julian Oes 2020-05-26 11:36:30 +02:00
parent 73dda496ef
commit d43e4d88c3
2 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -78,7 +78,12 @@ void AutopilotTester::wait_until_ready_local_position_only()
void AutopilotTester::store_home() void AutopilotTester::store_home()
{ {
request_ground_truth(); request_ground_truth();
_home = get_ground_truth_position(); 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)));
} }
void AutopilotTester::check_home_within(float acceptance_radius_m) void AutopilotTester::check_home_within(float acceptance_radius_m)
@ -241,12 +246,6 @@ bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::Pos
void AutopilotTester::request_ground_truth() void AutopilotTester::request_ground_truth()
{ {
CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
Telemetry::GroundTruth AutopilotTester::get_ground_truth_position()
{
return _telemetry->ground_truth();
} }
bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos,

3
test/mavsdk_tests/autopilot_tester.h
View File

@ -86,10 +86,9 @@ private:
const mavsdk::geometry::CoordinateTransformation::LocalCoordinate &local_coordinate, const mavsdk::geometry::CoordinateTransformation::LocalCoordinate &local_coordinate,
const MissionOptions &mission_options, const MissionOptions &mission_options,
const mavsdk::geometry::CoordinateTransformation &ct); const mavsdk::geometry::CoordinateTransformation &ct);
Telemetry::GroundTruth get_ground_truth_position();
bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m); bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m);
bool estimated_position_close_to(const Offboard::PositionNEDYaw &target_position, float acceptance_radius_m); bool estimated_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m);
bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m); bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m);
mavsdk::Mavsdk _mavsdk{}; mavsdk::Mavsdk _mavsdk{};