pbio/drv/legodev: Move category match to spec code.

This is constant information used in several places. Keeping it in one place is easier to maintain and less error prone. Also fixes Technic Large motor not being detected because it was missing on one of these (duplicated) lists. See pybricks/support#1131.
pybricks · Jul 3, 2023 · 9ad7b3a · 9ad7b3a
1 parent e0911e5
commit 9ad7b3a
Show file tree

Hide file tree

Showing 7 changed files with 147 additions and 107 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -4,6 +4,10 @@
 
 ## [Unreleased]
 
+- Fixed Technic (Extra) Large motors not working ([support#1131]) on all hubs.
+
+[support#1064]: https://github.com/pybricks/support/issues/1131
+
 ## [3.3.0b7] - 2023-06-30
 
 ### Added

diff --git a/lib/pbio/drv/legodev/legodev_ev3dev.c b/lib/pbio/drv/legodev/legodev_ev3dev.c
@@ -18,6 +18,7 @@
 #include <ev3dev_stretch/nxtcolor.h>
 
 #include "legodev_ev3dev.h"
+#include "legodev_spec.h"
 
 typedef struct {
     const pbdrv_legodev_ev3dev_sensor_platform_data_t *pdata;
@@ -83,10 +84,6 @@ pbio_error_t pbdrv_legodev_get_abs_angle(pbdrv_legodev_dev_t *legodev, pbio_angl
     return PBIO_ERROR_NOT_SUPPORTED;
 }
 
-bool type_id_matches(pbdrv_legodev_type_id_t *type, pbdrv_legodev_type_id_t actual_type) {
-    return false;
-}
-
 pbio_error_t pbdrv_legodev_get_device(pbio_port_id_t port_id, pbdrv_legodev_type_id_t *type_id, pbdrv_legodev_dev_t **legodev) {
     for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(devs); i++) {
         pbdrv_legodev_dev_t *candidate = &devs[i];
@@ -138,25 +135,6 @@ pbio_error_t pbdrv_legodev_get_info(pbdrv_legodev_dev_t *legodev, pbdrv_legodev_
     return PBIO_SUCCESS;
 }
 
-// Get the required mode switch time delay for a given sensor type and/or mode
-static uint32_t get_mode_switch_delay(pbdrv_legodev_type_id_t id, uint8_t mode) {
-    switch (id) {
-        case PBDRV_LEGODEV_TYPE_ID_EV3_COLOR_SENSOR:
-            return 30;
-        case PBDRV_LEGODEV_TYPE_ID_EV3_IR_SENSOR:
-            return 1100;
-        case PBDRV_LEGODEV_TYPE_ID_NXT_LIGHT_SENSOR:
-            return 20;
-        case PBDRV_LEGODEV_TYPE_ID_NXT_SOUND_SENSOR:
-            return 300;
-        case PBDRV_LEGODEV_TYPE_ID_NXT_ENERGY_METER:
-            return 200;
-        // Default delay for other sensors and modes:
-        default:
-            return 0;
-    }
-}
-
 pbio_error_t pbdrv_legodev_is_ready(pbdrv_legodev_dev_t *legodev) {
     if (legodev->is_motor) {
         return PBIO_SUCCESS;
@@ -167,7 +145,7 @@ pbio_error_t pbdrv_legodev_is_ready(pbdrv_legodev_dev_t *legodev) {
     }
 
     // Some device/mode pairs require time to discard stale data
-    uint32_t delay = get_mode_switch_delay(legodev->sensor->info.type_id, legodev->sensor->info.mode);
+    uint32_t delay = pbdrv_legodev_spec_stale_data_delay(legodev->sensor->info.type_id, legodev->sensor->info.mode);
     if (pbdrv_clock_get_ms() - legodev->sensor->mode_switch_time < delay) {
         return PBIO_ERROR_AGAIN;
     }

diff --git a/lib/pbio/drv/legodev/legodev_pup.c b/lib/pbio/drv/legodev/legodev_pup.c
@@ -29,6 +29,7 @@
 
 #include "legodev_pup.h"
 #include "legodev_pup_uart.h"
+#include "legodev_spec.h"
 
 /** The number of consecutive repeated detections needed for an affirmative ID. */
 #define AFFIRMATIVE_MATCH_COUNT 20
@@ -519,32 +520,17 @@ pbio_error_t pbdrv_legodev_get_abs_angle(pbdrv_legodev_dev_t *legodev, pbio_angl
 
 static bool type_id_matches(pbdrv_legodev_type_id_t *type, pbdrv_legodev_type_id_t actual_type) {
 
-    // Returns what was actually detected.
-    pbdrv_legodev_type_id_t desired_type = *type;
+    // Set return value to what was actually detected.
+    pbdrv_legodev_type_id_t desired = *type;
     *type = actual_type;
 
-    // Pass for any dc motor.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_DC_MOTOR) {
-        return actual_type == PBDRV_LEGODEV_TYPE_ID_LPF2_MMOTOR || actual_type == PBDRV_LEGODEV_TYPE_ID_LPF2_TRAIN;
+    // Test for category match.
+    if (pbdrv_legodev_spec_device_category_match(actual_type, desired)) {
+        return true;
     }
 
-    // Pass for any encoded motor.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_ENCODED_MOTOR) {
-        return actual_type == PBDRV_LEGODEV_TYPE_ID_INTERACTIVE_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_M_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_L_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_S_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_TECHNIC_M_ANGULAR_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR;
-    }
-
-    // Pass for any LEGO UART device.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_LUMP_UART) {
-        return actual_type > PBDRV_LEGODEV_TYPE_ID_LPF2_UNKNOWN_UART && actual_type <= PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR;
-    }
-
-    // Require an exact match.
-    return desired_type == actual_type;
+    // Otherwise require an exact match.
+    return desired == actual_type;
 }
 
 pbio_error_t pbdrv_legodev_get_device(pbio_port_id_t port_id, pbdrv_legodev_type_id_t *type_id, pbdrv_legodev_dev_t **legodev) {

diff --git a/lib/pbio/drv/legodev/legodev_spec.c b/lib/pbio/drv/legodev/legodev_spec.c
@@ -29,6 +29,18 @@ uint32_t pbdrv_legodev_spec_stale_data_delay(pbdrv_legodev_type_id_t id, uint8_t
             return mode == PBDRV_LEGODEV_MODE_PUP_COLOR_SENSOR__LIGHT ? 0 : 30;
         case PBDRV_LEGODEV_TYPE_ID_SPIKE_ULTRASONIC_SENSOR:
             return mode == PBDRV_LEGODEV_MODE_PUP_ULTRASONIC_SENSOR__LIGHT ? 0 : 50;
+        #if PBDRV_CONFIG_LEGODEV_EV3DEV
+        case PBDRV_LEGODEV_TYPE_ID_EV3_COLOR_SENSOR:
+            return 30;
+        case PBDRV_LEGODEV_TYPE_ID_EV3_IR_SENSOR:
+            return 1100;
+        case PBDRV_LEGODEV_TYPE_ID_NXT_LIGHT_SENSOR:
+            return 20;
+        case PBDRV_LEGODEV_TYPE_ID_NXT_SOUND_SENSOR:
+            return 300;
+        case PBDRV_LEGODEV_TYPE_ID_NXT_ENERGY_METER:
+            return 200;
+        #endif // PBDRV_CONFIG_LEGODEV_EV3DEV
         default:
             // Default delay for other sensors and modes.
             return 0;
@@ -56,6 +68,100 @@ uint32_t pbdrv_legodev_spec_data_set_delay(pbdrv_legodev_type_id_t id, uint8_t m
     return 10;
 }
 
+/**
+ * Checks if the device type is a motor with an absolute encoder.
+ *
+ * @param [in]  id          The device type ID.
+ * @return                  True if the device is an absolute encoded motor, false otherwise.
+ */
+static bool pbdrv_legodev_spec_device_is_abs_motor(pbdrv_legodev_type_id_t id) {
+    switch (id) {
+        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_XL_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_SPIKE_M_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_SPIKE_L_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_SPIKE_S_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_M_ANGULAR_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR:
+            return true;
+        default:
+            return false;
+    }
+}
+
+/**
+ * Checks if the device type is a motor with an encoder (absolute or relative).
+ *
+ * @param [in]  id          The device type ID.
+ * @return                  True if the device is an encoded motor, false otherwise.
+ */
+static bool pbdrv_legodev_spec_device_is_encoded_motor(pbdrv_legodev_type_id_t id) {
+
+    // Absolute motors are also encoded motors.
+    if (pbdrv_legodev_spec_device_is_abs_motor(id)) {
+        return true;
+    }
+
+    // Relative encoded motors, including those that do not report specs.
+    switch (id) {
+        #if PBDRV_CONFIG_LEGODEV_EV3DEV
+        case PBDRV_LEGODEV_TYPE_ID_EV3_LARGE_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_EV3_MEDIUM_MOTOR:
+        #endif // PBDRV_CONFIG_LEGODEV_EV3DEV
+        case PBDRV_LEGODEV_TYPE_ID_INTERACTIVE_MOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_MOVE_HUB_MOTOR:
+            return true;
+        default:
+            return false;
+    }
+}
+
+/**
+ * Checks if the device type is a dc motor.
+ *
+ * @param [in]  id          The device type ID.
+ * @return                  True if the device is a dc motor, false otherwise.
+ */
+static bool pbdrv_legodev_spec_device_is_dc_motor(pbdrv_legodev_type_id_t id) {
+    switch (id) {
+        case PBDRV_LEGODEV_TYPE_ID_LPF2_MMOTOR:
+        case PBDRV_LEGODEV_TYPE_ID_LPF2_TRAIN:
+        case PBDRV_LEGODEV_TYPE_ID_EV3DEV_DC_MOTOR:
+            return true;
+        default:
+            return false;
+    }
+}
+
+/**
+ * Checks if the device type is a UART device.
+ *
+ * @param [in]  id          The device type ID.
+ * @return                  True if the device is a LEGO UART device, false otherwise.
+ */
+static bool pbdrv_legodev_spec_device_is_uart_device(pbdrv_legodev_type_id_t id) {
+    return id > PBDRV_LEGODEV_TYPE_ID_LPF2_UNKNOWN_UART && id <= PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR;
+}
+
+/**
+ * Checks if the device type is in a given category of devices.
+ *
+ * @param [in]  id          The device type ID.
+ * @return                  True if the device is a motor, false otherwise.
+ */
+bool pbdrv_legodev_spec_device_category_match(pbdrv_legodev_type_id_t id, pbdrv_legodev_type_id_t category) {
+    switch (category) {
+        case PBDRV_LEGODEV_TYPE_ID_ANY_DC_MOTOR:
+            return pbdrv_legodev_spec_device_is_dc_motor(id);
+        case PBDRV_LEGODEV_TYPE_ID_ANY_ENCODED_MOTOR:
+            return pbdrv_legodev_spec_device_is_encoded_motor(id);
+        case PBDRV_LEGODEV_TYPE_ID_ANY_LUMP_UART:
+            return pbdrv_legodev_spec_device_is_uart_device(id);
+        default:
+            return false;
+    }
+}
+
 /**
  * Gets the desired default mode for a device.
  *
@@ -64,19 +170,16 @@ uint32_t pbdrv_legodev_spec_data_set_delay(pbdrv_legodev_type_id_t id, uint8_t m
  * @return                  Required delay in milliseconds.
  */
 uint8_t pbdrv_legodev_spec_default_mode(pbdrv_legodev_type_id_t id) {
+
+    if (pbdrv_legodev_spec_device_is_abs_motor(id)) {
+        return PBDRV_LEGODEV_MODE_PUP_ABS_MOTOR__CALIB;
+    }
+
     switch (id) {
         case PBDRV_LEGODEV_TYPE_ID_COLOR_DIST_SENSOR:
             return PBDRV_LEGODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I;
         case PBDRV_LEGODEV_TYPE_ID_INTERACTIVE_MOTOR:
             return PBDRV_LEGODEV_MODE_PUP_REL_MOTOR__POS;
-        case PBDRV_LEGODEV_TYPE_ID_SPIKE_M_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_SPIKE_L_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_SPIKE_S_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_XL_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_M_ANGULAR_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR:
-            return PBDRV_LEGODEV_MODE_PUP_ABS_MOTOR__CALIB;
         default:
             return 0;
     }
@@ -91,6 +194,11 @@ uint8_t pbdrv_legodev_spec_default_mode(pbdrv_legodev_type_id_t id) {
  * @return                  Power reqquirement capability flag.
  */
 pbdrv_legodev_capability_flags_t pbdrv_legodev_spec_basic_flags(pbdrv_legodev_type_id_t id) {
+
+    if (pbdrv_legodev_spec_device_is_abs_motor(id)) {
+        return PBDRV_LEGODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS;
+    }
+
     switch (id) {
         case PBDRV_LEGODEV_TYPE_ID_SPIKE_COLOR_SENSOR:
         case PBDRV_LEGODEV_TYPE_ID_SPIKE_ULTRASONIC_SENSOR:
@@ -99,14 +207,6 @@ pbdrv_legodev_capability_flags_t pbdrv_legodev_spec_basic_flags(pbdrv_legodev_ty
             return PBDRV_LEGODEV_CAPABILITY_FLAG_NEEDS_SUPPLY_PIN2;
         case PBDRV_LEGODEV_TYPE_ID_INTERACTIVE_MOTOR:
             return PBDRV_LEGODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS;
-        case PBDRV_LEGODEV_TYPE_ID_SPIKE_M_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_SPIKE_L_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_SPIKE_S_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_XL_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_M_ANGULAR_MOTOR:
-        case PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR:
-            return PBDRV_LEGODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS;
         default:
             return 0;
     }

diff --git a/lib/pbio/drv/legodev/legodev_spec.h b/lib/pbio/drv/legodev/legodev_spec.h
@@ -12,6 +12,8 @@ uint32_t pbdrv_legodev_spec_stale_data_delay(pbdrv_legodev_type_id_t id, uint8_t
 
 uint32_t pbdrv_legodev_spec_data_set_delay(pbdrv_legodev_type_id_t id, uint8_t mode);
 
+bool pbdrv_legodev_spec_device_category_match(pbdrv_legodev_type_id_t id, pbdrv_legodev_type_id_t category);
+
 uint8_t pbdrv_legodev_spec_default_mode(pbdrv_legodev_type_id_t id);
 
 pbdrv_legodev_capability_flags_t pbdrv_legodev_spec_basic_flags(pbdrv_legodev_type_id_t id);

diff --git a/lib/pbio/drv/legodev/legodev_test.c b/lib/pbio/drv/legodev/legodev_test.c
@@ -88,34 +88,18 @@ pbio_error_t pbdrv_legodev_get_abs_angle(pbdrv_legodev_dev_t *legodev, pbio_angl
 
 static bool type_id_matches(pbdrv_legodev_type_id_t *type, pbdrv_legodev_type_id_t actual_type) {
 
-    // Returns what was actually detected.
-    pbdrv_legodev_type_id_t desired_type = *type;
+    // Set return value to what was actually detected.
+    pbdrv_legodev_type_id_t desired = *type;
     *type = actual_type;
 
-    // Pass for any dc motor.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_DC_MOTOR) {
-        return actual_type == PBDRV_LEGODEV_TYPE_ID_LPF2_MMOTOR || actual_type == PBDRV_LEGODEV_TYPE_ID_LPF2_TRAIN;
+    // Test for category match.
+    if (pbdrv_legodev_spec_device_category_match(actual_type, desired)) {
+        return true;
     }
 
-    // Pass for any encoded motor.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_ENCODED_MOTOR) {
-        return actual_type == PBDRV_LEGODEV_TYPE_ID_INTERACTIVE_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_M_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_L_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_S_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_TECHNIC_M_ANGULAR_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR;
-    }
-
-    // Pass for any LEGO UART device.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_LUMP_UART) {
-        return actual_type > PBDRV_LEGODEV_TYPE_ID_LPF2_UNKNOWN_UART && actual_type <= PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR;
-    }
-
-    // Require an exact match.
-    return desired_type == actual_type;
+    // Otherwise require an exact match.
+    return desired == actual_type;
 }
-
 pbio_error_t pbdrv_legodev_get_device(pbio_port_id_t port_id, pbdrv_legodev_type_id_t *type_id, pbdrv_legodev_dev_t **legodev) {
     for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(devs); i++) {
         pbdrv_legodev_dev_t *candidate = &devs[i];

diff --git a/lib/pbio/drv/legodev/legodev_virtual.c b/lib/pbio/drv/legodev/legodev_virtual.c
@@ -25,6 +25,7 @@
 #include <pbdrv/legodev.h>
 
 #include "legodev_virtual.h"
+#include "legodev_spec.h"
 #include "../motor_driver/motor_driver_virtual_simulation.h"
 
 struct _pbdrv_legodev_dev_t {
@@ -76,32 +77,17 @@ pbio_error_t pbdrv_legodev_get_abs_angle(pbdrv_legodev_dev_t *legodev, pbio_angl
 
 static bool type_id_matches(pbdrv_legodev_type_id_t *type, pbdrv_legodev_type_id_t actual_type) {
 
-    // Returns what was actually detected.
-    pbdrv_legodev_type_id_t desired_type = *type;
+    // Set return value to what was actually detected.
+    pbdrv_legodev_type_id_t desired = *type;
     *type = actual_type;
 
-    // Pass for any dc motor.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_DC_MOTOR) {
-        return actual_type == PBDRV_LEGODEV_TYPE_ID_LPF2_MMOTOR || actual_type == PBDRV_LEGODEV_TYPE_ID_LPF2_TRAIN;
+    // Test for category match.
+    if (pbdrv_legodev_spec_device_category_match(actual_type, desired)) {
+        return true;
     }
 
-    // Pass for any encoded motor.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_ENCODED_MOTOR) {
-        return actual_type == PBDRV_LEGODEV_TYPE_ID_INTERACTIVE_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_M_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_L_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_SPIKE_S_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_TECHNIC_M_ANGULAR_MOTOR ||
-               actual_type == PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR;
-    }
-
-    // Pass for any LEGO UART device.
-    if (desired_type == PBDRV_LEGODEV_TYPE_ID_ANY_LUMP_UART) {
-        return actual_type > PBDRV_LEGODEV_TYPE_ID_LPF2_UNKNOWN_UART && actual_type <= PBDRV_LEGODEV_TYPE_ID_TECHNIC_L_ANGULAR_MOTOR;
-    }
-
-    // Require an exact match.
-    return desired_type == actual_type;
+    // Otherwise require an exact match.
+    return desired == actual_type;
 }
 
 pbio_error_t pbdrv_legodev_get_device(pbio_port_id_t port_id, pbdrv_legodev_type_id_t *type_id, pbdrv_legodev_dev_t **legodev) {