add step/dir input tests

Firmware/MotorControl/axis.cpp

@@ -338,7 +338,7 @@ bool Axis::run_closed_loop_control_loop() {
 
         return true;
     });
-    set_step_dir_active(false);
+    set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
     return check_for_errors();
 }
 
@@ -428,6 +428,7 @@ bool Axis::run_idle_loop() {
     // run_control_loop ignores missed modulation timing updates
     // if and only if we're in AXIS_STATE_IDLE
     safety_critical_disarm_motor_pwm(motor_);
+    set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
     run_control_loop([this]() {
         return true;
     });

Firmware/MotorControl/axis.hpp

@@ -68,8 +68,14 @@ public:
         bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
         bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
         bool startup_homing = false; //<! enable homing after calibration/startup
+
         bool enable_step_dir = false; //<! enable step/dir input after calibration
                                     //   For M0 this has no effect if enable_uart is true
+        bool step_dir_always_on = false; //<! Keep step/dir enabled while the motor is disabled.
+                                         //<! This is ignored if enable_step_dir is false.
+                                         //<! This setting only takes effect on a state transition
+                                         //<! into idle or out of closed loop control.
+
         float counts_per_step = 2.0f;
 
         float watchdog_timeout = 0.0f;  // [s] (0 disables watchdog)
@@ -120,7 +126,6 @@ public:
     void step_cb();
     void set_step_dir_active(bool enable);
     void decode_step_dir_pins();
-    void update_watchdog_settings();
 
     static void load_default_step_dir_pin_config(
         const AxisHardwareConfig_t& hw_config, Config_t* config);
@@ -276,6 +281,7 @@ public:
                 make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
                 make_protocol_property("startup_homing", &config_.startup_homing),
                 make_protocol_property("enable_step_dir", &config_.enable_step_dir),
+                make_protocol_property("step_dir_always_on", &config_.step_dir_always_on),
                 make_protocol_property("counts_per_step", &config_.counts_per_step),
                 make_protocol_property("watchdog_timeout", &config_.watchdog_timeout),
                 make_protocol_property("enable_watchdog", &config_.enable_watchdog),

tools/odrive/tests/step_dir_test.py

@@ -0,0 +1,92 @@
+
+import test_runner
+
+import struct
+import asyncio
+import time
+
+from fibre.utils import Logger
+from odrive.enums import *
+from test_runner import *
+
+class TestStepDir():
+    """
+    Tests Step/Dir input.
+    Not all possible combinations are tested, but each axis and each GPIO
+    participates in at least one test case.
+
+    The tests are conducted while the axis is in idle.
+    """
+    def get_test_cases(self, testrig: TestRig):
+        for odrive in testrig.get_components(ODriveComponent):
+            gpio_conns = [
+                list(testrig.get_connected_components((odrive.gpio1, False), LinuxGpioComponent)),
+                list(testrig.get_connected_components((odrive.gpio2, False), LinuxGpioComponent)),
+                list(testrig.get_connected_components((odrive.gpio3, False), LinuxGpioComponent)),
+                list(testrig.get_connected_components((odrive.gpio4, False), LinuxGpioComponent)),
+                list(testrig.get_connected_components((odrive.gpio5, False), LinuxGpioComponent)),
+                list(testrig.get_connected_components((odrive.gpio6, False), LinuxGpioComponent)),
+                list(testrig.get_connected_components((odrive.gpio7, False), LinuxGpioComponent)),
+                list(testrig.get_connected_components((odrive.gpio8, False), LinuxGpioComponent)),
+            ]
+
+            yield (odrive.axes[0], 1, gpio_conns[0], 2, gpio_conns[1])
+            yield (odrive.axes[0], 3, gpio_conns[2], 4, gpio_conns[3])
+            yield (odrive.axes[0], 5, gpio_conns[4], 6, gpio_conns[5]) # broken
+            yield (odrive.axes[0], 7, gpio_conns[6], 8, gpio_conns[7]) # broken
+            
+            yield (odrive.axes[1], 7, gpio_conns[6], 8, gpio_conns[7])
+
+    def run_test(self, axis: ODriveAxisComponent,  step_gpio_num: int, step_gpio: LinuxGpioComponent, dir_gpio_num: int, dir_gpio: LinuxGpioComponent, logger: Logger):
+        step_gpio.config(output=True)
+        step_gpio.write(False)
+        dir_gpio.config(output=True)
+        dir_gpio.write(True)
+
+        if axis.num == 0:
+            axis.parent.handle.config.enable_uart = False
+        axis.handle.config.enable_step_dir = True
+        axis.handle.config.step_dir_always_on = True # needed for testing
+        axis.handle.config.step_gpio_pin = step_gpio_num
+        axis.handle.config.dir_gpio_pin = dir_gpio_num
+        request_state(axis, AXIS_STATE_IDLE) # apply step_dir_always_on config
+
+
+        ref = axis.handle.controller.input_pos
+        axis.handle.config.counts_per_step = counts_per_step = 10
+
+        # On the RPi 4 a ~5kHz GPIO signal can be generated from Python
+
+        for i in range(100):
+            step_gpio.write(True)
+            step_gpio.write(False)
+            test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
+
+        ref = axis.handle.controller.input_pos
+        dir_gpio.write(False)
+
+        for i in range(100):
+            step_gpio.write(True)
+            step_gpio.write(False)
+            test_assert_eq(axis.handle.controller.input_pos, ref - (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
+
+        ref = axis.handle.controller.input_pos
+        dir_gpio.write(True)
+        axis.handle.config.counts_per_step = counts_per_step = 1
+
+        for i in range(100):
+            step_gpio.write(True)
+            step_gpio.write(False)
+            test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
+
+        ref = axis.handle.controller.input_pos
+        axis.handle.config.counts_per_step = counts_per_step = -1
+
+        for i in range(100):
+            step_gpio.write(True)
+            step_gpio.write(False)
+            test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * abs(counts_per_step))
+
+
+if __name__ == '__main__':
+    test_runner.run(TestStepDir())

tools/odrive/tests/test_runner.py

@@ -61,6 +61,10 @@ def disjoint_sets(list_of_sets: list):
 def is_list_like(arg):
     return hasattr(arg, '__iter__') and not isinstance(arg, str)
 
+def all_unique(lst):
+    seen = list()
+    return not any(i in seen or seen.append(i) for i in lst)
+
 # Test Components -------------------------------------------------------------#
 
 class Component(object):
@@ -72,8 +76,8 @@ class ODriveComponent(Component):
         self.handle = None
         self.yaml = yaml
         #self.axes = [ODriveAxisComponent(None), ODriveAxisComponent(None)]
-        self.encoders = [ODriveEncoderComponent(self, 0, None), ODriveEncoderComponent(self, 1, None)]
-        self.axes = [ODriveAxisComponent(self, 0, None), ODriveAxisComponent(self, 1, None)]
+        self.encoders = [ODriveEncoderComponent(self, 0, yaml['encoder0']), ODriveEncoderComponent(self, 1, yaml['encoder1'])]
+        self.axes = [ODriveAxisComponent(self, 0, yaml['motor0']), ODriveAxisComponent(self, 1, yaml['motor1'])]
         for i in range(1,9):
             self.__setattr__('gpio' + str(i), Component(self))
         self.can = Component(self)
@@ -123,19 +127,20 @@ class MotorComponent(Component):
         pass
 
 class ODriveAxisComponent(Component):
-    def __init__(self, odrv_ctx: ODriveComponent, num: int, yaml: dict):
+    def __init__(self, parent: ODriveComponent, num: int, yaml: dict):
+        Component.__init__(self, parent)
         self.handle = None
-        self.yaml = odrv_ctx.yaml[f'motor{num}'] # TODO: this is bad naming
-        self.odrv_ctx = odrv_ctx
+        self.yaml = yaml # TODO: this is bad naming
         self.num = num
 
     def prepare(self, logger: Logger):
-        self.odrv_ctx.prepare(logger)
+        self.parent.prepare(logger)
 
 class ODriveEncoderComponent(Component):
-    def __init__(self, odrv_ctx: ODriveComponent, num: int, yaml: dict):
+    def __init__(self, parent: ODriveComponent, num: int, yaml: dict):
+        Component.__init__(self, parent)
         self.handle = None
-        self.odrv_ctx = odrv_ctx
+        self.yaml = yaml
         self.num = num
         self.z = Component(self)
         self.a = Component(self)
@@ -145,7 +150,7 @@ class ODriveEncoderComponent(Component):
         return [('z', self.z), ('a', self.a), ('b', self.b)]
 
     def prepare(self, logger: Logger):
-        self.odrv_ctx.prepare(logger)
+        self.parent.prepare(logger)
 
 class EncoderComponent(Component):
     def __init__(self, parent: Component, yaml: dict):
@@ -311,6 +316,9 @@ class ProxiedComponent(Component):
     def __repr__(self):
         return testrig.get_component_name(self.impl) + ' (routed via ' + ', '.join((testrig.get_component_name(t) + ': ' + str(i.num) + ' => ' + str(o.num)) for t, i, o, n in self.gpio_tuples) + ')'
 
+    def __eq__(self, obj):
+        return isinstance(obj, ProxiedComponent) and (self.impl == obj.impl) #  and (self.gpio_tuples == obj.gpio_tuples)
+
     def prepare(self):
         for teensy, gpio_in, gpio_out, gpio_noise_enable in self.gpio_tuples:
             teensy.add_route(gpio_in, gpio_out, gpio_noise_enable)
@@ -480,24 +488,26 @@ def run_shell(command_line, logger, env=None, timeout=None):
         logger.error(result.stdout.decode(sys.stdout.encoding))
         raise TestFailed("command {} failed".format(command_line))
 
-def select_params(param_options):
-    params = []
 
+def get_combinations(param_options):
+    if len(param_options) > 0:
+        param = param_options[0]
+        if not is_list_like(param):
+            param = [param]
+        
+        for part1, part2 in itertools.product(param, get_combinations(param_options[1:]) if (len(param_options) > 1) else [()]):
+            if not isinstance(part1, tuple):
+                part1 = (part1,)
+            yield part1 + part2
+
+def select_params(param_options):
     # Select parameters from the resource list
     # (this could be arbitrarily complex to improve parallelization of the tests)
-    for param in param_options:
-        if is_list_like(param):
-            if len(param) == 0:
-                return None
-            else:
-                selection = param[0]
-                if is_list_like(selection):
-                    params = params + list(selection)
-                else:
-                    params.append(selection)
-        else:
-            params.append(param)
-    return params
+    for combination in get_combinations(param_options):
+        if all_unique(combination):
+            return list(combination)
+
+    return None
 
 def run(tests):
     if not isinstance(tests, list):
@@ -590,34 +600,37 @@ testrig = TestRig(test_rig_yaml, logger)
 
 
 if args.setup_host:
-    def export_gpio(gpio):
-        if not os.path.isdir("/sys/class/gpio/gpio{}".format(gpio)):
+    for gpio in testrig.get_components(LinuxGpioComponent):
+        num = gpio.num
+        logger.debug('exporting GPIO ' + str(num) + ' to user space...')
+        if not os.path.isdir("/sys/class/gpio/gpio{}".format(num)):
             with open("/sys/class/gpio/export", "w") as fp:
-                fp.write(str(gpio))
-        os.chmod("/sys/class/gpio/gpio{}/value".format(gpio), stat.S_IROTH | stat.S_IWOTH)
-        os.chmod("/sys/class/gpio/gpio{}/direction".format(gpio), stat.S_IROTH | stat.S_IWOTH)
+                fp.write(str(num))
+        os.chmod("/sys/class/gpio/gpio{}/value".format(num), stat.S_IROTH | stat.S_IWOTH)
+        os.chmod("/sys/class/gpio/gpio{}/direction".format(num), stat.S_IROTH | stat.S_IWOTH)
 
-    # TODO: read configuration from yaml file
-    export_gpio(20) # connected to Teensy GPIO
-    export_gpio(26) # connected to Teensy Program pin
+    for port in testrig.get_components(SerialPortComponent):
+        logger.debug('changing permissions on ' + port.yaml['port'] + '...')
+        os.chmod(port.yaml['port'], stat.S_IROTH | stat.S_IWOTH)
 
-    os.chmod("/dev/ttyS0", stat.S_IROTH | stat.S_IWOTH)
-
-    # This breaks the retarded teensy loader that shows up on every compile
-    if not os.path.isfile('/usr/share/arduino/hardware/tools/teensy_post_compile_old'):
-        os.rename('/usr/share/arduino/hardware/tools/teensy_post_compile', '/usr/share/arduino/hardware/tools/teensy_post_compile_old')
-    with open('/usr/share/arduino/hardware/tools/teensy_post_compile', 'w') as scr:
-        scr.write('#!/bin/bash\n')
-        scr.write('if [ "$ARDUINO_COMPILE_DESTINATION" != "" ]; then\n')
-        scr.write('  cp -r ${2#-path=}/*.ino.hex ${ARDUINO_COMPILE_DESTINATION}\n')
-        scr.write('fi\n')
-    os.chmod('/usr/share/arduino/hardware/tools/teensy_post_compile', stat.S_IRWXU | stat.S_IRGRP | stat.S_IXGRP | stat.S_IROTH | stat.S_IXOTH)
+    if len(list(testrig.get_components(TeensyComponent))):
+        # This breaks the annoying teensy loader that shows up on every compile
+        logger.debug('modifying teensyduino installation...')
+        if not os.path.isfile('/usr/share/arduino/hardware/tools/teensy_post_compile_old'):
+            os.rename('/usr/share/arduino/hardware/tools/teensy_post_compile', '/usr/share/arduino/hardware/tools/teensy_post_compile_old')
+        with open('/usr/share/arduino/hardware/tools/teensy_post_compile', 'w') as scr:
+            scr.write('#!/bin/bash\n')
+            scr.write('if [ "$ARDUINO_COMPILE_DESTINATION" != "" ]; then\n')
+            scr.write('  cp -r ${2#-path=}/*.ino.hex ${ARDUINO_COMPILE_DESTINATION}\n')
+            scr.write('fi\n')
+        os.chmod('/usr/share/arduino/hardware/tools/teensy_post_compile', stat.S_IRWXU | stat.S_IRGRP | stat.S_IXGRP | stat.S_IROTH | stat.S_IXOTH)
 
     # Bring up CAN interface(s)
     for intf in testrig.get_components(CanInterfaceComponent):
         name = intf.yaml['interface']
-        run_shell('ip link set dev {} down'.format(intf))
-        run_shell('ip link set dev {} type can bitrate 250000'.format(intf))
-        run_shell('ip link set dev {} type can loopback off'.format(intf))
-        run_shell('ip link set dev {} up'.format(intf))
+        logger.debug('bringing up {}...'.format(name))
+        run_shell('ip link set dev {} down'.format(name), logger)
+        run_shell('ip link set dev {} type can bitrate 250000'.format(name), logger)
+        run_shell('ip link set dev {} type can loopback off'.format(name), logger)
+        run_shell('ip link set dev {} up'.format(name), logger)
 

tools/test-rig-rpi.yaml

@@ -17,12 +17,13 @@ components:
         name: can0
         interface: can0
         connected-to: odrive.can
-      - {type: gpio, num: 20} # need to specify GPIOs explicitly for the generalpurpose type
+      - {type: gpio, num: 19} # need to specify GPIOs explicitly for the generalpurpose type
+      - {type: gpio, num: 20}
       - {type: gpio, num: 26}
 
-  - type: programmer
-    name: The Blue STLink/v2
-    id: '\x53\x3f\x75\x06\x49\x3f\x49\x51\x44\x54\x19\x3f'
+#  - type: programmer
+#    name: The Blue STLink/v2
+#    id: '\x53\x3f\x75\x06\x49\x3f\x49\x51\x44\x54\x19\x3f'
 
   - type: odrive
     name: odrive
@@ -65,11 +66,12 @@ connections:
   - ['teensy.gpio9', 'odrive.gpio2']
   - ['teensy.gpio8', 'odrive.gpio3']
   - ['teensy.gpio7', 'odrive.gpio4']
-  - ['teensy.gpio15', 'odrive.gpio5']
-  - ['teensy.gpio16', 'odrive.gpio6']
-  - ['teensy.gpio17', 'odrive.gpio7']
-  - ['teensy.gpio18', 'odrive.gpio8']
+  - ['teensy.gpio14', 'odrive.gpio5']
+  - ['teensy.gpio15', 'odrive.gpio6']
+  - ['teensy.gpio16', 'odrive.gpio7']
+  - ['teensy.gpio17', 'odrive.gpio8']
   - ['teensy.gpio4', 'rpi.gpio20']
+  - ['teensy.gpio5', 'rpi.gpio19']
   - ['teensy.gpio23', 'odrive.encoder0.z']
   - ['teensy.gpio22', 'odrive.encoder0.a']
   - ['teensy.gpio21', 'odrive.encoder0.b']