yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

CI: improve mavros SITL tests logging (#8714)

Browse Source 
* add more logging to help with #8556
* log subscribed topics on mission start and test exit (pass or fail)
* use mavlink enums everywhere to avoid maintaining dictionary mappings and to have readable values
* log when the FCU advances to next mission item without satisfying the position reached offset/radius
* some renaming for readability
* log more state value changes (connected and MAV_STATUS)
This commit is contained in:
Anthony Lamping
2018-01-17 17:54:32 -05:00
committed by Daniel Agar
parent 821eb5ac87
commit 85ba160757
4 changed files with 132 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -43,8 +43,9 @@ PKG = 'px4'
import rospy import rospy
from geometry_msgs.msg import Quaternion, Vector3 from geometry_msgs.msg import Quaternion, Vector3
from mavros_msgs.msg import AttitudeTarget, ExtendedState from mavros_msgs.msg import AttitudeTarget
from mavros_test_common import MavrosTestCommon from mavros_test_common import MavrosTestCommon
from pymavlink import mavutil
from std_msgs.msg import Header from std_msgs.msg import Header
from threading import Thread from threading import Thread
from tf.transformations import quaternion_from_euler from tf.transformations import quaternion_from_euler
@@ -72,7 +73,7 @@ class MavrosOffboardAttctlTest(MavrosTestCommon):
self.att_thread.start() self.att_thread.start()
def tearDown(self): def tearDown(self):
pass super(MavrosOffboardAttctlTest, self).tearDown()
# #
# Helper methods # Helper methods
@@ -107,8 +108,10 @@ class MavrosOffboardAttctlTest(MavrosTestCommon):
# make sure the simulation is ready to start the mission # make sure the simulation is ready to start the mission
self.wait_for_topics(60) self.wait_for_topics(60)
self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND, 10, -1) self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
10, -1)
self.log_topic_vars()
self.set_mode("OFFBOARD", 5) self.set_mode("OFFBOARD", 5)
self.set_arm(True, 5) self.set_arm(True, 5)

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

@@ -45,8 +45,8 @@ import rospy
import math import math
import numpy as np import numpy as np
from geometry_msgs.msg import PoseStamped, Quaternion from geometry_msgs.msg import PoseStamped, Quaternion
from mavros_msgs.msg import ExtendedState
from mavros_test_common import MavrosTestCommon from mavros_test_common import MavrosTestCommon
from pymavlink import mavutil
from std_msgs.msg import Header from std_msgs.msg import Header
from threading import Thread from threading import Thread
from tf.transformations import quaternion_from_euler from tf.transformations import quaternion_from_euler
@@ -77,7 +77,7 @@ class MavrosOffboardPosctlTest(MavrosTestCommon):
self.pos_thread.start() self.pos_thread.start()
def tearDown(self): def tearDown(self):
pass super(MavrosOffboardPosctlTest, self).tearDown()
# #
# Helper methods # Helper methods
@@ -155,8 +155,10 @@ class MavrosOffboardPosctlTest(MavrosTestCommon):
# make sure the simulation is ready to start the mission # make sure the simulation is ready to start the mission
self.wait_for_topics(60) self.wait_for_topics(60)
self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND, 10, -1) self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
10, -1)
self.log_topic_vars()
self.set_mode("OFFBOARD", 5) self.set_mode("OFFBOARD", 5)
self.set_arm(True, 5) self.set_arm(True, 5)

114
integrationtests/python_src/px4_it/mavros/mavros_test_common.py
View File

