Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

use separate altitude offset check in FW

Browse Source
This commit is contained in:
Andreas Antener
2016-05-15 16:33:44 +02:00
committed by Lorenz Meier
parent 05dc643f17
commit 57fa9d2070
1 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
integrationtests/python_src/px4_it/mavros/mission_test.py
+10 -7
View File
@@ -73,6 +73,7 @@ class MavrosMissionTest(unittest.TestCase):
self.home_alt = 0 self.home_alt = 0
self.mc_rad = 5 self.mc_rad = 5
self.fw_rad = 50 self.fw_rad = 50
self.fw_alt_rad = 10
self.last_alt_d = 9999 self.last_alt_d = 9999
self.last_pos_d = 9999 self.last_pos_d = 9999
@@ -111,7 +112,7 @@ class MavrosMissionTest(unittest.TestCase):
# #
# Helper methods # Helper methods
# #
def is_at_position(self, lat, lon, alt, offset): def is_at_position(self, lat, lon, alt, xy_offset, z_offset):
R = 6371000 # metres R = 6371000 # metres
rlat1 = math.radians(lat) rlat1 = math.radians(lat)
rlat2 = math.radians(self.global_position.latitude) rlat2 = math.radians(self.global_position.latitude)
@@ -135,7 +136,7 @@ class MavrosMissionTest(unittest.TestCase):
if self.last_alt_d > alt_d: if self.last_alt_d > alt_d:
self.last_alt_d = alt_d self.last_alt_d = alt_d
return d < offset and alt_d < offset return d < xy_offset and alt_d < z_offset
def reach_position(self, lat, lon, alt, timeout, index): def reach_position(self, lat, lon, alt, timeout, index):
# reset best distances # reset best distances
@@ -146,11 +147,13 @@ class MavrosMissionTest(unittest.TestCase):
count = 0 count = 0
while count < timeout: while count < timeout:
# use different radius matching vehicle state # use different radius matching vehicle state
radius = self.mc_rad xy_radius = self.mc_rad
z_radius = self.mc_rad
if self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW: if self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW:
radius = self.fw_rad xy_radius = self.fw_rad
z_radius = self.fw_alt_rad
if self.is_at_position(lat, lon, alt, radius): if self.is_at_position(lat, lon, alt, xy_radius, z_radius):
rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" % rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" %
(index, count, self.last_pos_d, self.last_alt_d)) (index, count, self.last_pos_d, self.last_alt_d))
break break
@@ -158,8 +161,8 @@ class MavrosMissionTest(unittest.TestCase):
self.rate.sleep() self.rate.sleep()
self.assertTrue(count < timeout, ("took too long to get to position " + self.assertTrue(count < timeout, ("took too long to get to position " +
"lat: %f, lon: %f, alt: %f, radius: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f" % "lat: %f, lon: %f, alt: %f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f" %
(lat, lon, alt, radius, timeout, index, self.last_pos_d, self.last_alt_d))) (lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d)))
def run_mission(self): def run_mission(self):
"""switch mode: armed | auto""" """switch mode: armed | auto"""