From 79fbcf8a1cff8b8e342a6169d6a11a74d452acc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Dec 2016 17:01:51 +0100 Subject: [PATCH] Tuning to integration testing for better reporting --- .../python_src/px4_it/mavros/mission_test.py | 23 ++++++++++++++++--- .../python_src/px4_it/mavros/vtol_new_1.txt | 4 ++-- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index f4db60d90d..1c15596583 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -111,7 +111,12 @@ class MavrosMissionTest(unittest.TestCase): self.has_global_pos = True def extended_state_callback(self, data): + + prev_state = self.extended_state.vtol_state; + self.extended_state = data + if (prev_state != self.extended_state.vtol_state): + print("VTOL state change: %d" % self.extended_state.vtol_state); # # Helper methods @@ -148,7 +153,7 @@ class MavrosMissionTest(unittest.TestCase): self.last_pos_d = 9999 rospy.loginfo("trying to reach waypoint " + - "lat: %f, lon: %f, alt: %f, timeout: %d, index: %d" % + "lat: %13.9f, lon: %13.9f, alt: %6.2f, timeout: %d, index: %d" % (lat, lon, alt, timeout, index)) # does it reach the position in X seconds? @@ -174,9 +179,20 @@ class MavrosMissionTest(unittest.TestCase): count = count + 1 self.rate.sleep() + vtol_state_string = "VTOL undefined" + + if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC): + vtol_state_string = "VTOL MC" + if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW): + vtol_state_string = "VTOL FW" + if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_MC): + vtol_state_string = "VTOL FW->MC" + if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_FW): + vtol_state_string = "VTOL MC->FW" + self.assertTrue(count < timeout, (("(%s) took too long to get to position " + - "lat: %f, lon: %f, alt: %f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f") % - (self.mission_name, lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d))) + "lat: %13.9f, lon: %13.9f, alt: %6.2f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f, VTOL state: %s") % + (self.mission_name, lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d, vtol_state_string))) def run_mission(self): """switch mode: auto and arm""" @@ -283,6 +299,7 @@ class MavrosMissionTest(unittest.TestCase): alt = waypoint.z_alt if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT: alt += self.home_alt + self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600, index) # check if VTOL transition happens if applicable diff --git a/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt index 527bd5f5ad..8165e35bc6 100644 --- a/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt +++ b/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt @@ -1,4 +1,4 @@ # New style transitions, takeoff not at home QGC WPL 110 -0 1 3 84 15.0 0 0 0 47.397731862593744 8.543269295853861 12.0 1 -1 0 3 85 0.0 0.0 -0.0 0.0 47.3990753922183 8.5432371088360526 0.0 1 +0 1 3 84 15.0 0 0 0 47.397731862593744 8.543269295853861 15.0 1 +1 0 3 85 0.0 0.0 -0.0 0.0 47.39651890962795 8.5433217125134888 0.0 1