From 3f4a6a23dda7d7f1a19ad3000005172b5f3216d5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Jul 2023 09:06:27 +1000 Subject: [PATCH] Tools: added battery fitting script --- Tools/scripts/battery_fit.py | 192 +++++++++++++++++++++++++++++++++++ 1 file changed, 192 insertions(+) create mode 100755 Tools/scripts/battery_fit.py diff --git a/Tools/scripts/battery_fit.py b/Tools/scripts/battery_fit.py new file mode 100755 index 0000000000..9cc17e3878 --- /dev/null +++ b/Tools/scripts/battery_fit.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python +''' +fit coefficients for battery percentate from resting voltage + +See AP_Scripting/applets/BattEstimate.lua +''' + +from argparse import ArgumentParser +parser = ArgumentParser(description=__doc__) +parser.add_argument("--no-graph", action='store_true', default=False, help='disable graph display') +parser.add_argument("--num-cells", type=int, default=0, help='cell count, zero for auto-detection') +parser.add_argument("--batidx", type=int, default=1, help='battery index') +parser.add_argument("--condition", default=None, help='match condition') +parser.add_argument("--final-pct", type=float, default=100.0, help='set final percentage in log') +parser.add_argument("--comparison", type=str, default=None, help='comparison coefficients') +parser.add_argument("log", metavar="LOG") + +args = parser.parse_args() + +import sys +import math +from pymavlink import mavutil +import numpy as np +import matplotlib.pyplot as pyplot + +def constrain(value, minv, maxv): + """Constrain a value to a range.""" + return max(min(value,maxv),minv) + +def SOC_model(cell_volt, c): + '''simple model of state of charge versus resting voltage. + With thanks to Roho for the form of the equation + https://electronics.stackexchange.com/questions/435837/calculate-battery-percentage-on-lipo-battery + ''' + p0 = 80.0 + p1 = c[2] + return constrain(c[0]*(1.0-1.0/math.pow(1+math.pow(cell_volt/c[1],p0),p1)),0,100) + +def fit_batt(data): + '''fit a set of battery data to the SOC model''' + from scipy import optimize + + def fit_error(p): + p = list(p) + ret = 0 + for (voltR,pct) in data: + error = pct - SOC_model(voltR, p) + ret += abs(error) + + ret /= len(data) + return ret + + p = [123.0, 3.7, 0.165] + bounds = [(100.0, 10000.0), (3.0,3.9), (0.001, 0.4)] + + (p,err,iterations,imode,smode) = optimize.fmin_slsqp(fit_error, p, bounds=bounds, iter=10000, full_output=True) + if imode != 0: + print("Fit failed: %s" % smode) + sys.exit(1) + return p + +def ExtractDataLog(logfile): + '''find battery fit parameters from a log file''' + print("Processing log %s" % logfile) + mlog = mavutil.mavlink_connection(logfile) + + Wh_total = 0.0 + last_t = None + data = [] + last_voltR = None + + while True: + msg = mlog.recv_match(type=['BAT'], condition=args.condition) + if msg is None: + break + + if msg.get_type() == 'BAT' and msg.Instance == args.batidx-1 and msg.VoltR > 1: + current = msg.Curr + voltR = msg.VoltR + if last_voltR is not None and voltR > last_voltR: + continue + last_voltR = voltR + power = current*voltR + t = msg.TimeUS*1.0e-6 + + if last_t is None: + last_t = t + continue + + dt = t - last_t + if dt < 0.5: + # 2Hz data is plenty + continue + last_t = t + Wh_total += (power*dt)/3600.0 + + data.append((voltR,Wh_total)) + + if len(data) == 0: + print("No data found") + sys.exit(1) + + # calculate total pack capacity based on final percentage + Wh_max = data[-1][1]/(args.final_pct*0.01) + + fit_data = [] + + for i in range(len(data)): + (voltR,Wh) = data[i] + SOC = 100-100*Wh/Wh_max + fit_data.append((voltR, SOC)) + + print("Loaded %u samples" % len(data)) + return fit_data + +def ExtractDataCSV(logfile): + '''find battery fit parameters from a CSV file''' + print("Processing CSV %s" % logfile) + lines = open(logfile,'r').readlines() + fit_data = [] + for line in lines: + line = line.strip() + if line.startswith("#"): + continue + v = line.split(',') + if len(v) != 2: + continue + if not v[0][0].isnumeric() or not v[1][0].isnumeric(): + continue + fit_data.append((float(v[1]),float(v[0]))) + return fit_data + +def BattFit(fit_data, num_cells): + + fit_data = [ (v/num_cells,p) for (v,p) in fit_data ] + + c = fit_batt(fit_data) + print("Coefficients C1=%.4f C2=%.4f C3=%.4f" % (c[0], c[1], c[2])) + + if args.no_graph: + return + + fig, axs = pyplot.subplots() + + np_volt = np.array([v for (v,p) in fit_data]) + np_pct = np.array([p for (v,p) in fit_data]) + axs.invert_xaxis() + axs.plot(np_volt, np_pct, label='SOC') + np_rem = np.zeros(0,dtype=float) + + # pad down to 3.2V to make it easier to visualise for logs that don't go to a low voltage + low_volt = np_volt[-1] + while low_volt > 3.2: + low_volt -= 0.1 + np_volt = np.append(np_volt, low_volt) + + for i in range(np_volt.size): + voltR = np_volt[i] + np_rem = np.append(np_rem, SOC_model(voltR, c)) + + axs.plot(np_volt, np_rem, label='SOC Fit') + + if args.comparison: + c2 = args.comparison.split(',') + c2 = [ float(x) for x in c2 ] + np_rem2 = np.zeros(0,dtype=float) + for i in range(np_volt.size): + voltR = np_volt[i] + np_rem2 = np.append(np_rem2, SOC_model(voltR, c2)) + axs.plot(np_volt, np_rem2, label='SOC Fit2') + + axs.legend(loc='upper left') + axs.set_title('Battery Fit') + + pyplot.show() + +def get_cell_count(data): + if args.num_cells != 0: + return args.num_cells + volts = [ v[0] for v in data ] + volts = sorted(volts) + num_cells = round(volts[-1]/4.2) + print("Max voltags %.1f num_cells %u" % (volts[-1], num_cells)) + return num_cells + +if args.log.upper().endswith(".CSV"): + fit_data = ExtractDataCSV(args.log) +else: + fit_data = ExtractDataLog(args.log) + +num_cells = get_cell_count(fit_data) +BattFit(fit_data, num_cells)