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

@@ -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 \

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 \

lib/pbio/drv/counter/counter_core.c

@@ -10,8 +10,8 @@
 #include <pbdrv/counter.h>
 #include <pbio/error.h>
 
-#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) {

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 <pbdrv/config.h>
+
+#if PBDRV_CONFIG_COUNTER_LPF2
+
+#include <stdint.h>
+
+#include <pbdrv/ioport.h>
+#include <pbio/error.h>
+#include <pbio/iodev.h>
+#include <pbio/util.h>
+
+#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

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 <pbdrv/config.h>
+
+#if PBDRV_CONFIG_COUNTER_LPF2
+
+#if !PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV
+#error Platform must define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV
+#endif
+
+#include <stdint.h>
+
+#include <pbdrv/counter.h>
+#include <pbio/port.h>
+
+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_

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.

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

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)

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,
     },
 };
 

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)

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,13 +134,24 @@ 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 },
         .button = PBIO_BUTTON_CENTER,
     }
 };
 
+// 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,
     },
 };
 

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)