extend CAN test: add watchdog and heartbeat tests

tools/odrive/tests/can_test.py

@@ -13,7 +13,7 @@ from test_runner import *
 # Each argument is described as tuple (name, format, scale).
 # Struct format codes: https://docs.python.org/2/library/struct.html
 command_set = {
-    'heartbeat': (0x001, [('error', 'I', 1), ('current_state', 'I', 1)]), # untested
+    'heartbeat': (0x001, [('error', 'I', 1), ('current_state', 'I', 1)]), # tested
     'estop': (0x002, []), # tested
     'get_motor_error': (0x003, [('motor_error', 'I', 1)]), # untested
     'get_encoder_error': (0x004, [('encoder_error', 'I', 1)]), # untested
@@ -21,8 +21,8 @@ command_set = {
     'set_node_id': (0x006, [('node_id', 'H', 1)]), # tested
     'set_requested_state': (0x007, [('requested_state', 'I', 1)]), # tested
     # 0x008 not yet implemented
-    'get_encoder_estimates': (0x009, [('encoder_pos_estimate', 'f', 1), ('encoder_vel_estimate', 'f', 1)]), # untested
-    'get_encoder_count': (0x00a, [('encoder_shadow_count', 'i', 1), ('encoder_count', 'i', 1)]), # untested
+    'get_encoder_estimates': (0x009, [('encoder_pos_estimate', 'f', 1), ('encoder_vel_estimate', 'f', 1)]), # partially tested
+    'get_encoder_count': (0x00a, [('encoder_shadow_count', 'i', 1), ('encoder_count', 'i', 1)]), # partially tested
     'set_controller_modes': (0x00b, [('control_mode', 'i', 1), ('input_mode', 'i', 1)]), # tested
     'set_input_pos': (0x00c, [('input_pos', 'i', 1), ('vel_ff', 'h', 0.1), ('cur_ff', 'h', 0.01)]), # tested
     'set_input_vel': (0x00d, [('input_vel', 'i', 0.01), ('cur_ff', 'h', 0.01)]), # tested
@@ -34,7 +34,7 @@ command_set = {
     'set_traj_a_per_css': (0x013, [('a_per_css', 'f', 1)]), # tested
     'get_iq': (0x014, [('iq_setpoint', 'f', 1), ('iq_measured', 'f', 1)]), # untested
     'get_sensorless_estimates': (0x015, [('sensorless_pos_estimate', 'f', 1), ('sensorless_vel_estimate', 'f', 1)]), # untested
-    'reboot': (0x016, []), # untested
+    'reboot': (0x016, []), # tested
     'get_vbus_voltage': (0x017, [('vbus_voltage', 'f', 1)]), # tested
     'clear_errors': (0x018, []), # partially tested
 }
@@ -52,7 +52,12 @@ def command(bus, node_id_, cmd_name, **kwargs):
     msg = can.Message(arbitration_id=((node_id_ << 5) | cmd_id), data=data)
     bus.send(msg)
 
-async def request(bus, node_id, cmd_name, timeout = 1.0):
+async def record_messages(bus, node_id, cmd_name, timeout = 5.0):
+    """
+    Returns an async generator that yields a dictionary for each CAN message that
+    is received, provided that the CAN ID matches the expected value.
+    """
+
     cmd_spec = command_set[cmd_name]
     cmd_id = cmd_spec[0]
     fmt = '<' + ''.join([f for (n, f, s) in cmd_spec[1]]) # all little endian
@@ -61,23 +66,37 @@ async def request(bus, node_id, cmd_name, timeout = 1.0):
     notifier = can.Notifier(bus, [reader], timeout = timeout, loop = asyncio.get_event_loop())
 
     try:
-        msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), data=[], is_remote_frame=True)
-        bus.send(msg)
-
         # The timeout in can.Notifier only triggers if no new messages are received at all,
         # so we need a second monitoring method.
         start = time.monotonic()
         while True:
             msg = await reader.get_message()
             if ((msg.arbitration_id == ((node_id << 5) | cmd_id)) and not msg.is_remote_frame):
-                break
+                fields = struct.unpack(fmt, msg.data[:(struct.calcsize(fmt))]) 
+                res = {n: (fields[i] * s) for (i, (n, f, s)) in enumerate(cmd_spec[1])}
+                res['t'] = time.monotonic()
+                yield res
             if (time.monotonic() - start) > timeout:
-                raise TimeoutError()
+                break
     finally:
         notifier.stop()
 
-    fields = struct.unpack(fmt, msg.data[:(struct.calcsize(fmt))]) 
-    return {n: (fields[i] * s) for (i, (n, f, s)) in enumerate(cmd_spec[1])}
+async def request(bus, node_id, cmd_name, timeout = 1.0):
+    cmd_spec = command_set[cmd_name]
+    cmd_id = cmd_spec[0]
+
+    msg_generator = record_messages(bus, node_id, cmd_name, timeout)
+
+    msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), data=[], is_remote_frame=True)
+    bus.send(msg)
+
+    async for msg in msg_generator:
+        return msg
+
+    raise TimeoutError()
+
+async def get_all(async_iterator):
+    return [x async for x in async_iterator]
 
 
 class TestSimpleCAN():
@@ -111,6 +130,10 @@ class TestSimpleCAN():
         fence()
         test_assert_eq(axis.config.can_node_id, node_id)
 
