Skip to content

Commit

Permalink
mavproxy_cmdlong: make command_int take all arguments for a command_int
Browse files Browse the repository at this point in the history
Like command_long you should be able to form up any message -
well-formed or not

Also make it easier to select a frame
  • Loading branch information
peterbarker committed Jan 3, 2018
1 parent b8cd8a7 commit 6c7051d
Showing 1 changed file with 35 additions and 16 deletions.
51 changes: 35 additions & 16 deletions MAVProxy/modules/mavproxy_cmdlong.py
Original file line number Diff line number Diff line change
Expand Up @@ -345,34 +345,53 @@ def cmd_long(self, args):

def cmd_command_int(self, args):
'''execute supplied command_int'''
if len(args) != 9:
if len(args) != 11:
print("num args{0}".format(len(args)))
print("Usage: command_int <command> frame param1 param2 param3 param4 x y z")
print("Usage: command_int frame command current autocontinue param1 param2 param3 param4 x y z")
print("e.g. command_int GLOBAL_RELATIVE_ALT DO_SET_HOME 0 0 0 0 0 0 -353632120 1491659330 0")
print("e.g. command_int GLOBAL MAV_CMD_DO_SET_ROI 0 0 0 0 0 0 5000000 5000000 500")
return
command = None

if args[0].isdigit():
command = int(args[0])
frame = int(args[0])
else:
try:
command = eval("mavutil.mavlink." + args[0])
# attempt to allow MAV_FRAME_GLOBAL for frame
frame = eval("mavutil.mavlink." + args[0])
except AttributeError as e:
try:
command = eval("mavutil.mavlink.MAV_CMD_" + args[0])
# attempt to allow GLOBAL for frame
frame = eval("mavutil.mavlink.MAV_FRAME_" + args[0])
except AttributeError as e:
pass

if command is None:
print("Unknown command_int ({0})".format(args[0]))
if frame is None:
print("Unknown frame ({0})".format(args[0]))
return

frame = int(args[1])
param1 = float(args[2])
param2 = float(args[3])
param3 = float(args[4])
param4 = float(args[5])
x = int(args[6])
y = int(args[7])
z = float(args[8])
command = None
if args[1].isdigit():
command = int(args[1])
else:
# let "command_int ... MAV_CMD_DO_SET_HOME ..." work
try:
command = eval("mavutil.mavlink." + args[1])
except AttributeError as e:
try:
# let "command_int ... DO_SET_HOME" work
command = eval("mavutil.mavlink.MAV_CMD_" + args[1])
except AttributeError as e:
pass

current = int(args[2])
autocontinue = int(args[3])
param1 = float(args[4])
param2 = float(args[5])
param3 = float(args[6])
param4 = float(args[7])
x = int(args[8])
y = int(args[9])
z = float(args[10])
self.master.mav.command_int_send(self.settings.target_system,
self.settings.target_component,
frame,
Expand Down

0 comments on commit 6c7051d

Please sign in to comment.