BattMonitor: add scripting backend

[robertlong13]

This PR adds a lua backend for battery monitors, opening up a lot more flexibility for custom PMU configurations. This has been on my to-do list for a while for making custom simulated power systems in SITL, but I finally decided to go for it when someone asked a question about custom battery monitor drivers on Discord.

This is by far my largest-scope PR for ArduPilot so far, so I expect this will take some massaging to get it in-line with best practices.

I mostly tried to follow the same ideas that went into the EFI scripting backend. A couple of notable differences:

1. I created a new state structure for scripting state. There are things in the existing BattMonitor_State that I don't need, and some things I needed to add to it. There were other ways to do this probably, but this felt cleanest.
2. There are other existing bindings for AP_BattMonitor, unlike how it was for AP_EFI, so I can't add a depends flag to remove the binding for get_backend if HAL_BATTMON_SCRIPTING_ENABLED is not set.
3. There is a lot of extra handling in place for optional fields.

[robertlong13]

This is an example driver, simulating a use-case where someone has 4 separate DC-to-DC converters, and implements 5 battery monitors. 1 aggregate monitor for the operator, and 4 individual monitors for logging/diagnosing problems. I'll need to add an example driver in the PR eventually, but I don't know if it should be this exact one, so I've left it out for now:

--[[ 
  BattMonitor Scripting backend example

  Set the following parameters to use this script:
    BATT_MONITOR 29
    BATT3_MONITOR 29
    BATT4_MONITOR 29
    BATT5_MONITOR 29
    BATT6_MONITOR 29
--]]

-- Check Script uses a miniumum firmware version
local SCRIPT_AP_VERSION = 4.5
local SCRIPT_NAME       = "BattMonitor: Example"

local VERSION = FWVersion:major() + (FWVersion:minor() * 0.1)

assert(VERSION >= SCRIPT_AP_VERSION, string.format('%s Requires: %s:%.1f. Found Version: %s', SCRIPT_NAME, FWVersion:type(), SCRIPT_AP_VERSION, VERSION))

local MAV_SEVERITY_ERROR = 3

local UPDATE_RATE = 5.0 -- Hz

function get_time_sec()
    return millis():tofloat() * 0.001
end

function get_voltage(idx)
    return 5.4 + 0.2 * math.random()
end

function get_current_amps(idx)
    return 1.1 + 0.05 * math.random()
end

function get_temperature(idx)
    return 35 + 0.2 * math.random()
end

-- Aggregate battery state, mins/maxes of all batteries
-- Uses BATTERY_MONITOR 1
backend_combined = battery:get_backend(0)
state_combined = BattMonitorScript_State()

