feat(core): Add Fuel gauge estimator into power_manager

[no changelog]

core/embed/sys/power_manager/stm32u5/power_manager.c

@@ -22,7 +22,10 @@
 #include <sys/systimer.h>
 #include <trezor_rtl.h>
 
+
 #include "power_manager_internal.h"
+#include "../../powerctl/npm1300/npm1300.h"
+#include "../../powerctl/stwlc38/stwlc38.h"
 
 // Global driver instance
 power_manager_driver_t g_power_manager = {
@@ -55,9 +58,20 @@ power_manager_status_t power_manager_init(power_manager_state_t initial_state) {
     return POWER_MANAGER_ERROR;
   }
 
+  // Clear fuel gauge state
+  memcpy(drv->fuel_gauge, 0, sizeof(fuel_gauge_state_t));
+
+  // Initialize fuel gauge
+  fuel_gauge_init(&(drv->fuel_gauge), POWER_MANAGER_FUEL_GAUGE_R,
+                  POWER_MANAGER_FUEL_GAUGE_Q,
+                  POWER_MANAGER_FUEL_GAUGE_R_AGGRESSIVE,
+                  POWER_MANAGER_FUEL_GAUGE_Q_AGGRESSIVE,
+                  POWER_MANAGER_FUEL_GAUGE_P_INIT);
+
   // Create monitoring timer
   drv->monitoring_timer = systimer_create(pm_monitoring_timer_handler, NULL);
-  systimer_set_periodic(drv->monitoring_timer, POWER_MANAGER_TIMER_PERIOD_MS);
+  systimer_set_periodic(drv->monitoring_timer,
+                        POWER_MANAGER_BATTERY_SAMPLING_PERIOD_MS);
 
   // Create shutdown timer
   drv->shutdown_timer = systimer_create(pm_shutdown_timer_handler, NULL);
@@ -171,6 +185,16 @@ power_manager_status_t power_manager_turn_on(void) {
     return POWER_MANAGER_REQUEST_REJECTED;
   }
 
+  irq_key_t irq_key = irq_lock();
+
+  pm_battery_initial_soc_guess();
+
+  // Set monitoiring timer with longer period
+  systimer_set_periodic(drv->monitoring_timer, POWER_MANAGER_TIMER_PERIOD_MS);
+
+  drv->fuel_gauge_initialized = true;
+  irq_unlock(irq_key);
+
   return POWER_MANAGER_OK;
 }
 
@@ -203,7 +227,6 @@ power_manager_status_t power_manager_get_report(
 }
 
 // Timer handlers
-
 static void pm_monitoring_timer_handler(void* context) {
   pm_monitor_power_sources();
 }

core/embed/sys/power_manager/stm32u5/power_manager_internal.h

@@ -25,28 +25,53 @@
 
 #include "../../powerctl/npm1300/npm1300.h"
 #include "../../powerctl/stwlc38/stwlc38.h"
+#include "../../powerctl/fuel_gauge/fuel_gauge.h"
 
 // Power manager thresholds & timings
 #define POWER_MANAGER_TIMER_PERIOD_MS 300
+#define POWER_MANAGER_BATTERY_SAMPLING_PERIOD_MS 100
 #define POWER_MANAGER_SHUTDOWN_TIMEOUT_MS 15000
 #define POWER_MANAGER_BATTERY_UNDERVOLT_THRESHOLD_V 3.0f
 #define POWER_MANAGER_BATTERY_UNDERVOLT_HYSTERESIS_V 0.5f
 #define POWER_MANAGER_BATTERY_LOW_THRESHOLD_V 3.15f
 #define POWER_MANAGER_BATTERY_LOW_RECOVERY_V 3.2f
+#define POWER_MANAGER_BATTERY_SAMPLING_BUF_SIZE 10
+
 #define POWER_MANAGER_WPC_CHARGE_CURR_STEP_MA 50
 #define POWER_MANAGER_WPC_CHARGE_CURR_STEP_TIMEOUT_MS 1000
+#define POWER_MANAGER_FUEL_GAUGE_R 3000.0f
+#define POWER_MANAGER_FUEL_GAUGE_Q 0.001f
+#define POWER_MANAGER_FUEL_GAUGE_R_AGGRESSIVE 3000.0f
+#define POWER_MANAGER_FUEL_GAUGE_Q_AGGRESSIVE 0.001f
+#define POWER_MANAGER_FUEL_GAUGE_P_INIT 0.1f
 
 // Event flag manipulation macros
 #define PM_SET_EVENT(flags, event) ((flags) |= (event))
 #define PM_CLEAR_EVENT(flags, event) ((flags) &= ~(event))
 #define PM_CLEAR_ALL_EVENTS(flags) ((flags) = 0)
 
