diff --git a/core/embed/sys/powerctl/fuel_gauge/battery_model.c b/core/embed/sys/powerctl/fuel_gauge/battery_model.c new file mode 100644 index 0000000000..67cc66f132 --- /dev/null +++ b/core/embed/sys/powerctl/fuel_gauge/battery_model.c @@ -0,0 +1,214 @@ +/* + * This file is part of the Trezor project, https://trezor.io/ + * + * Copyright (c) SatoshiLabs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +/* + * Battery Model Implementation + * Auto-generated from battery characterization data + */ + +#include "battery_model.h" +#include + +// Helper function for linear interpolation +static float linear_interpolate(float x, float x1, float y1, float x2, + float y2) { + // Prevent division by zero + if (fabsf(x2 - x1) < 1e-6f) { + return (y1 + y2) / 2.0f; // Return average if x values are too close + } + return y1 + (x - x1) * (y2 - y1) / (x2 - x1); +} + +// Helper function to calculate OCV for specific parameters and SOC +static float calc_ocv(const ocv_params_t* params, float soc) { + if (soc < BATTERY_SOC_BREAKPOINT_1) { + // First segment (rational) + return (params->a1 + params->b1 * soc) / (params->c1 + params->d1 * soc); + } else if (soc <= BATTERY_SOC_BREAKPOINT_2) { + // Middle segment (linear) + return params->m * soc + params->b; + } else { + // Third segment (rational) + return (params->a3 + params->b3 * soc) / (params->c3 + params->d3 * soc); + } +} + +// Helper function to calculate OCV slope for specific parameters and SOC +static float calc_ocv_slope(const ocv_params_t* params, float soc) { + if (soc < BATTERY_SOC_BREAKPOINT_1) { + // First segment (rational) + float denominator = params->c1 + params->d1 * soc; + return (params->b1 * params->c1 - params->a1 * params->d1) / + (denominator * denominator); + } else if (soc <= BATTERY_SOC_BREAKPOINT_2) { + // Middle segment (linear) + return params->m; + } else { + // Third segment (rational) + float denominator = params->c3 + params->d3 * soc; + return (params->b3 * params->c3 - params->a3 * params->d3) / + (denominator * denominator); + } +} + +// Helper function to calculate SOC from OCV for specific parameters +static float calc_soc_from_ocv(const ocv_params_t* params, float ocv) { + // Calculate breakpoint voltages + float ocv_breakpoint_1 = calc_ocv(params, BATTERY_SOC_BREAKPOINT_1); + float ocv_breakpoint_2 = calc_ocv(params, BATTERY_SOC_BREAKPOINT_2); + + if (ocv < ocv_breakpoint_1) { + // First segment (rational) + return (params->a1 - params->c1 * ocv) / (params->d1 * ocv - params->b1); + } else if (ocv <= ocv_breakpoint_2) { + // Middle segment (linear) + return (ocv - params->b) / params->m; + } else { + // Third segment (rational) + return (params->a3 - params->c3 * ocv) / (params->d3 * ocv - params->b3); + } +} + +float battery_rint(float temperature) { + // Calculate R_int using rational function: (a + b*t)/(c + d*t) + float a = BATTERY_R_INT_PARAMS.a; + float b = BATTERY_R_INT_PARAMS.b; + float c = BATTERY_R_INT_PARAMS.c; + float d = BATTERY_R_INT_PARAMS.d; + + return (a + b * temperature) / (c + d * temperature); +} + +float battery_total_capacity(float temperature) { + // Handle out-of-bounds temperatures + if (temperature <= BATTERY_TEMP_POINTS[0]) { + return BATTERY_OCV_PARAMS[0].total_capacity; + } + + if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMPERATURE_POINTS - 1]) { + return BATTERY_OCV_PARAMS[BATTERY_NUM_TEMPERATURE_POINTS - 1] + .total_capacity; + } + + // Find temperature bracket + for (int i = 0; i < BATTERY_NUM_TEMPERATURE_POINTS - 1; i++) { + if (temperature < BATTERY_TEMP_POINTS[i + 1]) { + return linear_interpolate(temperature, BATTERY_TEMP_POINTS[i], + BATTERY_OCV_PARAMS[i].total_capacity, + BATTERY_TEMP_POINTS[i + 1], + BATTERY_OCV_PARAMS[i + 1].total_capacity); + } + } + + // Should never reach here + return BATTERY_OCV_PARAMS[0].total_capacity; +} + +float battery_meas_to_ocv(float voltage_V, float current_mA, + float temperature) { + // Convert to mA to A by dividing by 1000 + float current_A = current_mA / 1000.0f; + + // Calculate OCV: V_OC = V_term + I * R_int + return voltage_V + (current_A * battery_rint(temperature)); +} + +float battery_ocv(float soc, float temperature) { + // Clamp SOC to valid range + soc = (soc < 0.0f) ? 0.0f : ((soc > 1.0f) ? 1.0f : soc); + + // Handle out-of-bounds temperatures + if (temperature <= BATTERY_TEMP_POINTS[0]) { + return calc_ocv(&BATTERY_OCV_PARAMS[0], soc); + } + + if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMPERATURE_POINTS - 1]) { + return calc_ocv(&BATTERY_OCV_PARAMS[BATTERY_NUM_TEMPERATURE_POINTS - 1], + soc); + } + + // Find temperature bracket and interpolate + for (int i = 0; i < BATTERY_NUM_TEMPERATURE_POINTS - 1; i++) { + if (temperature < BATTERY_TEMP_POINTS[i + 1]) { + float ocv_low = calc_ocv(&BATTERY_OCV_PARAMS[i], soc); + float ocv_high = calc_ocv(&BATTERY_OCV_PARAMS[i + 1], soc); + + return linear_interpolate(temperature, BATTERY_TEMP_POINTS[i], ocv_low, + BATTERY_TEMP_POINTS[i + 1], ocv_high); + } + } + + // Should never reach here + return calc_ocv(&BATTERY_OCV_PARAMS[0], soc); +} + +float battery_ocv_slope(float soc, float temperature) { + // Clamp SOC to valid range + soc = (soc < 0.0f) ? 0.0f : ((soc > 1.0f) ? 1.0f : soc); + + // Handle out-of-bounds temperatures + if (temperature <= BATTERY_TEMP_POINTS[0]) { + return calc_ocv_slope(&BATTERY_OCV_PARAMS[0], soc); + } + + if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMPERATURE_POINTS - 1]) { + return calc_ocv_slope( + &BATTERY_OCV_PARAMS[BATTERY_NUM_TEMPERATURE_POINTS - 1], soc); + } + + // Find temperature bracket and interpolate + for (int i = 0; i < BATTERY_NUM_TEMPERATURE_POINTS - 1; i++) { + if (temperature < BATTERY_TEMP_POINTS[i + 1]) { + float slope_low = calc_ocv_slope(&BATTERY_OCV_PARAMS[i], soc); + float slope_high = calc_ocv_slope(&BATTERY_OCV_PARAMS[i + 1], soc); + + return linear_interpolate(temperature, BATTERY_TEMP_POINTS[i], slope_low, + BATTERY_TEMP_POINTS[i + 1], slope_high); + } + } + + // Should never reach here + return calc_ocv_slope(&BATTERY_OCV_PARAMS[0], soc); +} + +float battery_soc(float ocv, float temperature) { + // Handle out-of-bounds temperatures + if (temperature <= BATTERY_TEMP_POINTS[0]) { + return calc_soc_from_ocv(&BATTERY_OCV_PARAMS[0], ocv); + } + + if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMPERATURE_POINTS - 1]) { + return calc_soc_from_ocv( + &BATTERY_OCV_PARAMS[BATTERY_NUM_TEMPERATURE_POINTS - 1], ocv); + } + + // Find temperature bracket and interpolate + for (int i = 0; i < BATTERY_NUM_TEMPERATURE_POINTS - 1; i++) { + if (temperature < BATTERY_TEMP_POINTS[i + 1]) { + float soc_low = calc_soc_from_ocv(&BATTERY_OCV_PARAMS[i], ocv); + float soc_high = calc_soc_from_ocv(&BATTERY_OCV_PARAMS[i + 1], ocv); + + return linear_interpolate(temperature, BATTERY_TEMP_POINTS[i], soc_low, + BATTERY_TEMP_POINTS[i + 1], soc_high); + } + } + + // Should never reach here + return calc_soc_from_ocv(&BATTERY_OCV_PARAMS[0], ocv); +} diff --git a/core/embed/sys/powerctl/fuel_gauge/battery_model.h b/core/embed/sys/powerctl/fuel_gauge/battery_model.h new file mode 100644 index 0000000000..dda7740397 --- /dev/null +++ b/core/embed/sys/powerctl/fuel_gauge/battery_model.h @@ -0,0 +1,211 @@ +/* + * This file is part of the Trezor project, https://trezor.io/ + * + * Copyright (c) SatoshiLabs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +/** + * Battery Model Lookup Tables + * Auto-generated from battery characterization data + */ + +#ifndef BATTERY_MODEL_H +#define BATTERY_MODEL_H + +#include +#include + +// Configuration +#define BATTERY_NUM_TEMPERATURE_POINTS 7 + +// Battery model parameters +// SOC breakpoints for piecewise functions +#define BATTERY_SOC_BREAKPOINT_1 0.25f +#define BATTERY_SOC_BREAKPOINT_2 0.8f + +// Temperature points array (in Celsius) +static const float BATTERY_TEMP_POINTS[BATTERY_NUM_TEMPERATURE_POINTS] = { + -9.02f, -2.30f, 4.57f, 13.04f, 17.51f, 27.17f, 37.03f}; + +// Internal resistance curve parameters (rational function parameters +// a+b*t)/(c+d*t) +typedef struct { + float a; + float b; + float c; + float d; +} rint_params_t; + +// OCV curve parameters for one temperature +typedef struct { + // m, b (linear segment) + float m; + float b; + // a1, b1, c1, d1 (first rational segment) + float a1; + float b1; + float c1; + float d1; + // a3, b3, c3, d3 (third rational segment) + float a3; + float b3; + float c3; + float d3; + // Total capacity at this temperature + float total_capacity; +} ocv_params_t; + +// Internal resistance curve parameters +static const rint_params_t BATTERY_R_INT_PARAMS = { + .a = -19.914535f, .b = -0.111745f, .c = -17.424596f, .d = -0.664215f}; + +// OCV curve parameters for each temperature +static const ocv_params_t BATTERY_OCV_PARAMS[BATTERY_NUM_TEMPERATURE_POINTS] = { + // Temperature: -9.02°C + {.m = 0.141258f, + .b = 3.190412f, + .a1 = 23.713014f, + .b1 = -30252.014861f, + .c1 = 6.822542f, + .d1 = -9376.243132f, + .a3 = 870.834698f, + .b3 = -770.217859f, + .c3 = 268.533412f, + .d3 = -239.304307f, + .total_capacity = 12.36f}, + // Temperature: -2.30°C + {.m = 0.147703f, + .b = 3.174024f, + .a1 = -25.237388f, + .b1 = 24.466968f, + .c1 = -7.971240f, + .d1 = 8.065657f, + .a3 = 1301.931501f, + .b3 = -1261.841781f, + .c3 = 398.187039f, + .d3 = -386.691292f, + .total_capacity = 66.17f}, + // Temperature: 4.57°C + {.m = 0.140456f, + .b = 3.195639f, + .a1 = 113.417606f, + .b1 = -92.151449f, + .c1 = 36.245689f, + .d1 = -33.083460f, + .a3 = -3814.963656f, + .b3 = 3754.803540f, + .c3 = -1156.843875f, + .d3 = 1139.555473f, + .total_capacity = 151.01f}, + // Temperature: 13.04°C + {.m = 0.137867f, + .b = 3.231006f, + .a1 = -149.212187f, + .b1 = -399.546027f, + .c1 = -47.886320f, + .d1 = -113.585027f, + .a3 = 1094.282489f, + .b3 = -1087.594536f, + .c3 = 327.867939f, + .d3 = -325.957816f, + .total_capacity = 245.24f}, + // Temperature: 17.51°C + {.m = 0.128165f, + .b = 3.231001f, + .a1 = 10.761174f, + .b1 = 75.344670f, + .c1 = 3.480805f, + .d1 = 22.358681f, + .a3 = 1120.933145f, + .b3 = -1116.536363f, + .c3 = 336.565790f, + .d3 = -335.323329f, + .total_capacity = 296.29f}, + // Temperature: 27.17°C + {.m = 0.111403f, + .b = 3.245045f, + .a1 = 167.692298f, + .b1 = 1476.743067f, + .c1 = 54.549004f, + .d1 = 437.954443f, + .a3 = 1106.075910f, + .b3 = -1100.920128f, + .c3 = 332.031558f, + .d3 = -330.558171f, + .total_capacity = 331.01f}, + // Temperature: 37.03°C + {.m = 0.113740f, + .b = 3.244924f, + .a1 = -58.731545f, + .b1 = -483.282822f, + .c1 = -18.980003f, + .d1 = -143.490387f, + .a3 = 1073.157307f, + .b3 = -1067.171796f, + .c3 = 322.017999f, + .d3 = -320.303753f, + .total_capacity = 344.33f}}; + +// Function declarations + +/** + * Calculate internal resistance at the given temperature + * @param temperature Battery temperature in Celsius + * @return Internal resistance in ohms + */ +float battery_rint(float temperature); + +/** + * Get battery total capacity at the given temperature + * @param temperature Battery temperature in Celsius + * @return Total capacity in mAh + */ +float battery_total_capacity(float temperature); + +/** + * Calculate OCV from measured voltage and current + * @param voltage_V Measured battery voltage in volts + * @param current_mA Measured battery current in mA (positive for discharge) + * @param temperature Battery temperature in Celsius + * @return Open circuit voltage (OCV) in volts + */ +float battery_meas_to_ocv(float voltage_V, float current_mA, float temperature); + +/** + * Get OCV for given SOC and temperature + * @param soc State of charge (0.0 to 1.0) + * @param temperature Battery temperature in Celsius + * @return Open circuit voltage in volts + */ +float battery_ocv(float soc, float temperature); + +/** + * Get the slope of the OCV curve at a given SOC and temperature + * @param soc State of charge (0.0 to 1.0) + * @param temperature Battery temperature in Celsius + * @return Slope of OCV curve (dOCV/dSOC) in volts + */ +float battery_ocv_slope(float soc, float temperature); + +/** + * Get SOC for given OCV and temperature + * @param ocv Open circuit voltage in volts + * @param temperature Battery temperature in Celsius + * @return State of charge (0.0 to 1.0) + */ +float battery_soc(float ocv, float temperature); + +#endif // BATTERY_MODEL_H diff --git a/core/embed/sys/powerctl/fuel_gauge/fuel_gauge.c b/core/embed/sys/powerctl/fuel_gauge/fuel_gauge.c new file mode 100644 index 0000000000..dab9535c45 --- /dev/null +++ b/core/embed/sys/powerctl/fuel_gauge/fuel_gauge.c @@ -0,0 +1,122 @@ +/* + * This file is part of the Trezor project, https://trezor.io/ + * + * Copyright (c) SatoshiLabs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include + +#include "battery_model.h" +#include "fuel_gauge.h" + +void fuel_gauge_init(fuel_gauge_state_t* state, float R, float Q, + float R_aggressive, float Q_aggressive, float P_init) { + state->R = R; + 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; +} + +void fuel_gauge_reset(fuel_gauge_state_t* state) { + // Reset state but keep filter parameters + state->soc = 0.0f; + state->soc_latched = 0.0f; +} + +void fuel_gauge_initial_guess(fuel_gauge_state_t* state, float voltage_V, + float current_mA, float temperature) { + // Calculate OCV from terminal voltage and current + float ocv = battery_meas_to_ocv(voltage_V, current_mA, temperature); + + // Get SOC from OCV using lookup + state->soc = battery_soc(ocv, temperature); + state->soc_latched = state->soc; +} + +float fuel_gauge_update(fuel_gauge_state_t* state, uint32_t dt, float voltage_V, + float current_mA, float temperature) { + // Choose filter parameters based on temperature and SOC + float R = state->R; + float Q = state->Q; + + if (temperature < 10.0f) { + // Cold temperature - use more conservative values + R = 10.0f; + Q = 0.01f; + } else if (state->soc_latched < 0.2f) { + // Low SOC - use aggressive values to track more closely + R = state->R_aggressive; + Q = state->Q_aggressive; + } + + // Convert milliseconds to seconds + float dt_sec = dt / 1000.0f; + + // Get total capacity at current temperature + float total_capacity = battery_total_capacity(temperature); + + // State prediction (coulomb counting) + // SOC_k+1 = SOC_k - (I*dt)/(3600*capacity) + float x_k1_k = + state->soc - (current_mA / (3600.0f * total_capacity)) * dt_sec; + + // Calculate Jacobian of measurement function h(x) = dOCV/dSOC + float h_jacobian = battery_ocv_slope(x_k1_k, temperature); + + // Error covariance prediction + float P_k1_k = state->P + Q; + + // Calculate innovation covariance + float S = h_jacobian * P_k1_k * h_jacobian + R; + + // Calculate Kalman gain + float K_k1_k = P_k1_k * h_jacobian / S; + + // Calculate predicted terminal voltage + float v_pred = battery_ocv(x_k1_k, temperature) - + (current_mA / 1000.0f) * battery_rint(temperature); + + // State update + float x_k1_k1 = x_k1_k + K_k1_k * (voltage_V - v_pred); + + // Error covariance update + float P_k1_k1 = (1.0f - K_k1_k * h_jacobian) * P_k1_k; + + // Enforce SOC boundaries + state->soc = (x_k1_k1 < 0.0f) ? 0.0f : ((x_k1_k1 > 1.0f) ? 1.0f : x_k1_k1); + state->P = P_k1_k1; + + // Update latched SOC based on current direction + if (current_mA > 0.0f) { + // Discharging, SOC should move only in negative direction + if (state->soc < state->soc_latched) { + state->soc_latched = state->soc; + } + } else { + // Charging, SOC should move only in positive direction + if (state->soc > state->soc_latched) { + state->soc_latched = state->soc; + } + } + + return state->soc_latched; +} diff --git a/core/embed/sys/powerctl/fuel_gauge/fuel_gauge.h b/core/embed/sys/powerctl/fuel_gauge/fuel_gauge.h new file mode 100644 index 0000000000..9f0e333f1b --- /dev/null +++ b/core/embed/sys/powerctl/fuel_gauge/fuel_gauge.h @@ -0,0 +1,77 @@ +/* + * This file is part of the Trezor project, https://trezor.io/ + * + * Copyright (c) SatoshiLabs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +// Fuel gauge state structure +typedef struct { + // State estimate (SOC) + float soc; + // Latched SOC (the one that gets reported) + float soc_latched; + // Error covariance + float P; + // Filter parameters + float R; // Measurement noise variance + float Q; // Process noise variance + float R_aggressive; // Aggressive measurement noise variance + float Q_aggressive; // Aggressive process noise variance +} fuel_gauge_state_t; + +/** + * Initialize the fuel gauge state + * @param state Pointer to fuel gauge state structure + * @param R Measurement noise variance + * @param Q Process noise variance + * @param R_aggressive Aggressive mode measurement noise variance + * @param Q_aggressive Aggressive mode process noise variance + * @param P_init Initial error covariance + */ +void fuel_gauge_init(fuel_gauge_state_t* state, float R, float Q, + float R_aggressive, float Q_aggressive, float P_init); + +/** + * Reset the fuel gauge state + * @param state Pointer to fuel gauge state structure + */ +void fuel_gauge_reset(fuel_gauge_state_t* state); + +/** + * Make initial SOC guess based on OCV + * @param state Pointer to fuel gauge state structure + * @param voltage_V Current battery voltage (V) + * @param current_mA Current battery current (mA), positive for discharge + * @param temperature Battery temperature (°C) + */ +void fuel_gauge_initial_guess(fuel_gauge_state_t* state, float voltage_V, + float current_mA, float temperature); + +/** + * Update the fuel gauge with new measurements + * @param state Pointer to fuel gauge state structure + * @param dt Time step in milliseconds + * @param voltage_V Current battery voltage (V) + * @param current_mA Current battery current (mA), positive for discharge + * @param temperature Battery temperature (°C) + * @return Updated SOC estimate (0.0 to 1.0) + */ +float fuel_gauge_update(fuel_gauge_state_t* state, uint32_t dt, float voltage_V, + float current_mA, float temperature);