mirror of
https://github.com/trezor/trezor-firmware.git
synced 2025-05-08 09:58:46 +00:00
feat(core): Introduce fuel gauge library [no changelog]
This commit is contained in:
kopecdav
kopecdav
parent
07c16858d4
commit
76cffbc5c6
214
core/embed/sys/powerctl/fuel_gauge/battery_model.c
Normal file
214
core/embed/sys/powerctl/fuel_gauge/battery_model.c
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Battery Model Implementation
|
||||
* Auto-generated from battery characterization data
|
||||
*/
|
||||
|
||||
#include "battery_model.h"
|
||||
#include <math.h>
|
||||
|
||||
// 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);
|
||||
}
|
||||
211
core/embed/sys/powerctl/fuel_gauge/battery_model.h
Normal file
211
core/embed/sys/powerctl/fuel_gauge/battery_model.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Battery Model Lookup Tables
|
||||
* Auto-generated from battery characterization data
|
||||
*/
|
||||
|
||||
#ifndef BATTERY_MODEL_H
|
||||
#define BATTERY_MODEL_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
// 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
|
||||
122
core/embed/sys/powerctl/fuel_gauge/fuel_gauge.c
Normal file
122
core/embed/sys/powerctl/fuel_gauge/fuel_gauge.c
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <trezor_types.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
77
core/embed/sys/powerctl/fuel_gauge/fuel_gauge.h
Normal file
77
core/embed/sys/powerctl/fuel_gauge/fuel_gauge.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <trezor_types.h>
|
||||
|
||||
// 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);
|
||||
Loading…
Reference in New Issue
Block a user