diff --git a/MAVProxy/mavproxy.py b/MAVProxy/mavproxy.py index 641e9a9eae..546905b7ea 100644 --- a/MAVProxy/mavproxy.py +++ b/MAVProxy/mavproxy.py @@ -267,6 +267,7 @@ def __init__(self): MPSetting('rallyalt', int, 90, 'Default Rally Altitude', range=(0,10000), increment=1), MPSetting('terrainalt', str, 'Auto', 'Use terrain altitudes', choice=['Auto','True','False']), MPSetting('guidedalt', int, 100, 'Default "Fly To" Altitude', range=(0,10000), increment=1), + MPSetting('guided_use_reposition', bool, True, 'Use MAV_CMD_DO_REPOSITION for guided fly-to'), MPSetting('rally_breakalt', int, 40, 'Default Rally Break Altitude', range=(0,10000), increment=1), MPSetting('rally_flags', int, 0, 'Default Rally Flags', range=(0,10000), increment=1), diff --git a/MAVProxy/modules/mavproxy_mode.py b/MAVProxy/modules/mavproxy_mode.py index 4ec12cca09..8b715ae938 100644 --- a/MAVProxy/modules/mavproxy_mode.py +++ b/MAVProxy/modules/mavproxy_mode.py @@ -84,6 +84,25 @@ def cmd_guided(self, args): altitude = float(args[0]) print("Guided %s %s" % (str(latlon), str(altitude))) + + if self.settings.guided_use_reposition: + self.master.mav.command_int_send( + self.settings.target_system, + self.settings.target_component, + self.module('wp').get_default_frame(), + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + 0, # current + 0, # autocontinue + -1, # p1 - ground speed, -1 is use-default + mavutil.mavlink.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, # p2 - flags + 0, # p3 - loiter radius for Planes, 0 is ignored + 0, # p4 - yaw - 0 is loiter clockwise + int(latlon[0]*1.0e7), + int(latlon[1]*1.0e7), + altitude + ) + return + self.master.mav.mission_item_int_send( self.settings.target_system, self.settings.target_component,