arno/trezor-firmware
1
0
Fork 0
mirror of https://github.com/trezor/trezor-firmware.git synced 2025-04-20 17:19:01 +00:00
Code Issues Releases Wiki Activity

Merge 38ce65ddc7 into f89e7670c5

Browse Source
This commit is contained in:
kopecdav 2025-04-19 14:30:12 +07:00 committed by GitHub
commit fd73dfe7ac
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
10 changed files with 834 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
core/SConscript.prodtest
View File

@ -137,6 +137,7 @@ SOURCE_PRODTEST = [
'embed/projects/prodtest/cmd/prodtest_bootloader.c',
'embed/projects/prodtest/cmd/prodtest_button.c',
'embed/projects/prodtest/cmd/prodtest_display.c',
'embed/projects/prodtest/cmd/prodtest_fuel_gauge.c',
'embed/projects/prodtest/cmd/prodtest_prodtest.c',
'embed/projects/prodtest/cmd/prodtest_get_cpuid.c',
'embed/projects/prodtest/cmd/prodtest_haptic.c',

19
core/embed/projects/prodtest/README.md
View File

@ -969,6 +969,7 @@ Example:
nfc-write_card <timeout_seconds>
# NFC reader on, put the card on the reader (timeout <timeout_seconds> s)
# Writting URI to NFC tag 7AF403
### unit-test-run
Prodtest have capability to verify the overall firmware functionality by running built-in unit tests which should excercise the basic
features of the firmware drivers. This command will run all registered unit tests and return 'OK' if all tests passed.
@ -991,3 +992,21 @@ Example:
# ut-pmic-init-deinit - Test PMIC driver initialization and deinitialization
OK
```
### fuel-gauge
Activates fuel gauge which monitors battery state of charge and reports it on
display and command line, exit the fuel gauge monitor with CTRL+C
Example:
fuel-gauge
# Initialize Fuel gauge.
PROGRESS 3.123 122.13 0.281
PROGRESS 3.125 122.18 0.281
PROGRESS 3.123 122.13 0.280
```

114
core/embed/projects/prodtest/cmd/prodtest_fuel_gauge.c Normal file
View File

@ -0,0 +1,114 @@
/*
* 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/>.
*/
#ifdef USE_POWERCTL
#include <rust_ui_prodtest.h>
#include <stdlib.h>
#include <trezor_rtl.h>
#include <rtl/cli.h>
#include <rtl/mini_printf.h>
#include <sys/systick.h>
#include "../../../sys/powerctl/fuel_gauge/fuel_gauge.h"
#include "../../../sys/powerctl/npm1300/npm1300.h"
static void prodtest_fuel_gauge(cli_t *cli) {
if (cli_arg_count(cli) > 0) {
cli_error_arg_count(cli);
return;
}
char display_text[100];
fuel_gauge_state_t fg;
float Q = 0.001f;
float R = 3000.0f;
float R_aggressive = 3000.0f;
float Q_aggressive = 0.001f;
float P_init = 0.1;
cli_trace(cli, "Initialize Fuel gauge.");
fuel_gauge_init(&fg, R, Q, R_aggressive, Q_aggressive, P_init);
npm1300_report_t report;
if (!npm1300_measure_sync(&report)) {
cli_error(cli, CLI_ERROR, "Failed to get measurement data from PMIC.");
return;
}
fuel_gauge_initial_guess(&fg, report.vbat, report.ibat, report.ntc_temp);
uint32_t tick = systick_ms();
systick_delay_ms(1000);
while (true) {
if (cli_aborted(cli)) {
cli_trace(cli, "Abort fuel gauge test.");
break;
}
if (!npm1300_measure_sync(&report)) {
cli_error(cli, CLI_ERROR, "Failed to get measurement data from PMIC.");
break;
}
fuel_gauge_update(&fg, systick_ms() - tick, report.vbat, report.ibat,
report.ntc_temp);
tick = systick_ms();
// Calculate the integer and fractional parts correctly
int vbat_int = (int)report.vbat;
int vbat_frac =
abs((int)((report.vbat - vbat_int) * 1000)); // Only 3 decimal places
int ibat_int = (int)report.ibat;
int ibat_frac =
abs((int)((report.ibat - ibat_int) * 1000)); // Only 3 decimal places
int soc_int = (int)fg.soc;
int soc_frac =
abs((int)((fg.soc - soc_int) * 1000)); // Only 3 decimal places
cli_progress(cli, "%d.%03d %d.%03d %d.%03d", vbat_int, vbat_frac, ibat_int,
ibat_frac, soc_int, soc_frac);
mini_snprintf(display_text, 100, "V: %d.%03d I: %d.%03d SOC: %d.%03d",
vbat_int, vbat_frac, ibat_int, ibat_frac, soc_int, soc_frac);
screen_prodtest_show_text(display_text, strlen(display_text));
// Wait a second
systick_delay_ms(1000);
}
}
// clang-format off
PRODTEST_CLI_CMD(
.name = "fuel-gauge",
.func = prodtest_fuel_gauge,
.info = "Test fuel gauge",
.args = ""
);
#endif // USE_POWERCTL

132
core/embed/sys/powerctl/fuel_gauge/battery_data_jyhpfl333838.h Normal file
View File

@ -0,0 +1,132 @@
/*
* 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 Data: JYHPFL333838
* Auto-generated from battery characterization data
* Contains lookup tables and parameters for the specific battery model
*/
#pragma once
#include <stdint.h>
/**
* Battery Specifications:
* Model: JYHPFL333838
* Chemistry: LiFePO4
*/
// Configuration
#define BATTERY_NUM_TEMP_POINTS 4
// 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_TEMP_POINTS] = {
0.46f, 10.05f, 25.03f, 29.80f};
// Internal resistance curve parameters (rational function parameters
// (a+b*t)/(c+d*t)
static const float BATTERY_R_INT_PARAMS[4] = {
// a, b, c, d for rational function (a + b*t)/(c + d*t)
314.561562f, 6.949454f, 329.002634f, 20.119285f};
// Discharge OCV curve parameters for each temperature
static const float BATTERY_OCV_DISCHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS][10] = {
// Temperature: 0.46°C
{
0.130028f, 3.193222f, // m, b (linear segment)
325.823911f, 1943.605327f, 106.271504f,
581.477397f, // a1, b1, c1, d1 (first rational segment)
-3083.838407f, 3072.517555f, -935.932995f,
932.663443f // a3, b3, c3, d3 (third rational segment)
},
// Temperature: 10.05°C
{
0.124523f, 3.204479f, // m, b (linear segment)
50.263715f, 294.491126f, 16.431238f,
87.428849f, // a1, b1, c1, d1 (first rational segment)
1157.754723f, -1154.042231f, 350.693046f,
-349.642784f // a3, b3, c3, d3 (third rational segment)
},
// Temperature: 25.03°C
{
0.131226f, 3.228685f, // m, b (linear segment)
-2761.545706f, -35244.718985f, -903.249559f,
-10580.169625f, // a1, b1, c1, d1 (first rational segment)
560.960239f, -560.104695f, 168.314326f,
-168.068451f // a3, b3, c3, d3 (third rational segment)
},
// Temperature: 29.80°C
{
0.128900f, 3.224999f, // m, b (linear segment)
147.603009f, 2106.738218f, 48.371431f,
634.565986f, // a1, b1, c1, d1 (first rational segment)
1088.145798f, -1086.632309f, 327.042181f,
-326.609182f // a3, b3, c3, d3 (third rational segment)
}};
// Charge OCV curve parameters for each temperature
static const float BATTERY_OCV_CHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS][10] = {
// Temperature: 0.46°C
{
0.189010f, 3.186844f, // m, b (linear segment)
429.481457f, 18113.325314f, 151.811299f,
5524.684908f, // a1, b1, c1, d1 (first rational segment)
-153.670897f, 124.507975f, -48.860495f,
40.830081f // a3, b3, c3, d3 (third rational segment)
},
// Temperature: 10.05°C
{
0.205185f, 3.279822f, // m, b (linear segment)
309.660606f, 16075.042838f, 108.161345f,
4764.914681f, // a1, b1, c1, d1 (first rational segment)
-117.943000f, -1804.883579f, -150.332984f,
-378.962265f // a3, b3, c3, d3 (third rational segment)
},
// Temperature: 25.03°C
{
0.171150f, 3.255314f, // m, b (linear segment)
21.579423f, 949.274458f, 7.473251f,
284.103459f, // a1, b1, c1, d1 (first rational segment)
-2905.691117f, 2320.320088f, -903.787409f,
743.029277f // a3, b3, c3, d3 (third rational segment)
},
// Temperature: 29.80°C
{
0.145942f, 3.256710f, // m, b (linear segment)
-150.766651f, -6156.646216f, -51.753537f,
-1845.616021f, // a1, b1, c1, d1 (first rational segment)
1326.452225f, -1158.930117f, 407.411820f,
-361.305515f // a3, b3, c3, d3 (third rational segment)
}};
// Battery capacity data for each temperature
static const float BATTERY_CAPACITY[BATTERY_NUM_TEMP_POINTS][2] = {
// Temperature: 0.46°C
{260.61f, 375.53f},
// Temperature: 10.05°C
{291.40f, 349.77f},
// Temperature: 25.03°C
{345.85f, 397.55f},
// Temperature: 29.80°C
{343.12f, 390.44f}};

