AP_Scripting: added BattEstimate lua script

this estimates state of charge from resting voltage while disarmed

libraries/AP_Scripting/applets/BattEstimate.lua

@@ -0,0 +1,249 @@
+--[[
+   battery state of charge (SOC) estimator based on resting voltage
+
+   See Tools/scripts/battery_fit.py for a tool to calculate the coefficients from a log
+--]]
+
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+
+local PARAM_TABLE_KEY = 14
+local PARAM_TABLE_PREFIX = "BATT_SOC"
+
+-- bind a parameter to a variable
+function bind_param(name)
+   local p = Parameter()
+   assert(p:init(name), string.format('could not find %s parameter', name))
+   return p
+end
+
+-- add a parameter and bind it to a variable
+function bind_add_param(name, idx, default_value)
+   assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+   return bind_param(PARAM_TABLE_PREFIX .. name)
+end
+
+-- setup quicktune specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 32), 'could not add param table')
+
+--[[
+  // @Param: BATT_SOC_COUNT
+  // @DisplayName: Count of SOC estimators
+  // @Description: Number of battery SOC estimators
+  // @Range: 0 4
+  // @User: Standard
+--]]
+local BATT_SOC_COUNT     = bind_add_param('_COUNT', 1, 0)
+
+if BATT_SOC_COUNT:get() <= 0 then
+   return
+end
+
+--[[
+  // @Param: BATT_SOC1_IDX
+  // @DisplayName: Battery estimator index
+  // @Description: Battery estimator index
+  // @Range: 0 4
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC1_NCELL
+  // @DisplayName: Battery estimator cell count
+  // @Description: Battery estimator cell count
+  // @Range: 0 48
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC1_C1
+  // @DisplayName: Battery estimator coefficient1
+  // @Description: Battery estimator coefficient1
+  // @Range: 100 200
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC1_C2
+  // @DisplayName: Battery estimator coefficient2
+  // @Description: Battery estimator coefficient2
+  // @Range: 2 5
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC2_IDX
+  // @DisplayName: Battery estimator index
+  // @Description: Battery estimator index
+  // @Range: 0 4
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC2_NCELL
+  // @DisplayName: Battery estimator cell count
+  // @Description: Battery estimator cell count
+  // @Range: 0 48
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC2_C1
+  // @DisplayName: Battery estimator coefficient1
+  // @Description: Battery estimator coefficient1
+  // @Range: 100 200
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC2_C2
+  // @DisplayName: Battery estimator coefficient2
+  // @Description: Battery estimator coefficient2
+  // @Range: 2 5
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC3_IDX
+  // @DisplayName: Battery estimator index
+  // @Description: Battery estimator index
+  // @Range: 0 4
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC3_NCELL
+  // @DisplayName: Battery estimator cell count
+  // @Description: Battery estimator cell count
+  // @Range: 0 48
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC3_C1
+  // @DisplayName: Battery estimator coefficient1
+  // @Description: Battery estimator coefficient1
+  // @Range: 100 200
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC3_C2
+  // @DisplayName: Battery estimator coefficient2
+  // @Description: Battery estimator coefficient2
+  // @Range: 2 5
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC4_IDX
+  // @DisplayName: Battery estimator index
+  // @Description: Battery estimator index
+  // @Range: 0 4
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC4_NCELL
+  // @DisplayName: Battery estimator cell count
+  // @Description: Battery estimator cell count
+  // @Range: 0 48
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC4_C1
+  // @DisplayName: Battery estimator coefficient1
+  // @Description: Battery estimator coefficient1
+  // @Range: 100 200
+  // @User: Standard
+--]]
+
+--[[
+  // @Param: BATT_SOC4_C2
+  // @DisplayName: Battery estimator coefficient2
+  // @Description: Battery estimator coefficient2
+  // @Range: 2 5
+  // @User: Standard
+--]]
+
+local params = {}
+local last_armed_ms = 0
+
+--[[
+   add parameters for an estimator
+--]]
+function add_estimator(i)
+   id = string.format("%u_", i)
+   pidx = 2+(i-1)*4
+   params[i] = {}
+   params[i]['IDX']   = bind_add_param(id .. "IDX",     pidx+0, 0)
+   params[i]['NCELL'] = bind_add_param(id .. "NCELL", pidx+1, 0)
+   params[i]['C1']    = bind_add_param(id .. "C1", pidx+2, 120.0)
+   params[i]['C2']    = bind_add_param(id .. "C2", pidx+3, 3.65)
+end
+
+local count = math.floor(BATT_SOC_COUNT:get())
+for i = 1, count do
+   add_estimator(i)
+end
+
+local function constrain(v, vmin, vmax)
+   return math.max(math.min(v, vmax), vmin)
+end
+
+--[[
+   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
+--]]
+local function SOC_model(cell_volt, c1, c2)
+    local p0 = 80.0
+    local p1 = 0.165
+    local soc = c1*(1.0-1.0/(1+(cell_volt/c2)^p0)^p1)
+    return constrain(soc, 0, 100)
+end
+
+--[[
+   update one estimator
+--]]
+local function update_estimator(i)
+   local idx = math.floor(params[i]['IDX']:get())
+   local ncell = math.floor(params[i]['NCELL']:get())
+   if idx <= 0 or ncell <= 0 then
+      return
+   end
+   local C1 = params[i]['C1']:get()
+   local C2 = params[i]['C2']:get()
+   local num_batts = battery:num_instances()
+   if idx > num_batts then
+      return
+   end
+   local voltR = battery:voltage_resting_estimate(idx-1)
+   local soc = SOC_model(voltR/ncell, C1, C2)
+   battery:reset_remaining(idx-1, soc)
+end
+
+--[[
+   main update function, called at 1Hz
+--]]
+function update()
+   local now_ms = millis()
+   if arming:is_armed() then
+      last_armed_ms = now_ms
+      return update, 1000
+   end
+   -- don't update for 10s after disarm, to get logging of charge recovery
+   if now_ms - last_armed_ms < 10000 then
+      return update, 1000
+   end
+   for i = 1, #params do
+      update_estimator(i)
+   end
+   return update, 1000
+end
+
+gcs:send_text(MAV_SEVERITY.INFO, string.format("Loaded BattEstimate for %u batteries", #params))
+
+-- start running update loop
+return update, 1000
+

libraries/AP_Scripting/applets/BattEstimate.md

@@ -0,0 +1,56 @@
+# Battery State of Charge Estimator
+
+This script implements a battery state of charge estimator based on
+resting voltage and a simple LiPo cell model.
+
+This allows the remaining battery percentage to be automatically set
+based on the resting voltage when disarmed.
+
+# Parameters
+
+You will need to start by setting BATT_SOC_COUNT to the number of
+estimators you want (how many batteries you want to do SoC estimation
+for).
+
+Then you should restart scripting or reboot and set the following
+parameters per SoC estimator.
+
+## BATT_SOCn_IDX
+
+The IDX is the battery index, starting at 1.
+
+## BATT_SOCn_NCELL
+
+Set the number of cells in your battery in the NCELL parameter
+
+## BATT_SOCn_C1
+
+C1 is the first coefficient from your fit of your battery
+
+## BATT_SOCn_C2
+
+C2 is the second coefficient from your fit of your battery
+
+# Usage
+
+You need to start by working out the coefficients C1 and C2 for your
+battery. You can do this by starting with a fully charged battery and
+slowly discharging it with LOG_DISARMED set to 1.
+
+Then run the resulting log through the script at
+Tools/scripts/battery_fit.py. You will need to tell the script the
+following:
+
+ - the number of cells
+ - the final percentage charge your log stops at
+ - the battery index you want to fit to (1 is the first battery)
+
+That will produce a graph and a set of coefficients like this:
+ - Coefficients C1=120.342 C2=3.643
+
+Use the C1 and C2 parameters in the parameters for this script.
+
+The remaining battery percentage is only set when disarmed, and won't
+be set till 10 seconds after you disarm from a flight.
+
+