-- Individual battery states, for logging
-- Use BATTERY_MONITORs 3, 4, 5, 6 
-- (if we use #2, it will show on the Mission Planner HUD, which we don't want)
backends = {}
states = {}

for i=1,4 do
    backends[i] = battery:get_backend(i+1) -- Starting from monitor 3 (index 2)
    states[i] = BattMonitorScript_State()
end

function update()
    local min_voltage = 1000 -- large number
    local total_current_amps = 0
    local max_temperature = -1000 -- small number
    
    -- Read voltage, current, and temperature from the 4 devices
    for i=1,4 do
        local voltage = get_voltage(i)
        local current_amps = get_current_amps(i)
        local temperature = get_temperature(i)

        -- update aggregate battery stat calculations
        min_voltage = math.min(min_voltage, voltage)
        total_current_amps = total_current_amps + current_amps
        max_temperature = math.max(max_temperature, temperature)

        -- update individual battery state
        states[i]:healthy(true)
        states[i]:voltage(voltage)
        states[i]:current_amps(current_amps)
        states[i]:temperature(temperature)
        states[i]:last_update_us(uint32_t(micros()))
    end

    -- update aggregate battery state
    state_combined:healthy(true)
    state_combined:voltage(min_voltage)
    state_combined:current_amps(total_current_amps)
    state_combined:temperature(max_temperature)
    state_combined:last_update_us(uint32_t(micros()))

    -- uncomment to test the failsafe and mavlink_faults fields
    -- after one minute, trigger a failsafe
    -- if get_time_sec() > 60 then
    --     state_combined:failsafe(1)
    --     state_combined:mavlink_faults(state_combined:mavlink_faults() | 16) -- set MAV_BATTERY_FAULT_OVER_TEMPERATURE
    -- end
    -- if get_time_sec() > 120 then
    --     state_combined:failsafe(2)
    --     state_combined:mavlink_faults(state_combined:mavlink_faults() & ~16) -- clear MAV_BATTERY_FAULT_OVER_TEMPERATURE
    --     state_combined:mavlink_faults(state_combined:mavlink_faults() | 32) -- set MAV_BATTERY_FAULT_UNDER_TEMPERATURE
    -- end

    -- update the backends
    for i=1,4 do
        backends[i]:handle_scripting(states[i])
    end
    backend_combined:handle_scripting(state_combined)
end

gcs:send_text(0, SCRIPT_NAME .. string.format(" loaded"))

-- wrapper around update(). This calls update() and if update faults
-- then an error is displayed, but the script is not stopped
function protected_wrapper()
    local success, err = pcall(update)
    if not success then
        gcs:send_text(MAV_SEVERITY_ERROR, "Internal Error: " .. err)
        -- when we fault we run the update function again after 1s, slowing it
        -- down a bit so we don't flood the console with errors
        return protected_wrapper, 1000
    end
    return protected_wrapper, 1000 / UPDATE_RATE
end

-- start running update loop
return protected_wrapper()

[IamPete1]

We have wanted this for ages! Thanks. Take a look at the way the EFI scripting backend works. The script builds a structure and sets the whole thing in one go. You would add the BattMonitor_State to scripting as a userdata and then add a virtual set state method to the battery monitor backed which only the scripting one would implement.

[robertlong13]

Thanks for the reply, Peter, I hope I understand you correctly,

I do have a virtual set state method (called handle_scripting) that is only implemented by the scripting backend, just like AP_EFI. With the EFI one, it copies the state passed in from scripting into a backend state, then immediately copies the backend state to the frontend state. Mine copies the passed state into the backend, then the backend state is copied to the frontend in an overridden read method (like the UAVCAN battery backend does; I think this is the right way to handle it with the way the parent battery monitor backend is currently written).

The reason I didn't use the existing BattMonitor_State is that there are extra things I need to pass into the Scripting backend (the battery monitor backend is a bit fractured). For one thing, cycle_count isn't stored in the existing BattMonitor_State. Another thing is that there is a wide variety of optional fields that not every backend handles, and I needed a way to get that from the script into the backend. I decided to do this by initializing the optional fields in my new BattMonitorScript_State with special values to automatically detect which fields are actually updated by the script. I could have created an object to handle feature capabilities, but that isn't much cleaner in the C++ side (I still need to create a new data structure and bind it into Lua), and makes the Lua side trickier---you have to remember to enable the features you are using.

The thing that is nagging me though is that there is always a binding to AP_BattMonitor::get_backend, even if HAL_BATTMON_SCRIPTING_ENABLED is false. AP_EFI::get_backend doesn't have this problem because the entire AP_EFI singleton binding depends on AP_EFI_SCRIPTING_ENABLED, but I can't do that with AP_BattMonitor because there are bindings we want to keep even if scripting drivers are disabled.

[IamPete1]

We could just always enable the scripting backend when scripting is enabled, we can always fix the binding generator in the future.

I see your point about BattMonitor_State we don't need var_info for example. You could make a sub structure for all backends that only contains the stuff actually related to the battery. But it is a bit tricky, for example should scripting provide a resting estimate, or should battery monitor use its estimator. Should scripting integrate to energy it's self or should it just provide voltage and current for battery monitor to do its own integration. Should the script trigger failsafes or should it battery monitor use its low and crit thresholds. @tridge Any thoughts?

[robertlong13]

Should scripting integrate to energy it's self or should it just provide voltage and current for battery monitor to do its own integration. Should the script trigger failsafes or should it battery monitor use its low and crit thresholds.

This implementation allows for optionally setting its own failsafe, mah consumption, wh consumption, and more, while automatically falling back to having the backend provide those if the script doesn't.

One of the few things I didn't implement was resting voltage and resistance. Those are handled by the parent backend class currently. It wouldn't be too hard to add those as optionally-handled by script if we want.

[robertlong13]

I added the ability to override resistance and/or resting voltage estimates in the script. This should allow @jschall to implement the EKF battery state estimation as a lua backend. We should probably add a way to use a lua script to combine such an estimator with existing backends (probably something for a separate PR), but as a start this would allow you to do it with analog inputs by manually reading the pins in the lua script.

[pedorich-n]

I am the person who asked about a custom battery monitor on Discord, and I can confirm that I was able to use the code from this PR and from the example to create custom battery monitors very easily! Much better than a custom driver in C.

I haven't used the aggregated state yet, but I can now read values from DC-DC converters using I2C and fake them into battery monitors.

[tridge]

needs a rebase

[tridge]

needs a rebase and an example script
@IamPete1 volunteered to write the example :-)

[robertlong13]

needs a rebase and an example script @IamPete1 volunteered to write the example :-)

I'm happy to write an example, or to have Peter do it. I could add the example I posted as a comment. Alternatively, I could try to implement a backend for a real device, such as the Millswood 250W PMU.

[tridge]

@robertlong13 I've done some updates on this and written a working example for a CAN battery monitor

[tridge]

note that the ANX driver needs a README and has some debug code in it that will be removed once tested

[IamPete1]

It would be nice to default the "invalid" values correctly. current_amps, consumed_mah, consumed_wh, temperature, cycle_count, capacity_remaining_pct all use none zero values. We could do it with a custom scripting constructor. This would mean the script writer just has to fill in what they have and it should all work.

[IamPete1]

LGTM