From 4b58aeb637b7d5eab218dee6d464b8f745426691 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 29 Apr 2020 14:49:29 +0200 Subject: [PATCH] add SPI encoder tests (AMS and CUI protocol) --- tools/odrive/enums.py | 2 +- tools/odrive/tests/encoder_test.py | 401 ++++++++++++++++++++++------- tools/odrive/tests/test_runner.py | 6 + tools/test-rig-rpi.yaml | 19 +- 4 files changed, 333 insertions(+), 95 deletions(-) diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 5a3f1b76..1292e48b 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -91,6 +91,6 @@ INPUT_MODE_MIRROR = 7 ENCODER_MODE_INCREMENTAL = 0x00 ENCODER_MODE_HALL = 0x01 ENCODER_MODE_SINCOS = 0x02 -#ENCODER_MODE_SPI_ABS_CUI = 0x100 # currently not functional +ENCODER_MODE_SPI_ABS_CUI = 0x100 ENCODER_MODE_SPI_ABS_AMS = 0x101 ENCODER_MODE_SPI_ABS_AEAT = 0x102 diff --git a/tools/odrive/tests/encoder_test.py b/tools/odrive/tests/encoder_test.py index d3f66949..b9af8ed6 100644 --- a/tools/odrive/tests/encoder_test.py +++ b/tools/odrive/tests/encoder_test.py @@ -10,82 +10,6 @@ from odrive.enums import * from test_runner import * -teensy_code_template = """ -void setup() { - pinMode({enc_a}, OUTPUT); - pinMode({enc_b}, OUTPUT); -} - -int cpr = 8192; -int rpm = 30; - -// the loop routine runs over and over again forever: -void loop() { - int microseconds_per_count = (1000000 * 60 / cpr / rpm); - - for (;;) { - digitalWrite({enc_a}, HIGH); - delayMicroseconds(microseconds_per_count); - digitalWrite({enc_b}, HIGH); - delayMicroseconds(microseconds_per_count); - digitalWrite({enc_a}, LOW); - delayMicroseconds(microseconds_per_count); - digitalWrite({enc_b}, LOW); - delayMicroseconds(microseconds_per_count); - } -} -""" - -teensy_code_template2 = """ -void setup() { - analogWriteResolution(10); - int freq = 150000000/1024; // ~146.5kHz PWM frequency - analogWriteFrequency({enc_sin}, freq); - analogWriteFrequency({enc_cos}, freq); -} - -int rpm = 60; -float pos = 0; - -void loop() { - pos += 0.001f * ((float)rpm / 60.0f); - if (pos > 1.0f) - pos -= 1.0f; - analogWrite({enc_sin}, (int)(512.0f + 512.0f * sin(2.0f * M_PI * pos))); - analogWrite({enc_cos}, (int)(512.0f + 512.0f * cos(2.0f * M_PI * pos))); - delay(1); -} -""" - -teensy_code_template3 = """ -void setup() { - pinMode({hall_a}, OUTPUT); - pinMode({hall_b}, OUTPUT); - pinMode({hall_c}, OUTPUT); - digitalWrite({hall_a}, HIGH); -} - -int cpr = 90; // 15 pole-pairs. Value suggested in hoverboard.md -int rpm = 60; -int microseconds_per_count = (1000000 * 60 / cpr / rpm); - -void loop() { - digitalWrite({hall_b}, HIGH); - delayMicroseconds(microseconds_per_count); - digitalWrite({hall_a}, LOW); - delayMicroseconds(microseconds_per_count); - digitalWrite({hall_c}, HIGH); - delayMicroseconds(microseconds_per_count); - digitalWrite({hall_b}, LOW); - delayMicroseconds(microseconds_per_count); - digitalWrite({hall_a}, HIGH); - delayMicroseconds(microseconds_per_count); - digitalWrite({hall_c}, LOW); - delayMicroseconds(microseconds_per_count); -} -""" - - class TestEncoderBase(): """ Base class for encoder tests. @@ -112,8 +36,6 @@ class TestEncoderBase(): encoder.pos_cpr, encoder.vel_estimate, ], duration=5.0) - - data = np.array(data) short_period = (abs(1 / true_rps) < 5.0) reverse = (true_rps < 0) @@ -130,7 +52,7 @@ class TestEncoderBase(): # encoder.phase slope, offset, fitted_curve = fit_sawtooth(data[:,(0,3)], pi if reverse else -pi, -pi if reverse else pi, sigma=5) - test_assert_eq(slope / 7, 2*pi*true_rps, accuracy=0.01) + test_assert_eq(slope / 7, 2*pi*true_rps, accuracy=0.05) test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.pos_estimate @@ -151,6 +73,31 @@ class TestEncoderBase(): +teensy_incremental_encoder_emulation_code = """ +void setup() { + pinMode({enc_a}, OUTPUT); + pinMode({enc_b}, OUTPUT); +} + +int cpr = 8192; +int rpm = 30; + +// the loop routine runs over and over again forever: +void loop() { + int microseconds_per_count = (1000000 * 60 / cpr / rpm); + + for (;;) { + digitalWrite({enc_a}, HIGH); + delayMicroseconds(microseconds_per_count); + digitalWrite({enc_b}, HIGH); + delayMicroseconds(microseconds_per_count); + digitalWrite({enc_a}, LOW); + delayMicroseconds(microseconds_per_count); + digitalWrite({enc_b}, LOW); + delayMicroseconds(microseconds_per_count); + } +} +""" class TestIncrementalEncoder(TestEncoderBase): @@ -173,10 +120,10 @@ class TestIncrementalEncoder(TestEncoderBase): yield (encoder, valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: int, teensy_gpio_b: int, logger: Logger): + def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, logger: Logger): true_cps = 8192*-0.5 # counts per second generated by the virtual encoder - code = teensy_code_template.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num)) + code = teensy_incremental_encoder_emulation_code.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num)) teensy.compile_and_program(code) if enc.handle.config.mode != ENCODER_MODE_INCREMENTAL: @@ -195,6 +142,27 @@ class TestIncrementalEncoder(TestEncoderBase): +teensy_sin_cos_encoder_emulation_code = """ +void setup() { + analogWriteResolution(10); + int freq = 150000000/1024; // ~146.5kHz PWM frequency + analogWriteFrequency({enc_sin}, freq); + analogWriteFrequency({enc_cos}, freq); +} + +float rps = 1.0f; +float pos = 0; + +void loop() { + pos += 0.001f * rps; + if (pos > 1.0f) + pos -= 1.0f; + analogWrite({enc_sin}, (int)(512.0f + 512.0f * sin(2.0f * M_PI * pos))); + analogWrite({enc_cos}, (int)(512.0f + 512.0f * cos(2.0f * M_PI * pos))); + delay(1); +} +""" + class TestSinCosEncoder(TestEncoderBase): def get_test_cases(self, testrig: TestRig): for odrive in testrig.get_components(ODriveComponent): @@ -213,7 +181,7 @@ class TestSinCosEncoder(TestEncoderBase): def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger): - code = teensy_code_template2.replace("{enc_sin}", str(teensy_gpio_sin.num)).replace("{enc_cos}", str(teensy_gpio_cos.num)) + code = teensy_sin_cos_encoder_emulation_code.replace("{enc_sin}", str(teensy_gpio_sin.num)).replace("{enc_cos}", str(teensy_gpio_cos.num)) teensy.compile_and_program(code) if enc.handle.config.mode != ENCODER_MODE_SINCOS: @@ -229,6 +197,34 @@ class TestSinCosEncoder(TestEncoderBase): +teensy_hall_effect_encoder_emulation_code = """ +void setup() { + pinMode({hall_a}, OUTPUT); + pinMode({hall_b}, OUTPUT); + pinMode({hall_c}, OUTPUT); + digitalWrite({hall_a}, HIGH); +} + +int cpr = 90; // 15 pole-pairs. Value suggested in hoverboard.md +float rps = 1.0f; +int us_per_count = (1000000.0f / cpr / rps); + +void loop() { + digitalWrite({hall_b}, HIGH); + delayMicroseconds(us_per_count); + digitalWrite({hall_a}, LOW); + delayMicroseconds(us_per_count); + digitalWrite({hall_c}, HIGH); + delayMicroseconds(us_per_count); + digitalWrite({hall_b}, LOW); + delayMicroseconds(us_per_count); + digitalWrite({hall_a}, HIGH); + delayMicroseconds(us_per_count); + digitalWrite({hall_c}, LOW); + delayMicroseconds(us_per_count); +} +""" + class TestHallEffectEncoder(TestEncoderBase): def get_test_cases(self, testrig: TestRig): @@ -251,11 +247,11 @@ class TestHallEffectEncoder(TestEncoderBase): yield (encoder, valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: int, teensy_gpio_b: int, teensy_gpio_c: int, logger: Logger): + def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, teensy_gpio_c: TeensyGpio, logger: Logger): true_cpr = 90 true_rps = -1.0 - code = teensy_code_template3.replace("{hall_a}", str(teensy_gpio_a.num)).replace("{hall_b}", str(teensy_gpio_b.num)).replace("{hall_c}", str(teensy_gpio_c.num)) + code = teensy_hall_effect_encoder_emulation_code.replace("{hall_a}", str(teensy_gpio_a.num)).replace("{hall_b}", str(teensy_gpio_b.num)).replace("{hall_c}", str(teensy_gpio_c.num)) teensy.compile_and_program(code) if enc.handle.config.mode != ENCODER_MODE_HALL: @@ -270,9 +266,242 @@ class TestHallEffectEncoder(TestEncoderBase): enc.handle.config.cpr = 8192 + +# This encoder emulation mimics the specification given in the following datasheets: +# +# With {mode} == ENCODER_MODE_SPI_ABS_CUI: +# AMT23xx: https://www.cuidevices.com/product/resource/amt23.pdf +# +# With {mode} == ENCODER_MODE_SPI_ABS_AMS: +# AS5047P: https://ams.com/documents/20143/36005/AS5047P_DS000324_2-00.pdf/a7d44138-51f1-2f6e-c8b6-2577b369ace8 +# AS5048A/AS5048B: https://ams.com/documents/20143/36005/AS5048_DS000298_4-00.pdf/910aef1f-6cd3-cbda-9d09-41f152104832 +# => Only the read command on address 0x3fff is currently implemented. + +teensy_spi_encoder_emulation_code = """ +#define ENCODER_MODE_SPI_ABS_CUI 0x100 +#define ENCODER_MODE_SPI_ABS_AMS 0x101 +#define ENCODER_MODE_SPI_ABS_AEAT 0x102 + +static float rps = 1.0f; +static uint32_t cpr = 16384; +static uint32_t us_per_revolution = (uint32_t)(1000000.0f / rps); +static uint16_t spi_txd = 0; // first output word: NOP +static uint32_t zerotime = 0; + +void setup() { + pinMode({ncs}, INPUT_PULLUP); +} + +uint16_t get_pos_now() { + uint32_t time = micros(); + return ((uint64_t)((time - zerotime) % us_per_revolution)) * cpr / us_per_revolution; +} + + +#if {mode} == ENCODER_MODE_SPI_ABS_AMS + +uint8_t ams_parity(uint16_t v) { + v ^= v >> 8; + v ^= v >> 4; + v ^= v >> 2; + v ^= v >> 1; + return v & 1; +} + +uint16_t handle_command(uint16_t cmd) { + const uint16_t ERROR_RESPONSE = 0xc000; // error flag and parity bit set + + if (ams_parity(cmd)) { + return ERROR_RESPONSE; + } + + if (!(cmd & 14)) { // write not supported + return ERROR_RESPONSE; + } + + uint16_t addr = cmd & 0x3fff; + uint16_t data; + + switch (addr) { + case 0x3fff: data = get_pos_now(); break; + default: return ERROR_RESPONSE; + } + + return data | (ams_parity(data) << 15); +} + +#endif + +#if {mode} == ENCODER_MODE_SPI_ABS_CUI + +uint8_t cui_parity(uint16_t v) { + v ^= v >> 8; + v ^= v >> 4; + v ^= v >> 2; + return ~v & 3; +} + +uint16_t handle_command(uint16_t cmd) { + (void) cmd; // input not used on CUI + + // Test the cui_parity function itself with the example given in the datasheet + if ((0x21AB | (cui_parity(0x21AB) << 14)) != 0x61AB) { + return 0x0000; + } + + uint16_t data = get_pos_now(); + return data | (cui_parity(data) << 14); +} + +#endif + + +void loop() { + while (digitalReadFast({reset})) { + zerotime = micros(); + } + + if (!digitalReadFast({ncs})) { + static uint16_t spi_rxd = 0; + + pinMode({miso}, OUTPUT); + + for (;;) { + while (!digitalReadFast({sck})) + if (digitalReadFast({ncs})) + goto cs_deasserted; + + // Rising edge: Push output bit + + bool output_bit = spi_txd & 0x8000; + digitalWriteFast({miso}, output_bit); + spi_txd <<= 1; + + while (digitalReadFast({sck})) + if (digitalReadFast({ncs})) + goto cs_deasserted; + + // Falling edge: Sample input bit (only in AMS mode) + +#if {mode} == ENCODER_MODE_SPI_ABS_AMS + bool input_bit = digitalReadFast({mosi}); + spi_rxd <<= 1; + if (input_bit) { + spi_rxd |= 1; + } else { + spi_rxd &= ~1; + } +#endif + } + +cs_deasserted: + // chip deselected: Process command + pinMode({miso}, INPUT); + + spi_txd = handle_command(spi_rxd); + } +} +""" + +class TestSpiEncoder(TestEncoderBase): + def __init__(self, mode: int): + self.mode = mode + + def get_test_cases(self, testrig: TestRig): + for odrive in testrig.get_components(ODriveComponent): + for encoder in odrive.encoders: + odrive_ncs_gpio = odrive.gpio7 # this GPIO choice is completely arbitrary + gpio_conns = [ + testrig.get_connected_components(odrive.sck, TeensyGpio), + testrig.get_connected_components(odrive.miso, TeensyGpio), + testrig.get_connected_components(odrive.mosi, TeensyGpio), + testrig.get_connected_components(odrive_ncs_gpio, TeensyGpio), + ] + + valid_combinations = [] + for combination in itertools.product(*gpio_conns): + if (len(set(c.parent for c in combination)) != 1): + continue + teensy = combination[0].parent + reset_pin_options = [] + for gpio in teensy.gpios: + for local_gpio in testrig.get_connected_components(gpio, LinuxGpioComponent): + reset_pin_options.append((gpio, local_gpio)) + valid_combinations.append((teensy, *combination, reset_pin_options)) + + yield (encoder, 7, valid_combinations) + + + def run_test(self, enc: EncoderComponent, odrive_ncs_gpio: int, teensy: TeensyComponent, teensy_gpio_sck: TeensyGpio, teensy_gpio_miso: TeensyGpio, teensy_gpio_mosi: TeensyGpio, teensy_gpio_ncs: TeensyGpio, teensy_gpio_reset: TeensyGpio, reset_gpio: LinuxGpioComponent, logger: Logger): + true_cpr = 16384 + true_rps = 1.0 + + reset_gpio.config(output=True) # hold encoder and disable its SPI + reset_gpio.write(True) + + code = (teensy_spi_encoder_emulation_code + .replace("{sck}", str(teensy_gpio_sck.num)) + .replace("{miso}", str(teensy_gpio_miso.num)) + .replace("{mosi}", str(teensy_gpio_mosi.num)) + .replace("{ncs}", str(teensy_gpio_ncs.num)) + .replace("{reset}", str(teensy_gpio_reset.num)) + .replace("{mode}", str(self.mode))) + teensy.compile_and_program(code) + + logger.debug(f'Configuring absolute encoder in mode 0x{self.mode:x}...') + enc.handle.config.mode = self.mode + enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio + enc.handle.config.cpr = true_cpr + enc.parent.save_config_and_reboot() + + time.sleep(1.0) + + logger.debug('Testing absolute readings and SPI errors...') + + # Encoder is still disabled - expect recurring error + enc.handle.error = 0 + time.sleep(0.002) + # This fails from time to time because the pull-up on the ODrive only manages + # to pull MISO to 1.8V, leaving it in the undefined range. + test_assert_eq(enc.handle.error, errors.encoder.ERROR_ABS_SPI_COM_FAIL) + + # Enable encoder and expect error to go away + reset_gpio.write(False) + release_time = time.monotonic() + enc.handle.error = 0 + time.sleep(0.002) + test_assert_eq(enc.handle.error, 0) + + # Check absolute position after 1.5s + time.sleep(1.5) + true_delta_t = time.monotonic() - release_time + test_assert_eq(enc.handle.pos_abs, (true_delta_t * true_rps * true_cpr) % true_cpr, range = true_cpr*0.001) + + test_assert_eq(enc.handle.error, 0) + reset_gpio.write(True) + time.sleep(0.002) + test_assert_eq(enc.handle.error, errors.encoder.ERROR_ABS_SPI_COM_FAIL) + reset_gpio.write(False) + release_time = time.monotonic() + enc.handle.error = 0 + time.sleep(0.002) + test_assert_eq(enc.handle.error, 0) + + # Check absolute position after 1.5s + time.sleep(1.5) + true_delta_t = time.monotonic() - release_time + test_assert_eq(enc.handle.pos_abs, (true_delta_t * true_rps * true_cpr) % true_cpr, range = true_cpr*0.001) + + self.run_generic_encoder_test(enc.handle, true_cpr, true_rps) + enc.handle.config.cpr = 8192 + + + if __name__ == '__main__': test_runner.run([ - TestIncrementalEncoder(), - TestSinCosEncoder(), + #TestIncrementalEncoder(), + #TestSinCosEncoder(), TestHallEffectEncoder(), + TestSpiEncoder(ENCODER_MODE_SPI_ABS_AMS), + TestSpiEncoder(ENCODER_MODE_SPI_ABS_CUI), ]) diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index ea863d97..56b9e663 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -194,6 +194,9 @@ class ODriveComponent(Component): for i in range(1,9): self.__setattr__('gpio' + str(i), Component(self)) self.can = Component(self) + self.sck = Component(self) + self.miso = Component(self) + self.mosi = Component(self) def get_subcomponents(self): for enc_ctx in self.encoders: @@ -203,6 +206,9 @@ class ODriveComponent(Component): for i in range(1,9): yield ('gpio' + str(i)), getattr(self, 'gpio' + str(i)) yield 'can', self.can + yield 'spi.sck', self.sck + yield 'spi.miso', self.miso + yield 'spi.mosi', self.mosi def prepare(self, logger: Logger): """ diff --git a/tools/test-rig-rpi.yaml b/tools/test-rig-rpi.yaml index 33588274..4fb0b4e3 100644 --- a/tools/test-rig-rpi.yaml +++ b/tools/test-rig-rpi.yaml @@ -65,18 +65,18 @@ components: connections: - ['odrive.can', 'rpi.can0'] - ['teensy.program', 'rpi.gpio26'] - - ['teensy.gpio11', 'rpi.uart0.tx'] - - ['teensy.gpio12', 'rpi.uart0.rx'] - - ['teensy.gpio10', 'odrive.gpio1'] - - ['teensy.gpio9', 'odrive.gpio2'] - - ['teensy.gpio8', 'odrive.gpio3'] - - ['teensy.gpio7', 'odrive.gpio4'] + - ['teensy.gpio12', 'rpi.uart0.tx'] + - ['teensy.gpio13', 'rpi.uart0.rx'] + - ['teensy.gpio11', 'odrive.gpio1'] + - ['teensy.gpio10', 'odrive.gpio2'] + - ['teensy.gpio9', 'odrive.gpio3'] + - ['teensy.gpio8', 'odrive.gpio4'] - ['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.gpio6', 'rpi.gpio20'] + - ['teensy.gpio7', 'rpi.gpio19'] - ['teensy.gpio23', 'odrive.encoder0.z'] - ['teensy.gpio22', 'odrive.encoder0.a'] - ['teensy.gpio21', 'odrive.encoder0.b'] @@ -86,6 +86,9 @@ connections: - ['teensy.gpio0', 'real_encoder.z'] - ['teensy.gpio1', 'real_encoder.a'] - ['teensy.gpio2', 'real_encoder.b'] + - ['teensy.gpio3', 'odrive.spi.mosi'] + - ['teensy.gpio4', 'odrive.spi.miso'] + - ['teensy.gpio5', 'odrive.spi.sck'] - ['odrive.axis0', 'D5065-270KV_0'] - ['D5065-270KV_0', 'real_encoder'] - ['odrive.gpio3', 'lpf0']