feat(core): store fuel gauge covariance into backup ram [no changelog]

core/embed/sys/backup_ram/backup_ram.c

@@ -148,7 +148,7 @@ backup_ram_status_t backup_ram_erase_unused(void) {
 }
 
 backup_ram_status_t backup_ram_store_power_manager_data(
-    const backup_ram_power_manager_data_t *pm_data) {
+  const backup_ram_power_manager_data_t *pm_data) {
   backup_ram_driver_t *drv = &backup_ram_driver;
 
   if (!drv->initialized) {

core/embed/sys/backup_ram/inc/sys/backup_ram.h

@@ -44,7 +44,8 @@ typedef enum {
  * after power loss.
  */
 typedef struct {
-  float soc;  // Captured state of charge <0, 1>
+  float soc;  // Captured fuel gauge state of charge <0, 1>
+  float P;    // Captured fuel gauge covariance
   bool bat_critical;
   // Captures RTC time at which SOC was captured
   uint32_t last_capture_timestamp;

core/embed/sys/power_manager/fuel_gauge/fuel_gauge.c

@@ -27,11 +27,11 @@ void fuel_gauge_init(fuel_gauge_state_t* state, float R, float Q,
   state->Q = Q;
   state->R_aggressive = R_aggressive;
   state->Q_aggressive = Q_aggressive;
-  state->P = P_init;
 
   // Initialize state
   state->soc = 0.0f;
   state->soc_latched = 0.0f;
+  state->P = P_init; // Initial error covariance
 }
 
 void fuel_gauge_reset(fuel_gauge_state_t* state) {
@@ -40,10 +40,11 @@ void fuel_gauge_reset(fuel_gauge_state_t* state) {
   state->soc_latched = 0.0f;
 }
 
-void fuel_gauge_set_soc(fuel_gauge_state_t* state, float soc) {
+void fuel_gauge_set_soc(fuel_gauge_state_t* state, float soc, float P) {
   // Set SOC directly
   state->soc = soc;
   state->soc_latched = soc;
+  state->P = P; // Set error covariance
 }
 
 void fuel_gauge_initial_guess(fuel_gauge_state_t* state, float voltage_V,

core/embed/sys/power_manager/fuel_gauge/fuel_gauge.h

@@ -59,7 +59,7 @@ void fuel_gauge_reset(fuel_gauge_state_t* state);
  * @param state Pointer to EKF state structure
  * @param soc State of charge (0.0 to 1.0)
  */
-void fuel_gauge_set_soc(fuel_gauge_state_t* state, float soc);
+void fuel_gauge_set_soc(fuel_gauge_state_t* state, float soc, float P);
 
 /**
  * Make initial SOC guess based on OCV

core/embed/sys/power_manager/inc/sys/power_manager.h

@@ -101,7 +101,7 @@ pm_status_t pm_turn_on(void);
 pm_status_t pm_get_report(pm_report_t* report);
 pm_status_t pm_charging_enable(void);
 pm_status_t pm_charging_disable(void);
-pm_status_t pm_store_data_to_backup_ram(void);
+pm_status_t pm_store_data_to_backup_ram();
 pm_status_t pm_wait_until_active(uint32_t timeout_ms);
 pm_status_t pm_wakeup_flags_set(pm_wakeup_flags_t flags);
 pm_status_t pm_wakeup_flags_reset(void);

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

@@ -77,7 +77,7 @@ pm_status_t pm_init(bool inherit_state) {
       backup_ram_read_power_manager_data(&pm_recovery_data);
 
   if (status == BACKUP_RAM_OK) {
-    fuel_gauge_set_soc(&drv->fuel_gauge, pm_recovery_data.soc);
+    fuel_gauge_set_soc(&drv->fuel_gauge, pm_recovery_data.soc, pm_recovery_data.P);
   } else {
     // Wait for 1s to sample battery data
     systick_delay_ms(1000);
@@ -228,11 +228,11 @@ pm_status_t pm_hibernate(void) {
   drv->request_hibernate = true;
   pm_process_state_machine();
 
-  if (drv->state != PM_STATE_HIBERNATE) {
-    return PM_REQUEST_REJECTED;
-  }
+  systick_delay_ms(50);
+
+  // Whenever hibernation request fall through, request was rejected
+  return PM_REQUEST_REJECTED;
 
-  return PM_OK;
 }
 
 pm_status_t pm_turn_on(void) {
@@ -324,7 +324,7 @@ pm_status_t pm_charging_disable(void) {
   return PM_OK;
 }
 
-pm_status_t pm_store_data_to_backup_ram(void) {
+pm_status_t pm_store_data_to_backup_ram() {
   pm_driver_t* drv = &g_pm;
 
   if (!drv->initialized) {
@@ -333,12 +333,15 @@ pm_status_t pm_store_data_to_backup_ram(void) {
 
   backup_ram_power_manager_data_t pm_data = {0};
 
+  // Fuel gauge state
   if (drv->battery_critical) {
     pm_data.soc = 0;
   } else {
     pm_data.soc = drv->fuel_gauge.soc;
   }
+  pm_data.P = drv->fuel_gauge.P;
 
+  // Power manager state
   pm_data.bat_critical = drv->battery_critical;
   pm_data.bootloader_exit_state = drv->state;
 

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

@@ -105,21 +105,16 @@ void pm_enter_active(pm_driver_t* drv) {
 }
 
 pm_internal_state_t pm_handle_state_active(pm_driver_t* drv) {
+
   // Handle hibernate request
   if (drv->request_hibernate) {
     drv->request_hibernate = false;
-
     return PM_STATE_HIBERNATE;
   }
 
   // Handle suspend request
   if (drv->request_suspend) {
     drv->request_suspend = false;
-
-    if (drv->usb_connected || drv->wireless_connected) {
-      return PM_STATE_CHARGING;
-    }
-
     return PM_STATE_SUSPEND;
   }
 
@@ -138,21 +133,16 @@ void pm_enter_power_save(pm_driver_t* drv) {
 }
 
 pm_internal_state_t pm_handle_state_power_save(pm_driver_t* drv) {
+
   // Handle hibernate request
   if (drv->request_hibernate) {
     drv->request_hibernate = false;
-
     return PM_STATE_HIBERNATE;
   }
 
   // Handle suspend request
   if (drv->request_suspend) {
     drv->request_suspend = false;
-
-    if (drv->usb_connected || drv->wireless_connected) {
-      return PM_STATE_CHARGING;
-    }
-
     return PM_STATE_SUSPEND;
   }
 
@@ -170,6 +160,13 @@ pm_internal_state_t pm_handle_state_power_save(pm_driver_t* drv) {
 }
 
 pm_internal_state_t pm_handle_state_shutting_down(pm_driver_t* drv) {
+
+  // System is shutting down, but user can still hibernate the device early.
+  if (drv->request_hibernate) {
+      drv->request_hibernate = false;
+      return PM_STATE_HIBERNATE;
+  }
+
   // Return to power save if external power or battery recovered
   if (drv->usb_connected || !drv->battery_critical) {
     return PM_STATE_POWER_SAVE;
@@ -253,6 +250,7 @@ pm_internal_state_t pm_handle_state_hibernate(pm_driver_t* drv) {
 // State enter/exit actions
 
 void pm_enter_report_low_battery(pm_driver_t* drv) {
+
   // Set backlight to minimum
   backlight_set_max_level(0);
 
@@ -273,15 +271,14 @@ void pm_exit_shutting_down(pm_driver_t* drv) {
 void pm_enter_suspend(pm_driver_t* drv) {
 
   pm_control_suspend();
-  // Not implemented yet
+
 }
 
 void pm_enter_hibernate(pm_driver_t* drv) {
 
-  // TODO: Store power manager data with request to hibernate
-  // after reboot?
+  // Store power manager data with request to hibernate, power manager
+  // will try to hibernate immediately after reboot.
   pm_store_data_to_backup_ram();
-
   reboot_device();
 
 }