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

simplify vtol transition check, more log msgs

This commit is contained in:
Anthony Lamping 2017-12-30 13:17:01 -05:00 committed by Lorenz Meier
parent f9e7c66718
commit ab5a268ca5
3 changed files with 19 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py
View File

@ -221,6 +221,10 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
#
def test_attctl(self):
"""Test offboard attitude control"""
# boundary to cross
boundary_x = 5
boundary_y = 5
boundary_z = -5
# delay starting the mission
self.wait_for_topics(30)
@ -231,15 +235,17 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
self.set_arm(True, 5)
rospy.loginfo("run mission")
rospy.loginfo("attempting to cross boundary | x: {0}, y: {1}, z: {2}".
format(boundary_x, boundary_y, boundary_z))
# does it cross expected boundaries in 'timeout' seconds?
timeout = 12 # (int) seconds
loop_freq = 10 # Hz
rate = rospy.Rate(loop_freq)
crossed = False
for i in xrange(timeout * loop_freq):
if (self.local_position.pose.position.x > 5 and
self.local_position.pose.position.z > 5 and
self.local_position.pose.position.y < -5):
if (self.local_position.pose.position.x > boundary_x and
self.local_position.pose.position.z > boundary_y and
self.local_position.pose.position.y < boundary_z):
rospy.loginfo("boundary crossed | seconds: {0} of {1}".format(
i / loop_freq, timeout))
crossed = True
@ -248,7 +254,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
rate.sleep()
self.assertTrue(crossed, (
"took too long to cross boundaries | x: {0}, y: {1}, z: {2} | timeout(seconds): {3}".
"took too long to cross boundaries | current position x: {0}, y: {1}, z: {2} | timeout(seconds): {3}".
format(self.local_position.pose.position.x,
self.local_position.pose.position.y,
self.local_position.pose.position.z, timeout)))

10
integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py
View File

@ -231,6 +231,8 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
self.pos.pose.position.x = x
self.pos.pose.position.y = y
self.pos.pose.position.z = z
rospy.loginfo("attempting to reach position | x: {0}, y: {1}, z: {2}".
format(x, y, z))
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = 0 # North
@ -246,17 +248,15 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
if self.is_at_position(self.pos.pose.position.x,
self.pos.pose.position.y,
self.pos.pose.position.z, 1):
rospy.loginfo(
"position reached | x: {0}, y: {1}, z: {2} | seconds: {3} of {4}".
format(self.pos.pose.position.x, self.pos.pose.position.y,
self.pos.pose.position.z, i / loop_freq, timeout))
rospy.loginfo("position reached | seconds: {0} of {1}".format(
i / loop_freq, timeout))
reached = True
break
rate.sleep()
self.assertTrue(reached, (
"took too long to get to position | x: {0}, y: {1}, z: {2} | timeout(seconds): {3}".
"took too long to get to position | current position x: {0}, y: {1}, z: {2} | timeout(seconds): {3}".
format(self.local_position.pose.position.x,
self.local_position.pose.position.y,
self.local_position.pose.position.z, timeout)))

21
integrationtests/python_src/px4_it/mavros/mission_test.py
View File

@ -346,7 +346,7 @@ class MavrosMissionTest(unittest.TestCase):
if self.is_at_position(lat, lon, alt, xy_radius, z_radius):
reached = True
rospy.loginfo(
"position reached | pos_d: {0}, alt_d: {1}, index: {2} | seconds: {3} of {4}".
"position reached | pos_d: {0:.2f}, alt_d: {1:.2f}, index: {2} | seconds: {3} of {4}".
format(self.last_pos_d, self.last_alt_d, index, i /
loop_freq, timeout))
break
@ -354,7 +354,7 @@ class MavrosMissionTest(unittest.TestCase):
rate.sleep()
self.assertTrue(reached, (
"({0}) took too long to get to position | lat: {1:13.9f}, lon: {2:13.9f}, alt: {3:6.2f}, xy off: {4}, z off: {5}, pos_d: {6}, alt_d: {7}, VTOL state: {8}, index: {9} | timeout(seconds): {10}".
"({0}) took too long to get to position | lat: {1:13.9f}, lon: {2:13.9f}, alt: {3:6.2f}, xy off: {4}, z off: {5}, pos_d: {6:.2f}, alt_d: {7:.2f}, VTOL state: {8}, index: {9} | timeout(seconds): {10}".
format(self.mission_name, lat, lon, alt, xy_radius, z_radius,
self.last_pos_d, self.last_alt_d,
self.VTOL_STATES.get(self.extended_state.vtol_state), index,
@ -392,7 +392,7 @@ class MavrosMissionTest(unittest.TestCase):
if self.extended_state.landed_state == desired_landed_state:
landed_state_confirmed = True
rospy.loginfo(
"landed state confirmed | state: {0}, index{1}".format(
"landed state confirmed | state: {0}, index: {1}".format(
self.LAND_STATES.get(desired_landed_state), index))
break
@ -414,20 +414,7 @@ class MavrosMissionTest(unittest.TestCase):
rate = rospy.Rate(loop_freq)
transitioned = False
for i in xrange(timeout * loop_freq):
# transition to MC
if (transition == ExtendedState.VTOL_STATE_MC and
self.extended_state.vtol_state ==
ExtendedState.VTOL_STATE_MC):
rospy.loginfo(
"transitioned | index: {0} | seconds: {1} of {2}".format(
index, i / loop_freq, timeout))
transitioned = True
break
# transition to FW
if (transition == ExtendedState.VTOL_STATE_FW and
self.extended_state.vtol_state ==
ExtendedState.VTOL_STATE_FW):
if transition == self.extended_state.vtol_state:
rospy.loginfo(
"transitioned | index: {0} | seconds: {1} of {2}".format(
index, i / loop_freq, timeout))