Properly handle disjoint QueryResolution._extra ranges

mjbots · Jun 30, 2022 · 1c078d8 · 1c078d8
1 parent 4c0e1c4
commit 1c078d8
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 4 deletions.
diff --git a/lib/python/moteus/moteus.py b/lib/python/moteus/moteus.py
@@ -606,11 +606,13 @@ def _make_query_data(self):
             c3.maybe_write()
 
         if len(qr._extra):
+            min_val = int(min(qr._extra.keys()))
+            max_val = int(max(qr._extra.keys()))
             c4 = mp.WriteCombiner(
-                writer, 0x10, int(min(qr._extra.keys())),
-                [qr._extra[y] for y in
-                 sorted(list([int(x) for x in qr._extra.keys()]))])
-            for _ in qr._extra.keys():
+                writer, 0x10, min_val,
+                [qr._extra.get(i, mp.IGNORE)
+                 for i in range(min_val, max_val +1)])
+            for _ in range(min_val, max_val + 1):
                 c4.maybe_write()
 
         return buf.getvalue()

diff --git a/lib/python/moteus/test/moteus_test.py b/lib/python/moteus/test/moteus_test.py
@@ -206,6 +206,28 @@ def test_make_read_gpio(self):
             result.data,
             bytes([0x12, 0x5e]))
 
+    def test_query_extra_disjoint(self):
+        qr = mot.QueryResolution()
+        qr._extra = {
+            0x030 : mot.mp.F32,
+            0x031 : mot.mp.F32,
+            0x033 : mot.mp.F32,
+            0x034 : mot.mp.F32,
+            0x035 : mot.mp.F32,
+            0x036 : mot.mp.F32,
+        }
+
+        dut = mot.Controller(query_resolution=qr)
+        result = dut.make_query()
+        self.assertEqual(
+            result.data,
+            bytes([
+                0x11, 0x00,
+                0x1f, 0x01,
+                0x13, 0x0d,
+                0x1e, 0x30,
+                0x1c, 0x04, 0x33, ]))
+
 
 if __name__ == '__main__':
     unittest.main()