pbio/drv/battery/battery_ev3.c: Implement EV3 battery driver

bricks/_common/sources.mk

@@ -232,6 +232,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\
 	src/task.c \
 	src/trajectory.c \
 	src/util.c \
+	sys/battery_temp.c \
 	sys/battery.c \
 	sys/bluetooth.c \
 	sys/command.c \

lib/pbio/drv/battery/battery_ev3.c

@@ -7,6 +7,7 @@
 
 #include <stdbool.h>
 
+#include <pbdrv/adc.h>
 #include <pbdrv/battery.h>
 #include <pbio/error.h>
 #include <pbdrv/gpio.h>
@@ -15,22 +16,40 @@
 #include <tiam1808/hw/hw_syscfg0_AM1808.h>
 
 static const pbdrv_gpio_t battery_type_gpio = PBDRV_GPIO_EV3_PIN(19, 7, 4, 8, 8);
+static const pbdrv_gpio_t battery_voltage_measure_en_gpio = PBDRV_GPIO_EV3_PIN(1, 7, 4, 0, 6);
+
+#define PBDRV_EV3_BATTERY_VOLTAGE_ADC_CH    4
+#define PBDRV_EV3_BATTERY_CURRENT_ADC_CH    3
 
 void pbdrv_battery_init(void) {
     pbdrv_gpio_alt(&battery_type_gpio, SYSCFG_PINMUX19_PINMUX19_7_4_GPIO8_8);
+    pbdrv_gpio_out_high(&battery_voltage_measure_en_gpio);
 }
 
 pbio_error_t pbdrv_battery_get_voltage_now(uint16_t *value) {
-    // TODO. Calculate battery voltage based on ADC channel 4.
-    // For now, return nominal value for now so we don't trigger low battery shutdown.
-    *value = 7200;
+    uint16_t value_raw;
+    pbio_error_t err = pbdrv_adc_get_ch(PBDRV_EV3_BATTERY_VOLTAGE_ADC_CH, &value_raw);
+    if (err != PBIO_SUCCESS) {
+        return err;
+    }
+    // Battery voltage is read through a voltage divider consisting of two
+    // 100 K resistors, which halves the voltage. The ADC returns 10 LSBs,
+    // where full scale is 5 V.
+    *value = ((uint32_t)(value_raw) * 2 * 5000) / 1023;
     return PBIO_SUCCESS;
 }
 
 pbio_error_t pbdrv_battery_get_current_now(uint16_t *value) {
-    // TODO. Calculate battery current based on ADC channel 3.
-    *value = 0;
-    return PBIO_ERROR_NOT_IMPLEMENTED;
+    uint16_t value_raw;
+    pbio_error_t err = pbdrv_adc_get_ch(PBDRV_EV3_BATTERY_CURRENT_ADC_CH, &value_raw);
+    if (err != PBIO_SUCCESS) {
+        return err;
+    }
+    // Battery current is read across a 0.05 ohm equivalent shunt resistor
+    // which is then connected to an op-amp configured with a gain of 16
+    // (non-inverting). This yields 1 A = 0.8 V.
+    *value = ((uint32_t)(value_raw) * 5000 * 10) / (1023 * 8);
+    return PBIO_SUCCESS;
 }
 
 pbio_error_t pbdrv_battery_get_type(pbdrv_battery_type_t *value) {

lib/pbio/drv/block_device/block_device_ev3.c

@@ -79,11 +79,11 @@ enum {
  */
 enum {
     // The maximum ADC clock speed according to the datasheet is 20 MHz.
-    // However, because the SPI peripheral does not have a fractional clock generator,
-    // the closest achievable in-spec speed is a division factor of 8.
-    //
-    // 150 MHz / 8 = 18.75 MHz actual
-    ADC_SPI_CLK_SPEED = 20000000,
+    // However, likely due to capacitance, it takes more time to settle even
+    // when maxing out other delays like C2T/T2C/WDELAY. Slowing down the
+    // SCLK adds even more settling time since each CS triggers a conversion
+    // and triggers the acquisition of the previous conversion.
+    ADC_SPI_CLK_SPEED = 1000000,
     // Time between the end of one SPI operation and start of the next.
     ADC_SAMPLE_PERIOD = 2,
 };
@@ -914,6 +914,15 @@ pbio_error_t ev3_spi_process_thread(pbio_os_state_t *state, void *context) {
     // failure, it can reset the user data to factory defaults, and save it
     // properly on shutdown.
     pbdrv_block_device_load_err = err;
+
+    // Read one set of ADC samples before continuing boot.
+    // This ensures that e.g. the low-battery warning doesn't falsely trigger.
+    pbdrv_block_device_ev3_spi_begin_for_adc(
+        channel_cmd,
+        channel_data,
+        PBDRV_CONFIG_ADC_EV3_ADC_NUM_CHANNELS + PBDRV_ADC_EV3_NUM_DELAY_SAMPLES);
+    PBIO_OS_AWAIT_WHILE(state, (spi_dev.status & SPI_STATUS_WAIT_ANY));
+
     pbio_busy_count_down();
 
     pbio_os_timer_set(&timer, ADC_SAMPLE_PERIOD);

lib/pbio/include/pbio/protocol.h

@@ -329,6 +329,18 @@ typedef enum {
      * @since Pybricks Profile v1.4.0
      */
     PBIO_PYBRICKS_STATUS_BLE_HOST_CONNECTED = 9,
+    /**
+     * Battery temperature is critically high.
+     *
+     * @since Pybricks Profile v1.5.0
+     */
+    PBIO_PYBRICKS_STATUS_BATTERY_HIGH_TEMP_SHUTDOWN = 10,
+    /**
+     * Battery temperature is high.
+     *
+     * @since Pybricks Profile v1.5.0
+     */
+    PBIO_PYBRICKS_STATUS_BATTERY_HIGH_TEMP_WARNING = 11,
     /** Total number of indications. */
     NUM_PBIO_PYBRICKS_STATUS,
 } pbio_pybricks_status_flags_t;

lib/pbio/platform/ev3/pbsysconfig.h

@@ -6,6 +6,7 @@
 #define PBSYS_CONFIG_FEATURE_BUILTIN_USER_PROGRAM_IMU_CALIBRATION  (0)
 #define PBSYS_CONFIG_FEATURE_PROGRAM_FORMAT_MULTI_MPY_V6           (1)
 #define PBSYS_CONFIG_FEATURE_PROGRAM_FORMAT_MULTI_MPY_V6_3_NATIVE  (0)
+#define PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION        (1)
 #define PBSYS_CONFIG_HMI_NUM_SLOTS                  (5)
 #define PBSYS_CONFIG_HOST                           (1)
 #define PBSYS_CONFIG_HOST_STDIN_BUF_SIZE            (21)

lib/pbio/sys/battery.c

@@ -1,5 +1,5 @@
 // SPDX-License-Identifier: MIT
-// Copyright (c) 2018-2022 The Pybricks Authors
+// Copyright (c) 2018-2025 The Pybricks Authors
 
 // Provides battery status indication and shutdown on low battery.
 
@@ -8,10 +8,12 @@
 
 #include <pbdrv/battery.h>
 #include <pbio/battery.h>
+#include <pbio/os.h>
 #include <pbdrv/charger.h>
 #include <pbdrv/config.h>
 #include <pbdrv/clock.h>
 #include <pbdrv/usb.h>
+#include <pbsys/config.h>
 #include <pbsys/status.h>
 
 // These values are for Alkaline (AA/AAA) batteries
@@ -25,7 +27,22 @@
 #define LIION_LOW_MV            6800    // 3.4V per cell
 #define LIION_CRITICAL_MV       6000    // 3.0V per cell
 
+#if PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION
+
+#include <math.h>
+
+#define PBSYS_BATTERY_TEMP_TIMER_PERIOD_MS 400
+
+static pbio_os_timer_t pbsys_battery_temp_timer;
+
+extern float pbsys_battery_temp_update(float V_bat, float I_bat);
+
+#endif // PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION
+
 void pbsys_battery_init(void) {
+    #if PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION
+    pbio_os_timer_set(&pbsys_battery_temp_timer, PBSYS_BATTERY_TEMP_TIMER_PERIOD_MS);
+    #endif
 }
 
 /**
@@ -60,6 +77,44 @@ void pbsys_battery_poll(void) {
     if (pbsys_status_test_debounce(PBIO_PYBRICKS_STATUS_BATTERY_LOW_VOLTAGE_SHUTDOWN, true, 3000)) {
         pbsys_status_set(PBIO_PYBRICKS_STATUS_SHUTDOWN_REQUEST);
     }
+
+    #if PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION
+    if (!is_liion && pbio_os_timer_is_expired(&pbsys_battery_temp_timer)) {
+        pbio_os_timer_extend(&pbsys_battery_temp_timer);
+
+        uint16_t voltage_mv;
+        uint16_t current_ma;
+        if (pbdrv_battery_get_voltage_now(&voltage_mv) == PBIO_SUCCESS &&
+            pbdrv_battery_get_current_now(&current_ma) == PBIO_SUCCESS) {
+
+            static float old_temp;
+            float new_temp = pbsys_battery_temp_update(voltage_mv / 1000.0f, current_ma / 1000.0f);
+            if (fabsf(new_temp - old_temp) > 0.1f) {
+                old_temp = new_temp;
+            }
+
+            const float high_temp_warning = 25.0f;
+            const float high_temp_critical = 30.0f;
+
+            if (old_temp >= high_temp_warning) {
+                pbsys_status_set(PBIO_PYBRICKS_STATUS_BATTERY_HIGH_TEMP_WARNING);
+            } else if (old_temp < high_temp_warning) {
+                pbsys_status_clear(PBIO_PYBRICKS_STATUS_BATTERY_HIGH_TEMP_WARNING);
+            }
+
+            if (old_temp >= high_temp_critical) {
+                pbsys_status_set(PBIO_PYBRICKS_STATUS_BATTERY_HIGH_TEMP_SHUTDOWN);
+            } else if (old_temp < high_temp_critical) {
+                pbsys_status_clear(PBIO_PYBRICKS_STATUS_BATTERY_HIGH_TEMP_SHUTDOWN);
+            }
+
+            // Shut down on high temperature so we don't damage AAA batteries.
+            if (pbsys_status_test_debounce(PBIO_PYBRICKS_STATUS_BATTERY_HIGH_TEMP_SHUTDOWN, true, 3000)) {
+                pbsys_status_set(PBIO_PYBRICKS_STATUS_SHUTDOWN_REQUEST);
+            }
+        }
+    }
+    #endif
 }
 
 /**