diff --git a/inc/sp140/globals.h b/inc/sp140/globals.h index 3ff430c2..f3c9b8c2 100644 --- a/inc/sp140/globals.h +++ b/inc/sp140/globals.h @@ -32,6 +32,11 @@ uint32_t _eRPM = 0; uint16_t _inPWM = 0; uint16_t _outPWM = 0; +// Battery information +unsigned int cellsInSeries = 0; +unsigned int cellsInParallel = 0; +unsigned int exactCapacityWh = 0; + // ESC Telemetry float prevVolts = 0; float prevAmps = 0; diff --git a/inc/sp140/shared-config.h b/inc/sp140/shared-config.h index ce98ca70..b076f902 100644 --- a/inc/sp140/shared-config.h +++ b/inc/sp140/shared-config.h @@ -4,6 +4,7 @@ // Batt setting now configurable by user. Read from device data #define BATT_MIN_V 60.0 // 24 * 2.5V per cell +#define CELL_CAPACITY_WH 15.5 // Calibration #define MAMP_OFFSET 200 diff --git a/inc/sp140/voltage-curves.h b/inc/sp140/voltage-curves.h new file mode 100644 index 00000000..008dbb29 --- /dev/null +++ b/inc/sp140/voltage-curves.h @@ -0,0 +1,193 @@ +// Copyright 2023 +#ifndef INC_VOLTAGE_CURVES_H_ +#define INC_VOLTAGE_CURVES_H_ + +// Voltage curves at different currents for a single battery cell based on product data sheet +// Energy lookup table is total energy minus integral of voltage curve at each point +// Data from https://www.molicel.com/wp-content/uploads/INR21700P42A-V4-80092.pdf +typedef struct { + const float current; + const float *const voltageCurve; // voltage at voltageCurve[i] corresponds to discharge = i * 50 (mAh) + const float *const energyLookup; // remaining energy at specific voltage (Wh), same length as voltageCurve + const float totalEnergy; // total energy of battery cell (decreases at higher current due to heat loss) + const int numPoints; +} VoltageCurve; + +const float _0_84A_VOLTAGE_CURVE[] = { + 4.1746144, 4.129601, 4.1050863, 4.0857577, 4.077724, 4.0685687, + 4.064714, 4.056633, 4.0552087, 4.055017, 4.0469356, 4.0469356, + 4.042087, 4.0357985, 4.030712, 4.0195327, 4.0156155, 3.9969745, + 3.9816196, 3.9587073, 3.948181, 3.9325078, 3.924103, 3.911155, + 3.897928, 3.8837006, 3.87562, 3.867713, 3.857877, 3.8497498, + 3.8340952, 3.8255184, 3.8119152, 3.80546, 3.7931652, 3.7785158, + 3.7689712, 3.7608678, 3.7512062, 3.7349863, 3.7272768, 3.7122865, + 3.7043037, 3.6949084, 3.6784623, 3.670328, 3.6545455, 3.6460936, + 3.6355476, 3.6283426, 3.6202934, 3.6121814, 3.6047, 3.5960193, + 3.5862072, 3.57892, 3.5723896, 3.5621874, 3.5515337, 3.537837, + 3.5297542, 3.5216837, 3.5057023, 3.494145, 3.481332, 3.4650154, + 3.4535213, 3.4392476, 3.424676, 3.4166222, 3.406926, 3.3977017, + 3.3859127, 3.367578, 3.3354974, 3.3067477, 3.269035, 3.233233, + 3.1855989, 3.1283264, 3.0746572, 3.0079076, 2.935368, 2.8415823, + 2.6789198 +}; +const float _0_84A_ENERGY_LOOKUP[] = { + 15.464685, 15.25708, 15.051212, 14.846441, 14.642355, 14.438697, + 14.235365, 14.032331, 13.8295355, 13.62678, 13.424231, 13.221884, + 13.019659, 12.817712, 12.616049, 12.414793, 12.213914, 12.013599, + 11.814135, 11.615626, 11.4179535, 11.220937, 11.024522, 10.82864, + 10.633413, 10.438872, 10.244889, 10.051306, 9.858166, 9.665476, + 9.473379, 9.281889, 9.090953, 8.900518, 8.710553, 8.521261, + 8.332574, 8.144328, 7.956526, 7.769371, 7.5828147, 7.396826, + 7.211411, 7.0264306, 6.8420963, 6.6583767, 6.4752545, 6.2927384, + 6.1106977, 5.9291005, 5.7478843, 5.5670724, 5.3866506, 5.2066326, + 5.0270767, 4.8479486, 4.669166, 4.490802, 4.3129587, 4.1357245, + 3.9590344, 3.7827487, 3.607064, 3.4320679, 3.257681, 3.084022, + 2.9110587, 2.7387395, 2.5671415, 2.3961089, 2.2255204, 2.0554047, + 1.8858142, 1.716977, 1.5494001, 1.3833439, 1.2189493, 1.0563927, + 0.8959219, 0.73807377, 0.58299917, 0.43093503, 0.28532478, 0.13801256, + 0.0 +}; + +const float _4_2A_VOLTAGE_CURVE[] = { + 4.1164317, 4.0588408, 4.0305204, 4.0125675, 3.9968245, 3.9968238, + 3.9823055, 3.9766607, 3.9766607, 3.9677424, 3.9645758, 3.9612777, + 3.9612777, 3.9529183, 3.9418828, 3.9321654, 3.9268913, 3.9071343, + 3.891789, 3.8756242, 3.8659148, 3.8509874, 3.8420107, 3.829988, + 3.8176718, 3.8061042, 3.7971275, 3.7881508, 3.7705674, 3.7705674, + 3.767335, 3.749557, 3.7443159, 3.7306695, 3.7253141, 3.7073607, + 3.6983843, 3.689573, 3.6714542, 3.6624775, 3.6524806, 3.6445243, + 3.626571, 3.6175942, 3.5996408, 3.5927868, 3.5879018, 3.5733933, + 3.5636306, 3.552243, 3.5427706, 3.5283515, 3.5216744, 3.5168397, + 3.5071297, 3.492583, 3.484476, 3.4808502, 3.472109, 3.4521806, + 3.4473376, 3.4408634, 3.4360166, 3.4264388, 3.4093611, 3.398861, + 3.38599, 3.384201, 3.3662477, 3.3503585, 3.3503585, 3.3419952, + 3.3213644, 3.3117821, 3.2858427, 3.256723, 3.218311, 3.1882393, + 3.1403384, 3.085144, 3.0270107, 2.9308786, 2.8269744, 2.6391382, + 2.3249552 +}; +const float _4_2A_ENERGY_LOOKUP[] = { + 15.13936, 14.9349785, 14.732744, 14.53971, 14.331466, 14.131624, + 13.932146, 13.741131, 13.530368, 13.335731, 13.137423, 12.939276, + 12.741212, 12.543357, 12.345987, 12.149136, 11.95266, 11.756809, + 11.561836, 11.367651, 11.174112, 10.98119, 10.777325, 10.5970335, + 10.405842, 10.215247, 10.032769, 9.831759, 9.646571, 9.458043, + 9.269595, 9.081673, 8.894326, 8.707452, 8.521052, 8.327803, + 8.153768, 7.9653745, 7.7850294, 7.59068, 7.415121, 7.225399, + 7.050893, 6.8589225, 6.689317, 6.5095067, 6.3299894, 6.150957, + 5.9725313, 5.7946343, 5.617259, 5.440481, 5.2642307, 5.088268, + 4.912668, 4.7376757, 4.563249, 4.389116, 4.215292, 4.042185, + 3.8696969, 3.6974916, 3.5255697, 3.3540084, 3.1831133, 3.0129077, + 2.8432865, 2.6740317, 2.4951448, 2.3373046, 2.1697867, 2.002478, + 1.8425572, 1.6700954, 1.5051548, 1.3415906, 1.1797148, 1.019551, + 0.8613366, 0.70569956, 0.55289567, 0.40394846, 0.2600021, 0.13154846, + 0.0 +}; + +const float _10A_VOLTAGE_CURVE[] = { + 4.021544, 3.937097, 3.91284, 3.8982913, 3.8898132, 3.8852828, + 3.8788521, 3.876952, 3.8705826, 3.8691893, 3.8626902, 3.859458, + 3.852993, 3.846527, 3.8432128, 3.8322246, 3.8240573, 3.8075285, + 3.7984154, 3.785114, 3.7718325, 3.7592669, 3.7446897, 3.7320395, + 3.7163374, 3.7107694, 3.7022507, 3.6930342, 3.6816804, 3.6748397, + 3.6647232, 3.6542938, 3.6445243, 3.6366048, 3.6218748, 3.6119523, + 3.6015162, 3.5879357, 3.5782738, 3.5686371, 3.5547576, 3.5393915, + 3.5297632, 3.5167475, 3.5055048, 3.4908752, 3.481271, 3.4715717, + 3.461801, 3.452213, 3.439249, 3.4278316, 3.4198546, 3.4118223, + 3.4020767, 3.3956628, 3.3874347, 3.3778198, 3.3688047, 3.3584354, + 3.3487458, 3.3383005, 3.3292768, 3.3196783, 3.312019, 3.296985, + 3.2873223, 3.2773812, 3.2634811, 3.2469163, 3.2388206, 3.2198048, + 3.2077317, 3.183698, 3.1611385, 3.132227, 3.1001198, 3.0654805, + 3.0111265, 2.9529216, 2.881508, 2.7997704, 2.6882768, 2.5103097 +}; +const float _10A_ENERGY_LOOKUP[] = { + 14.580459, 14.381493, 14.185244, 13.989965, 13.795263, 13.600885, + 13.406782, 13.212887, 13.019198, 12.825705, 12.632407, 12.439354, + 12.246543, 12.054054, 11.861811, 11.669925, 11.470861, 11.2877035, + 11.097555, 10.907967, 10.719043, 10.530766, 10.343166, 10.156248, + 9.958866, 9.784329, 9.599004, 9.414122, 9.229754, 9.045841, + 8.862352, 8.679377, 8.500555, 8.314887, 8.133425, 7.952579, + 7.772242, 7.592506, 7.4133506, 7.234678, 7.056593, 6.8792396, + 6.7025104, 6.5263476, 6.3507915, 6.175882, 6.0015783, 5.8277574, + 5.6544228, 5.4815726, 5.309286, 5.137609, 4.966417, 4.7956247, + 4.6252775, 4.455334, 4.2857566, 4.1166253, 3.9479597, 3.7797785, + 3.612099, 3.444923, 3.2782335, 3.1120095, 2.946217, 2.780992, + 2.6163843, 2.4522667, 2.2887452, 2.1259854, 1.9638418, 1.8023763, + 1.6416878, 1.481902, 1.3232812, 1.165947, 1.0101384, 0.85599834, + 0.70408314, 0.55498195, 0.41203845, 0.26716584, 0.12996466, 0.0 +}; + +const float _20A_VOLTAGE_CURVE[] = { + 3.852993, 3.775158, 3.744758, 3.7350113, 3.7285466, 3.72854, + 3.7204657, 3.7204657, 3.7204657, 3.7188494, 3.7107677, 3.7075791, + 3.7075343, 3.7026875, 3.6946077, 3.6899178, 3.678311, 3.6703641, + 3.66067, 3.6541235, 3.6380396, 3.6283426, 3.6186485, 3.605716, + 3.5952039, 3.5847173, 3.5752053, 3.564258, 3.5540614, 3.5475338, + 3.5378373, 3.528076, 3.5169456, 3.5076632, 3.4973578, 3.4846323, + 3.4739864, 3.4635506, 3.448868, 3.440853, 3.4298346, 3.4150314, + 3.4045706, 3.392419, 3.3826787, 3.3729918, 3.360417, 3.3503585, + 3.336853, 3.3332775, 3.322917, 3.3134315, 3.301752, 3.2921739, + 3.2824845, 3.2759678, 3.2681108, 3.2598548, 3.2501547, 3.2436962, + 3.233997, 3.2244403, 3.2131405, 3.20312, 3.194052, 3.1855073, + 3.1758103, 3.1677964, 3.1531954, 3.1417575, 3.1273284, 3.1111548, + 3.096621, 3.0721636, 3.0486014, 3.0207028, 2.9880097, 2.956401, + 2.9111023, 2.8618124, 2.8022275, 2.728905, 2.6301615, 2.4685817 +}; +const float _20A_ENERGY_LOOKUP[] = { + 14.069057, 13.878353, 13.690355, 13.503362, 13.316772, 13.130345, + 12.94412, 12.758097, 12.572067, 12.386079, 12.200338, 12.014879, + 11.829502, 11.644246, 11.459313, 11.2747, 11.090495, 10.906778, + 10.723502, 10.540632, 10.358328, 10.176669, 9.995494, 9.814885, + 9.634862, 9.455364, 9.276365, 9.097879, 8.919921, 8.742381, + 8.565247, 8.388599, 8.212474, 8.036859, 7.861733, 7.6871834, + 7.5132174, 7.3397794, 7.166969, 6.9947257, 6.8229585, 6.651837, + 6.481347, 6.3114223, 6.1420445, 5.973153, 5.8048177, 5.6370482, + 5.469868, 5.303115, 5.1367097, 4.9708014, 4.805422, 4.6405735, + 4.476207, 4.312246, 4.1486435, 3.9854445, 3.8226943, 3.660348, + 3.4984057, 3.3369448, 3.1760054, 3.0155988, 2.8556695, 2.6961803, + 2.5371475, 2.3785574, 2.2205327, 2.0631588, 1.9064316, 1.7504694, + 1.595275, 1.4410554, 1.2880363, 1.1363038, 0.98608595, 0.83747566, + 0.6907881, 0.5464652, 0.40486422, 0.26105475, 0.1351167, 0.0 +}; + +const float _30A_VOLTAGE_CURVE[] = { + 3.7204657, 3.619926, 3.5960522, 3.587938, 3.586322, 3.586322, + 3.586322, 3.586322, 3.586322, 3.586322, 3.5798573, 3.5798569, + 3.5750086, 3.5717764, 3.5636954, 3.558968, 3.5568585, 3.5459173, + 3.537837, 3.529765, 3.5216742, 3.5119746, 3.5022857, 3.4893508, + 3.4805644, 3.47158, 3.4580796, 3.4473636, 3.4392488, 3.4311678, + 3.4168158, 3.4133122, 3.400481, 3.3914967, 3.3826516, 3.3713946, + 3.3584738, 3.3487375, 3.3405216, 3.3276732, 3.32171, 3.3083696, + 3.2961507, 3.2840946, 3.2760134, 3.2664099, 3.253803, 3.2436764, + 3.231598, 3.2277014, 3.2178311, 3.2081788, 3.1968985, 3.1920352, + 3.182406, 3.176502, 3.1658943, 3.1613383, 3.15141, 3.143487, + 3.1355228, 3.1273243, 3.1192489, 3.1076612, 3.096152, 3.08692, + 3.0787444, 3.0652418, 3.0528855, 3.0367794, 3.0188894, 3.0065687, + 2.9867172, 2.9641163, 2.9429958, 2.905992, 2.8766382, 2.8398225, + 2.7962463, 2.7378814, 2.6820629, 2.5946772, 2.4775584 +}; +const float _30A_ENERGY_LOOKUP[] = { + 13.46207, 13.278561, 13.098161, 12.918561, 12.739204, 12.559889, + 12.380572, 12.201257, 12.018354, 11.842625, 11.66347, 11.484477, + 11.305605, 11.126936, 10.948549, 10.770482, 10.5925865, 10.415017, + 10.237924, 10.0612335, 9.884948, 9.709106, 9.53375, 9.358959, + 9.1847105, 9.010907, 8.837666, 8.66503, 8.492865, 8.321104, + 8.149904, 7.9791512, 7.8088064, 7.639007, 7.469653, 7.300802, + 7.1325555, 6.964875, 6.7976437, 6.6309385, 6.464704, 6.298952, + 6.133839, 5.969333, 5.8053303, 5.64177, 5.4787645, 5.3163276, + 5.1544456, 4.992963, 4.831825, 4.6711745, 4.5110474, 4.351324, + 4.191963, 4.0329905, 3.8744307, 3.7162497, 3.5584311, 3.4010587, + 3.2440834, 3.0875123, 2.9313478, 2.775675, 2.6205797, 2.466003, + 2.3118613, 2.1582618, 2.0053086, 1.8530669, 1.7016752, 1.5510387, + 1.4012066, 1.2524357, 1.1047579, 0.9585332, 0.81396747, 0.671056, + 0.5301542, 0.38626695, 0.25618827, 0.12426977, 0.0 +}; + +const VoltageCurve VOLTAGE_CURVES[] = { + {0.84, _0_84A_VOLTAGE_CURVE, _0_84A_ENERGY_LOOKUP, 15.464685, 85}, + {4.2, _4_2A_VOLTAGE_CURVE, _4_2A_ENERGY_LOOKUP, 15.13936, 85}, + {10, _10A_VOLTAGE_CURVE, _10A_ENERGY_LOOKUP, 14.580459, 84}, + {20, _20A_VOLTAGE_CURVE, _20A_ENERGY_LOOKUP, 14.069045, 84}, + {30, _30A_VOLTAGE_CURVE, _30A_ENERGY_LOOKUP, 13.46207, 83} +}; + +#endif // INC_VOLTAGE_CURVES_H diff --git a/src/sp140/power.ino b/src/sp140/power.ino index 74b753a8..b31bb57d 100644 --- a/src/sp140/power.ino +++ b/src/sp140/power.ino @@ -1,33 +1,63 @@ // Copyright 2021 // OpenPPG -// simple set of data points from load testing -// maps voltage to battery percentage +#include "../../inc/sp140/voltage-curves.h" +#include "../../inc/sp140/globals.h" + +// estimate remaining battery percent using cell manufacturer's voltage curves float getBatteryPercent(float voltage) { - float battPercent = 0; - - if (voltage > 94.8) { - battPercent = mapd(voltage, 94.8, 99.6, 90, 100); - } else if (voltage > 93.36) { - battPercent = mapd(voltage, 93.36, 94.8, 80, 90); - } else if (voltage > 91.68) { - battPercent = mapd(voltage, 91.68, 93.36, 70, 80); - } else if (voltage > 89.76) { - battPercent = mapd(voltage, 89.76, 91.68, 60, 70); - } else if (voltage > 87.6) { - battPercent = mapd(voltage, 87.6, 89.76, 50, 60); - } else if (voltage > 85.2) { - battPercent = mapd(voltage, 85.2, 87.6, 40, 50); - } else if (voltage > 82.32) { - battPercent = mapd(voltage, 82.32, 85.2, 30, 40); - } else if (voltage > 80.16) { - battPercent = mapd(voltage, 80.16, 82.32, 20, 30); - } else if (voltage > 78) { - battPercent = mapd(voltage, 78, 80.16, 10, 20); - } else if (voltage > 60.96) { - battPercent = mapd(voltage, 60.96, 78, 0, 10); + float current = telemetryData.amps; + const float temperature = ambientTempC; + // calculate cell voltage and current (assume evenly distributed) + voltage = voltage / cellsInSeries; + current = current / cellsInParallel; + + // voltage curves measured at 23C - transpose curve by adding cold temperature correction term + // voltage has greater drop rate when <0C than between 0C and 10C + if (temperature < 0) { + // 8.4% voltage drop from 23C to -20C + voltage = voltage * mapd(temperature, 23, -20, 1, 1.084); + } + else if (temperature < 10) { + // 3.5% voltage drop from 23C to 0C + voltage = voltage * mapd(temperature, 23, 0, 1, 1.035); + } + + // find the two voltage curves with the closest current + const int numCurves = sizeof(VOLTAGE_CURVES) / sizeof(*VOLTAGE_CURVES); + const VoltageCurve *lower = VOLTAGE_CURVES; + const VoltageCurve *higher = VOLTAGE_CURVES + 1; + const VoltageCurve *end = VOLTAGE_CURVES + numCurves - 1; // ensure both pointers are valid + while (higher != end && current > higher->current) { + lower++; + higher++; + } + + // interpolate between the two curves + float energyLowerCurrent = getEnergy(voltage, lower->voltageCurve, lower->energyLookup, lower->numPoints); + float energyHigherCurrent = getEnergy(voltage, higher->voltageCurve, higher->energyLookup, higher->numPoints); + float remainingEnergy = mapd(current, lower->current, higher->current, energyLowerCurrent, energyHigherCurrent); + float totalEnergy = mapd(current, lower->current, higher->current, lower->totalEnergy, higher->totalEnergy); + return constrain(remainingEnergy / totalEnergy * 100, 0, 100); +} + + +// given a voltage, voltage curve, energy lookup, and length of curve, return the energy (Wh) +float getEnergy(float voltage, const float *voltageCurve, const float *energyLookup, const int len) { + // find the closest voltage in the curve (voltages are in descending order) + const float *higherVoltage = voltageCurve; + const float *lowerVoltage = voltageCurve + 1; + const float *higherEnergy = energyLookup; + const float *lowerEnergy = energyLookup + 1; + const float *end = voltageCurve + len - 1; // ensure both pointers are valid + while (lowerVoltage != end && voltage < *lowerVoltage) { + higherVoltage++; + lowerVoltage++; + higherEnergy++; + lowerEnergy++; } - return constrain(battPercent, 0, 100); + // remove remaining energy in between both data points + return mapd(voltage, *higherVoltage, *lowerVoltage, *higherEnergy, *lowerEnergy); } diff --git a/src/sp140/sp140-helpers.ino b/src/sp140/sp140-helpers.ino index b9adfff0..8f2bde31 100644 --- a/src/sp140/sp140-helpers.ino +++ b/src/sp140/sp140-helpers.ino @@ -1,4 +1,5 @@ // Copyright 2020 +#include "../../inc/sp140/shared-config.h" // track flight timer void handleFlightTime() { @@ -440,12 +441,31 @@ int limitedThrottle(int current, int last, int threshold) { // ring buffer for voltage readings float getBatteryVoltSmoothed() { float avg = 0.0; - - if (voltageBuffer.isEmpty()) { return avg; } - using index_t = decltype(voltageBuffer)::index_t; for (index_t i = 0; i < voltageBuffer.size(); i++) { avg += voltageBuffer[i] / voltageBuffer.size(); } return avg; } + +float getBatteryPercentSmoothed() { + float avg = 0.0; + using index_t = decltype(batteryPercentBuffer)::index_t; + for (index_t i = 0; i < batteryPercentBuffer.size(); i++) { + avg += batteryPercentBuffer[i] / batteryPercentBuffer.size(); + } + return avg; +} + +// Update battery information +void updateBatteryInfo() { + cellsInSeries = 24; + if (deviceData.batt_size == 2000) { + cellsInParallel = 6; + } + // default to battery size of 4000Wh + else { + cellsInParallel = 10; + } + exactCapacityWh = cellsInSeries * cellsInParallel * CELL_CAPACITY_WH; +} diff --git a/src/sp140/sp140.ino b/src/sp140/sp140.ino index a1e99281..0c6cd63b 100644 --- a/src/sp140/sp140.ino +++ b/src/sp140/sp140.ino @@ -59,6 +59,7 @@ ButtonConfig* buttonConfig = button_top.getButtonConfig(); #endif CircularBuffer voltageBuffer; +CircularBuffer batteryPercentBuffer; CircularBuffer potBuffer; Thread ledBlinkThread = Thread(); @@ -131,6 +132,7 @@ void setup() { #endif refreshDeviceData(); setup140(); + updateBatteryInfo(); #ifdef M0_PIO Watchdog.reset(); #endif @@ -392,7 +394,9 @@ void updateDisplay() { display.setTextColor(BLACK); float avgVoltage = getBatteryVoltSmoothed(); - batteryPercent = getBatteryPercent(avgVoltage); // multi-point line + float currentBatteryPercent = getBatteryPercent(avgVoltage); + batteryPercentBuffer.push(currentBatteryPercent); + batteryPercent = getBatteryPercentSmoothed(); // change battery color based on charge int batt_width = map((int)batteryPercent, 0, 100, 0, 108); display.fillRect(0, 0, batt_width, 36, batt2color(batteryPercent));