From ae927acbda0982713b58c90f7d955578a46d1a02 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 13 Jun 2022 13:09:14 +0200 Subject: [PATCH] drv/counter_lpf2: Rework uartdev counter. The purpose of this driver is to clean up the counter mixed in with uartdev, which was getting hard to follow. We will be using a single motor mode going forward. By dropping the hardcoded multi-mode setup and readout, we can treat motors like any other LPF2 sensor. This also removes user restrictions on setting the mode. This also reduces code size by about 250 bytes. --- bricks/nxt/Makefile | 1 + bricks/stm32/stm32.mk | 1 + lib/pbio/drv/counter/counter_core.c | 4 +- lib/pbio/drv/counter/counter_lpf2.c | 124 +++++++++++++++ lib/pbio/drv/counter/counter_lpf2.h | 39 +++++ lib/pbio/include/pbio/iodev.h | 30 ++++ lib/pbio/include/pbio/uartdev.h | 1 - lib/pbio/platform/city_hub/pbdrvconfig.h | 2 + lib/pbio/platform/city_hub/platform.c | 16 +- lib/pbio/platform/debug/pbdrvconfig.h | 2 + lib/pbio/platform/debug/platform.c | 13 +- lib/pbio/platform/essential_hub/pbdrvconfig.h | 2 + lib/pbio/platform/essential_hub/platform.c | 17 +- lib/pbio/platform/move_hub/pbdrvconfig.h | 2 + lib/pbio/platform/move_hub/platform.c | 15 +- lib/pbio/platform/prime_hub/pbdrvconfig.h | 2 + lib/pbio/platform/prime_hub/platform.c | 37 ++++- lib/pbio/platform/technic_hub/pbdrvconfig.h | 2 + lib/pbio/platform/technic_hub/platform.c | 27 +++- lib/pbio/src/uartdev.c | 146 ++---------------- lib/pbio/src/uartdev.h | 23 --- lib/pbio/test/src/uartdev.c | 70 +-------- pybricks/util_pb/pb_device_stm32.c | 28 ++++ 23 files changed, 357 insertions(+), 247 deletions(-) create mode 100644 lib/pbio/drv/counter/counter_lpf2.c create mode 100644 lib/pbio/drv/counter/counter_lpf2.h delete mode 100644 lib/pbio/src/uartdev.h diff --git a/bricks/nxt/Makefile b/bricks/nxt/Makefile index 31a938c24..54a0565ef 100644 --- a/bricks/nxt/Makefile +++ b/bricks/nxt/Makefile @@ -211,6 +211,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ src/dcmotor.c \ src/drivebase.c \ src/error.c \ + src/iodev.c \ src/integrator.c \ src/logger.c \ src/main.c \ diff --git a/bricks/stm32/stm32.mk b/bricks/stm32/stm32.mk index 052aaa779..3aeae579d 100644 --- a/bricks/stm32/stm32.mk +++ b/bricks/stm32/stm32.mk @@ -433,6 +433,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ drv/clock/clock_stm32.c \ drv/core.c \ drv/counter/counter_core.c \ + drv/counter/counter_lpf2.c \ drv/counter/counter_stm32f0_gpio_quad_enc.c \ drv/gpio/gpio_stm32f0.c \ drv/gpio/gpio_stm32f4.c \ diff --git a/lib/pbio/drv/counter/counter_core.c b/lib/pbio/drv/counter/counter_core.c index 661837079..f2a6f93b4 100644 --- a/lib/pbio/drv/counter/counter_core.c +++ b/lib/pbio/drv/counter/counter_core.c @@ -10,8 +10,8 @@ #include #include -#include "../src/uartdev.h" #include "counter_ev3dev_stretch_iio.h" +#include "counter_lpf2.h" #include "counter_nxt.h" #include "counter_stm32f0_gpio_quad_enc.h" #include "counter_test.h" @@ -22,11 +22,11 @@ static pbdrv_counter_dev_t pbdrv_counter_devs[PBDRV_CONFIG_COUNTER_NUM_DEV]; void pbdrv_counter_init(void) { pbdrv_counter_ev3dev_stretch_iio_init(pbdrv_counter_devs); + pbdrv_counter_lpf2_init(pbdrv_counter_devs); pbdrv_counter_nxt_init(pbdrv_counter_devs); pbdrv_counter_stm32f0_gpio_quad_enc_init(pbdrv_counter_devs); pbdrv_counter_test_init(pbdrv_counter_devs); pbdrv_counter_virtual_init(pbdrv_counter_devs); - pbio_uartdev_counter_init(pbdrv_counter_devs); } pbio_error_t pbdrv_counter_get_dev(uint8_t id, pbdrv_counter_dev_t **dev) { diff --git a/lib/pbio/drv/counter/counter_lpf2.c b/lib/pbio/drv/counter/counter_lpf2.c new file mode 100644 index 000000000..13981ce3f --- /dev/null +++ b/lib/pbio/drv/counter/counter_lpf2.c @@ -0,0 +1,124 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2022 The Pybricks Authors + +// LPF2 Counter driver +// +// This driver parses type-specific position readouts from LPF2 motors. +// +// This driver currently requires that pbdrv_counter_lpf2_get_count gets +// called often enough to observe full-rotation transitions of motors that +// use absolute encoders. + +#include + +#if PBDRV_CONFIG_COUNTER_LPF2 + +#include + +#include +#include +#include +#include + +#include "counter.h" +#include "counter_lpf2.h" + +typedef struct { + int32_t rotations; + int32_t last_abs_pos; +} private_data_t; + +#define PBDRV_COUNTER_UNKNOWN_ABS_ANGLE (INT32_MAX) + +static private_data_t private_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV]; + +static pbio_error_t pbdrv_counter_lpf2_get_count(pbdrv_counter_dev_t *dev, int32_t *count) { + + private_data_t *priv = dev->priv; + const pbdrv_counter_lpf2_platform_data_t *pdata = dev->pdata; + + // Get iodev, and test if still connected. + pbio_iodev_t *iodev; + pbio_error_t err = pbdrv_ioport_get_iodev(pdata->port_id, &iodev); + if (err != PBIO_SUCCESS) { + return err; + } + + // Ensure we are still dealing with a motor. + if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(iodev)) { + return PBIO_ERROR_NO_DEV; + } + + // Ensure that we are on expected mode. + uint8_t mode_id = PBIO_IODEV_IS_ABS_MOTOR(iodev) ? + PBIO_IODEV_MODE_PUP_ABS_MOTOR__APOS: + PBIO_IODEV_MODE_PUP_REL_MOTOR__POS; + if (iodev->mode != mode_id) { + return PBIO_ERROR_INVALID_OP; + } + + // Get LPF2 data buffer. + uint8_t *data; + pbio_iodev_get_data(iodev, &data); + + // For incremental encoders, we can just return the LPF2 data directly. + if (!PBIO_IODEV_IS_ABS_MOTOR(iodev)) { + *count = pbio_get_uint32_le(data); + priv->last_abs_pos = PBDRV_COUNTER_UNKNOWN_ABS_ANGLE; + return PBIO_SUCCESS; + } + + // For absolute encoders, we need to keep track of whole rotations. + int32_t abs_pos = (int16_t)pbio_get_uint16_le(data); + + // Update rotation counter as encoder passes through -180 + if (abs_pos < -90 && priv->last_abs_pos > 90) { + priv->rotations += 1; + } + if (abs_pos > 90 && priv->last_abs_pos < -90) { + priv->rotations -= 1; + } + priv->last_abs_pos = abs_pos; + + // The total count is the position in this circle plus full rotations. + *count = priv->rotations * 360 + abs_pos; + return PBIO_SUCCESS; +} + +static pbio_error_t pbdrv_counter_lpf2_get_abs_count(pbdrv_counter_dev_t *dev, int32_t *count) { + private_data_t *priv = dev->priv; + + // Get total count value, which also updates absolute count value. + pbio_error_t err = pbdrv_counter_lpf2_get_count(dev, count); + if (err != PBIO_SUCCESS) { + return err; + } + + if (priv->last_abs_pos == PBDRV_COUNTER_UNKNOWN_ABS_ANGLE) { + return PBIO_ERROR_NOT_SUPPORTED; + } + + *count = priv->last_abs_pos; + + return PBIO_SUCCESS; +} + +static const pbdrv_counter_funcs_t pbdrv_counter_lpf2_funcs = { + .get_count = pbdrv_counter_lpf2_get_count, + .get_abs_count = pbdrv_counter_lpf2_get_abs_count, +}; + +void pbdrv_counter_lpf2_init(pbdrv_counter_dev_t *devs) { + for (int i = 0; i < PBIO_ARRAY_SIZE(private_data); i++) { + const pbdrv_counter_lpf2_platform_data_t *pdata = + &pbdrv_counter_lpf2_platform_data[i]; + private_data_t *priv = &private_data[i]; + pbdrv_counter_dev_t *dev = &devs[pdata->counter_id]; + + dev->pdata = pdata; + dev->funcs = &pbdrv_counter_lpf2_funcs; + dev->priv = priv; + } +} + +#endif // PBDRV_CONFIG_COUNTER_LPF2 diff --git a/lib/pbio/drv/counter/counter_lpf2.h b/lib/pbio/drv/counter/counter_lpf2.h new file mode 100644 index 000000000..d9e8e9268 --- /dev/null +++ b/lib/pbio/drv/counter/counter_lpf2.h @@ -0,0 +1,39 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2022 The Pybricks Authors + +#ifndef _INTERNAL_PVDRV_COUNTER_LPF2_H_ +#define _INTERNAL_PVDRV_COUNTER_LPF2_H_ + +#include + +#if PBDRV_CONFIG_COUNTER_LPF2 + +#if !PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV +#error Platform must define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV +#endif + +#include + +#include +#include + +typedef struct { + /** The ID of the counter device provided by this instance. */ + uint8_t counter_id; + /** The I/O port identifier this counter is associated with. */ + pbio_port_id_t port_id; +} pbdrv_counter_lpf2_platform_data_t; + +// defined in platform/*/platform.c +extern const pbdrv_counter_lpf2_platform_data_t + pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV]; + +void pbdrv_counter_lpf2_init(pbdrv_counter_dev_t *devs); + +#else // PBDRV_CONFIG_COUNTER_LPF2 + +#define pbdrv_counter_lpf2_init(devs) + +#endif // PBDRV_CONFIG_COUNTER_LPF2 + +#endif // _INTERNAL_PVDRV_COUNTER_LPF2_H_ diff --git a/lib/pbio/include/pbio/iodev.h b/lib/pbio/include/pbio/iodev.h index 22c337dc7..784cc0383 100644 --- a/lib/pbio/include/pbio/iodev.h +++ b/lib/pbio/include/pbio/iodev.h @@ -344,6 +344,28 @@ enum { PBIO_IODEV_MODE_PUP_COLOR_LIGHT_MATRIX__TRANS = 3, }; +/** + * Modes for Powered Up motors with relative encoders + */ +enum { + PBIO_IODEV_MODE_PUP_REL_MOTOR__POWER = 0, + PBIO_IODEV_MODE_PUP_REL_MOTOR__SPEED = 1, + PBIO_IODEV_MODE_PUP_REL_MOTOR__POS = 2, + PBIO_IODEV_MODE_PUP_REL_MOTOR__TEST = 3, +}; + +/** + * Modes for Powered Up motors with absolute encoders + */ +enum { + PBIO_IODEV_MODE_PUP_ABS_MOTOR__POWER = 0, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__SPEED = 1, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__POS = 2, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__APOS = 3, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__CALIB = 4, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__STATS = 5, +}; + /** * Data types used by I/O devices. */ @@ -441,6 +463,14 @@ typedef enum { */ #define PBIO_IODEV_IS_FEEDBACK_MOTOR(d) ((d)->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS) +/** + * Macro for testing if I/O device is a motor with absolute position feedback. + * + * @param [in] d Pointer to pbio_iodev_t. + */ +#define PBIO_IODEV_IS_ABS_MOTOR(d) ((d)->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS) + + /** * Mapping flags that describe the input and output values of an I/O device. diff --git a/lib/pbio/include/pbio/uartdev.h b/lib/pbio/include/pbio/uartdev.h index 74b50a186..975f7d809 100644 --- a/lib/pbio/include/pbio/uartdev.h +++ b/lib/pbio/include/pbio/uartdev.h @@ -13,7 +13,6 @@ typedef struct { uint8_t uart_id; /**< The ID of a UART device used by this uartdev */ - uint8_t counter_id; /**< The ID of a counter device provided by this uartdev */ } pbio_uartdev_platform_data_t; #if PBIO_CONFIG_UARTDEV diff --git a/lib/pbio/platform/city_hub/pbdrvconfig.h b/lib/pbio/platform/city_hub/pbdrvconfig.h index 8769ba300..fc6d83fa5 100644 --- a/lib/pbio/platform/city_hub/pbdrvconfig.h +++ b/lib/pbio/platform/city_hub/pbdrvconfig.h @@ -31,6 +31,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (2) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (2) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F0 (1) diff --git a/lib/pbio/platform/city_hub/platform.c b/lib/pbio/platform/city_hub/platform.c index f776c1e8c..f1cdd5d9a 100644 --- a/lib/pbio/platform/city_hub/platform.c +++ b/lib/pbio/platform/city_hub/platform.c @@ -9,6 +9,7 @@ #include "../../drv/bluetooth/bluetooth_stm32_cc2640.h" #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/motor_driver/motor_driver_hbridge_pwm.h" @@ -161,6 +162,19 @@ void EXTI2_3_IRQHandler(void) { EXTI->PR = EXTI_PR_PR2; } +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -340,11 +354,9 @@ void USART3_6_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_ID_0, - .counter_id = COUNTER_PORT_A, }, [1] = { .uart_id = UART_ID_1, - .counter_id = COUNTER_PORT_B, }, }; diff --git a/lib/pbio/platform/debug/pbdrvconfig.h b/lib/pbio/platform/debug/pbdrvconfig.h index 8a28fd6e8..2d6ab9755 100644 --- a/lib/pbio/platform/debug/pbdrvconfig.h +++ b/lib/pbio/platform/debug/pbdrvconfig.h @@ -20,6 +20,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (1) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (1) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F4 (1) diff --git a/lib/pbio/platform/debug/platform.c b/lib/pbio/platform/debug/platform.c index 8c81687d0..9612f2ed2 100644 --- a/lib/pbio/platform/debug/platform.c +++ b/lib/pbio/platform/debug/platform.c @@ -13,6 +13,7 @@ #include "../../drv/adc/adc_stm32_hal.h" #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/pwm/pwm_stm32_tim.h" #include "../../drv/uart/uart_stm32f4_ll_irq.h" @@ -133,6 +134,8 @@ void DMA2_Stream0_IRQHandler(void) { pbdrv_adc_stm32_hal_handle_irq(); } +// buttons + const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTON_GPIO_NUM_BUTTON] = { [0] = { .gpio = { .bank = GPIOC, .pin = 13 }, @@ -140,6 +143,15 @@ const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTO } }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [0] = { + .counter_id = 0, + .port_id = PBIO_PORT_ID_1, + }, +}; + // LED // TODO: these are move/city hub values @@ -230,7 +242,6 @@ const pbdrv_uart_stm32f4_ll_irq_platform_data_t const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_ID_0, - .counter_id = COUNTER_DEV_0, }, }; diff --git a/lib/pbio/platform/essential_hub/pbdrvconfig.h b/lib/pbio/platform/essential_hub/pbdrvconfig.h index 5a5828ea8..272cbee04 100644 --- a/lib/pbio/platform/essential_hub/pbdrvconfig.h +++ b/lib/pbio/platform/essential_hub/pbdrvconfig.h @@ -44,6 +44,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (2) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (2) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F4 (1) diff --git a/lib/pbio/platform/essential_hub/platform.c b/lib/pbio/platform/essential_hub/platform.c index 6f02a7ba3..a17d5c6c0 100644 --- a/lib/pbio/platform/essential_hub/platform.c +++ b/lib/pbio/platform/essential_hub/platform.c @@ -17,6 +17,7 @@ #include "../../drv/bluetooth/bluetooth_btstack.h" #include "../../drv/button/button_gpio.h" #include "../../drv/charger/charger_mp2639a.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/motor_driver/motor_driver_hbridge_pwm.h" @@ -150,6 +151,19 @@ const pbdrv_charger_mp2639a_platform_data_t pbdrv_charger_mp2639a_platform_data .ib_adc_ch = 3, }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -381,15 +395,12 @@ void USART3_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [COUNTER_PORT_A] = { .uart_id = UART_PORT_A, - .counter_id = COUNTER_PORT_A, }, [COUNTER_PORT_B] = { .uart_id = UART_PORT_B, - .counter_id = COUNTER_PORT_B, }, }; - // STM32 HAL integration // bootloader gives us 16MHz clock diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index aadaf2a22..50da36cf1 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -32,6 +32,8 @@ #define PBDRV_CONFIG_COUNTER_NUM_DEV (4) #define PBDRV_CONFIG_COUNTER_STM32F0_GPIO_QUAD_ENC (1) #define PBDRV_CONFIG_COUNTER_STM32F0_GPIO_QUAD_ENC_NUM_DEV (2) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (2) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F0 (1) diff --git a/lib/pbio/platform/move_hub/platform.c b/lib/pbio/platform/move_hub/platform.c index 50ba05133..df6751612 100644 --- a/lib/pbio/platform/move_hub/platform.c +++ b/lib/pbio/platform/move_hub/platform.c @@ -8,6 +8,7 @@ #include #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/counter/counter_stm32f0_gpio_quad_enc.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" @@ -67,6 +68,17 @@ const pbdrv_counter_stm32f0_gpio_quad_enc_platform_data_t }, }; +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [0] = { + .counter_id = COUNTER_PORT_C, + .port_id = PBIO_PORT_ID_C, + }, + [1] = { + .counter_id = COUNTER_PORT_D, + .port_id = PBIO_PORT_ID_D, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -289,13 +301,12 @@ void USART3_4_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_ID_0, - .counter_id = COUNTER_PORT_C, }, [1] = { .uart_id = UART_ID_1, - .counter_id = COUNTER_PORT_D, }, }; + #endif // PBIO_CONFIG_UARTDEV // special memory addresses defined in linker script diff --git a/lib/pbio/platform/prime_hub/pbdrvconfig.h b/lib/pbio/platform/prime_hub/pbdrvconfig.h index 5fa23762e..2944602bb 100644 --- a/lib/pbio/platform/prime_hub/pbdrvconfig.h +++ b/lib/pbio/platform/prime_hub/pbdrvconfig.h @@ -46,6 +46,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (6) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (6) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F4 (1) diff --git a/lib/pbio/platform/prime_hub/platform.c b/lib/pbio/platform/prime_hub/platform.c index bcbfc8323..cdee36a71 100644 --- a/lib/pbio/platform/prime_hub/platform.c +++ b/lib/pbio/platform/prime_hub/platform.c @@ -15,6 +15,7 @@ #include "../../drv/bluetooth/bluetooth_btstack_uart_block_stm32_hal.h" #include "../../drv/bluetooth/bluetooth_btstack.h" #include "../../drv/charger/charger_mp2639a.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_array_pwm.h" #include "../../drv/led/led_dual.h" @@ -144,6 +145,35 @@ const pbdrv_charger_mp2639a_platform_data_t pbdrv_charger_mp2639a_platform_data .iset_pwm_ch = 1, }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, + [COUNTER_PORT_C] = { + .counter_id = COUNTER_PORT_C, + .port_id = PBIO_PORT_ID_C, + }, + [COUNTER_PORT_D] = { + .counter_id = COUNTER_PORT_D, + .port_id = PBIO_PORT_ID_D, + }, + [COUNTER_PORT_E] = { + .counter_id = COUNTER_PORT_E, + .port_id = PBIO_PORT_ID_E, + }, + [COUNTER_PORT_F] = { + .counter_id = COUNTER_PORT_F, + .port_id = PBIO_PORT_ID_F, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -622,31 +652,24 @@ void UART10_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [COUNTER_PORT_A] = { .uart_id = UART_PORT_A, - .counter_id = COUNTER_PORT_A, }, [COUNTER_PORT_B] = { .uart_id = UART_PORT_B, - .counter_id = COUNTER_PORT_B, }, [COUNTER_PORT_C] = { .uart_id = UART_PORT_C, - .counter_id = COUNTER_PORT_C, }, [COUNTER_PORT_D] = { .uart_id = UART_PORT_D, - .counter_id = COUNTER_PORT_D, }, [COUNTER_PORT_E] = { .uart_id = UART_PORT_E, - .counter_id = COUNTER_PORT_E, }, [COUNTER_PORT_F] = { .uart_id = UART_PORT_F, - .counter_id = COUNTER_PORT_F, }, }; - // STM32 HAL integration // bootloader gives us 16MHz clock diff --git a/lib/pbio/platform/technic_hub/pbdrvconfig.h b/lib/pbio/platform/technic_hub/pbdrvconfig.h index ac410cf4b..51ad75fc6 100644 --- a/lib/pbio/platform/technic_hub/pbdrvconfig.h +++ b/lib/pbio/platform/technic_hub/pbdrvconfig.h @@ -36,6 +36,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (4) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (4) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32L4 (1) diff --git a/lib/pbio/platform/technic_hub/platform.c b/lib/pbio/platform/technic_hub/platform.c index 618ab3e5e..ea35d8d08 100644 --- a/lib/pbio/platform/technic_hub/platform.c +++ b/lib/pbio/platform/technic_hub/platform.c @@ -9,6 +9,7 @@ #include "../../drv/battery/battery_adc.h" #include "../../drv/bluetooth/bluetooth_stm32_cc2640.h" #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/motor_driver/motor_driver_hbridge_pwm.h" @@ -183,6 +184,27 @@ const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTO }, }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, + [COUNTER_PORT_C] = { + .counter_id = COUNTER_PORT_C, + .port_id = PBIO_PORT_ID_C, + }, + [COUNTER_PORT_D] = { + .counter_id = COUNTER_PORT_D, + .port_id = PBIO_PORT_ID_D, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -548,23 +570,18 @@ void LPUART1_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_PORT_A, - .counter_id = COUNTER_PORT_A, }, [1] = { .uart_id = UART_PORT_B, - .counter_id = COUNTER_PORT_B, }, [2] = { .uart_id = UART_PORT_C, - .counter_id = COUNTER_PORT_C, }, [3] = { .uart_id = UART_PORT_D, - .counter_id = COUNTER_PORT_D, }, }; - // STM32 HAL integration // Default clock is 4MHz on boot diff --git a/lib/pbio/src/uartdev.c b/lib/pbio/src/uartdev.c index 0525eb2a8..cdb3fe76a 100644 --- a/lib/pbio/src/uartdev.c +++ b/lib/pbio/src/uartdev.c @@ -41,7 +41,6 @@ #include "pbio/port.h" #include "pbio/uartdev.h" #include "pbio/util.h" -#include "../drv/counter/counter.h" #include #define EV3_UART_MAX_MESSAGE_SIZE (LUMP_MAX_MSG_SIZE + 3) @@ -139,16 +138,12 @@ typedef enum { * @new_baud_rate: New baud rate that will be set with ev3_uart_change_bitrate * @info_flags: Flags indicating what information has already been read * from the data. - * @tacho_count: The tacho count received from an LPF2 motor - * @tacho_offset: Tacho count offset to account for unexpected jumps in LPF2 tacho data * @abs_pos: The absolute position received from an LPF2 motor * @tx_msg: Buffer to hold messages transmitted to the device * @rx_msg: Buffer to hold messages received from the device * @rx_msg_size: Size of the current message being received * @ext_mode: Extra mode adder for Powered Up devices (for modes > LUMP_MAX_MODE) * @write_cmd_size: The size parameter received from a WRITE command - * @tacho_rate: The tacho rate received from an LPF2 motor - * @max_tacho_rate: The "100%" rate received from an LPF2 motor * @last_err: data->msg to be printed in case of an error. * @err_count: Total number of errors that have occurred * @num_data_err: Number of bad reads when receiving DATA data->msgs. @@ -158,8 +153,6 @@ typedef enum { * @mode_change_tx_done: Flag to keep ev3_uart_set_mode_end() blocked until * mode has actually changed * @speed_payload: Buffer for holding baud rate change message data - * @mode_combo_payload: Buffer for holding mode combo message data - * @mode_combo_size: Actual size of mode combo message */ typedef struct { pbio_iodev_t iodev; @@ -176,16 +169,11 @@ typedef struct { uint8_t new_mode; uint32_t new_baud_rate; uint32_t info_flags; - int32_t tacho_count; - int32_t tacho_offset; - int16_t abs_pos; uint8_t *tx_msg; uint8_t *rx_msg; uint8_t rx_msg_size; uint8_t ext_mode; uint8_t write_cmd_size; - int8_t tacho_rate; - int32_t max_tacho_rate; DBG_ERR(const char *last_err); uint32_t err_count; uint32_t num_data_err; @@ -193,8 +181,6 @@ typedef struct { bool tx_busy; bool mode_change_tx_done; uint8_t speed_payload[4]; - uint8_t mode_combo_payload[5]; - uint8_t mode_combo_size; } uartdev_port_data_t; enum { @@ -214,8 +200,6 @@ static uint8_t bufs[PBIO_CONFIG_UARTDEV_NUM_DEV][NUM_BUF][EV3_UART_MAX_MESSAGE_S static uartdev_port_data_t dev_data[PBIO_CONFIG_UARTDEV_NUM_DEV]; -static pbdrv_counter_dev_t *counter_devs; - #define PBIO_PT_WAIT_READY(pt, expr) PT_WAIT_UNTIL((pt), (expr) != PBIO_ERROR_AGAIN) pbio_error_t pbio_uartdev_get(uint8_t id, pbio_iodev_t **iodev) { @@ -490,9 +474,7 @@ static void pbio_uartdev_parse_msg(uartdev_port_data_t *data) { } // REVISIT: this is potentially an array of combos - data->info->mode_combos = data->rx_msg[3] << 8 | data->rx_msg[2]; - - debug_pr("mode combos: %04x\n", data->info->mode_combos); + debug_pr("mode combos: %04x\n", data->rx_msg[3] << 8 | data->rx_msg[2]); break; case LUMP_INFO_UNK9: @@ -505,12 +487,10 @@ static void pbio_uartdev_parse_msg(uartdev_port_data_t *data) { goto err; } - // first 3 parameters look like PID constants - data->max_tacho_rate = pbio_get_uint32_le(data->rx_msg + 14); - + // first 3 parameters look like PID constants, 4th is max tacho_rate debug_pr("motor parameters: %" PRIu32 " %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", pbio_get_uint32_le(data->rx_msg + 2), pbio_get_uint32_le(data->rx_msg + 6), - pbio_get_uint32_le(data->rx_msg + 10), data->max_tacho_rate); + pbio_get_uint32_le(data->rx_msg + 10), pbio_get_uint32_le(data->rx_msg + 14)); break; case LUMP_INFO_UNK11: @@ -563,37 +543,16 @@ static void pbio_uartdev_parse_msg(uartdev_port_data_t *data) { goto err; } - if (PBIO_IODEV_IS_FEEDBACK_MOTOR(&data->iodev) && data->write_cmd_size > 0) { - data->tacho_rate = data->rx_msg[1]; - - // Decode the tacho count data message - int32_t tacho_count_msg = pbio_get_uint32_le(data->rx_msg + 2); - - // Sometimes, the incremental tacho data unexpectedly jumps by multiples - // of -/+360, so add a correction if an impossibly high change is detected. - while (tacho_count_msg - data->tacho_offset - data->tacho_count < -270) { - data->tacho_offset -= 360; - } - while (tacho_count_msg - data->tacho_offset - data->tacho_count > 270) { - data->tacho_offset += 360; - } - // The counter driver must return the corrected count - data->tacho_count = tacho_count_msg - data->tacho_offset; - - if (data->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS) { - data->abs_pos = data->rx_msg[7] << 8 | data->rx_msg[6]; - } - } else { - if (mode >= data->info->num_modes) { - DBG_ERR(data->last_err = "Invalid mode received"); - goto err; - } - data->iodev.mode = mode; - if (mode == data->new_mode) { - memcpy(data->iodev.bin_data, data->rx_msg + 1, msg_size - 2); - } + if (mode >= data->info->num_modes) { + DBG_ERR(data->last_err = "Invalid mode received"); + goto err; + } + data->iodev.mode = mode; + if (mode == data->new_mode) { + memcpy(data->iodev.bin_data, data->rx_msg + 1, msg_size - 2); } + // setting type_id in info struct lets external modules know a device is connected and receiving good data data->info->type_id = data->type_id; @@ -742,9 +701,6 @@ static PT_THREAD(pbio_uartdev_update(uartdev_port_data_t * data)) { data->info->capability_flags = PBIO_IODEV_CAPABILITY_FLAG_NONE; data->ext_mode = 0; data->status = PBIO_UARTDEV_STATUS_SYNCING; - // default max tacho rate for BOOST external motor since it is the only - // motor that does not send this info - data->max_tacho_rate = 1400; pbdrv_uart_flush(data->uart); @@ -899,37 +855,6 @@ static PT_THREAD(pbio_uartdev_update(uartdev_port_data_t * data)) { // reset data rx thread PT_INIT(&data->data_pt); - if (PBIO_IODEV_IS_FEEDBACK_MOTOR(&data->iodev)) { - data->mode_combo_size = __builtin_popcount(data->info->mode_combos) + 2; - data->mode_combo_payload[0] = 0x20 | (data->mode_combo_size - 2); // mode combo command, x modes - data->mode_combo_payload[1] = 0; // combo index - data->mode_combo_payload[2] = 1 << 4 | 0; // mode 1, dataset 0 - data->mode_combo_payload[3] = 2 << 4 | 0; // mode 2, dataset 0 - data->mode_combo_payload[4] = 3 << 4 | 0; // mode 3, dataset 0 - // HACK: we are cheating here and assuming that all mode combinations - // are consecutive, starting with mode 1 and data->mode_combo_size - // chops off any unused mode (i.e. APOS) - - // setup motor to send position and speed data - PBIO_PT_WAIT_READY(&data->pt, - err = ev3_uart_begin_tx_msg(data, LUMP_MSG_TYPE_CMD, LUMP_CMD_WRITE, - data->mode_combo_payload, data->mode_combo_size)); - if (err != PBIO_SUCCESS) { - DBG_ERR(data->last_err = "UART Tx begin error during motor"); - goto err; - } - PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart)); - if (err != PBIO_SUCCESS) { - data->tx_busy = false; - DBG_ERR(data->last_err = "UART Tx end error during motor"); - goto err; - } - data->tx_busy = false; - - // Reset tacho offset - data->tacho_offset = 0; - } - // Turn on power for devices that need it if (data->motor_driver) { if (data->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_NEEDS_SUPPLY_PIN1) { @@ -1051,10 +976,6 @@ static pbio_error_t ev3_uart_set_mode_begin(pbio_iodev_t *iodev, uint8_t mode) { uartdev_port_data_t *port_data = PBIO_CONTAINER_OF(iodev, uartdev_port_data_t, iodev); pbio_error_t err; - // User mode change for motors is not supported - if (PBIO_IODEV_IS_FEEDBACK_MOTOR(iodev)) { - return PBIO_ERROR_NOT_SUPPORTED; - } err = ev3_uart_begin_tx_msg(port_data, LUMP_MSG_TYPE_CMD, LUMP_CMD_SELECT, &mode, 1); if (err != PBIO_SUCCESS) { @@ -1146,45 +1067,6 @@ static const pbio_iodev_ops_t pbio_uartdev_ops = { .write_cancel = ev3_uart_write_cancel, }; -static pbio_error_t pbio_uartdev_get_count(pbdrv_counter_dev_t *dev, int32_t *count) { - uartdev_port_data_t *port_data = dev->priv; - - if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(&port_data->iodev)) { - return PBIO_ERROR_NO_DEV; - } - - *count = port_data->tacho_count; - - return PBIO_SUCCESS; -} - -static pbio_error_t pbio_uartdev_get_abs_count(pbdrv_counter_dev_t *dev, int32_t *count) { - uartdev_port_data_t *port_data = dev->priv; - - if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(&port_data->iodev)) { - return PBIO_ERROR_NO_DEV; - } - - if (!(port_data->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS)) { - return PBIO_ERROR_NOT_SUPPORTED; - } - - *count = port_data->abs_pos; - - return PBIO_SUCCESS; -} - -static const pbdrv_counter_funcs_t pbio_uartdev_counter_funcs = { - .get_count = pbio_uartdev_get_count, - .get_abs_count = pbio_uartdev_get_abs_count, -}; - -void pbio_uartdev_counter_init(pbdrv_counter_dev_t *devs) { - // Since this has async init via contiki process, we need to save this - // pointer for later. - counter_devs = devs; -} - static PT_THREAD(pbio_uartdev_init(struct pt *pt, uint8_t id)) { const pbio_uartdev_platform_data_t *pdata = &pbio_uartdev_platform_data[id]; uartdev_port_data_t *port_data = &dev_data[id]; @@ -1200,12 +1082,6 @@ static PT_THREAD(pbio_uartdev_init(struct pt *pt, uint8_t id)) { port_data->tx_msg = &bufs[id][BUF_TX_MSG][0]; port_data->rx_msg = &bufs[id][BUF_RX_MSG][0]; - // It is not guaranteed that pbio_uartdev_counter_init() is called before pbio_uartdev_init() - PT_WAIT_UNTIL(pt, counter_devs != NULL); - pbdrv_counter_dev_t *counter = &counter_devs[pdata->counter_id]; - counter->funcs = &pbio_uartdev_counter_funcs; - counter->priv = port_data; - PT_END(pt); } diff --git a/lib/pbio/src/uartdev.h b/lib/pbio/src/uartdev.h deleted file mode 100644 index 047e10805..000000000 --- a/lib/pbio/src/uartdev.h +++ /dev/null @@ -1,23 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2020 The Pybricks Authors - -// Internal functions for LEGO UART I/O devices. - -#ifndef _PBIO_UARTDEV_H_ -#define _PBIO_UARTDEV_H_ - -#include - -#if PBIO_CONFIG_UARTDEV - -#include - -void pbio_uartdev_counter_init(pbdrv_counter_dev_t *devs); - -#else // PBIO_CONFIG_UARTDEV - -#define pbio_uartdev_counter_init(devs) - -#endif // PBIO_CONFIG_UARTDEV - -#endif // _PBIO_UARTDEV_H_ diff --git a/lib/pbio/test/src/uartdev.c b/lib/pbio/test/src/uartdev.c index 9f42e4438..2b50fbe49 100644 --- a/lib/pbio/test/src/uartdev.c +++ b/lib/pbio/test/src/uartdev.c @@ -1,5 +1,5 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2019-2021 The Pybricks Authors +// Copyright (c) 2019-2022 The Pybricks Authors #include #include @@ -12,7 +12,6 @@ #include #include -#include #include #include #include @@ -332,12 +331,6 @@ static PT_THREAD(test_boost_color_distance_sensor(struct pt *pt)) { PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_ERROR_NO_DEV); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_ERROR_NO_DEV); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -354,7 +347,6 @@ static PT_THREAD(test_boost_color_distance_sensor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_COLOR_DIST_SENSOR); tt_want_uint_op(iodev->info->num_modes, ==, 11); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 6 | 1 << 3 | 1 << 2 | 1 << 1 | 1 << 0); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_NONE); tt_want_uint_op(iodev->mode, ==, 0); @@ -494,9 +486,7 @@ static PT_THREAD(test_boost_interactive_motor(struct pt *pt)) { static const uint8_t msg34[] = { 0x04 }; // ACK - static const uint8_t msg35[] = { 0x54, 0x22, 0x00, 0x10, 0x20, 0xB9 }; // WRITE mode combo - - static const uint8_t msg36[] = { 0xD8, 0x9C, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0xBB }; // DATA speed and position combo + static const uint8_t msg36[] = { 0xD0, 0xFF, 0xFF, 0xFF, 0xFF, 0x2F }; // DATA length 4, mode 0 static const uint8_t msg37[] = { 0x02 }; // NACK @@ -566,22 +556,8 @@ static PT_THREAD(test_boost_interactive_motor(struct pt *pt)) { test_uart_dev.baud == 115200; })); - // motors get WRITE message to setup mode combos - SIMULATE_TX_MSG(msg35); - - // same message is received in respose along with data message - SIMULATE_RX_MSG(msg35); - SIMULATE_RX_MSG(msg36); - PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, -1); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_ERROR_NOT_SUPPORTED); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -597,7 +573,6 @@ static PT_THREAD(test_boost_interactive_motor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR); tt_want_uint_op(iodev->info->num_modes, ==, 4); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 2 | 1 << 1); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_IS_DC_OUTPUT | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_SPEED | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS); tt_want_uint_op(iodev->mode, ==, 0); @@ -680,9 +655,7 @@ static PT_THREAD(test_technic_large_motor(struct pt *pt)) { static const uint8_t msg55[] = { 0x04 }; // ACK - static const uint8_t msg56[] = { 0x5C, 0x23, 0x00, 0x10, 0x20, 0x30, 0x00, 0x00, 0x00, 0x80 }; // WRITE mode combo - - static const uint8_t msg57[] = { 0xD8, 0x64, 0xFF, 0xFF, 0xFF, 0xFF, 0x17, 0x00, 0x00, 0x54 }; // DATA speed and position combo + static const uint8_t msg57[] = { 0xD0, 0xFF, 0xFF, 0xFF, 0xFF, 0x2F }; // DATA length 4, mode 0 static const uint8_t msg58[] = { 0x02 }; // NACK @@ -765,23 +738,8 @@ static PT_THREAD(test_technic_large_motor(struct pt *pt)) { // wait for ACK SIMULATE_TX_MSG(msg55); - // motors get WRITE message to setup mode combos - SIMULATE_TX_MSG(msg56); - - // same message is received in respose along with data message - SIMULATE_RX_MSG(msg56); - SIMULATE_RX_MSG(msg57); - PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, -1); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, 23); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -797,7 +755,6 @@ static PT_THREAD(test_technic_large_motor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_TECHNIC_L_MOTOR); tt_want_uint_op(iodev->info->num_modes, ==, 6); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 3 | 1 << 2 | 1 << 1); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_IS_DC_OUTPUT | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_SPEED | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS); tt_want_uint_op(iodev->mode, ==, 0); @@ -886,9 +843,7 @@ static PT_THREAD(test_technic_xl_motor(struct pt *pt)) { static const uint8_t msg55[] = { 0x04 }; // ACK - static const uint8_t msg56[] = { 0x5C, 0x23, 0x00, 0x10, 0x20, 0x30, 0x00, 0x00, 0x00, 0x80 }; // WRITE mode combo - - static const uint8_t msg57[] = { 0xD8, 0x64, 0xFF, 0xFF, 0xFF, 0xFF, 0x41, 0x00, 0x00, 0x02 }; // DATA speed and position combo + static const uint8_t msg57[] = { 0xD0, 0xFF, 0xFF, 0xFF, 0xFF, 0x2F }; // DATA length 4, mode 0 static const uint8_t msg58[] = { 0x02 }; // NACK @@ -971,23 +926,8 @@ static PT_THREAD(test_technic_xl_motor(struct pt *pt)) { // wait for ACK SIMULATE_TX_MSG(msg55); - // motors get WRITE message to setup mode combos - SIMULATE_TX_MSG(msg56); - - // same message is received in respose along with data message - SIMULATE_RX_MSG(msg56); - SIMULATE_RX_MSG(msg57); - PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, -1); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, 65); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -1003,7 +943,6 @@ static PT_THREAD(test_technic_xl_motor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_TECHNIC_XL_MOTOR); tt_want_uint_op(iodev->info->num_modes, ==, 6); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 3 | 1 << 2 | 1 << 1); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_IS_DC_OUTPUT | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_SPEED | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS); tt_want_uint_op(iodev->mode, ==, 0); @@ -1045,7 +984,6 @@ struct testcase_t pbio_uartdev_tests[] = { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[] = { [0] = { .uart_id = 0, - .counter_id = 0, }, }; diff --git a/pybricks/util_pb/pb_device_stm32.c b/pybricks/util_pb/pb_device_stm32.c index c378d25ac..2cb0c8bf3 100644 --- a/pybricks/util_pb/pb_device_stm32.c +++ b/pybricks/util_pb/pb_device_stm32.c @@ -225,4 +225,32 @@ int8_t pb_device_get_mode_id_from_str(pb_device_t *pbdev, const char *mode_str) } void pb_device_setup_motor(pbio_port_id_t port, bool is_servo) { + + // Get the iodevice + pbio_iodev_t *iodev; + pbio_error_t err; + + // Set up device + while ((err = pbdrv_ioport_get_iodev(port, &iodev)) == PBIO_ERROR_AGAIN) { + mp_hal_delay_ms(50); + } + pb_assert(err); + + // Only motors are allowed. + if (!PBIO_IODEV_IS_DC_OUTPUT(iodev)) { + pb_assert(PBIO_ERROR_NO_DEV); + } + + // If it's a DC motor, no further setup is needed. + if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(iodev)) { + return; + } + + // Choose mode based on device capabilities. + uint8_t mode_id = PBIO_IODEV_IS_ABS_MOTOR(iodev) ? + PBIO_IODEV_MODE_PUP_ABS_MOTOR__APOS: + PBIO_IODEV_MODE_PUP_REL_MOTOR__POS; + + // Activate mode. + set_mode(iodev, mode_id); }