diff --git a/MAVProxy/modules/mavproxy_cmdlong.py b/MAVProxy/modules/mavproxy_cmdlong.py index 14ad11126a..a466c87e8b 100644 --- a/MAVProxy/modules/mavproxy_cmdlong.py +++ b/MAVProxy/modules/mavproxy_cmdlong.py @@ -1,13 +1,19 @@ #!/usr/bin/env python3 -'''command long''' +''' +command long + +AP_FLAKE8_CLEAN +''' + +import copy +import math +import time -import time, os -from numpy import equal from pymavlink import mavutil -from math import * from MAVProxy.modules.lib import mp_module + class CmdlongModule(mp_module.MPModule): def __init__(self, mpstate): super(CmdlongModule, self).__init__(mpstate, "cmdlong", public=True) @@ -31,9 +37,15 @@ def __init__(self, mpstate): self.add_command('pause', self.cmd_pause, "pause AUTO/GUIDED modes") self.add_command('resume', self.cmd_resume, "resume AUTO/GUIDED modes") + self.last_command_retry_ms = 0 + self.command_opaque_id_seq = 1 + self.command_queue = {} # ordered by timestamp in values + # we only permit retries after seeing a non-zero opaque ID: + self.command_retry_permitted = 0 + def cmd_long_commands(self): atts = dir(mavutil.mavlink) - atts = filter( lambda x : x.lower().startswith("mav_cmd"), atts) + atts = filter(lambda x : x.lower().startswith("mav_cmd"), atts) ret = [] for att in atts: ret.append(att) @@ -82,7 +94,7 @@ def cmd_parachute(self, args): 'enable' : mavutil.mavlink.PARACHUTE_ENABLE, 'disable' : mavutil.mavlink.PARACHUTE_DISABLE, 'release' : mavutil.mavlink.PARACHUTE_RELEASE - } + } if not args[0] in cmds: print(usage) return @@ -118,7 +130,7 @@ def cmd_cammsg(self, args): params = [0, 0, 0, 0, 1, 0, 0] # fill in any args passed by user - for i in range(min(len(args),len(params))): + for i in range(min(len(args), len(params))): params[i] = float(args[i]) print("Sent DIGICAM_CONTROL CMD_LONG") @@ -146,9 +158,9 @@ def cmd_engine(self, args): args[0] = '1' if args[0] == 'stop': args[0] = '0' - + # fill in any args passed by user - for i in range(min(len(args),len(params))): + for i in range(min(len(args), len(params))): params[i] = float(args[i]) self.master.mav.command_long_send( @@ -175,7 +187,7 @@ def cmd_cammsg_old(self, args): def cmd_do_change_speed(self, args): '''speed value''' - if ( len(args) != 1): + if (len(args) != 1): print("Usage: setspeed SPEED_VALUE") return @@ -197,7 +209,7 @@ def cmd_do_change_speed(self, args): def cmd_condition_yaw(self, args): '''yaw angle angular_speed angle_mode''' - if ( len(args) != 3): + if (len(args) != 3): print("Usage: yaw ANGLE ANGULAR_SPEED MODE:[0 absolute / 1 relative]") return @@ -231,15 +243,15 @@ def cmd_velocity(self, args): z_mps = float(args[2]) print("x:%f, y:%f, z:%f" % (x_mps, y_mps, z_mps)) self.master.mav.set_position_target_local_ned_send( - 0, # system time in milliseconds - self.settings.target_system, # target system - 0, # target component - 8, # coordinate frame MAV_FRAME_BODY_NED - 4039, # type mask (vel only) - 0, 0, 0, # position x,y,z - x_mps, y_mps, z_mps, # velocity x,y,z - 0, 0, 0, # accel x,y,z - 0, 0) # yaw, yaw rate + 0, # system time in milliseconds + self.settings.target_system, # target system + 0, # target component + 8, # coordinate frame MAV_FRAME_BODY_NED + 4039, # type mask (vel only) + 0, 0, 0, # position x,y,z + x_mps, y_mps, z_mps, # velocity x,y,z + 0, 0, 0, # accel x,y,z + 0, 0) # yaw, yaw rate def cmd_position(self, args): '''position x-m y-m z-m''' @@ -253,15 +265,15 @@ def cmd_position(self, args): z_m = float(args[2]) print("x:%f, y:%f, z:%f" % (x_m, y_m, z_m)) self.master.mav.set_position_target_local_ned_send( - 0, # system time in milliseconds - self.settings.target_system, # target system - 0, # target component - 8, # coordinate frame MAV_FRAME_BODY_NED - 3576, # type mask (pos only) - x_m, y_m, z_m, # position x,y,z - 0, 0, 0, # velocity x,y,z - 0, 0, 0, # accel x,y,z - 0, 0) # yaw, yaw rate + 0, # system time in milliseconds + self.settings.target_system, # target system + 0, # target component + 8, # coordinate frame MAV_FRAME_BODY_NED + 3576, # type mask (pos only) + x_m, y_m, z_m, # position x,y,z + 0, 0, 0, # velocity x,y,z + 0, 0, 0, # accel x,y,z + 0, 0) # yaw, yaw rate def cmd_attitude(self, args): '''attitude mask q0 q1 q2 q3 roll_rate pitch_rate yaw_rate thrust''' @@ -308,15 +320,15 @@ def cmd_attitude(self, args): att_target = [q0, q1, q2, q3] self.master.mav.set_attitude_target_send( - 0, # system time in milliseconds - self.settings.target_system, # target system - 0, # target component - mask, # type mask - att_target, # quaternion attitude - radians(roll_rate), # body roll rate - radians(pitch_rate), # body pitch rate - radians(yaw_rate), # body yaw rate - thrust) # thrust + 0, # system time in milliseconds + self.settings.target_system, # target system + 0, # target component + mask, # type mask + att_target, # quaternion attitude + math.radians(roll_rate), # body roll rate + math.radians(pitch_rate), # body pitch rate + math.radians(yaw_rate), # body yaw rate + thrust) # thrust def cmd_posvel(self, args): '''posvel mapclick vN vE vD''' @@ -338,7 +350,7 @@ def cmd_posvel(self, args): vD = float(args[2]) ignoremask = ignoremask & 455 - print("ignoremask",ignoremask) + print("ignoremask", ignoremask) print(latlon) self.master.mav.set_position_target_global_int_send( 0, # system time in ms @@ -370,7 +382,7 @@ def cmd_pause(self, args): def cmd_resume(self, args): '''resume AUTO/GUIDED modes''' - self.master.mav.command_long_send( + c = mavutil.mavlink.MAVLink_command_long_message( self.settings.target_system, # target_system self.settings.target_component, # target_component mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, # command @@ -382,6 +394,7 @@ def cmd_resume(self, args): 0, # param5 0, # param6 0) # param7 + self.run_COMMAND_INT_OR_LONG(c, retries=2) def cmd_long(self, args): '''execute supplied command long''' @@ -394,10 +407,10 @@ def cmd_long(self, args): else: try: command = getattr(mavutil.mavlink, args[0]) - except AttributeError as e: + except AttributeError: try: command = getattr(mavutil.mavlink, "MAV_CMD_" + args[0]) - except AttributeError as e: + except AttributeError: pass if command is None: @@ -408,10 +421,10 @@ def cmd_long(self, args): if not args[1].isdigit(): try: args[1] = getattr(mavutil.mavlink, "MAVLINK_MSG_ID_" + args[1]) - except AttributeError as e: + except AttributeError: pass - floating_args = [ float(x) for x in args[1:] ] + floating_args = [float(x) for x in args[1:]] while len(floating_args) < 7: floating_args.append(float(0)) self.master.mav.command_long_send(self.settings.target_system, @@ -437,11 +450,11 @@ def cmd_command_int(self, args): try: # attempt to allow MAV_FRAME_GLOBAL for frame frame = getattr(mavutil.mavlink, args[0]) - except AttributeError as e: + except AttributeError: try: # attempt to allow GLOBAL for frame frame = getattr(mavutil.mavlink, "MAV_FRAME_" + args[0]) - except AttributeError as e: + except AttributeError: pass if frame is None: @@ -455,15 +468,15 @@ def cmd_command_int(self, args): # let "command_int ... MAV_CMD_DO_SET_HOME ..." work try: command = getattr(mavutil.mavlink, args[1]) - except AttributeError as e: + except AttributeError: try: # let "command_int ... DO_SET_HOME" work command = getattr(mavutil.mavlink, "MAV_CMD_" + args[1]) - except AttributeError as e: + except AttributeError: pass - current = int(args[2]) - autocontinue = int(args[3]) + # current = int(args[2]) + # autocontinue = int(args[3]) param1 = float(args[4]) param2 = float(args[5]) param3 = float(args[6]) @@ -485,6 +498,90 @@ def cmd_command_int(self, args): y, z) + class PendingCommand(): + def __init__(self, command, retries=None, retry_interval=5): + self.command = command + self.retries = retries + self.send_count = 0 + self.last_send_time = 0 + self.retry_interval = retry_interval + + def timeout_expired(self): + now = time.time() + return now - self.last_send_time > self.retry_interval + + def send(self, mav): + mav.send(self.command) + self.send_count += 1 + self.last_send_time = time.time() + + def dead(self): + if not self.timeout_expired: + return False + if self.retries is None: + return True + if self.send_count > self.retries: + return True + return False + + def run_COMMAND_INT_OR_LONG(self, command, retries=None): + '''fills command.command_opaque_id in, sends it to + self.master.mav, adds it to the pending command list''' + command.command_opaque_id = self.command_opaque_id_seq + self.command_opaque_id_seq += 1 + if self.command_opaque_id_seq == 0: + self.command_opaque_id_seq = 1 + p = CmdlongModule.PendingCommand(command, retries) + print(f"Adding {command.command_opaque_id} to queue") + self.command_queue[command.command_opaque_id] = p + p.send(self.master.mav) + + def handle_COMMAND_ACK(self, m): + '''remove and acked command from the pending list''' + if m.command_opaque_id == 0: + # not supplied + return + self.command_retry_permitted = 1 + + if m.command_opaque_id not in self.command_queue: + print(f"Received ack for unknown opaque ID={m.command_opaque_id}") + return + + print(f"Received ack={m} for command {self.command_queue[m.command_opaque_id]}") + # self.command_queue[m.command_opaque_id].ack_callback(m) + if m.result == mavutil.mavlink.MAV_RESULT_IN_PROGRESS: + return + del self.command_queue[m.command_opaque_id] + + def retry_commands(self): + for opaque_id, pending_command in self.command_queue.items(): + if not pending_command.timeout_expired(): + continue + print(f"retrying {pending_command}") + pending_command.send(self.master.mav) + + items = copy.copy(self.command_queue).items() + for opaque_id, pending_command in items: + if not pending_command.dead(): + continue + del self.command_queue[opaque_id] + + def idle_task(self): + if self.master is None: + return + now = time.time() + if now - self.last_command_retry_ms > 1: + self.retry_commands() + self.last_command_retry_ms = now + + def mavlink_packet(self, m): + '''handle an incoming mavlink packet''' + mtype = m.get_type() + + if mtype in ["COMMAND_ACK"]: + return self.handle_COMMAND_ACK(m) + + def init(mpstate): '''initialise module''' return CmdlongModule(mpstate)