284
core/embed/sys/powerctl/fuel_gauge/battery_model.c Normal file
View File

@ -0,0 +1,284 @@
/*
* 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 "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);
}
// Calculate OCV for specific parameters and SOC
static float calc_ocv(const float* params, float soc) {
if (soc < BATTERY_SOC_BREAKPOINT_1) {
// First segment (rational function): (a1 + b1*x)/(c1 + d1*x)
float a1 = params[2];
float b1 = params[3];
float c1 = params[4];
float d1 = params[5];
return (a1 + b1 * soc) / (c1 + d1 * soc);
} else if (soc <= BATTERY_SOC_BREAKPOINT_2) {
// Middle segment (linear function): m*x + b
float m = params[0];
float b = params[1];
return m * soc + b;
} else {
// Third segment (rational function): (a3 + b3*x)/(c3 + d3*x)
float a3 = params[6];
float b3 = params[7];
float c3 = params[8];
float d3 = params[9];
return (a3 + b3 * soc) / (c3 + d3 * soc);
}
}
// Calculate OCV slope for specific parameters and SOC
static float calc_ocv_slope(const float* params, float soc) {
if (soc < BATTERY_SOC_BREAKPOINT_1) {
// First segment (rational function derivative)
float a1 = params[2];
float b1 = params[3];
float c1 = params[4];
float d1 = params[5];
float denominator = c1 + d1 * soc;
return (b1 * c1 - a1 * d1) / (denominator * denominator);
} else if (soc <= BATTERY_SOC_BREAKPOINT_2) {
// Middle segment (linear function derivative)
float m = params[0];
return m;
} else {
// Third segment (rational function derivative)
float a3 = params[6];
float b3 = params[7];
float c3 = params[8];
float d3 = params[9];
float denominator = c3 + d3 * soc;
return (b3 * c3 - a3 * d3) / (denominator * denominator);
}
}
// Calculate SOC from OCV for specific parameters
static float calc_soc_from_ocv(const float* 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);
// Extract parameters
float m = params[0];
float b = params[1];
float a1 = params[2];
float b1 = params[3];
float c1 = params[4];
float d1 = params[5];
float a3 = params[6];
float b3 = params[7];
float c3 = params[8];
float d3 = params[9];
if (ocv < ocv_breakpoint_1) {
// First segment (rational function inverse)
return (a1 - c1 * ocv) / (d1 * ocv - b1);
} else if (ocv <= ocv_breakpoint_2) {
// Middle segment (linear function inverse)
return (ocv - b) / m;
} else {
// Third segment (rational function inverse)
return (a3 - c3 * ocv) / (d3 * ocv - b3);
}
}
float battery_rint(float temperature) {
// Calculate R_int using rational function: (a + b*t)/(c + d*t)
float a = BATTERY_R_INT_PARAMS[0];
float b = BATTERY_R_INT_PARAMS[1];
float c = BATTERY_R_INT_PARAMS[2];
float d = BATTERY_R_INT_PARAMS[3];
return (a + b * temperature) / (c + d * temperature);
}
float battery_total_capacity(float temperature, bool discharging_mode) {
// Handle out-of-bounds temperatures
if (temperature <= BATTERY_TEMP_POINTS[0]) {
return BATTERY_CAPACITY[0][discharging_mode ? 0 : 1];
}
if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMP_POINTS - 1]) {
return BATTERY_CAPACITY[BATTERY_NUM_TEMP_POINTS - 1]
[discharging_mode ? 0 : 1];
}
// Find temperature bracket
for (int i = 0; i < BATTERY_NUM_TEMP_POINTS - 1; i++) {
if (temperature < BATTERY_TEMP_POINTS[i + 1]) {
return linear_interpolate(
temperature, BATTERY_TEMP_POINTS[i],
BATTERY_CAPACITY[i][discharging_mode ? 0 : 1],
BATTERY_TEMP_POINTS[i + 1],
BATTERY_CAPACITY[i + 1][discharging_mode ? 0 : 1]);
}
}
// Should never reach here
return BATTERY_CAPACITY[0][discharging_mode ? 0 : 1];
}
float battery_meas_to_ocv(float voltage_V, float current_mA,
float temperature) {
// Convert 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, bool discharging_mode) {
// 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]) {
const float* params = discharging_mode ? BATTERY_OCV_DISCHARGE_PARAMS[0]
: BATTERY_OCV_CHARGE_PARAMS[0];
return calc_ocv(params, soc);
}
if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMP_POINTS - 1]) {
const float* params =
discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS - 1]
: BATTERY_OCV_CHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS - 1];
return calc_ocv(params, soc);
}
// Find temperature bracket and interpolate
for (int i = 0; i < BATTERY_NUM_TEMP_POINTS - 1; i++) {
if (temperature < BATTERY_TEMP_POINTS[i + 1]) {
const float* params_low = discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[i]
: BATTERY_OCV_CHARGE_PARAMS[i];
const float* params_high = discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[i + 1]
: BATTERY_OCV_CHARGE_PARAMS[i + 1];
float ocv_low = calc_ocv(params_low, soc);
float ocv_high = calc_ocv(params_high, soc);
return linear_interpolate(temperature, BATTERY_TEMP_POINTS[i], ocv_low,
BATTERY_TEMP_POINTS[i + 1], ocv_high);
}
}
// Should never reach here
const float* params = discharging_mode ? BATTERY_OCV_DISCHARGE_PARAMS[0]
: BATTERY_OCV_CHARGE_PARAMS[0];
return calc_ocv(params, soc);
}
float battery_ocv_slope(float soc, float temperature, bool discharging_mode) {
// 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]) {
const float* params = discharging_mode ? BATTERY_OCV_DISCHARGE_PARAMS[0]
: BATTERY_OCV_CHARGE_PARAMS[0];
return calc_ocv_slope(params, soc);
}
if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMP_POINTS - 1]) {
const float* params =
discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS - 1]
: BATTERY_OCV_CHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS - 1];
return calc_ocv_slope(params, soc);
}
// Find temperature bracket and interpolate
for (int i = 0; i < BATTERY_NUM_TEMP_POINTS - 1; i++) {
if (temperature < BATTERY_TEMP_POINTS[i + 1]) {
const float* params_low = discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[i]
: BATTERY_OCV_CHARGE_PARAMS[i];
const float* params_high = discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[i + 1]
: BATTERY_OCV_CHARGE_PARAMS[i + 1];
float slope_low = calc_ocv_slope(params_low, soc);
float slope_high = calc_ocv_slope(params_high, soc);
return linear_interpolate(temperature, BATTERY_TEMP_POINTS[i], slope_low,
BATTERY_TEMP_POINTS[i + 1], slope_high);
}
}
// Should never reach here
const float* params = discharging_mode ? BATTERY_OCV_DISCHARGE_PARAMS[0]
: BATTERY_OCV_CHARGE_PARAMS[0];
return calc_ocv_slope(params, soc);
}
float battery_soc(float ocv, float temperature, bool discharging_mode) {
// Handle out-of-bounds temperatures
if (temperature <= BATTERY_TEMP_POINTS[0]) {
const float* params = discharging_mode ? BATTERY_OCV_DISCHARGE_PARAMS[0]
: BATTERY_OCV_CHARGE_PARAMS[0];
return calc_soc_from_ocv(params, ocv);
}
if (temperature >= BATTERY_TEMP_POINTS[BATTERY_NUM_TEMP_POINTS - 1]) {
const float* params =
discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS - 1]
: BATTERY_OCV_CHARGE_PARAMS[BATTERY_NUM_TEMP_POINTS - 1];
return calc_soc_from_ocv(params, ocv);
}
// Find temperature bracket and interpolate
for (int i = 0; i < BATTERY_NUM_TEMP_POINTS - 1; i++) {
if (temperature < BATTERY_TEMP_POINTS[i + 1]) {
const float* params_low = discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[i]
: BATTERY_OCV_CHARGE_PARAMS[i];
const float* params_high = discharging_mode
? BATTERY_OCV_DISCHARGE_PARAMS[i + 1]
: BATTERY_OCV_CHARGE_PARAMS[i + 1];
float soc_low = calc_soc_from_ocv(params_low, ocv);
float soc_high = calc_soc_from_ocv(params_high, ocv);
return linear_interpolate(temperature, BATTERY_TEMP_POINTS[i], soc_low,
BATTERY_TEMP_POINTS[i + 1], soc_high);
}
}
// Should never reach here
const float* params = discharging_mode ? BATTERY_OCV_DISCHARGE_PARAMS[0]
: BATTERY_OCV_CHARGE_PARAMS[0];
return calc_soc_from_ocv(params, ocv);
}

77
core/embed/sys/powerctl/fuel_gauge/battery_model.h Normal file
View 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>
// Include the battery data header - this will be selected at compile time
// based on which battery is being used
#include "battery_data_jyhpfl333838.h"
/**
* 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 and discharge mode
* @param temperature Battery temperature in Celsius
* @param discharging_mode true if discharging, false if charging
* @return Total capacity in mAh
*/
float battery_total_capacity(float temperature, bool discharging_mode);
/**
* 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
* @param discharging_mode true if discharging, false if charging
* @return Open circuit voltage in volts
*/
float battery_ocv(float soc, float temperature, bool discharging_mode);
/**
* 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
* @param discharging_mode true if discharging, false if charging
* @return Slope of OCV curve (dOCV/dSOC) in volts
*/
float battery_ocv_slope(float soc, float temperature, bool discharging_mode);
/**
* Get SOC for given OCV and temperature
* @param ocv Open circuit voltage in volts
* @param temperature Battery temperature in Celsius
* @param discharging_mode true if discharging, false if charging
* @return State of charge (0.0 to 1.0)
*/
float battery_soc(float ocv, float temperature, bool discharging_mode);

