|
| 1 | +// SPDX-License-Identifier: GPL-2.0-only |
| 2 | +// Copyright (C) 2010-2013 The LEGO Group |
| 3 | +// Copyright (c) 2017-2018,2021 David Lechner <[email protected]> |
| 4 | +// Copyright (C) 2025 The Pybricks Authors |
| 5 | + |
| 6 | +#include <pbsys/config.h> |
| 7 | + |
| 8 | +// LEGO MINDSTORMS EV3 battery temperature estimation from lms2012 |
| 9 | +#if PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION |
| 10 | + |
| 11 | +/** |
| 12 | + * Function for estimating new battery temperature based on measurements |
| 13 | + * of battery voltage and battery current. |
| 14 | + * @param [in] V_bat Battery voltage (volts) |
| 15 | + * @param [in] I_bat Battery current (amps) |
| 16 | + * @returns Estimated battery temperature (degrees Celsius) |
| 17 | + */ |
| 18 | +float pbsys_battery_temp_update(float V_bat, float I_bat) { |
| 19 | + static struct { |
| 20 | + /** Keeps track of sample index since power-on. */ |
| 21 | + uint32_t index; |
| 22 | + /** Running mean current. */ |
| 23 | + float I_bat_mean; |
| 24 | + /** Battery temperature. */ |
| 25 | + float T_bat; |
| 26 | + /** EV3 electronics temperature. */ |
| 27 | + float T_elec; |
| 28 | + /** Old internal resistance of the battery model. */ |
| 29 | + float R_bat_model_old; |
| 30 | + /** Internal resistance of the batteries. */ |
| 31 | + float R_bat; |
| 32 | + // Flag that prevents initialization of R_bat when the battery is charging |
| 33 | + bool has_passed_7v5_flag; |
| 34 | + } bat_temp; |
| 35 | + |
| 36 | + /*************************** Model parameters *******************************/ |
| 37 | + // Approx. initial internal resistance of 6 Energizer industrial batteries: |
| 38 | + const float R_bat_init = 0.63468f; |
| 39 | + // Batteries' heat capacity: |
| 40 | + const float heat_cap_bat = 136.6598f; |
| 41 | + // Newtonian cooling constant for electronics: |
| 42 | + const float K_bat_loss_to_elec = -0.0003f; // -0.000789767; |
| 43 | + // Newtonian heating constant for electronics: |
| 44 | + const float K_bat_gain_from_elec = 0.001242896f; // 0.001035746; |
| 45 | + // Newtonian cooling constant for environment: |
| 46 | + const float K_bat_to_room = -0.00012f; |
| 47 | + // Battery power Boost |
| 48 | + const float battery_power_boost = 1.7f; |
| 49 | + // Battery R_bat negative gain |
| 50 | + const float R_bat_neg_gain = 1.00f; |
| 51 | + |
| 52 | + // Slope of electronics lossless heating curve (linear!!!) [Deg.C / s]: |
| 53 | + const float K_elec_heat_slope = 0.0123175f; |
| 54 | + // Newtonian cooling constant for battery packs: |
| 55 | + const float K_elec_loss_to_bat = -0.004137487f; |
| 56 | + // Newtonian heating constant for battery packs: |
| 57 | + const float K_elec_gain_from_bat = 0.002027574f; // 0.00152068; |
| 58 | + // Newtonian cooling constant for environment: |
| 59 | + const float K_elec_to_room = -0.001931431f; // -0.001843639; |
| 60 | + |
| 61 | + // NB: This time must match PBSYS_BATTERY_TEMP_TIMER_PERIOD_MS |
| 62 | + const float sample_period = 0.4f; // Algorithm update period in seconds |
| 63 | + |
| 64 | + float R_bat_model; // Internal resistance of the battery model |
| 65 | + float slope_A; // Slope obtained by linear interpolation |
| 66 | + float intercept_b; // Offset obtained by linear interpolation |
| 67 | + const float I_1A = 0.05f; // Current carrying capacity at bottom of the curve |
| 68 | + const float I_2A = 2.0f; // Current carrying capacity at the top of the curve |
| 69 | + |
| 70 | + float R_1A; // Internal resistance of the batteries at 1A and V_bat |
| 71 | + float R_2A; // Internal resistance of the batteries at 2A and V_bat |
| 72 | + |
| 73 | + float dT_bat_own; // Batteries' own heat |
| 74 | + float dT_bat_loss_to_elec; // Batteries' heat loss to electronics |
| 75 | + float dT_bat_gain_from_elec; // Batteries' heat gain from electronics |
| 76 | + float dT_bat_loss_to_room; // Batteries' cooling from environment |
| 77 | + |
| 78 | + float dT_elec_own; // Electronics' own heat |
| 79 | + float dT_elec_loss_to_bat; // Electronics' heat loss to the battery pack |
| 80 | + float dT_elec_gain_from_bat; // Electronics' heat gain from battery packs |
| 81 | + float dT_elec_loss_to_room; // Electronics' heat loss to the environment |
| 82 | + |
| 83 | + /***************************************************************************/ |
| 84 | + |
| 85 | + // Update the average current: I_bat_mean |
| 86 | + if (bat_temp.index > 0) { |
| 87 | + bat_temp.I_bat_mean = (bat_temp.index * bat_temp.I_bat_mean + I_bat) / (bat_temp.index + 1); |
| 88 | + } else { |
| 89 | + bat_temp.I_bat_mean = I_bat; |
| 90 | + } |
| 91 | + |
| 92 | + bat_temp.index++; |
| 93 | + |
| 94 | + // Calculate R_1A as a function of V_bat (internal resistance at 1A continuous) |
| 95 | + R_1A = 0.014071f * (V_bat * V_bat * V_bat * V_bat) |
| 96 | + - 0.335324f * (V_bat * V_bat * V_bat) |
| 97 | + + 2.933404f * (V_bat * V_bat) |
| 98 | + - 11.243047f * V_bat |
| 99 | + + 16.897461f; |
| 100 | + |
| 101 | + // Calculate R_2A as a function of V_bat (internal resistance at 2A continuous) |
| 102 | + R_2A = 0.014420f * (V_bat * V_bat * V_bat * V_bat) |
| 103 | + - 0.316728f * (V_bat * V_bat * V_bat) |
| 104 | + + 2.559347f * (V_bat * V_bat) |
| 105 | + - 9.084076f * V_bat |
| 106 | + + 12.794176f; |
| 107 | + |
| 108 | + // Calculate the slope by linear interpolation between R_1A and R_2A |
| 109 | + slope_A = (R_1A - R_2A) / (I_1A - I_2A); |
| 110 | + |
| 111 | + // Calculate intercept by linear interpolation between R1_A and R2_A |
| 112 | + intercept_b = R_1A - slope_A * R_1A; |
| 113 | + |
| 114 | + // Reload R_bat_model: |
| 115 | + R_bat_model = slope_A * bat_temp.I_bat_mean + intercept_b; |
| 116 | + |
| 117 | + // Calculate batteries' internal resistance: R_bat |
| 118 | + if (V_bat > 7.5 && !bat_temp.has_passed_7v5_flag) { |
| 119 | + bat_temp.R_bat = R_bat_init; // 7.5 V not passed a first time |
| 120 | + } else { |
| 121 | + // Only update R_bat with positive outcomes: R_bat_model - R_bat_model_old |
| 122 | + // R_bat updated with the change in model R_bat is not equal value in the model! |
| 123 | + if ((R_bat_model - bat_temp.R_bat_model_old) > 0) { |
| 124 | + bat_temp.R_bat += R_bat_model - bat_temp.R_bat_model_old; |
| 125 | + } else { // The negative outcome of R_bat_model added to only part of R_bat |
| 126 | + bat_temp.R_bat += R_bat_neg_gain * (R_bat_model - bat_temp.R_bat_model_old); |
| 127 | + } |
| 128 | + // Make sure we initialize R_bat later |
| 129 | + bat_temp.has_passed_7v5_flag = true; |
| 130 | + } |
| 131 | + |
| 132 | + // Save R_bat_model for use in the next function call |
| 133 | + bat_temp.R_bat_model_old = R_bat_model; |
| 134 | + |
| 135 | + // pbdrv_uart_debug_printf("%c %f %f %f %f %f %f\r\n", bat_temp.has_passed_7v5_flag ? 'Y' : 'N', |
| 136 | + // (double)R_1A, (double)R_2A, (double)slope_A, (double)intercept_b, |
| 137 | + // (double)(R_bat_model - bat_temp.R_bat_model_old), (double)bat_temp.R_bat); |
| 138 | + |
| 139 | + /**** Calculate the 4 types of temperature change for the batteries ****/ |
| 140 | + |
| 141 | + // Calculate the batteries' own temperature change |
| 142 | + dT_bat_own = bat_temp.R_bat * I_bat * I_bat * sample_period * battery_power_boost / heat_cap_bat; |
| 143 | + |
| 144 | + // Calculate the batteries' heat loss to the electronics |
| 145 | + if (bat_temp.T_bat > bat_temp.T_elec) { |
| 146 | + dT_bat_loss_to_elec = K_bat_loss_to_elec * (bat_temp.T_bat - bat_temp.T_elec) * sample_period; |
| 147 | + } else { |
| 148 | + dT_bat_loss_to_elec = 0.0f; |
| 149 | + } |
| 150 | + |
| 151 | + // Calculate the batteries' heat gain from the electronics |
| 152 | + if (bat_temp.T_bat < bat_temp.T_elec) { |
| 153 | + dT_bat_gain_from_elec = K_bat_gain_from_elec * (bat_temp.T_elec - bat_temp.T_bat) * sample_period; |
| 154 | + } else { |
| 155 | + dT_bat_gain_from_elec = 0.0f; |
| 156 | + } |
| 157 | + |
| 158 | + // Calculate the batteries' heat loss to environment |
| 159 | + dT_bat_loss_to_room = K_bat_to_room * bat_temp.T_bat * sample_period; |
| 160 | + |
| 161 | + /**** Calculate the 4 types of temperature change for the electronics ****/ |
| 162 | + |
| 163 | + // Calculate the electronics' own temperature change |
| 164 | + dT_elec_own = K_elec_heat_slope * sample_period; |
| 165 | + |
| 166 | + // Calculate the electronics' heat loss to the batteries |
| 167 | + if (bat_temp.T_elec > bat_temp.T_bat) { |
| 168 | + dT_elec_loss_to_bat = K_elec_loss_to_bat * (bat_temp.T_elec - bat_temp.T_bat) * sample_period; |
| 169 | + } else { |
| 170 | + dT_elec_loss_to_bat = 0.0f; |
| 171 | + } |
| 172 | + |
| 173 | + // Calculate the electronics' heat gain from the batteries |
| 174 | + if (bat_temp.T_elec < bat_temp.T_bat) { |
| 175 | + dT_elec_gain_from_bat = K_elec_gain_from_bat * (bat_temp.T_bat - bat_temp.T_elec) * sample_period; |
| 176 | + } else { |
| 177 | + dT_elec_gain_from_bat = 0.0f; |
| 178 | + } |
| 179 | + |
| 180 | + // Calculate the electronics' heat loss to the environment |
| 181 | + dT_elec_loss_to_room = K_elec_to_room * bat_temp.T_elec * sample_period; |
| 182 | + |
| 183 | + /*****************************************************************************/ |
| 184 | + |
| 185 | + // pbdrv_uart_debug_printf("%f %f %f %f %f <> %f %f %f %f %f\r\n", |
| 186 | + // (double)dT_bat_own, (double)dT_bat_loss_to_elec, |
| 187 | + // (double)dT_bat_gain_from_elec, (double)dT_bat_loss_to_room, (double)bat_temp.T_bat, |
| 188 | + // (double)dT_elec_own, (double)dT_elec_loss_to_bat, (double)dT_elec_gain_from_bat, |
| 189 | + // (double)dT_elec_loss_to_room, (double)bat_temp.T_elec); |
| 190 | + |
| 191 | + // Refresh battery temperature |
| 192 | + bat_temp.T_bat += dT_bat_own + dT_bat_loss_to_elec + dT_bat_gain_from_elec + dT_bat_loss_to_room; |
| 193 | + |
| 194 | + // Refresh electronics temperature |
| 195 | + bat_temp.T_elec += dT_elec_own + dT_elec_loss_to_bat + dT_elec_gain_from_bat + dT_elec_loss_to_room; |
| 196 | + |
| 197 | + return bat_temp.T_bat; |
| 198 | +} |
| 199 | + |
| 200 | +#endif // PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION |
0 commit comments