mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Scripting: added BattEstimate lua script
this estimates state of charge from resting voltage while disarmed
This commit is contained in:
Andrew Tridgell
parent
3f4a6a23dd
commit
5722cb584d
282
libraries/AP_Scripting/applets/BattEstimate.lua
Normal file
282
libraries/AP_Scripting/applets/BattEstimate.lua
Normal file
@ -0,0 +1,282 @@
|
|||||||
|
--[[
|
||||||
|
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_SOC1_C3
|
||||||
|
// @DisplayName: Battery estimator coefficient3
|
||||||
|
// @Description: Battery estimator coefficient3
|
||||||
|
// @Range: 0.01 0.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_SOC2_C3
|
||||||
|
// @DisplayName: Battery estimator coefficient3
|
||||||
|
// @Description: Battery estimator coefficient3
|
||||||
|
// @Range: 0.01 0.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_SOC3_C3
|
||||||
|
// @DisplayName: Battery estimator coefficient3
|
||||||
|
// @Description: Battery estimator coefficient3
|
||||||
|
// @Range: 0.01 0.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
|
||||||
|
--]]
|
||||||
|
|
||||||
|
--[[
|
||||||
|
// @Param: BATT_SOC4_C3
|
||||||
|
// @DisplayName: Battery estimator coefficient3
|
||||||
|
// @Description: Battery estimator coefficient3
|
||||||
|
// @Range: 0.01 0.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)*5
|
||||||
|
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, 111.56)
|
||||||
|
params[i]['C2'] = bind_add_param(id .. "C2", pidx+3, 3.65)
|
||||||
|
params[i]['C3'] = bind_add_param(id .. "C3", pidx+4, 0.205)
|
||||||
|
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, c3)
|
||||||
|
local p0 = 80.0
|
||||||
|
local soc = c1*(1.0-1.0/(1+(cell_volt/c2)^p0)^c3)
|
||||||
|
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 C3 = params[i]['C3']: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, C3)
|
||||||
|
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
|
||||||
|
|
||||||
62
libraries/AP_Scripting/applets/BattEstimate.md
Normal file
62
libraries/AP_Scripting/applets/BattEstimate.md
Normal file
@ -0,0 +1,62 @@
|
|||||||
|
# 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
|
||||||
|
|
||||||
|
## BATT_SOCn_C3
|
||||||
|
|
||||||
|
C3 is the second coefficient from your fit of your battery
|
||||||
|
|
||||||
|
# Usage
|
||||||
|
|
||||||
|
You need to start by working out the coefficients C1, C2 and C3 for your
|
||||||
|
battery. You can do this by starting with a fully charged battery and
|
||||||
|
slowly discharging it with LOG_DISARMED set to 1. Alternatively you
|
||||||
|
can provide a CSV file with battery percentage in the first column and
|
||||||
|
voltage in the 2nd column.
|
||||||
|
|
||||||
|
Then run the resulting log or csv file 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=111.5629 C2=3.6577 C3=0.2048
|
||||||
|
|
||||||
|
Use the C1, C2 and C3 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.
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user