mavproxy_cmdlong.py: add possibility of retrying commands

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)