+// Power manager battery sampling data structure)
+typedef struct {
+  float vbat;      // Battery voltage [V]
+  float ibat;      // Battery current [mA]
+  float ntc_temp;  // NTC temperature [°C]
+} power_manager_sampling_data_t;
+
 // Power manager core driver structure
 typedef struct {
   bool initialized;
   power_manager_state_t state;
   power_manager_event_t event_flags;
 
+  // Fuel gauge
+  fuel_gauge_state_t fuel_gauge;
+  bool fuel_gauge_initialized;
+  power_manager_sampling_data_t bat_sampling_buf[
+                                POWER_MANAGER_BATTERY_SAMPLING_BUF_SIZE];
+  uint8_t bat_sampling_buf_tail_idx = 0;
+  uint8_t bat_sampling_buf_head_idx = 0;
+
+
   // Battery charging state
   uint16_t charging_current_target_ma;
   uint32_t charging_target_timestamp;
@@ -89,6 +114,7 @@ void pm_monitor_power_sources(void);
 void pm_process_state_machine(void);
 void pm_pmic_data_ready(void* context, npm1300_report_t* report);
 void pm_charging_controller(power_manager_driver_t* drv);
+void pm_battery_initial_soc_guess(void);
 
 // State handlers
 power_manager_state_t pm_handle_state_active(power_manager_driver_t* drv);

core/embed/sys/power_manager/stm32u5/power_monitoring.c

@@ -23,11 +23,29 @@
 
 #include "../../powerctl/npm1300/npm1300.h"
 #include "../../powerctl/stwlc38/stwlc38.h"
+#include "../../powerctl/fuel_gauge/fuel_gauge.h"
 #include "power_manager_internal.h"
 
 void pm_monitor_power_sources(void) {
   power_manager_driver_t* drv = &g_power_manager;
 
+  // Check if PMIC data is ready, otherwise skip processing
+  if(!drv->pmic_measurement_ready) {
+    return;
+  }
+
+  // Update fuel gauge state
+  if (drv->fuel_gauge_initialized) {
+    fuel_gauge_update(&drv->fuel_gauge, POWER_MANAGER_BATTERY_SAMPLING_PERIOD_MS,
+                      drv->pmic_data.vbat, drv->pmic_data.ibat,
+                      drv->pmic_data.ntc_temp);
+  } else {
+
+    // Battery sampling period, collect data before initial guess is made.
+    fuel_gauge_initial_guess(&drv->fuel_gauge, drv->pmic_data.vbat,
+                             drv->pmic_data.ibat, drv->pmic_data.ntc_temp);
+  }
+
   // Check USB power source status
   if (drv->pmic_data.usb_status != 0x0) {
     if (!drv->usb_connected) {
@@ -79,12 +97,13 @@ void pm_monitor_power_sources(void) {
   // Run battery charging controller
   pm_charging_controller(drv);
 
-  // Request fresh measurements from PMIC
+  // Process state machine with updated battery and power source information
+  pm_process_state_machine();
+
+  // Request fresh measurements
   npm1300_measure(pm_pmic_data_ready, NULL);
   drv->pmic_measurement_ready = false;
 
-  // Process state machine with updated battery and power source information
-  pm_process_state_machine();
 }
 
 // PMIC measurement callback
@@ -145,3 +164,66 @@ void pm_charging_controller(power_manager_driver_t* drv) {
     npm1300_set_charging(true);
   }
 }
+
+void pm_battery_sampling(float vbat, float ibat, float ntc_temp) {
+  power_manager_driver_t* drv = &g_power_manager;
+
+  // Store battery data in the buffer
+  drv->bat_sampling_buf[bat_sampling_buf_head_idx].vbat = vbat;
+  drv->bat_sampling_buf[bat_sampling_buf_head_idx].ibat = ibat;
+  drv->bat_sampling_buf[bat_sampling_buf_head_idx].ntc_temp = ntc_temp;
+
+  // Update head index
+  drv->bat_sampling_buf_head_idx++;
+  if (bat_sampling_buf_head_idx >= POWER_MANAGER_BAT_DATA_BUF_SIZE) {
+    bat_sampling_buf_head_idx = 0;
+  }
+
+  // Check if the buffer is full
+  if (bat_sampling_buf_head_idx == bat_sampling_buf_tail_idx) {
+    // Buffer is full, move tail index forward
+    bat_sampling_buf_tail_idx++;
+    if (bat_sampling_buf_tail_idx >= POWER_MANAGER_BAT_DATA_BUF_SIZE) {
+      bat_sampling_buf_tail_idx = 0;
+    }
+  }
+
+}
+
+void pm_battery_initial_soc_guess(void){
+  power_manager_driver_t* drv = &g_power_manager;
+
+  // Check if the buffer is full
+  if (bat_sampling_buf_head_idx == bat_sampling_buf_tail_idx) {
+    // Buffer is empty, no data to process
+    return;
+  }
+
+  // Calculate average voltage, current and temperature from the sampling
+  // buffer and run the fuel gauge initial guess
+  uint8_t buf_idx = bat_sampling_buf_tail_idx;
+  uint8_t samples_count = 0;
+  float vbat_g, ibat_g, ntc_temp_g = 0.0f;
+  while(bat_sampling_buf_head_idx != buf_idx) {
+
+    vbat_g += drv->bat_sampling_buf[buf_idx].vbat;
+    ibat_g += drv->bat_sampling_buf[buf_idx].ibat;
+    ntc_temp_g += drv->bat_sampling_buf[buf_idx].ntc_temp;
+
+    buf_idx++;
+    if (buf_idx >= POWER_MANAGER_BAT_DATA_BUF_SIZE) {
+      buf_idx = 0;
+    }
+
+    samples_count++;
+
+  }
+
+  // Calculate average values
+  vbat_g /= samples_count;
+  ibat_g /= samples_count;
+  ntc_temp_g /= samples_count;
+
+  fuel_gauge_initial_guess(&drv->fuel_gauge, vbat_g, ibat_g, ntc_temp_g);
+
+}