126
core/embed/sys/powerctl/fuel_gauge/fuel_gauge.c Normal file
View File

@ -0,0 +1,126 @@
/*
* 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 "fuel_gauge.h"
#include <math.h>
#include "battery_model.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) {
// Determine if we're in discharge mode
bool discharging_mode = current_mA >= 0.0f;
// 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, discharging_mode);
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) {
// Determine if we're in discharge mode
bool discharging_mode = current_mA >= 0.0f;
// 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, discharging_mode);
// 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, discharging_mode);
// 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, discharging_mode) -
(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
View 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 EKF 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 EKF state
* @param state Pointer to EKF state structure
*/
void fuel_gauge_reset(fuel_gauge_state_t* state);
/**
* Make initial SOC guess based on OCV
* @param state Pointer to EKF 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 EKF 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);

2
core/site_scons/models/T3W1/trezor_t3w1_revB.py
View File

@ -236,6 +236,8 @@ def configure(
sources += [
"embed/sys/powerctl/npm1300/npm1300.c",
"embed/sys/powerctl/fuel_gauge/fuel_gauge.c",
"embed/sys/powerctl/fuel_gauge/battery_model.c",
"embed/sys/powerctl/stwlc38/stwlc38.c",
"embed/sys/powerctl/stwlc38/stwlc38_patching.c",
"embed/sys/powerctl/stm32u5/powerctl.c",

2
core/site_scons/models/T3W1/trezor_t3w1_revC.py
View File

@ -236,6 +236,8 @@ def configure(
sources += [
"embed/sys/powerctl/npm1300/npm1300.c",
"embed/sys/powerctl/fuel_gauge/fuel_gauge.c",
"embed/sys/powerctl/fuel_gauge/battery_model.c",
"embed/sys/powerctl/stwlc38/stwlc38.c",
"embed/sys/powerctl/stwlc38/stwlc38_patching.c",
"embed/sys/powerctl/stm32u5/powerctl.c",