mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
AP_Scripting: added get_cell_voltage() API and arming check example
this allows for cell imbalance to be checked as an arming check using a lua script
This commit is contained in:
parent
4d821b2c91
commit
a190dfe24a
@ -2718,6 +2718,12 @@ function battery:healthy(instance) end
|
||||
---@return integer
|
||||
function battery:num_instances() end
|
||||
|
||||
-- get individual cell voltage
|
||||
---@param instance integer
|
||||
---@param cell integer
|
||||
---@return number|nil
|
||||
function battery:get_cell_voltage(instance, cell) end
|
||||
|
||||
|
||||
-- desc
|
||||
---@class arming
|
||||
|
48
libraries/AP_Scripting/examples/cell_balance_check.lua
Normal file
48
libraries/AP_Scripting/examples/cell_balance_check.lua
Normal file
@ -0,0 +1,48 @@
|
||||
--[[
|
||||
script implementing pre-arm check that batteries are well balanced
|
||||
--]]
|
||||
|
||||
local MAX_CELL_DEVIATION = 0.2
|
||||
|
||||
local auth_id = arming:get_aux_auth_id()
|
||||
|
||||
local NCELLS_MAX = 16
|
||||
|
||||
function check_cell_balance(bnum)
|
||||
local min_volt = -1
|
||||
local max_volt = -1
|
||||
for c=0,NCELLS_MAX do
|
||||
local voltage = battery:get_cell_voltage(bnum, c)
|
||||
if not voltage then
|
||||
break
|
||||
end
|
||||
if min_volt == -1 or min_volt > voltage then
|
||||
min_volt = voltage
|
||||
end
|
||||
if max_volt == -1 or max_volt < voltage then
|
||||
max_volt = voltage
|
||||
end
|
||||
end
|
||||
local vdiff = max_volt - min_volt
|
||||
if vdiff > MAX_CELL_DEVIATION then
|
||||
arming:set_aux_auth_failed(auth_id, string.format("Batt[%u] imbalance %.1fV", bnum+1, vdiff))
|
||||
return false
|
||||
end
|
||||
return true
|
||||
end
|
||||
|
||||
function update()
|
||||
local num_batts = battery:num_instances()
|
||||
local ok = true
|
||||
for i=0,num_batts do
|
||||
if not check_cell_balance(i) then
|
||||
ok = false
|
||||
end
|
||||
end
|
||||
if ok then
|
||||
arming:set_aux_auth_passed(auth_id)
|
||||
end
|
||||
return update, 500
|
||||
end
|
||||
|
||||
return update()
|
@ -90,6 +90,7 @@ singleton AP_BattMonitor method overpower_detected boolean uint8_t 0 ud->num_ins
|
||||
singleton AP_BattMonitor method get_temperature boolean float'Null uint8_t 0 ud->num_instances()
|
||||
singleton AP_BattMonitor method get_cycle_count boolean uint8_t 0 ud->num_instances() uint16_t'Null
|
||||
singleton AP_BattMonitor method reset_remaining boolean uint8_t 0 ud->num_instances() float 0 100
|
||||
singleton AP_BattMonitor method get_cell_voltage boolean uint8_t'skip_check uint8_t'skip_check float'Null
|
||||
|
||||
include AP_GPS/AP_GPS.h
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user