@@ -9,26 +9,11 @@ from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, State, \
WaypointList WaypointList
from mavros_msgs.srv import CommandBool, ParamGet, SetMode, WaypointClear, \ from mavros_msgs.srv import CommandBool, ParamGet, SetMode, WaypointClear, \
WaypointPush WaypointPush
from pymavlink import mavutil
from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import NavSatFix
class MavrosTestCommon(unittest.TestCase): class MavrosTestCommon(unittest.TestCase):
# dictionaries correspond to mavros ExtendedState msg
LAND_STATES = {
0: 'UNDEFINED',
1: 'ON_GROUND',
2: 'IN_AIR',
3: 'TAKEOFF',
4: 'LANDING'
}
VTOL_STATES = {
0: 'VTOL UNDEFINED',
1: 'VTOL MC-to-FW',
2: 'VTOL FW-to-MC',
3: 'VTOL MC',
4: 'VTOL FW'
}
def __init__(self, *args): def __init__(self, *args):
super(MavrosTestCommon, self).__init__(*args) super(MavrosTestCommon, self).__init__(*args)
@@ -91,6 +76,9 @@ class MavrosTestCommon(unittest.TestCase):
self.state_sub = rospy.Subscriber('mavros/state', State, self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback) self.state_callback)
def tearDown(self):
self.log_topic_vars()
# #
# Callback functions # Callback functions
# #
@@ -104,13 +92,15 @@ class MavrosTestCommon(unittest.TestCase):
def extended_state_callback(self, data): def extended_state_callback(self, data):
if self.extended_state.vtol_state != data.vtol_state: if self.extended_state.vtol_state != data.vtol_state:
rospy.loginfo("VTOL state changed from {0} to {1}".format( rospy.loginfo("VTOL state changed from {0} to {1}".format(
self.VTOL_STATES.get(self.extended_state.vtol_state), mavutil.mavlink.enums['MAV_VTOL_STATE']
self.VTOL_STATES.get(data.vtol_state))) [self.extended_state.vtol_state].name, mavutil.mavlink.enums[
'MAV_VTOL_STATE'][data.vtol_state].name))
if self.extended_state.landed_state != data.landed_state: if self.extended_state.landed_state != data.landed_state:
rospy.loginfo("landed state changed from {0} to {1}".format( rospy.loginfo("landed state changed from {0} to {1}".format(
self.LAND_STATES.get(self.extended_state.landed_state), mavutil.mavlink.enums['MAV_LANDED_STATE']
self.LAND_STATES.get(data.landed_state))) [self.extended_state.landed_state].name, mavutil.mavlink.enums[
'MAV_LANDED_STATE'][data.landed_state].name))
self.extended_state = data self.extended_state = data
@@ -150,10 +140,20 @@ class MavrosTestCommon(unittest.TestCase):
rospy.loginfo("armed state changed from {0} to {1}".format( rospy.loginfo("armed state changed from {0} to {1}".format(
self.state.armed, data.armed)) self.state.armed, data.armed))
if self.state.connected != data.connected:
rospy.loginfo("connected changed from {0} to {1}".format(
self.state.connected, data.connected))
if self.state.mode != data.mode: if self.state.mode != data.mode:
rospy.loginfo("mode changed from {0} to {1}".format( rospy.loginfo("mode changed from {0} to {1}".format(
self.state.mode, data.mode)) self.state.mode, data.mode))
if self.state.system_status != data.system_status:
rospy.loginfo("system_status changed from {0} to {1}".format(
mavutil.mavlink.enums['MAV_STATE'][
self.state.system_status].name, mavutil.mavlink.enums[
'MAV_STATE'][data.system_status].name))
self.state = data self.state = data
# mavros publishes a disconnected state message on init # mavros publishes a disconnected state message on init
@@ -173,9 +173,8 @@ class MavrosTestCommon(unittest.TestCase):
for i in xrange(timeout * loop_freq): for i in xrange(timeout * loop_freq):
if self.state.armed == arm: if self.state.armed == arm:
arm_set = True arm_set = True
rospy.loginfo( rospy.loginfo("set arm success | seconds: {0} of {1}".format(
"set arm success | new arm: {0}, old arm: {1} | seconds: {2} of {3}". i / loop_freq, timeout))
format(arm, old_arm, i / loop_freq, timeout))
break break
else: else:
try: try:
@@ -201,9 +200,8 @@ class MavrosTestCommon(unittest.TestCase):
for i in xrange(timeout * loop_freq): for i in xrange(timeout * loop_freq):
if self.state.mode == mode: if self.state.mode == mode:
mode_set = True mode_set = True
rospy.loginfo( rospy.loginfo("set mode success | seconds: {0} of {1}".format(
"set mode success | new mode: {0}, old mode: {1} | seconds: {2} of {3}". i / loop_freq, timeout))
format(mode, old_mode, i / loop_freq, timeout))
break break
else: else:
try: try:
@@ -240,53 +238,52 @@ class MavrosTestCommon(unittest.TestCase):
"failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}". "failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}".
format(self.sub_topics_ready, timeout))) format(self.sub_topics_ready, timeout)))
def wait_on_landed_state(self, desired_landed_state, timeout, index): def wait_for_landed_state(self, desired_landed_state, timeout, index):
rospy.loginfo( rospy.loginfo("waiting for landed state | state: {0}, index: {1}".
"waiting for landed state | state: {0}, index: {1}".format( format(mavutil.mavlink.enums['MAV_LANDED_STATE'][
self.LAND_STATES.get(desired_landed_state), index)) desired_landed_state].name, index))
loop_freq = 10 # Hz loop_freq = 10 # Hz
rate = rospy.Rate(loop_freq) rate = rospy.Rate(loop_freq)
landed_state_confirmed = False landed_state_confirmed = False
for i in xrange(timeout * loop_freq): for i in xrange(timeout * loop_freq):
if self.extended_state.landed_state == desired_landed_state: if self.extended_state.landed_state == desired_landed_state:
landed_state_confirmed = True landed_state_confirmed = True
rospy.loginfo( rospy.loginfo("landed state confirmed | seconds: {0} of {1}".
"landed state confirmed | state: {0}, index: {1} | seconds: {2} of {3}". format(i / loop_freq, timeout))
format(
self.LAND_STATES.get(desired_landed_state), index, i /
loop_freq, timeout))
break break
rate.sleep() rate.sleep()
self.assertTrue(landed_state_confirmed, ( self.assertTrue(landed_state_confirmed, (
"landed state not detected | desired: {0}, current: {1} | index: {2}, timeout(seconds): {3}". "landed state not detected | desired: {0}, current: {1} | index: {2}, timeout(seconds): {3}".
format( format(mavutil.mavlink.enums['MAV_LANDED_STATE'][
self.LAND_STATES.get(desired_landed_state), desired_landed_state].name, mavutil.mavlink.enums[
self.LAND_STATES.get(self.extended_state.landed_state), index, 'MAV_VTOL_STATE'][self.extended_state.landed_state].name,
timeout))) index, timeout)))
def wait_on_transition(self, transition, timeout, index): def wait_for_vtol_state(self, transition, timeout, index):
"""Wait for VTOL transition, timeout(int): seconds""" """Wait for VTOL transition, timeout(int): seconds"""
rospy.loginfo( rospy.loginfo(
"waiting for VTOL transition | transition: {0}, index: {1}".format( "waiting for VTOL transition | transition: {0}, index: {1}".format(
self.VTOL_STATES.get(transition), index)) mavutil.mavlink.enums['MAV_VTOL_STATE'][
transition].name, index))
loop_freq = 10 # Hz loop_freq = 10 # Hz
rate = rospy.Rate(loop_freq) rate = rospy.Rate(loop_freq)
transitioned = False transitioned = False
for i in xrange(timeout * loop_freq): for i in xrange(timeout * loop_freq):
if transition == self.extended_state.vtol_state: if transition == self.extended_state.vtol_state:
rospy.loginfo( rospy.loginfo("transitioned | seconds: {0} of {1}".format(
"transitioned | index: {0} | seconds: {1} of {2}".format( i / loop_freq, timeout))
index, i / loop_freq, timeout))
transitioned = True transitioned = True
break break
rate.sleep() rate.sleep()
self.assertTrue(transitioned, ( self.assertTrue(transitioned, (
"transition not detected | index: {0} | timeout(seconds): {1}". "transition not detected | desired: {0}, current: {1} | index: {2} timeout(seconds): {3}".
format(index, timeout))) format(mavutil.mavlink.enums['MAV_VTOL_STATE'][transition].name,
mavutil.mavlink.enums['MAV_VTOL_STATE'][
self.extended_state.vtol_state].name, index, timeout)))
def clear_wps(self, timeout): def clear_wps(self, timeout):
"""timeout(int): seconds""" """timeout(int): seconds"""
@@ -362,8 +359,9 @@ class MavrosTestCommon(unittest.TestCase):
if res.success: if res.success:
self.mav_type = res.value.integer self.mav_type = res.value.integer
rospy.loginfo( rospy.loginfo(
"MAV_TYPE received | value: {0} | seconds: {1} of {2}". "MAV_TYPE received | type: {0} | seconds: {1} of {2}".
format(self.mav_type, i / loop_freq, timeout)) format(mavutil.mavlink.enums['MAV_TYPE'][self.mav_type]
.name, i / loop_freq, timeout))
break break
except rospy.ServiceException as e: except rospy.ServiceException as e:
rospy.logerr(e) rospy.logerr(e)
@@ -373,3 +371,23 @@ class MavrosTestCommon(unittest.TestCase):
self.assertTrue(res.success, ( self.assertTrue(res.success, (
"MAV_TYPE param get failed | timeout(seconds): {0}".format(timeout) "MAV_TYPE param get failed | timeout(seconds): {0}".format(timeout)
)) ))
def log_topic_vars(self):
"""log the state of topic variables"""
rospy.loginfo("========================")
rospy.loginfo("===== topic values =====")
rospy.loginfo("========================")
rospy.loginfo("altitude:\n{}".format(self.altitude))
rospy.loginfo("========================")
rospy.loginfo("extended_state:\n{}".format(self.extended_state))
rospy.loginfo("========================")
rospy.loginfo("global_position:\n{}".format(self.global_position))
rospy.loginfo("========================")
rospy.loginfo("home_position:\n{}".format(self.home_position))
rospy.loginfo("========================")
rospy.loginfo("local_position:\n{}".format(self.local_position))
rospy.loginfo("========================")
rospy.loginfo("mission_wp:\n{}".format(self.mission_wp))
rospy.loginfo("========================")
rospy.loginfo("state:\n{}".format(self.state))
rospy.loginfo("========================")

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