+        axis.encoder.set_linear_count(123)
+        test_assert_eq(my_req('get_encoder_estimates')['encoder_pos_estimate'], 123.0, accuracy=0.01)
+        test_assert_eq(my_req('get_encoder_count')['encoder_shadow_count'], 123.0, accuracy=0.01)
+
         my_cmd('clear_errors')
         fence()
         test_assert_eq(axis.error, 0)
@@ -176,6 +199,27 @@ class TestSimpleCAN():
         fence()
         test_assert_eq(axis.controller.config.inertia, 55.086, range=0.0001)
 
+        # any CAN cmd will feed the watchdog
+        test_watchdog(axis, lambda: my_cmd('set_input_current', input_current=0.0), logger)
+
+        logger.debug('testing heartbeat...')
+        # note that this will include the heartbeats that were received during the
+        # watchdog test (which takes 4.8s).
+        heartbeats = asyncio.run(get_all(record_messages(canbus.handle, node_id, 'heartbeat', timeout = 1.0)))
+        test_assert_eq(len(heartbeats), 5.8 / 0.1, accuracy=0.05)
+        test_assert_eq([msg['error'] for msg in heartbeats[0:35]], [0] * 35) # before watchdog expiry
+        test_assert_eq([msg['error'] for msg in heartbeats[-10:]], [errors.axis.ERROR_WATCHDOG_TIMER_EXPIRED] * 10) # after watchdog expiry
+        test_assert_eq([msg['current_state'] for msg in heartbeats], [1] * len(heartbeats))
+
+        logger.debug('testing reboot...')
+        my_cmd('reboot')
+        time.sleep(0.5)
+        if len(odrive.handle._remote_attributes) != 0:
+            raise TestFailed("device didn't seem to reboot")
+        odrive.handle = None
+        time.sleep(2.0)
+        odrive.prepare(logger)
+
 
 if __name__ == '__main__':
     test_runner.run(TestSimpleCAN())

tools/odrive/tests/test_runner.py

@@ -6,6 +6,7 @@ sys.path.append(os.path.join(os.path.dirname(__file__), '..', '..'))
 
 import stat
 import odrive
+from odrive.enums import *
 import fibre
 from fibre import Logger, Event
 import argparse
@@ -153,6 +154,29 @@ def test_curve_fit(data, fitted_curve, max_mean_err, inlier_range, max_outliers)
         save_log(np.concatenate([data, np.array([fitted_curve]).transpose()], 1))
         raise TestFailed("curve fit has too many outliers (err > {}): {} > {}".format(inlier_range, outliers, max_outliers))
 
+def test_watchdog(axis, feed_func, logger: Logger):
+    """
+    Tests the watchdog of one axis, using the provided function to feed the watchdog.
+    This test assumes that the testing host has no more than 300ms random delays.
+    """
+    start = time.monotonic()
+    axis.config.enable_watchdog = False
+    axis.error = 0
+    axis.config.watchdog_timeout = 1.0
+    axis.watchdog_feed()
+    axis.config.enable_watchdog = True
+    test_assert_eq(axis.error, 0)
+    for _ in range(5): # keep the watchdog alive for 3.5 seconds
+        time.sleep(0.7)
+        logger.debug('feeding watchdog at {}s'.format(time.monotonic() - start))
+        feed_func()
+        err = axis.error
+        logger.debug('checking error at {}s'.format(time.monotonic() - start))
+        test_assert_eq(err, 0)
+        
+    logger.debug('letting watchdog expire...')
+    time.sleep(1.3) # let the watchdog expire
+    test_assert_eq(axis.error, errors.axis.ERROR_WATCHDOG_TIMER_EXPIRED)
 
 # Test Components -------------------------------------------------------------#
 

tools/odrive/tests/uart_ascii_test.py

@@ -131,25 +131,7 @@ class TestUartAscii():
             test_assert_eq(float(response.split()[0]), odrive.handle.axis0.encoder.pos_estimate, accuracy=0.001)
             test_assert_eq(float(response.split()[1]), odrive.handle.axis0.encoder.vel_estimate, accuracy=0.001)
 
-
-            # Test watchdog (assumes that the testing host has no more than 300ms random delays)
-            start = time.monotonic()
-            odrive.handle.axis0.config.enable_watchdog = False
-            odrive.handle.axis0.error = 0
-            odrive.handle.axis0.config.watchdog_timeout = 1.0
-            odrive.handle.axis0.watchdog_feed()
-            odrive.handle.axis0.config.enable_watchdog = True
-            test_assert_eq(odrive.handle.axis0.error, 0)
-            for _ in range(5): # keep the watchdog alive for 3.5 seconds
-                time.sleep(0.7)
-                print('feeding watchdog at {}s'.format(time.monotonic() - start))
-                ser.write(b'u 0\n')
-                err = odrive.handle.axis0.error
-                print('checking error at {}s'.format(time.monotonic() - start))
-                test_assert_eq(err, 0)
-                
-            time.sleep(1.3) # let the watchdog expire
-            test_assert_eq(odrive.handle.axis0.error, errors.axis.ERROR_WATCHDOG_TIMER_EXPIRED)
+            test_watchdog(odrive.handle.axis0, lambda: ser.write(b'u 0\n'), logger)
             test_assert_eq(ser.readline(), b'') # check if the device remained silent during the test