diff --git a/core/embed/sys/power_manager/inc/sys/power_manager.h b/core/embed/sys/power_manager/inc/sys/power_manager.h index d1bc83846e..04da541290 100644 --- a/core/embed/sys/power_manager/inc/sys/power_manager.h +++ b/core/embed/sys/power_manager/inc/sys/power_manager.h @@ -86,4 +86,4 @@ const char* power_manager_get_state_name(power_manager_state_t state); power_manager_status_t power_manager_suspend(void); power_manager_status_t power_manager_hibernate(void); power_manager_status_t power_manager_turn_on(void); -power_manager_status_t power_manager_get_report(power_manager_report_t* report); +power_manager_status_t power_manager_get_report(power_manager_report_t* report); \ No newline at end of file diff --git a/core/embed/sys/power_manager/stm32u5/power_manager.c b/core/embed/sys/power_manager/stm32u5/power_manager.c index edf82a028b..43842f9990 100644 --- a/core/embed/sys/power_manager/stm32u5/power_manager.c +++ b/core/embed/sys/power_manager/stm32u5/power_manager.c @@ -22,7 +22,10 @@ #include #include + #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(); } diff --git a/core/embed/sys/power_manager/stm32u5/power_manager_internal.h b/core/embed/sys/power_manager/stm32u5/power_manager_internal.h index 765e23dd33..68febc15af 100644 --- a/core/embed/sys/power_manager/stm32u5/power_manager_internal.h +++ b/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); diff --git a/core/embed/sys/power_manager/stm32u5/power_monitoring.c b/core/embed/sys/power_manager/stm32u5/power_monitoring.c index 4dfacba541..9f66ec7294 100644 --- a/core/embed/sys/power_manager/stm32u5/power_monitoring.c +++ b/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); + +} \ No newline at end of file