diff --git a/lib/python/moteus/moteus.py b/lib/python/moteus/moteus.py index fe8e6e1c..efc72519 100644 --- 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 index e3ffccec..a6b3b028 100644 --- 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()