@@ -51,7 +51,7 @@ import px4tools
import sys import sys
from mavros import mavlink from mavros import mavlink
from mavros.mission import QGroundControlWP from mavros.mission import QGroundControlWP
from mavros_msgs.msg import ExtendedState, Mavlink, Waypoint from mavros_msgs.msg import Mavlink, Waypoint
from mavros_test_common import MavrosTestCommon from mavros_test_common import MavrosTestCommon
from pymavlink import mavutil from pymavlink import mavutil
from threading import Thread from threading import Thread
@@ -151,11 +151,9 @@ class MavrosMissionTest(MavrosTestCommon):
def setUp(self): def setUp(self):
super(self.__class__, self).setUp() super(self.__class__, self).setUp()
self.mc_rad = 5 self.mc_rad = 5 # meters
self.fw_rad = 60 self.fw_rad = 60
self.fw_alt_rad = 10 self.fw_alt_rad = 10
self.last_alt_d = None
self.last_pos_d = None
self.mission_name = "" self.mission_name = ""
self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
@@ -170,7 +168,7 @@ class MavrosMissionTest(MavrosTestCommon):
self.hb_thread.start() self.hb_thread.start()
def tearDown(self): def tearDown(self):
pass super(MavrosMissionTest, self).tearDown()
# #
# Helper methods # Helper methods
@@ -200,65 +198,73 @@ class MavrosMissionTest(MavrosTestCommon):
d = R * c d = R * c
alt_d = abs(alt - self.altitude.amsl) alt_d = abs(alt - self.altitude.amsl)
# remember best distances
if not self.last_pos_d or self.last_pos_d > d:
self.last_pos_d = d
if not self.last_alt_d or self.last_alt_d > alt_d:
self.last_alt_d = alt_d
rospy.logdebug("d: {0}, alt_d: {1}".format(d, alt_d)) rospy.logdebug("d: {0}, alt_d: {1}".format(d, alt_d))
return d < xy_offset and alt_d < z_offset return (d < xy_offset and alt_d < z_offset), d, alt_d
def reach_position(self, lat, lon, alt, timeout, index): def reach_position(self, lat, lon, alt, timeout, index):
"""alt(amsl): meters, timeout(int): seconds""" """alt(amsl): meters, timeout(int): seconds"""
# reset best distances
self.last_alt_d = None
self.last_pos_d = None
rospy.loginfo( rospy.loginfo(
"trying to reach waypoint | lat: {0:.9f}, lon: {1:.9f}, alt: {2:.2f}, index: {3}". "trying to reach waypoint | lat: {0:.9f}, lon: {1:.9f}, alt: {2:.2f}, index: {3}".
format(lat, lon, alt, index)) format(lat, lon, alt, index))
best_pos_xy_d = None
best_pos_z_d = None
fcu_advanced = False
# does it reach the position in 'timeout' seconds? # does it reach the position in 'timeout' seconds?
loop_freq = 2 # Hz loop_freq = 2 # Hz
rate = rospy.Rate(loop_freq) rate = rospy.Rate(loop_freq)
reached = False
for i in xrange(timeout * loop_freq): for i in xrange(timeout * loop_freq):
# use FW radius if VTOL in FW or transition or FW # use FW radius if VTOL in FW or transition or FW
if (self.mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING or if (self.mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING or
self.extended_state.vtol_state == self.extended_state.vtol_state ==
ExtendedState.VTOL_STATE_FW or mavutil.mavlink.MAV_VTOL_STATE_FW or
self.extended_state.vtol_state == self.extended_state.vtol_state ==
ExtendedState.VTOL_STATE_TRANSITION_TO_MC or mavutil.mavlink.MAV_VTOL_STATE_TRANSITION_TO_MC or
self.extended_state.vtol_state == self.extended_state.vtol_state ==
ExtendedState.VTOL_STATE_TRANSITION_TO_FW): mavutil.mavlink.MAV_VTOL_STATE_TRANSITION_TO_FW):
xy_radius = self.fw_rad xy_radius = self.fw_rad
z_radius = self.fw_alt_rad z_radius = self.fw_alt_rad
else: # assume MC else: # assume MC
xy_radius = self.mc_rad xy_radius = self.mc_rad
z_radius = self.mc_rad z_radius = self.mc_rad
if self.is_at_position(lat, lon, alt, xy_radius, z_radius): reached, pos_xy_d, pos_z_d = self.is_at_position(
reached = True lat, lon, alt, xy_radius, z_radius)
# remember best distances
if not best_pos_xy_d or best_pos_xy_d > pos_xy_d:
best_pos_xy_d = pos_xy_d
if not best_pos_z_d or best_pos_z_d > pos_z_d:
best_pos_z_d = pos_z_d
if reached:
rospy.loginfo( rospy.loginfo(
"position reached | pos_d: {0:.2f}, alt_d: {1:.2f}, index: {2} | seconds: {3} of {4}". "position reached | pos_xy_d: {0:.2f}, pos_z_d: {1:.2f}, index: {2} | seconds: {3} of {4}".
format(self.last_pos_d, self.last_alt_d, index, i / format(pos_xy_d, pos_z_d, index, i / loop_freq, timeout))
loop_freq, timeout))
break break
elif not fcu_advanced and (index < self.mission_wp.current_seq or (
index == len(self.mission_wp.waypoints) and
self.mission_wp.current_seq == 0)):
# FCU advanced to the next mission item, or finished mission
fcu_advanced = True
rospy.loginfo(
"FCU advanced to mission item index {0} when checking position | xy off: {1}, z off: {2}, pos_xy_d: {3:.2f}, pos_z_d: {4:.2f},".
format(self.mission_wp.current_seq, xy_radius, z_radius,
pos_xy_d, pos_z_d))
rate.sleep() rate.sleep()
self.assertTrue(reached, ( self.assertTrue(
"took too long to get to position | lat: {0:.9f}, lon: {1:.9f}, alt: {2:.2f}, xy off: {3}, z off: {4}, pos_d: {5:.2f}, alt_d: {6:.2f}, index: {7} | timeout(seconds): {8}". reached,
format(lat, lon, alt, xy_radius, z_radius, self.last_pos_d, "took too long to get to position | lat: {0:.9f}, lon: {1:.9f}, alt: {2:.2f}, xy off: {3}, z off: {4}, current pos_xy_d: {5:.2f}, current pos_z_d: {6:.2f}, best pos_xy_d: {7:.2f}, best pos_z_d: {8:.2f}, index: {9} | timeout(seconds): {10}".
self.last_alt_d, index, timeout))) format(lat, lon, alt, xy_radius, z_radius, pos_xy_d, pos_z_d,
best_pos_xy_d, best_pos_z_d, index, timeout))
# #
# Test method # Test method
# #
def test_mission(self): def test_mission(self):
"""Test mission""" """Test mission"""
if len(sys.argv) < 2: if len(sys.argv) < 2:
self.fail("usage: mission_test.py mission_file") self.fail("usage: mission_test.py mission_file")
return return
@@ -275,18 +281,21 @@ class MavrosMissionTest(MavrosTestCommon):
# make sure the simulation is ready to start the mission # make sure the simulation is ready to start the mission
self.wait_for_topics(60) self.wait_for_topics(60)
self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND, 10, -1) self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
10, -1)
self.wait_for_mav_type(10) self.wait_for_mav_type(10)
# push waypoints to FCU and start mission # push waypoints to FCU and start mission
self.send_wps(wps, 30) self.send_wps(wps, 30)
self.log_topic_vars()
self.set_mode("AUTO.MISSION", 5) self.set_mode("AUTO.MISSION", 5)
self.set_arm(True, 5) self.set_arm(True, 5)
rospy.loginfo("run mission {0}".format(self.mission_name)) rospy.loginfo("run mission {0}".format(self.mission_name))
for index, waypoint in enumerate(wps): for index, waypoint in enumerate(wps):
# only check position for waypoints where this makes sense # only check position for waypoints where this makes sense
if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or waypoint.frame == Waypoint.FRAME_GLOBAL: if (waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or
waypoint.frame == Waypoint.FRAME_GLOBAL):
alt = waypoint.z_alt alt = waypoint.z_alt
if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT: if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
alt += self.altitude.amsl - self.altitude.relative alt += self.altitude.amsl - self.altitude.relative
@@ -295,21 +304,23 @@ class MavrosMissionTest(MavrosTestCommon):
index) index)
# check if VTOL transition happens if applicable # check if VTOL transition happens if applicable
if waypoint.command == 84 or waypoint.command == 85 or waypoint.command == 3000: if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF or
transition = waypoint.param1 waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND
or waypoint.command ==
mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION):
transition = waypoint.param1 # used by MAV_CMD_DO_VTOL_TRANSITION
if waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF: # VTOL takeoff implies transition to FW
transition = mavutil.mavlink.MAV_VTOL_STATE_FW
if waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: # VTOL land implies transition to MC
transition = mavutil.mavlink.MAV_VTOL_STATE_MC
if waypoint.command == 84: # VTOL takeoff implies transition to FW self.wait_for_vtol_state(transition, 60, index)
transition = ExtendedState.VTOL_STATE_FW
if waypoint.command == 85: # VTOL land implies transition to MC
transition = ExtendedState.VTOL_STATE_MC
self.wait_on_transition(transition, 60, index)
# after reaching position, wait for landing detection if applicable # after reaching position, wait for landing detection if applicable
if waypoint.command == 85 or waypoint.command == 21: if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND or
self.wait_on_landed_state(ExtendedState.LANDED_STATE_ON_GROUND, waypoint.command == mavutil.mavlink.MAV_CMD_NAV_LAND):
60, index) self.wait_for_landed_state(
mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, 60, index)
self.set_arm(False, 5) self.set_arm(False, 5)
self.clear_wps(5) self.clear_wps(5)