diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index aead55901..69544ae3d 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -57,7 +57,7 @@ PYBRICKS_PYBRICKS_SRC_C = $(addprefix pybricks/,\ hubs/pb_type_virtualhub.c \ iodevices/pb_module_iodevices.c \ iodevices/pb_type_iodevices_analogsensor.c \ - iodevices/pb_type_iodevices_i2cdevice.c \ + iodevices/pb_type_i2c_device.c \ iodevices/pb_type_iodevices_lwp3device.c \ iodevices/pb_type_iodevices_pupdevice.c \ iodevices/pb_type_iodevices_xbox_controller.c \ diff --git a/bricks/ev3/mpconfigport.h b/bricks/ev3/mpconfigport.h index f7255a1ab..8a694abbc 100644 --- a/bricks/ev3/mpconfigport.h +++ b/bricks/ev3/mpconfigport.h @@ -30,6 +30,7 @@ #define PYBRICKS_PY_EXPERIMENTAL (1) #define PYBRICKS_PY_HUBS (1) #define PYBRICKS_PY_IODEVICES (1) +#define PYBRICKS_PY_IODEVICES_I2CDEVICE (1) #define PYBRICKS_PY_IODEVICES_XBOX_CONTROLLER (0) #define PYBRICKS_PY_MEDIA (1) #define PYBRICKS_PY_NXTDEVICES (1) diff --git a/lib/pbio/drv/i2c/i2c_ev3.c b/lib/pbio/drv/i2c/i2c_ev3.c index 409a65d70..dfbb6bec8 100644 --- a/lib/pbio/drv/i2c/i2c_ev3.c +++ b/lib/pbio/drv/i2c/i2c_ev3.c @@ -27,7 +27,7 @@ #include "../drv/rproc/rproc.h" #include "../rproc/rproc_ev3.h" -#define DEBUG 1 +#define DEBUG 0 #if DEBUG #include #include @@ -43,6 +43,9 @@ // Rounded up to a nice power of 2 and multiple of cache lines #define PRU_I2C_MAX_BYTES_PER_TXN 512 +// Number of times we try the operation before NAK is raised as an IO error. +#define PRU_I2C_MAX_NUM_TRIES_ON_NAK (2) + static uint8_t pbdrv_i2c_buffers[PBDRV_RPROC_EV3_PRU1_NUM_I2C_BUSES][PRU_I2C_MAX_BYTES_PER_TXN] PBDRV_DMA_BUF; struct _pbdrv_i2c_dev_t { @@ -50,6 +53,8 @@ struct _pbdrv_i2c_dev_t { volatile bool is_busy; bool is_initialized; uint8_t pru_i2c_idx; + pbio_os_timer_t timer; + size_t try_count; }; static pbdrv_i2c_dev_t i2c_devs[PBDRV_RPROC_EV3_PRU1_NUM_I2C_BUSES]; @@ -105,7 +110,7 @@ pbio_error_t pbdrv_i2c_write_then_read( uint8_t dev_addr, const uint8_t *wdata, size_t wlen, - uint8_t *rdata, + uint8_t **rdata, size_t rlen, bool nxt_quirk) { @@ -114,7 +119,7 @@ pbio_error_t pbdrv_i2c_write_then_read( if (wlen && !wdata) { return PBIO_ERROR_INVALID_ARG; } - if (rlen && !rdata) { + if (*rdata) { return PBIO_ERROR_INVALID_ARG; } if (wlen > 0xff || rlen > 0xff) { @@ -129,45 +134,59 @@ pbio_error_t pbdrv_i2c_write_then_read( if (wlen) { memcpy(i2c_dev->buffer, wdata, wlen); } - i2c_dev->is_busy = true; - pbdrv_cache_prepare_before_dma(i2c_dev->buffer, PRU_I2C_MAX_BYTES_PER_TXN); - - // Kick off transfer - pbdrv_rproc_ev3_pru1_shared_ram.i2c[i2c_dev->pru_i2c_idx].flags = PBDRV_RPROC_EV3_PRU1_I2C_PACK_FLAGS( - dev_addr, - rlen, - wlen, - PBDRV_RPROC_EV3_PRU1_I2C_CMD_START | (nxt_quirk ? PBDRV_RPROC_EV3_PRU1_I2C_CMD_NXT_QUIRK : 0) - ); - - // Wait for transfer to finish - PBIO_OS_AWAIT_WHILE(state, i2c_dev->is_busy); - - uint32_t flags = pbdrv_rproc_ev3_pru1_shared_ram.i2c[i2c_dev->pru_i2c_idx].flags; - debug_pr("i2c %d done flags %08x\r\n", i2c_dev->pru_i2c_idx, flags); - if (!(flags & PBDRV_RPROC_EV3_PRU1_I2C_STAT_DONE)) { - debug_pr("i2c %d not actually done???\r\n", i2c_dev->pru_i2c_idx); - return PBIO_ERROR_FAILED; - } - switch (flags & PBDRV_RPROC_EV3_PRU1_I2C_STAT_MASK) { - case PBDRV_RPROC_EV3_PRU1_I2C_STAT_OK: - break; - case PBDRV_RPROC_EV3_PRU1_I2C_STAT_TIMEOUT: + + for (i2c_dev->try_count = 0; i2c_dev->try_count < PRU_I2C_MAX_NUM_TRIES_ON_NAK; i2c_dev->try_count++) { + + if (nxt_quirk) { + // NXT sensors affected by the quirk can't be accessed too quickly. + // The timer is set after awaiting so we don't unnecessarily slow + // down code that polls less frequently. + PBIO_OS_AWAIT_UNTIL(state, pbio_os_timer_is_expired(&i2c_dev->timer)); + pbio_os_timer_set(&i2c_dev->timer, 100); + } + + i2c_dev->is_busy = true; + pbdrv_cache_prepare_before_dma(i2c_dev->buffer, PRU_I2C_MAX_BYTES_PER_TXN); + + // Kick off transfer + pbdrv_rproc_ev3_pru1_shared_ram.i2c[i2c_dev->pru_i2c_idx].flags = PBDRV_RPROC_EV3_PRU1_I2C_PACK_FLAGS( + dev_addr, + rlen, + wlen, + PBDRV_RPROC_EV3_PRU1_I2C_CMD_START | (nxt_quirk ? PBDRV_RPROC_EV3_PRU1_I2C_CMD_NXT_QUIRK : 0) + ); + + // Wait for transfer to finish + PBIO_OS_AWAIT_WHILE(state, i2c_dev->is_busy); + + uint32_t flags = pbdrv_rproc_ev3_pru1_shared_ram.i2c[i2c_dev->pru_i2c_idx].flags; + debug_pr("i2c %d done flags %08x\r\n", i2c_dev->pru_i2c_idx, flags); + if (!(flags & PBDRV_RPROC_EV3_PRU1_I2C_STAT_DONE)) { + debug_pr("i2c %d not actually done\r\n", i2c_dev->pru_i2c_idx); + return PBIO_ERROR_FAILED; + } + + flags &= PBDRV_RPROC_EV3_PRU1_I2C_STAT_MASK; + + if (flags == PBDRV_RPROC_EV3_PRU1_I2C_STAT_OK) { + // Success, exit loop. + pbdrv_cache_prepare_after_dma(i2c_dev->buffer, PRU_I2C_MAX_BYTES_PER_TXN); + if (rlen) { + *rdata = &i2c_dev->buffer[wlen]; + } + return PBIO_SUCCESS; + } else if (flags == PBDRV_RPROC_EV3_PRU1_I2C_STAT_TIMEOUT) { return PBIO_ERROR_TIMEDOUT; - case PBDRV_RPROC_EV3_PRU1_I2C_STAT_NAK: - return PBIO_ERROR_IO; - default: - debug_pr("i2c %d unknown error occurred???\r\n", i2c_dev->pru_i2c_idx); + } else if (flags != PBDRV_RPROC_EV3_PRU1_I2C_STAT_NAK) { + // Many known NXT devices occasionally get NAK. Retrying usually + // helps. Everything else is unexpected and raised immediately. + debug_pr("i2c %d unknown error occurred\r\n", i2c_dev->pru_i2c_idx); return PBIO_ERROR_FAILED; + } } - // If we got here, there's no error. Copy RX data. - pbdrv_cache_prepare_after_dma(i2c_dev->buffer, PRU_I2C_MAX_BYTES_PER_TXN); - if (rlen) { - memcpy(rdata, &i2c_dev->buffer[wlen], rlen); - } - - PBIO_OS_ASYNC_END(PBIO_SUCCESS); + // Didn't succeed even after trying allowed number of times. + PBIO_OS_ASYNC_END(PBIO_ERROR_IO); } static pbio_os_process_t ev3_i2c_init_process; @@ -217,6 +236,7 @@ void pbdrv_i2c_init(void) { i2c->pru_i2c_idx = i; i2c->buffer = pbdrv_i2c_buffers[i]; i2c->is_initialized = true; + pbio_os_timer_set(&i2c->timer, 0); } pbio_busy_count_up(); diff --git a/lib/pbio/drv/ioport/ioport.c b/lib/pbio/drv/ioport/ioport.c index dbd7a7ff9..0f15ce0d8 100644 --- a/lib/pbio/drv/ioport/ioport.c +++ b/lib/pbio/drv/ioport/ioport.c @@ -10,41 +10,42 @@ #include #include -pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_uart_dev_t *uart_dev, pbdrv_ioport_p5p6_mode_t mode) { +/** + * Resets pins to the default state, which is input with the buffer disabled. + */ +static pbio_error_t pbdrv_ioport_p5p6_pin_reset(const pbdrv_ioport_pins_t *pins) { - if (mode == PBDRV_IOPORT_P5P6_MODE_GPIO_ADC) { + if (!pins) { + return PBIO_ERROR_NOT_SUPPORTED; + } - // Disables UART IRQ if it was enabled. - if (uart_dev) { - // Revisit: If we call this we should also re-enable IRQs - // when starting a new UART operation. - // pbdrv_uart_stop(uart_dev); - } + pbdrv_gpio_input(&pins->p5); + pbdrv_gpio_input(&pins->p6); + pbdrv_gpio_input(&pins->uart_tx); + pbdrv_gpio_input(&pins->uart_rx); + pbdrv_gpio_out_high(&pins->uart_buf); - // Reset pins if this port has GPIO pins. - if (!pins) { - return PBIO_ERROR_NOT_SUPPORTED; - } + // These should be set by default already, but it seems that the + // bootloader on the Technic hub changes these and causes wrong + // detection if we don't make sure pull is disabled. + pbdrv_gpio_set_pull(&pins->p5, PBDRV_GPIO_PULL_NONE); + pbdrv_gpio_set_pull(&pins->p6, PBDRV_GPIO_PULL_NONE); + pbdrv_gpio_set_pull(&pins->uart_buf, PBDRV_GPIO_PULL_NONE); + pbdrv_gpio_set_pull(&pins->uart_tx, PBDRV_GPIO_PULL_NONE); + pbdrv_gpio_set_pull(&pins->uart_rx, PBDRV_GPIO_PULL_NONE); - pbdrv_gpio_input(&pins->p5); - pbdrv_gpio_input(&pins->p6); - pbdrv_gpio_input(&pins->uart_tx); - pbdrv_gpio_input(&pins->uart_rx); - pbdrv_gpio_out_high(&pins->uart_buf); + return PBIO_SUCCESS; +} - // These should be set by default already, but it seems that the - // bootloader on the Technic hub changes these and causes wrong - // detection if we don't make sure pull is disabled. - pbdrv_gpio_set_pull(&pins->p5, PBDRV_GPIO_PULL_NONE); - pbdrv_gpio_set_pull(&pins->p6, PBDRV_GPIO_PULL_NONE); - pbdrv_gpio_set_pull(&pins->uart_buf, PBDRV_GPIO_PULL_NONE); - pbdrv_gpio_set_pull(&pins->uart_tx, PBDRV_GPIO_PULL_NONE); - pbdrv_gpio_set_pull(&pins->uart_rx, PBDRV_GPIO_PULL_NONE); +pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_ioport_p5p6_mode_t mode) { - return PBIO_SUCCESS; + pbio_error_t err; + + if (mode == PBDRV_IOPORT_P5P6_MODE_GPIO_ADC) { + // This is the same as the default mode. + return pbdrv_ioport_p5p6_pin_reset(pins); } else if (mode == PBDRV_IOPORT_P5P6_MODE_UART) { - // First reset all pins to inputs by going to GPIO mode recursively. - pbio_error_t err = pbdrv_ioport_p5p6_set_mode(pins, uart_dev, PBDRV_IOPORT_P5P6_MODE_GPIO_ADC); + err = pbdrv_ioport_p5p6_pin_reset(pins); if (err != PBIO_SUCCESS) { return err; } @@ -53,12 +54,23 @@ pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_u pbdrv_gpio_alt(&pins->uart_tx, pins->uart_tx_alt_uart); pbdrv_gpio_out_low(&pins->uart_buf); return PBIO_SUCCESS; + } else if (mode == PBDRV_IOPORT_P5P6_MODE_I2C) { + err = pbdrv_ioport_p5p6_pin_reset(pins); + if (err != PBIO_SUCCESS) { + return err; + } + // Required for EV3 I2C implementation. + pbdrv_gpio_out_low(&pins->p5); + pbdrv_gpio_input(&pins->p5); + pbdrv_gpio_out_low(&pins->p6); + pbdrv_gpio_input(&pins->p6); + return PBIO_SUCCESS; } else if (mode == PBDRV_IOPORT_P5P6_MODE_QUADRATURE) { - // In PoweredUP, this is only used for two motors in boost. Its counter - // driver does all the required setup. Its mode can never change. The - // initial driver init does not check errors for default modes since - // they are supported by definition. We can return an error for all - // other ports. + // Ports with this mode support only this mode and nothing else. The + // counter drivers are automatically started on boot. This mode is only + // set at port init when default ports are set, for which the return + // value is not checked. This mode should never change at runtime + // either, so always just return an error. return PBIO_ERROR_NOT_SUPPORTED; } diff --git a/lib/pbio/include/pbdrv/i2c.h b/lib/pbio/include/pbdrv/i2c.h index c14a93267..8864e0348 100644 --- a/lib/pbio/include/pbdrv/i2c.h +++ b/lib/pbio/include/pbdrv/i2c.h @@ -47,8 +47,9 @@ pbio_error_t pbdrv_i2c_get_instance(uint8_t id, pbdrv_i2c_dev_t **i2c_dev); * If this is not 0 and \p rlen is not 0, * a write will be sent followed by a read in * a single transaction. - * @param [out] rdata Buffer for data read from the device. - * Can be null if \p rlen is 0. + * @param [out] rdata Pointer to data read from the device. Only valid + * immediately on successfull completion. + * Returns null if \p rlen is 0 or the operation failed. * @param [in] rlen Size of \p rdata. * @param [in] nxt_quirk Whether to use NXT I2C transaction quirk. * @return ::PBIO_SUCCESS on success. @@ -59,7 +60,7 @@ pbio_error_t pbdrv_i2c_write_then_read( uint8_t dev_addr, const uint8_t *wdata, size_t wlen, - uint8_t *rdata, + uint8_t **rdata, size_t rlen, bool nxt_quirk); @@ -76,7 +77,7 @@ static inline pbio_error_t pbdrv_i2c_write_then_read( uint8_t dev_addr, const uint8_t *wdata, size_t wlen, - uint8_t *rdata, + uint8_t **rdata, size_t rlen, bool nxt_quirk) { return PBIO_ERROR_NOT_SUPPORTED; diff --git a/lib/pbio/include/pbdrv/ioport.h b/lib/pbio/include/pbdrv/ioport.h index c4dd749a7..c9599fd93 100644 --- a/lib/pbio/include/pbdrv/ioport.h +++ b/lib/pbio/include/pbdrv/ioport.h @@ -138,13 +138,12 @@ static inline void pbdrv_ioport_enable_vcc(bool enable) { * pins to inputs even if GPIO mode is already active. * * @param [in] pins The pins to set the mode for. - * @param [in] uart_dev The UART device to use for UART mode. * @param [in] mode The mode state to set. * @return ::PBIO_SUCCESS on success, otherwise * ::PBIO_ERROR_NOT_SUPPORTED if the mode is not supported. * ::PBIO_ERROR_INVALID_OP if changing the mode is not permitted in the current state. */ -pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_uart_dev_t *uart_dev, pbdrv_ioport_p5p6_mode_t mode); +pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_ioport_p5p6_mode_t mode); extern const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPORT_NUM_DEV]; diff --git a/lib/pbio/include/pbio/port_dcm.h b/lib/pbio/include/pbio/port_dcm.h index 3900a387f..e05f15682 100644 --- a/lib/pbio/include/pbio/port_dcm.h +++ b/lib/pbio/include/pbio/port_dcm.h @@ -71,6 +71,10 @@ static inline pbio_port_dcm_t *pbio_port_dcm_init_instance(uint8_t index) { } static inline pbio_error_t pbio_port_dcm_assert_type_id(pbio_port_dcm_t *dcm, lego_device_type_id_t *type_id) { + // Fixme: need a DCM implementation for CI tests. For now just pass LUMP. + if (*type_id == LEGO_DEVICE_TYPE_ID_ANY_LUMP_UART) { + return PBIO_SUCCESS; + } return PBIO_ERROR_NOT_SUPPORTED; } diff --git a/lib/pbio/platform/ev3/platform.c b/lib/pbio/platform/ev3/platform.c index 42add1845..e65c80bed 100644 --- a/lib/pbio/platform/ev3/platform.c +++ b/lib/pbio/platform/ev3/platform.c @@ -322,7 +322,7 @@ const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPOR #if PBDRV_CONFIG_UART_DEBUG_FIRST_PORT .supported_modes = PBIO_PORT_MODE_UART, #else // PBDRV_CONFIG_UART_DEBUG_FIRST_PORT - .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_LEGO_DCM, + .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_I2C | PBIO_PORT_MODE_LEGO_DCM, #endif }, { @@ -345,7 +345,7 @@ const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPOR .adc_p1 = 8, .adc_p6 = 7, }, - .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_LEGO_DCM, + .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_I2C | PBIO_PORT_MODE_LEGO_DCM, }, { .port_id = PBIO_PORT_ID_3, @@ -367,7 +367,7 @@ const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPOR .adc_p1 = 10, .adc_p6 = 9, }, - .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_LEGO_DCM, + .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_I2C | PBIO_PORT_MODE_LEGO_DCM, }, { .port_id = PBIO_PORT_ID_4, @@ -389,7 +389,7 @@ const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPOR .adc_p1 = 12, .adc_p6 = 11, }, - .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_LEGO_DCM, + .supported_modes = PBIO_PORT_MODE_UART | PBIO_PORT_MODE_I2C | PBIO_PORT_MODE_LEGO_DCM, }, }; diff --git a/lib/pbio/src/port.c b/lib/pbio/src/port.c index c34b7bddf..bd7755a07 100644 --- a/lib/pbio/src/port.c +++ b/lib/pbio/src/port.c @@ -92,7 +92,22 @@ struct _pbio_port_t { static pbio_port_t ports[PBIO_CONFIG_PORT_NUM_DEV]; -pbio_error_t pbio_port_process_pup_thread(pbio_os_state_t *state, void *context) { +static bool pbio_port_dcm_test_type_id(pbio_port_t *port, lego_device_type_id_t range) { + return pbio_port_dcm_assert_type_id(port->connection_manager, &range) == PBIO_SUCCESS; +} + +/** + * This is the high level process that monitors and drives official LEGO + * devices that support some form of automatic detection. There is one process + * for each port, running in parallel. + * + * It starts with a platform-specific auto-detection routine by setting, + * clearing and testing several of the GPIO and ADC pins. Besides detection, + * this routine also monitors simple analog devices. This routine exists once + * a "smart" device is detected that requires going through a LEGO UART or I2C + * protocol. + */ +static pbio_error_t pbio_port_process_lego_dcm_thread(pbio_os_state_t *state, void *context) { pbio_port_t *port = context; @@ -108,28 +123,30 @@ pbio_error_t pbio_port_process_pup_thread(pbio_os_state_t *state, void *context) // peripherals are available and initialized. for (;;) { - - // Run passive device connection manager until UART device is detected. - pbdrv_ioport_p5p6_set_mode(port->pdata->pins, port->uart_dev, PBDRV_IOPORT_P5P6_MODE_GPIO_ADC); + // Run passive device connection manager until smart device is detected. + pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_GPIO_ADC); + pbio_port_p1p2_set_power(port, PBIO_PORT_POWER_REQUIREMENTS_NONE); PBIO_OS_AWAIT(state, &port->child1, err = pbio_port_dcm_thread(&port->child1, &port->timer, port->connection_manager, port->pdata->pins)); - // Synchronize with LUMP data stream from sensor and parse device info. - pbdrv_ioport_p5p6_set_mode(port->pdata->pins, port->uart_dev, PBDRV_IOPORT_P5P6_MODE_UART); - PBIO_OS_AWAIT(state, &port->child1, err = pbio_port_lump_sync_thread(&port->child1, port->lump_dev, port->uart_dev, &port->timer)); - if (err != PBIO_SUCCESS) { - // Synchronization failed. Retry. - continue; + // Active device detected. Check type to decide next steps. + if (pbio_port_dcm_test_type_id(port, LEGO_DEVICE_TYPE_ID_ANY_LUMP_UART)) { + + // Synchronize with LUMP data stream from sensor and parse device info. + pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_UART); + PBIO_OS_AWAIT(state, &port->child1, err = pbio_port_lump_sync_thread(&port->child1, port->lump_dev, port->uart_dev, &port->timer)); + if (err != PBIO_SUCCESS) { + // Synchronization failed. Retry. + continue; + } + + // Exchange sensor data with the LUMP device until it is disconnected. + // The send thread detects this when the keep alive messages time out. + pbio_port_p1p2_set_power(port, pbio_port_lump_get_power_requirements(port->lump_dev)); + PBIO_OS_AWAIT_RACE(state, &port->child1, &port->child2, + pbio_port_lump_data_recv_thread(&port->child1, port->lump_dev, port->uart_dev), + pbio_port_lump_data_send_thread(&port->child2, port->lump_dev, port->uart_dev, &port->timer) + ); } - - // Exchange sensor data with the LUMP device until it is disconnected. - // The send thread detects this when the keep alive messages time out. - pbio_port_p1p2_set_power(port, pbio_port_lump_get_power_requirements(port->lump_dev)); - PBIO_OS_AWAIT_RACE(state, &port->child1, &port->child2, - pbio_port_lump_data_recv_thread(&port->child1, port->lump_dev, port->uart_dev), - pbio_port_lump_data_send_thread(&port->child2, port->lump_dev, port->uart_dev, &port->timer) - ); - - pbio_port_p1p2_set_power(port, PBIO_PORT_POWER_REQUIREMENTS_NONE); } // Unreachable. @@ -198,18 +215,40 @@ pbio_error_t pbio_port_get_uart_dev(pbio_port_t *port, pbdrv_uart_dev_t **uart_d /** * Gets the I2C interface of the port. * + * Also initializes GPIO pins in I2C mode. + * * @param [in] port The port instance. * @param [out] i2c_dev The I2C device. * @return ::PBIO_SUCCESS on success, otherwise * ::PBIO_ERROR_NOT_SUPPORTED if this port does not support I2C. + * ::PBIO_ERROR_INVALID_OP if this port is not in a compatible mode. + * ::PBIO_ERROR_NO_DEV if it is in LEGO mode but no I2C device is detected. */ pbio_error_t pbio_port_get_i2c_dev(pbio_port_t *port, pbdrv_i2c_dev_t **i2c_dev) { - // User access to I2C device is only allowed in direct I2C mode. - if (port->mode != PBIO_PORT_MODE_I2C) { + + if (!port->i2c_dev) { + return PBIO_ERROR_NOT_SUPPORTED; + } + + // In case of direct access without device type checks, start right away. + if (port->mode == PBIO_PORT_MODE_I2C) { + *i2c_dev = port->i2c_dev; + return pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_I2C); + } + + // Otherwise we must be in LEGO mode. + if (port->mode != PBIO_PORT_MODE_LEGO_DCM) { return PBIO_ERROR_INVALID_OP; } + + // And must have detected an I2C device. + lego_device_type_id_t type_id = LEGO_DEVICE_TYPE_ID_NXT_I2C; + pbio_error_t err = pbio_port_dcm_assert_type_id(port->connection_manager, &type_id); + if (err != PBIO_SUCCESS) { + return err; + } *i2c_dev = port->i2c_dev; - return PBIO_SUCCESS; + return pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_I2C); } /** @@ -380,8 +419,8 @@ static void pbio_port_init_one_port(pbio_port_t *port) { // Optionally used by some ports to get angle information. pbdrv_counter_get_dev(port->pdata->counter_driver_index, &port->counter); - // Configure basic quadrature-only ports such as BOOST A&B or NXT A&B&C - // without device kind and type id detection. + // Configure basic quadrature-only ports such as BOOST A&B or NXT/EV3 motor + // ports. May also support device detection in their counter driver. if (port->pdata->supported_modes == PBIO_PORT_MODE_QUADRATURE) { pbio_port_set_mode(port, PBIO_PORT_MODE_QUADRATURE); return; @@ -507,13 +546,13 @@ pbio_error_t pbio_port_set_mode(pbio_port_t *port, pbio_port_mode_t mode) { switch (mode) { case PBIO_PORT_MODE_NONE: - pbdrv_ioport_p5p6_set_mode(port->pdata->pins, port->uart_dev, PBDRV_IOPORT_P5P6_MODE_GPIO_ADC); + pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_GPIO_ADC); pbio_port_p1p2_set_power(port, PBIO_PORT_POWER_REQUIREMENTS_NONE); return PBIO_SUCCESS; case PBIO_PORT_MODE_LEGO_DCM: // Physical modes for this mode will be set by the process so this // is all we need to do here. - pbio_os_process_init(&port->process, pbio_port_process_pup_thread); + pbio_os_process_init(&port->process, pbio_port_process_lego_dcm_thread); // Returning e-again allows user module to wait for the port to be // ready after first entering LEGO mode, avoiding NODEV errors when // switching from direct access modes back to LEGO mode. @@ -521,10 +560,12 @@ pbio_error_t pbio_port_set_mode(pbio_port_t *port, pbio_port_mode_t mode) { case PBIO_PORT_MODE_UART: // Enable UART on the port. No process needed here. User can // access UART from their own event loop. - pbdrv_ioport_p5p6_set_mode(port->pdata->pins, port->uart_dev, PBDRV_IOPORT_P5P6_MODE_UART); - return PBIO_SUCCESS; + return pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_UART); + case PBIO_PORT_MODE_I2C: + // Enable I2C on the port. User controlled; no process needed here. + return pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_I2C); case PBIO_PORT_MODE_QUADRATURE: - return PBIO_SUCCESS; + return pbdrv_ioport_p5p6_set_mode(port->pdata->pins, PBDRV_IOPORT_P5P6_MODE_QUADRATURE); default: return PBIO_ERROR_NOT_SUPPORTED; } diff --git a/lib/pbio/src/port_dcm_ev3.c b/lib/pbio/src/port_dcm_ev3.c index 2eeabdf1f..f97023f27 100644 --- a/lib/pbio/src/port_dcm_ev3.c +++ b/lib/pbio/src/port_dcm_ev3.c @@ -330,119 +330,116 @@ pbio_error_t pbio_port_dcm_thread(pbio_os_state_t *state, pbio_os_timer_t *timer PBIO_OS_ASYNC_BEGIN(state); - for (;;) { - - debug_pr("Start device scan\n"); - dcm->category = DCM_CATEGORY_NONE; - dcm->connected = false; - - // Wait for any device to be connected. - for (dcm->count = 0; dcm->count < DCM_LOOP_STEADY_STATE_COUNT; dcm->count++) { - pbio_port_dcm_pin_state_t pin_state = pbio_port_dcm_get_state(pins); - pbio_port_dcm_category_t category = pbio_port_dcm_get_category(pin_state); - if (category != dcm->category || category == DCM_CATEGORY_NONE) { - dcm->count = 0; - dcm->category = category; - } - PBIO_OS_AWAIT_MS(state, timer, DCM_LOOP_TIME_MS); + debug_pr("Start device scan\n"); + dcm->category = DCM_CATEGORY_NONE; + dcm->connected = false; + + // Wait for any device to be connected. + for (dcm->count = 0; dcm->count < DCM_LOOP_STEADY_STATE_COUNT; dcm->count++) { + pbio_port_dcm_pin_state_t pin_state = pbio_port_dcm_get_state(pins); + pbio_port_dcm_category_t category = pbio_port_dcm_get_category(pin_state); + if (category != dcm->category || category == DCM_CATEGORY_NONE) { + dcm->count = 0; + dcm->category = category; } - debug_pr("Device kind detected: %d\n", dcm->category); - dcm->connected = true; + PBIO_OS_AWAIT_MS(state, timer, DCM_LOOP_TIME_MS); + } + debug_pr("Device kind detected: %d\n", dcm->category); + dcm->connected = true; - // Now run processes for devices that require a process, and otherwise - // wait for disconnection. + // Now run processes for devices that require a process, and otherwise + // wait for disconnection. - if (dcm->category == DCM_CATEGORY_LUMP) { - debug_pr("Continue as LUMP process\n"); - // Exit EV3 device manager, letting LUMP manager take over. - // That process runs until the device no longer reports data. - return PBIO_SUCCESS; - } + if (dcm->category == DCM_CATEGORY_LUMP) { + debug_pr("Continue as LUMP process\n"); + // Exit EV3 device manager, letting LUMP manager take over. + // That process runs until the device no longer reports data. + return PBIO_SUCCESS; + } - if (dcm->category == DCM_CATEGORY_NXT_TEMPERATURE) { - // This device has no way to passively detect disconnection, so - // we need to monitor I2C transactions to see if it is still there. - debug_pr("Starting NXT temperature sensor process.\n"); - // TODO. - debug_pr("Stopped NXT temperature sensor process.\n"); - continue; - } + if (dcm->category == DCM_CATEGORY_NXT_TEMPERATURE) { + // This device has no way to passively detect disconnection, so + // we need to monitor I2C transactions to see if it is still there. + debug_pr("Starting NXT temperature sensor process.\n"); + // TODO. + debug_pr("Stopped NXT temperature sensor process.\n"); + return PBIO_SUCCESS; + } - if (dcm->category == DCM_CATEGORY_NXT_LIGHT) { - debug_pr("Reading NXT Light Sensor until disconnected.\n"); - // While plugged in, get reflected and ambient light intensity. - while (!pbdrv_gpio_input(&pins->p2)) { - // Reflected intensity. - pbdrv_gpio_out_high(&pins->p5); - PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); - dcm->nxt_rgba.r = pbio_port_dcm_get_mv(pins, 1); - - // Ambient intensity. - pbdrv_gpio_out_low(&pins->p5); - PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); - dcm->nxt_rgba.a = pbio_port_dcm_get_mv(pins, 1); - } - continue; + if (dcm->category == DCM_CATEGORY_NXT_LIGHT) { + debug_pr("Reading NXT Light Sensor until disconnected.\n"); + // While plugged in, get reflected and ambient light intensity. + while (!pbdrv_gpio_input(&pins->p2)) { + // Reflected intensity. + pbdrv_gpio_out_high(&pins->p5); + PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); + dcm->nxt_rgba.r = pbio_port_dcm_get_mv(pins, 1); + + // Ambient intensity. + pbdrv_gpio_out_low(&pins->p5); + PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); + dcm->nxt_rgba.a = pbio_port_dcm_get_mv(pins, 1); } + return PBIO_SUCCESS; + } - if (dcm->category == DCM_CATEGORY_NXT_COLOR) { - debug_pr("Initializing NXT Color Sensor.\n"); - - // The original firmware has a reset sequence where p6 is high and - // then p5 is toggled twice. It also works with 8 toggles, we can - // just use the send function with 0xff to achieve the same effect. - PBIO_OS_AWAIT(state, &dcm->child, pbio_port_dcm_nxt_color_tx_msg(&dcm->child, &dcm->nxt_color_state, pins, timer, 0xff)); - PBIO_OS_AWAIT_MS(state, timer, 100); + if (dcm->category == DCM_CATEGORY_NXT_COLOR) { + debug_pr("Initializing NXT Color Sensor.\n"); - // Set to full color mode. - PBIO_OS_AWAIT(state, &dcm->child, pbio_port_dcm_nxt_color_tx_msg(&dcm->child, &dcm->nxt_color_state, pins, timer, 13)); + // The original firmware has a reset sequence where p6 is high and + // then p5 is toggled twice. It also works with 8 toggles, we can + // just use the send function with 0xff to achieve the same effect. + PBIO_OS_AWAIT(state, &dcm->child, pbio_port_dcm_nxt_color_tx_msg(&dcm->child, &dcm->nxt_color_state, pins, timer, 0xff)); + PBIO_OS_AWAIT_MS(state, timer, 100); - // Receive all calibration info. - for (dcm->count = 0; dcm->count < sizeof(pbio_port_dcm_nxt_color_sensor_data_t); dcm->count++) { - PBIO_OS_AWAIT(state, &dcm->child, - pbio_port_dcm_nxt_color_rx_msg(&dcm->child, &dcm->nxt_color_state, pins, timer, (uint8_t *)&dcm->nxt_color_state.data + dcm->count)); - } + // Set to full color mode. + PBIO_OS_AWAIT(state, &dcm->child, pbio_port_dcm_nxt_color_tx_msg(&dcm->child, &dcm->nxt_color_state, pins, timer, 13)); - // REVISIT: Test checksum and exit on failure. - debug_pr("Finished initializing NXT Color Sensor.\n"); + // Receive all calibration info. + for (dcm->count = 0; dcm->count < sizeof(pbio_port_dcm_nxt_color_sensor_data_t); dcm->count++) { + PBIO_OS_AWAIT(state, &dcm->child, + pbio_port_dcm_nxt_color_rx_msg(&dcm->child, &dcm->nxt_color_state, pins, timer, (uint8_t *)&dcm->nxt_color_state.data + dcm->count)); + } - // While plugged in, toggle through available colors and measure intensity. - while (!pbdrv_gpio_input(&pins->p2)) { + // REVISIT: Test checksum and exit on failure. + debug_pr("Finished initializing NXT Color Sensor.\n"); - pbdrv_gpio_out_low(&pins->p5); - PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); - dcm->nxt_rgba.a = pbio_port_dcm_get_mv(pins, 6); + // While plugged in, toggle through available colors and measure intensity. + while (!pbdrv_gpio_input(&pins->p2)) { - pbdrv_gpio_out_high(&pins->p5); - PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); - dcm->nxt_rgba.r = pbio_port_dcm_get_mv(pins, 6); + pbdrv_gpio_out_low(&pins->p5); + PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); + dcm->nxt_rgba.a = pbio_port_dcm_get_mv(pins, 6); - pbdrv_gpio_out_low(&pins->p5); - PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 2000)); - dcm->nxt_rgba.g = pbio_port_dcm_get_mv(pins, 6); + pbdrv_gpio_out_high(&pins->p5); + PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); + dcm->nxt_rgba.r = pbio_port_dcm_get_mv(pins, 6); - pbdrv_gpio_out_high(&pins->p5); - PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); - dcm->nxt_rgba.b = pbio_port_dcm_get_mv(pins, 6); - } pbdrv_gpio_out_low(&pins->p5); - continue; + PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 2000)); + dcm->nxt_rgba.g = pbio_port_dcm_get_mv(pins, 6); + + pbdrv_gpio_out_high(&pins->p5); + PBIO_OS_AWAIT(state, &dcm->child, pbdrv_adc_await_new_samples(&dcm->child, &timer->start, 200)); + dcm->nxt_rgba.b = pbio_port_dcm_get_mv(pins, 6); } + pbdrv_gpio_out_low(&pins->p5); + return PBIO_SUCCESS; + } - // For everything else, disconnection is detected by just one pin going - // high rather than all pins going back to the none state. This is - // because other pins are used for data transfer and may vary between - // high/low during normal operation. - for (dcm->count = 0; dcm->count < DCM_LOOP_DISCONNECT_COUNT; dcm->count++) { - // Monitor P5 for EV3 analog, P2 for all NXT sensors. - const pbdrv_gpio_t *gpio = dcm->category == DCM_CATEGORY_EV3_ANALOG ? &pins->p5 : &pins->p2; - if (!pbdrv_gpio_input(gpio)) { - dcm->count = 0; - } - PBIO_OS_AWAIT_MS(state, timer, DCM_LOOP_TIME_MS); + // For everything else, disconnection is detected by just one pin going + // high rather than all pins going back to the none state. This is + // because other pins are used for data transfer and may vary between + // high/low during normal operation. + for (dcm->count = 0; dcm->count < DCM_LOOP_DISCONNECT_COUNT; dcm->count++) { + // Monitor P5 for EV3 analog, P2 for all NXT sensors. + const pbdrv_gpio_t *gpio = dcm->category == DCM_CATEGORY_EV3_ANALOG ? &pins->p5 : &pins->p2; + if (!pbdrv_gpio_input(gpio)) { + dcm->count = 0; } - debug_pr("Device disconnected\n"); + PBIO_OS_AWAIT_MS(state, timer, DCM_LOOP_TIME_MS); } + debug_pr("Device disconnected\n"); PBIO_OS_ASYNC_END(PBIO_SUCCESS); } @@ -455,8 +452,8 @@ pbio_port_dcm_t *pbio_port_dcm_init_instance(uint8_t index) { return dcm; } -static pbio_error_t must_equal(lego_device_type_id_t candidate, lego_device_type_id_t expected) { - return candidate == expected ? PBIO_SUCCESS : PBIO_ERROR_NO_DEV; +static pbio_error_t matches_category(pbio_port_dcm_t *dcm, pbio_port_dcm_category_t expected) { + return dcm->category == expected ? PBIO_SUCCESS : PBIO_ERROR_NO_DEV; } pbio_error_t pbio_port_dcm_assert_type_id(pbio_port_dcm_t *dcm, lego_device_type_id_t *expected_type_id) { @@ -475,45 +472,32 @@ pbio_error_t pbio_port_dcm_assert_type_id(pbio_port_dcm_t *dcm, lego_device_type } // Still busy detecting. - if (!dcm->connected) { + if (!dcm->connected || dcm->category == DCM_CATEGORY_NONE) { return PBIO_ERROR_NO_DEV; } - switch (dcm->category) { - case DCM_CATEGORY_NONE: - return PBIO_ERROR_NO_DEV; - case DCM_CATEGORY_EV3_ANALOG: - // This is the only known EV3 analog sensor. - return must_equal(*expected_type_id, LEGO_DEVICE_TYPE_ID_EV3_TOUCH_SENSOR); - case DCM_CATEGORY_NXT_COLOR: - // Only one sensor in this category. - return must_equal(*expected_type_id, LEGO_DEVICE_TYPE_ID_NXT_COLOR_SENSOR); - case DCM_CATEGORY_NXT_TEMPERATURE: - // Only one sensor in this category. - return must_equal(*expected_type_id, LEGO_DEVICE_TYPE_ID_NXT_TEMPERATURE_SENSOR); - case DCM_CATEGORY_NXT_LIGHT: - // For legacy compatibility, allow anything wired like a NXT light - // sensor to be detected and used as a generic analog sensor. - if (*expected_type_id == LEGO_DEVICE_TYPE_ID_NXT_ANALOG) { - *expected_type_id = LEGO_DEVICE_TYPE_ID_NXT_LIGHT_SENSOR; - return PBIO_SUCCESS; - } else { - // If a specific type is requested, it must be the light sensor. - return must_equal(*expected_type_id, LEGO_DEVICE_TYPE_ID_NXT_LIGHT_SENSOR); - } - case DCM_CATEGORY_NXT_ANALOG_OTHER: - // In principle, the sound sensor can be distinguished using ADCP6, - // but we pass both here for simplicity. - if (*expected_type_id == LEGO_DEVICE_TYPE_ID_NXT_ANALOG || - *expected_type_id == LEGO_DEVICE_TYPE_ID_NXT_SOUND_SENSOR || - *expected_type_id == LEGO_DEVICE_TYPE_ID_NXT_TOUCH_SENSOR) { - return PBIO_SUCCESS; - } else { - return PBIO_ERROR_NO_DEV; - } - case DCM_CATEGORY_NXT_I2C: - // TODO: Check standard LEGO I2C device ID. - return PBIO_ERROR_NO_DEV; + switch (*expected_type_id) { + case LEGO_DEVICE_TYPE_ID_ANY_LUMP_UART: + return matches_category(dcm, DCM_CATEGORY_LUMP); + case LEGO_DEVICE_TYPE_ID_EV3_TOUCH_SENSOR: + return matches_category(dcm, DCM_CATEGORY_EV3_ANALOG); + case LEGO_DEVICE_TYPE_ID_NXT_COLOR_SENSOR: + return matches_category(dcm, DCM_CATEGORY_NXT_COLOR); + case LEGO_DEVICE_TYPE_ID_NXT_TEMPERATURE_SENSOR: + return matches_category(dcm, DCM_CATEGORY_NXT_TEMPERATURE); + case LEGO_DEVICE_TYPE_ID_NXT_LIGHT_SENSOR: + return matches_category(dcm, DCM_CATEGORY_NXT_LIGHT); + case LEGO_DEVICE_TYPE_ID_NXT_SOUND_SENSOR: + return matches_category(dcm, DCM_CATEGORY_NXT_ANALOG_OTHER); + case LEGO_DEVICE_TYPE_ID_NXT_ANALOG: + // Allow any NXT analog device as well as anything wired like the + // NXT Light Sensor. + return (dcm->category == DCM_CATEGORY_NXT_ANALOG_OTHER || dcm->category == DCM_CATEGORY_NXT_LIGHT) ? + PBIO_SUCCESS : PBIO_ERROR_NO_DEV; + case LEGO_DEVICE_TYPE_ID_NXT_I2C: + // Temperature sensor is also an I2C sensor. + return (dcm->category == DCM_CATEGORY_NXT_I2C || dcm->category == DCM_CATEGORY_NXT_TEMPERATURE) ? + PBIO_SUCCESS : PBIO_ERROR_NO_DEV; default: return PBIO_ERROR_NO_DEV; } diff --git a/lib/pbio/src/port_dcm_pup.c b/lib/pbio/src/port_dcm_pup.c index e9724caec..2af2136b8 100644 --- a/lib/pbio/src/port_dcm_pup.c +++ b/lib/pbio/src/port_dcm_pup.c @@ -74,6 +74,8 @@ static const lego_device_type_id_t legodev_pup_type_id_lookup[3][3] = { * Thread that detects the device type. It monitors the ID1 and ID2 pins * on the port to see when devices are connected or disconnected. * + * It ends once a LEGO UART device is connected or nothing is connected. + * * @param [in] state The process thread state. * @param [in] timer The timer to use for timing. * @param [in] dcm The device connection manager. @@ -86,8 +88,10 @@ pbio_error_t pbio_port_dcm_thread(pbio_os_state_t *state, pbio_os_timer_t *timer dcm->prev_type_id = LEGO_DEVICE_TYPE_ID_NONE; dcm->dev_id_match_count = 0; - // Keep running until a UART device is definitively found. - while (dcm->dev_id_match_count < AFFIRMATIVE_MATCH_COUNT || dcm->prev_type_id != LEGO_DEVICE_TYPE_ID_LPF2_UNKNOWN_UART) { + // Repeat until we definitively have a UART device or no device is + // detected. So it stays here as long as a passive motor device is + // attached. + while (dcm->dev_id_match_count < AFFIRMATIVE_MATCH_COUNT || (dcm->prev_type_id != LEGO_DEVICE_TYPE_ID_LPF2_UNKNOWN_UART && dcm->prev_type_id != LEGO_DEVICE_TYPE_ID_NONE)) { dcm->type_id = LEGO_DEVICE_TYPE_ID_NONE; dcm->dev_id1_group = DEV_ID_GROUP_OPEN; @@ -281,11 +285,6 @@ pbio_error_t pbio_port_dcm_thread(pbio_os_state_t *state, pbio_os_timer_t *timer #endif } - // When DCM completes and proceeds to UART in a separate process, disallow - // getting any devices from here. This will cause the device assertion to - // raise. - dcm->dev_id_match_count = 0; - PBIO_OS_ASYNC_END(PBIO_SUCCESS); } @@ -322,7 +321,12 @@ pbio_error_t pbio_port_dcm_assert_type_id(pbio_port_dcm_t *dcm, lego_device_type *type_id = dcm->prev_type_id; return PBIO_SUCCESS; } - // fallthrough + return PBIO_ERROR_NO_DEV; + case LEGO_DEVICE_TYPE_ID_LPF2_UNKNOWN_UART: + if (*type_id == LEGO_DEVICE_TYPE_ID_ANY_LUMP_UART) { + return PBIO_SUCCESS; + } + return PBIO_ERROR_NO_DEV; default: return PBIO_ERROR_NO_DEV; } diff --git a/pybricks/iodevices/iodevices.h b/pybricks/iodevices/iodevices.h index 83aedef99..5d65e4b7c 100644 --- a/pybricks/iodevices/iodevices.h +++ b/pybricks/iodevices/iodevices.h @@ -13,6 +13,25 @@ extern const mp_obj_type_t pb_type_iodevices_PUPDevice; extern const mp_obj_type_t pb_type_uart_device; +#if PYBRICKS_PY_IODEVICES_I2CDEVICE + +extern const mp_obj_type_t pb_type_i2c_device; +/** + * Given a completed I2C operation, maps the resulting read buffer to an object + * of a desired form. For example, it could map two bytes to a single floating + * point value representing temperature. + * + * @param [in] data The data read. + * @param [in] len The data length. + */ +typedef mp_obj_t (*pb_type_i2c_device_return_map_t)(const uint8_t *data, size_t len); + +mp_obj_t pb_type_i2c_device_make_new(mp_obj_t port_in, uint8_t address, bool custom, bool powered, bool nxt_quirk); +mp_obj_t pb_type_i2c_device_start_operation(mp_obj_t i2c_device_obj, const uint8_t *write_data, size_t write_len, size_t read_len, pb_type_i2c_device_return_map_t return_map); +void pb_type_i2c_device_assert_string_at_register(mp_obj_t i2c_device_obj, uint8_t reg, const char *string); + +#endif + #if PYBRICKS_PY_PUPDEVICES extern const mp_obj_type_t pb_type_iodevices_LWP3Device; @@ -24,7 +43,6 @@ extern const mp_obj_type_t pb_type_iodevices_XboxController; extern const mp_obj_type_t pb_type_iodevices_AnalogSensor; extern const mp_obj_type_t pb_type_iodevices_Ev3devSensor; -extern const mp_obj_type_t pb_type_iodevices_I2CDevice; #endif // PYBRICKS_PY_EV3DEVICES diff --git a/pybricks/iodevices/pb_module_iodevices.c b/pybricks/iodevices/pb_module_iodevices.c index 049a28f30..fe5640dae 100644 --- a/pybricks/iodevices/pb_module_iodevices.c +++ b/pybricks/iodevices/pb_module_iodevices.c @@ -12,6 +12,9 @@ static const mp_rom_map_elem_t iodevices_globals_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_iodevices) }, { MP_ROM_QSTR(MP_QSTR_PUPDevice), MP_ROM_PTR(&pb_type_iodevices_PUPDevice) }, { MP_ROM_QSTR(MP_QSTR_UARTDevice), MP_ROM_PTR(&pb_type_uart_device) }, + #if PYBRICKS_PY_IODEVICES_I2CDEVICE + { MP_ROM_QSTR(MP_QSTR_I2CDevice), MP_ROM_PTR(&pb_type_i2c_device) }, + #endif #if PYBRICKS_PY_PUPDEVICES && PYBRICKS_PY_PUPDEVICES_REMOTE { MP_ROM_QSTR(MP_QSTR_LWP3Device), MP_ROM_PTR(&pb_type_iodevices_LWP3Device) }, #endif @@ -22,7 +25,6 @@ static const mp_rom_map_elem_t iodevices_globals_table[] = { { MP_ROM_QSTR(MP_QSTR_LUMPDevice), MP_ROM_PTR(&pb_type_iodevices_PUPDevice) }, { MP_ROM_QSTR(MP_QSTR_AnalogSensor), MP_ROM_PTR(&pb_type_iodevices_AnalogSensor) }, { MP_ROM_QSTR(MP_QSTR_Ev3devSensor), MP_ROM_PTR(&pb_type_iodevices_Ev3devSensor) }, - { MP_ROM_QSTR(MP_QSTR_I2CDevice), MP_ROM_PTR(&pb_type_iodevices_I2CDevice) }, #if PYBRICKS_PY_COMMON_MOTORS { MP_ROM_QSTR(MP_QSTR_DCMotor), MP_ROM_PTR(&pb_type_DCMotor) }, #endif diff --git a/pybricks/iodevices/pb_type_i2c_device.c b/pybricks/iodevices/pb_type_i2c_device.c new file mode 100644 index 000000000..40a178d27 --- /dev/null +++ b/pybricks/iodevices/pb_type_i2c_device.c @@ -0,0 +1,292 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2025 The Pybricks Authors + +#include "py/mpconfig.h" + +#if PYBRICKS_PY_IODEVICES && PYBRICKS_PY_IODEVICES_I2CDEVICE + +#include "py/mphal.h" +#include "py/objstr.h" + + +#include +#include + +#include +#include +#include + +#include +#include +#include + +// Object representing a pybricks.iodevices.I2CDevice instance. +typedef struct { + mp_obj_base_t base; + /** + * The following are buffered parameters for one ongoing I2C operation, See + * ::pbdrv_i2c_write_then_read for details on each parameter. We need to + * buffer these arguments so we can keep calling the protothread until it + * is complete. We don't need to buffer the write data here because it is + * immediately copied to the driver on the first call to the protothread. + */ + pbdrv_i2c_dev_t *i2c_dev; + pbio_os_state_t state; + uint8_t address; + bool nxt_quirk; + pb_type_i2c_device_return_map_t return_map; + size_t write_len; + size_t read_len; + uint8_t *read_buf; +} device_obj_t; + +// pybricks.iodevices.I2CDevice.__init__ +mp_obj_t pb_type_i2c_device_make_new(mp_obj_t port_in, uint8_t address, bool custom, bool powered, bool nxt_quirk) { + + pb_module_tools_assert_blocking(); + + // Get the port instance. + pbio_port_id_t port_id = pb_type_enum_get_value(port_in, &pb_enum_type_Port); + pbio_port_t *port; + pb_assert(pbio_port_get_port(port_id, &port)); + + // Set the port mode to LEGO DCM or raw I2C mode. + pbio_error_t err = pbio_port_set_mode(port, custom ? PBIO_PORT_MODE_I2C : PBIO_PORT_MODE_LEGO_DCM); + if (err == PBIO_ERROR_AGAIN) { + // If coming from a different mode, give port some time to get started. + // This happens when the user has a custom device and decides to switch + // back to LEGO mode. This should be rare, so we can afford to wait. + mp_hal_delay_ms(1000); + err = pbio_port_set_mode(port, PBIO_PORT_MODE_LEGO_DCM); + } + pb_assert(err); + + pbdrv_i2c_dev_t *i2c_dev; + pb_assert(pbio_port_get_i2c_dev(port, &i2c_dev)); + + device_obj_t *device = mp_obj_malloc(device_obj_t, &pb_type_i2c_device); + device->i2c_dev = i2c_dev; + device->address = address; + device->nxt_quirk = nxt_quirk; + if (powered) { + pbio_port_p1p2_set_power(port, PBIO_PORT_POWER_REQUIREMENTS_BATTERY_VOLTAGE_P1_POS); + } + + return MP_OBJ_FROM_PTR(device); +} + +// wrapper to parse args for __init__ +static mp_obj_t make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { + PB_PARSE_ARGS_CLASS(n_args, n_kw, args, + PB_ARG_REQUIRED(port), + PB_ARG_REQUIRED(address), + PB_ARG_DEFAULT_FALSE(custom), + PB_ARG_DEFAULT_FALSE(powered), + PB_ARG_DEFAULT_FALSE(nxt_quirk) + ); + + return pb_type_i2c_device_make_new(port_in, mp_obj_get_int(address_in), mp_obj_is_true(custom_in), mp_obj_is_true(powered_in), mp_obj_is_true(nxt_quirk_in)); +} + +// Object representing the iterable that is returned when calling an I2C +// method. This object can then be awaited (iterated). It has a reference to +// the device from which it was created. Only one operation can be active at +// one time. +typedef struct { + mp_obj_base_t base; + mp_obj_t device_obj; +} operation_obj_t; + +static mp_obj_t operation_close(mp_obj_t op_in) { + // Close is not implemented but needs to exist. + operation_obj_t *op = MP_OBJ_TO_PTR(op_in); + (void)op; + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_1(operation_close_obj, operation_close); + +static pbio_error_t operation_iterate_once(device_obj_t *device) { + return pbdrv_i2c_write_then_read( + &device->state, device->i2c_dev, + device->address, + NULL, // Already memcpy'd on initial iteration. No need to provide here. + device->write_len, + &device->read_buf, + device->read_len, + device->nxt_quirk + ); +} + +static mp_obj_t operation_iternext(mp_obj_t op_in) { + operation_obj_t *op = MP_OBJ_TO_PTR(op_in); + device_obj_t *device = MP_OBJ_TO_PTR(op->device_obj); + + pbio_error_t err = operation_iterate_once(device); + + // Yielded, keep going. + if (err == PBIO_ERROR_AGAIN) { + return mp_const_none; + } + + // Raises on Timeout and other I/O errors. Proceeds on success. + pb_assert(err); + + // For no return map, return basic stop iteration, which results None. + if (!device->return_map) { + return MP_OBJ_STOP_ITERATION; + } + + // Set return value via stop iteration. + return mp_make_stop_iteration(device->return_map(device->read_buf, device->read_len)); +} + +static const mp_rom_map_elem_t operation_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&operation_close_obj) }, +}; +MP_DEFINE_CONST_DICT(operation_locals_dict, operation_locals_dict_table); + +MP_DEFINE_CONST_OBJ_TYPE(operation_type, + MP_QSTR_I2COperation, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, operation_iternext, + locals_dict, &operation_locals_dict); + +mp_obj_t pb_type_i2c_device_start_operation(mp_obj_t i2c_device_obj, const uint8_t *write_data, size_t write_len, size_t read_len, pb_type_i2c_device_return_map_t return_map) { + + pb_assert_type(i2c_device_obj, &pb_type_i2c_device); + device_obj_t *device = MP_OBJ_TO_PTR(i2c_device_obj); + + // Kick off the operation. This will immediately raise if a transaction is + // in progress. + pbio_os_state_t state = 0; + uint8_t *read_buf = NULL; + pbio_error_t err = pbdrv_i2c_write_then_read( + &state, device->i2c_dev, device->address, + (uint8_t *)write_data, write_len, + &read_buf, read_len, device->nxt_quirk); + + // Expect yield after the initial call. + if (err == PBIO_SUCCESS) { + pb_assert(PBIO_ERROR_FAILED); + } else if (err != PBIO_ERROR_AGAIN) { + pb_assert(err); + } + + // The initial operation above can fail if an I2C transaction is already in + // progress. If so, we don't want to reset it state or allow the return + // result to be garbage collected. Now that the first iteration succeeded, + // save the state and assign the new result buffer. + device->read_len = read_len; + device->write_len = write_len; + device->state = state; + device->read_buf = NULL; + device->return_map = return_map; + + // If runloop active, return an awaitable object. + if (pb_module_tools_run_loop_is_active()) { + operation_obj_t *operation = mp_obj_malloc(operation_obj_t, &operation_type); + operation->device_obj = MP_OBJ_FROM_PTR(device); + return MP_OBJ_FROM_PTR(operation); + } + + // Otherwise block and wait for the result here. + while ((err = operation_iterate_once(device)) == PBIO_ERROR_AGAIN) { + MICROPY_EVENT_POLL_HOOK; + } + pb_assert(err); + + if (!device->return_map) { + return mp_const_none; + } + return device->return_map(device->read_buf, device->read_len); +} + +/** + * Helper utility to verify that expected device is attached by asserting an + * expected manufacturer or id string. Raises ::PBIO_ERROR_NO_DEV if expected + * string is not found. + */ +void pb_type_i2c_device_assert_string_at_register(mp_obj_t i2c_device_obj, uint8_t reg, const char *string) { + pb_module_tools_assert_blocking(); + + const uint8_t write_data[] = { reg }; + mp_obj_t result = pb_type_i2c_device_start_operation(i2c_device_obj, write_data, MP_ARRAY_SIZE(write_data), strlen(string) - 1, mp_obj_new_bytes); + + size_t result_len; + const char *result_data = mp_obj_str_get_data(result, &result_len); + + if (memcmp(string, result_data, strlen(string) - 1)) { + pb_assert(PBIO_ERROR_NO_DEV); + } +} + +// pybricks.iodevices.I2CDevice.read +static mp_obj_t read(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, + device_obj_t, device, + PB_ARG_DEFAULT_NONE(reg), + PB_ARG_DEFAULT_INT(length, 1) + ); + + // Write payload is one byte representing the register we want to read, + // or no write for reg=None. + uint8_t *write_data = reg_in == mp_const_none ? + NULL : + &(uint8_t) { mp_obj_get_int(reg_in) }; + size_t write_len = reg_in == mp_const_none ? 0 : 1; + + return pb_type_i2c_device_start_operation(MP_OBJ_FROM_PTR(device), write_data, write_len, pb_obj_get_positive_int(length_in), mp_obj_new_bytes); +} +static MP_DEFINE_CONST_FUN_OBJ_KW(read_obj, 0, read); + +// pybricks.iodevices.I2CDevice.write +static mp_obj_t write(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, + device_obj_t, device, + PB_ARG_DEFAULT_NONE(reg), + PB_ARG_DEFAULT_NONE(data) + ); + + // Treat none data as empty bytes. + if (data_in == mp_const_none) { + data_in = mp_const_empty_bytes; + } + + // Assert data is bytes. + size_t user_len; + const char *user_data = mp_obj_str_get_data(data_in, &user_len); + + // No register given, write data as is. + if (reg_in == mp_const_none) { + return pb_type_i2c_device_start_operation(MP_OBJ_FROM_PTR(device), (const uint8_t *)user_data, user_len, 0, NULL); + } + + // Otherwise need to prefix write data with given register. We're limiting + // write data to 32 bytes in this case. To send more data, the user can + // use reg=None and prefix the address to the data themselves. + uint8_t write_data[33]; + if (user_len > MP_ARRAY_SIZE(write_data) - 1) { + pb_assert(PBIO_ERROR_INVALID_ARG); + } + write_data[0] = pb_obj_get_positive_int(reg_in); + memcpy(&write_data[1], user_data, user_len); + + return pb_type_i2c_device_start_operation(MP_OBJ_FROM_PTR(device), write_data, user_len + 1, 0, NULL); +} +static MP_DEFINE_CONST_FUN_OBJ_KW(write_obj, 0, write); + +// dir(pybricks.iodevices.I2CDevice) +static const mp_rom_map_elem_t locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_read), MP_ROM_PTR(&read_obj) }, + { MP_ROM_QSTR(MP_QSTR_write), MP_ROM_PTR(&write_obj) }, +}; +static MP_DEFINE_CONST_DICT(locals_dict, locals_dict_table); + +// type(pybricks.iodevices.I2CDevice) +MP_DEFINE_CONST_OBJ_TYPE(pb_type_i2c_device, + MP_QSTR_I2CDevice, + MP_TYPE_FLAG_NONE, + make_new, make_new, + locals_dict, &locals_dict); + +#endif // PYBRICKS_PY_IODEVICES && PYBRICKS_PY_IODEVICES_I2CDEVICE diff --git a/pybricks/iodevices/pb_type_iodevices_i2cdevice.c b/pybricks/iodevices/pb_type_iodevices_i2cdevice.c deleted file mode 100644 index a97de23ab..000000000 --- a/pybricks/iodevices/pb_type_iodevices_i2cdevice.c +++ /dev/null @@ -1,164 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018-2023 The Pybricks Authors - -#include "py/mpconfig.h" - -#if PYBRICKS_PY_IODEVICES && PYBRICKS_PY_EV3DEVDEVICES - - -#include "py/objstr.h" - -#include -#include - -#include -#include -#include -#include - -#include "pbsmbus.h" - -// pybricks.iodevices.I2CDevice class object -typedef struct _iodevices_I2CDevice_obj_t { - pb_type_device_obj_base_t device_base; - smbus_t *bus; - int8_t address; -} iodevices_I2CDevice_obj_t; - -// pybricks.iodevices.I2CDevice.__init__ -static mp_obj_t iodevices_I2CDevice_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { - PB_PARSE_ARGS_CLASS(n_args, n_kw, args, - PB_ARG_REQUIRED(port), - PB_ARG_REQUIRED(address)); - - // Get device, which inits I2C port - iodevices_I2CDevice_obj_t *self = mp_obj_malloc(iodevices_I2CDevice_obj_t, type); - pb_type_device_init_class(&self->device_base, port_in, LEGO_DEVICE_TYPE_ID_CUSTOM_I2C); - - // Get selected I2C Address - mp_int_t address = mp_obj_get_int(address_in); - if (address < 0 || address > 255) { - pb_assert(PBIO_ERROR_INVALID_ARG); - } - self->address = address; - - // Get the smbus, which on ev3dev is zero based sensor port number + 3. - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - pb_assert(pb_smbus_get(&self->bus, port - PBIO_PORT_ID_1 + 3)); - - return MP_OBJ_FROM_PTR(self); -} - -// pybricks.iodevices.I2CDevice.read -static mp_obj_t iodevices_I2CDevice_read(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - - PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, - iodevices_I2CDevice_obj_t, self, - PB_ARG_REQUIRED(reg), - PB_ARG_DEFAULT_INT(length, 1)); - - // Get requested data length - mp_int_t length = mp_obj_get_int(length_in); - if (length < 0 || length > PB_SMBUS_BLOCK_MAX) { - pb_assert(PBIO_ERROR_INVALID_ARG); - } - - // First, deal with the case where no register is given - if (reg_in == mp_const_none) { - uint8_t data; - switch (length) { - case 0: - // No register, no data, so quick-read: - pb_assert(pb_smbus_read_quick(self->bus, self->address)); - return mp_const_none; - case 1: - // No register, read 1 byte: - pb_assert(pb_smbus_read_no_reg(self->bus, self->address, &data)); - return mp_obj_new_bytes(&data, 1); - default: - pb_assert(PBIO_ERROR_INVALID_ARG); - return mp_const_none; - } - } - - // There was a register, so get it - mp_int_t reg = mp_obj_get_int(reg_in); - if (reg < 0 || reg > 255) { - pb_assert(PBIO_ERROR_INVALID_ARG); - } - - // Read the given amount of bytes - uint8_t buf[PB_SMBUS_BLOCK_MAX]; - - pb_assert(pb_smbus_read_bytes(self->bus, self->address, reg, length, buf)); - - return mp_obj_new_bytes(buf, length); -} -static MP_DEFINE_CONST_FUN_OBJ_KW(iodevices_I2CDevice_read_obj, 1, iodevices_I2CDevice_read); - -// pybricks.iodevices.I2CDevice.write -static mp_obj_t iodevices_I2CDevice_write(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - - PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, - iodevices_I2CDevice_obj_t, self, - PB_ARG_REQUIRED(reg), - PB_ARG_DEFAULT_NONE(data)); - - // No data means an empty byte array - if (data_in == mp_const_none) { - data_in = mp_obj_new_bytes(NULL, 0); - } - - // Assert that data argument are bytes - if (!(mp_obj_is_str_or_bytes(data_in) || mp_obj_is_type(data_in, &mp_type_bytearray))) { - pb_assert(PBIO_ERROR_INVALID_ARG); - } - - // Get data and length - GET_STR_DATA_LEN(data_in, data, data_len); - - // Len 0 with no register given - if (data_len == 0 && reg_in == mp_const_none) { - pb_assert(pb_smbus_write_quick(self->bus, self->address)); - return mp_const_none; - } - - // Len 1 with no register given - if (data_len == 1 && reg_in == mp_const_none) { - pb_smbus_write_no_reg(self->bus, self->address, data[0]); - return mp_const_none; - } - - // Get register - mp_int_t reg = mp_obj_get_int(reg_in); - if (reg < 0 || reg > 255) { - pb_assert(PBIO_ERROR_INVALID_ARG); - } - - // Len 0 and register given just means that register is the data - if (data_len == 0) { - pb_smbus_write_no_reg(self->bus, self->address, reg); - return mp_const_none; - } - - // Otherwise send a block of data - pb_assert(pb_smbus_write_bytes(self->bus, self->address, reg, data_len, data)); - return mp_const_none; -} -static MP_DEFINE_CONST_FUN_OBJ_KW(iodevices_I2CDevice_write_obj, 1, iodevices_I2CDevice_write); - -// dir(pybricks.iodevices.I2CDevice) -static const mp_rom_map_elem_t iodevices_I2CDevice_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_read), MP_ROM_PTR(&iodevices_I2CDevice_read_obj) }, - { MP_ROM_QSTR(MP_QSTR_write), MP_ROM_PTR(&iodevices_I2CDevice_write_obj) }, -}; -static MP_DEFINE_CONST_DICT(iodevices_I2CDevice_locals_dict, iodevices_I2CDevice_locals_dict_table); - -// type(pybricks.iodevices.I2CDevice) -MP_DEFINE_CONST_OBJ_TYPE(pb_type_iodevices_I2CDevice, - MP_QSTR_I2CDevice, - MP_TYPE_FLAG_NONE, - make_new, iodevices_I2CDevice_make_new, - locals_dict, &iodevices_I2CDevice_locals_dict); - -#endif // PYBRICKS_PY_IODEVICES && PYBRICKS_PY_EV3DEVDEVICES diff --git a/pybricks/nxtdevices/nxtdevices.h b/pybricks/nxtdevices/nxtdevices.h index 13e861f23..be52df4fc 100644 --- a/pybricks/nxtdevices/nxtdevices.h +++ b/pybricks/nxtdevices/nxtdevices.h @@ -13,13 +13,13 @@ extern const mp_obj_type_t pb_type_nxtdevices_TouchSensor; extern const mp_obj_type_t pb_type_nxtdevices_LightSensor; extern const mp_obj_type_t pb_type_nxtdevices_ColorSensor; +extern const mp_obj_type_t pb_type_nxtdevices_UltrasonicSensor; +extern const mp_obj_type_t pb_type_nxtdevices_TemperatureSensor; #if PYBRICKS_PY_EV3DEVDEVICES extern const mp_obj_type_t pb_type_nxtdevices_EnergyMeter; extern const mp_obj_type_t pb_type_nxtdevices_SoundSensor; -extern const mp_obj_type_t pb_type_nxtdevices_TemperatureSensor; -extern const mp_obj_type_t pb_type_nxtdevices_UltrasonicSensor; int32_t analog_scale(int32_t mvolts, int32_t mvolts_min, int32_t mvolts_max, bool invert); diff --git a/pybricks/nxtdevices/pb_module_nxtdevices.c b/pybricks/nxtdevices/pb_module_nxtdevices.c index cfb9aef3c..ab2d8fb7f 100644 --- a/pybricks/nxtdevices/pb_module_nxtdevices.c +++ b/pybricks/nxtdevices/pb_module_nxtdevices.c @@ -16,11 +16,11 @@ static const mp_rom_map_elem_t nxtdevices_globals_table[] = { { MP_ROM_QSTR(MP_QSTR_TouchSensor), MP_ROM_PTR(&pb_type_nxtdevices_TouchSensor) }, { MP_ROM_QSTR(MP_QSTR_LightSensor), MP_ROM_PTR(&pb_type_nxtdevices_LightSensor) }, { MP_ROM_QSTR(MP_QSTR_ColorSensor), MP_ROM_PTR(&pb_type_nxtdevices_ColorSensor) }, + { MP_ROM_QSTR(MP_QSTR_UltrasonicSensor), MP_ROM_PTR(&pb_type_nxtdevices_UltrasonicSensor) }, + { MP_ROM_QSTR(MP_QSTR_TemperatureSensor), MP_ROM_PTR(&pb_type_nxtdevices_TemperatureSensor)}, #if PYBRICKS_PY_EV3DEVDEVICES { MP_ROM_QSTR(MP_QSTR_EnergyMeter), MP_ROM_PTR(&pb_type_nxtdevices_EnergyMeter) }, { MP_ROM_QSTR(MP_QSTR_SoundSensor), MP_ROM_PTR(&pb_type_nxtdevices_SoundSensor) }, - { MP_ROM_QSTR(MP_QSTR_TemperatureSensor), MP_ROM_PTR(&pb_type_nxtdevices_TemperatureSensor)}, - { MP_ROM_QSTR(MP_QSTR_UltrasonicSensor), MP_ROM_PTR(&pb_type_nxtdevices_UltrasonicSensor) }, #endif }; static MP_DEFINE_CONST_DICT(pb_module_nxtdevices_globals, nxtdevices_globals_table); diff --git a/pybricks/nxtdevices/pb_type_nxtdevices_temperaturesensor.c b/pybricks/nxtdevices/pb_type_nxtdevices_temperaturesensor.c index c695c1627..711d93c1a 100644 --- a/pybricks/nxtdevices/pb_type_nxtdevices_temperaturesensor.c +++ b/pybricks/nxtdevices/pb_type_nxtdevices_temperaturesensor.c @@ -1,21 +1,27 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2018-2023 The Pybricks Authors +// Copyright (c) 2018-2025 The Pybricks Authors #include "py/mpconfig.h" -#if PYBRICKS_PY_NXTDEVICES && PYBRICKS_PY_EV3DEVDEVICES +#if PYBRICKS_PY_NXTDEVICES + +#include "py/mphal.h" + +#include +#include #include -#include +#include #include #include #include -#include +#include // pybricks.nxtdevices.TemperatureSensor class object typedef struct _nxtdevices_TemperatureSensor_obj_t { - pb_type_device_obj_base_t device_base; + mp_obj_base_t base; + mp_obj_t *i2c_device_obj; } nxtdevices_TemperatureSensor_obj_t; // pybricks.nxtdevices.TemperatureSensor.__init__ @@ -24,21 +30,31 @@ static mp_obj_t nxtdevices_TemperatureSensor_make_new(const mp_obj_type_t *type, PB_ARG_REQUIRED(port)); nxtdevices_TemperatureSensor_obj_t *self = mp_obj_malloc(nxtdevices_TemperatureSensor_obj_t, type); - pb_type_device_init_class(&self->device_base, port_in, LEGO_DEVICE_TYPE_ID_NXT_TEMPERATURE_SENSOR); + self->i2c_device_obj = pb_type_i2c_device_make_new(port_in, 0x4C, true, false, false); + + // Set resolution to 0.125 degrees as a fair balance between speed and accuracy. + const uint8_t write_data[] = { 0x01, (1 << 6) | (0 << 5) }; + pb_type_i2c_device_start_operation(self->i2c_device_obj, write_data, MP_ARRAY_SIZE(write_data), 0, NULL); + return MP_OBJ_FROM_PTR(self); } +static mp_obj_t map_temperature(const uint8_t *data, size_t len) { + int16_t combined = ((uint16_t)data[0] << 8) | data[1]; + return mp_obj_new_float_from_f((combined >> 4) / 16.0f); +} + // pybricks.nxtdevices.TemperatureSensor.temperature static mp_obj_t nxtdevices_TemperatureSensor_temperature(mp_obj_t self_in) { - int16_t *temperature_scaled = pb_type_device_get_data_blocking(self_in, LEGO_DEVICE_MODE_NXT_TEMPERATURE_SENSOR_CELSIUS); - // FIXME: ENDIANNESS - return mp_obj_new_float((temperature_scaled[0] >> 4) / 16.0); + nxtdevices_TemperatureSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); + const uint8_t write_data[] = { 0x00 }; + return pb_type_i2c_device_start_operation(self->i2c_device_obj, write_data, MP_ARRAY_SIZE(write_data), 2, map_temperature); } static MP_DEFINE_CONST_FUN_OBJ_1(nxtdevices_TemperatureSensor_temperature_obj, nxtdevices_TemperatureSensor_temperature); -// dir(pybricks.ev3devices.TemperatureSensor) +// dir(pybricks.nxtdevices.TemperatureSensor) static const mp_rom_map_elem_t nxtdevices_TemperatureSensor_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_temperature), MP_ROM_PTR(&nxtdevices_TemperatureSensor_temperature_obj) }, + { MP_ROM_QSTR(MP_QSTR_temperature), MP_ROM_PTR(&nxtdevices_TemperatureSensor_temperature_obj) }, }; static MP_DEFINE_CONST_DICT(nxtdevices_TemperatureSensor_locals_dict, nxtdevices_TemperatureSensor_locals_dict_table); @@ -49,4 +65,4 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_nxtdevices_TemperatureSensor, make_new, nxtdevices_TemperatureSensor_make_new, locals_dict, &nxtdevices_TemperatureSensor_locals_dict); -#endif // PYBRICKS_PY_NXTDEVICES && PYBRICKS_PY_EV3DEVDEVICES +#endif // PYBRICKS_PY_NXTDEVICES diff --git a/pybricks/nxtdevices/pb_type_nxtdevices_ultrasonicsensor.c b/pybricks/nxtdevices/pb_type_nxtdevices_ultrasonicsensor.c index c3fc02365..467a45dec 100644 --- a/pybricks/nxtdevices/pb_type_nxtdevices_ultrasonicsensor.c +++ b/pybricks/nxtdevices/pb_type_nxtdevices_ultrasonicsensor.c @@ -1,20 +1,27 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2018-2023 The Pybricks Authors +// Copyright (c) 2018-2025 The Pybricks Authors #include "py/mpconfig.h" -#if PYBRICKS_PY_NXTDEVICES && PYBRICKS_PY_EV3DEVDEVICES +#if PYBRICKS_PY_NXTDEVICES + +#include "py/mphal.h" + +#include +#include #include +#include #include #include #include -#include +#include // pybricks.nxtdevices.UltrasonicSensor class object typedef struct _nxtdevices_UltrasonicSensor_obj_t { - pb_type_device_obj_base_t device_base; + mp_obj_base_t base; + mp_obj_t *i2c_device_obj; } nxtdevices_UltrasonicSensor_obj_t; // pybricks.nxtdevices.UltrasonicSensor.__init__ @@ -23,14 +30,26 @@ static mp_obj_t nxtdevices_UltrasonicSensor_make_new(const mp_obj_type_t *type, PB_ARG_REQUIRED(port)); nxtdevices_UltrasonicSensor_obj_t *self = mp_obj_malloc(nxtdevices_UltrasonicSensor_obj_t, type); - pb_type_device_init_class(&self->device_base, port_in, LEGO_DEVICE_TYPE_ID_NXT_ULTRASONIC_SENSOR); + self->i2c_device_obj = pb_type_i2c_device_make_new(port_in, 0x01, false, true, true); + + // NXT Ultrasonic Sensor appears to need some time after initializing I2C pins before it can receive data. + mp_hal_delay_ms(100); + + pb_type_i2c_device_assert_string_at_register(self->i2c_device_obj, 0x08, "LEGO"); + pb_type_i2c_device_assert_string_at_register(self->i2c_device_obj, 0x10, "Sonar"); + return MP_OBJ_FROM_PTR(self); } +static mp_obj_t map_distance(const uint8_t *data, size_t len) { + return mp_obj_new_int(data[0] * 10); +} + // pybricks.nxtdevices.UltrasonicSensor.distance static mp_obj_t nxtdevices_UltrasonicSensor_distance(mp_obj_t self_in) { - int8_t *distance = pb_type_device_get_data_blocking(self_in, LEGO_DEVICE_MODE_NXT_ULTRASONIC_SENSOR__DIST_CM); - return mp_obj_new_int(distance[0] * 10); + nxtdevices_UltrasonicSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); + const uint8_t write_data[] = { 0x42 }; + return pb_type_i2c_device_start_operation(self->i2c_device_obj, write_data, MP_ARRAY_SIZE(write_data), 1, map_distance); } static MP_DEFINE_CONST_FUN_OBJ_1(nxtdevices_UltrasonicSensor_distance_obj, nxtdevices_UltrasonicSensor_distance); @@ -47,4 +66,4 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_nxtdevices_UltrasonicSensor, make_new, nxtdevices_UltrasonicSensor_make_new, locals_dict, &nxtdevices_UltrasonicSensor_locals_dict); -#endif // PYBRICKS_PY_NXTDEVICES && PYBRICKS_PY_EV3DEVDEVICES +#endif // PYBRICKS_PY_NXTDEVICES