ardupilot/libraries/AP_Scripting/examples/gen_control.lua

212 lines
6.0 KiB
Lua
Raw Normal View History

--[[
control throttle on a generator to achieve a target battery voltage
This is meant to replace throttle governors on some hybrid drone
generators. It monitors battery voltage and controls the throttle
of the generator to maintain a target voltage using a PI controller
--]]
-- luacheck: only 0
UPDATE_RATE_HZ = 10
-- setup a parameter block
local PARAM_TABLE_KEY = 73
local PARAM_TABLE_PREFIX = 'GENCTL_'
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table')
-- 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
GENCTL_ENABLE = bind_add_param('ENABLE', 1, 0)
if GENCTL_ENABLE:get() <= 0 then
return
end
-- MIN, MAX and IDLE PWM for throttle output
GENCTL_PWM_MIN = bind_add_param('PWM_MIN', 2, 1000)
GENCTL_PWM_MAX = bind_add_param('PWM_MAX', 3, 2000)
GENCTL_PWM_IDLE = bind_add_param('PWM_IDLE', 4, 1200)
-- P and I gains for controller
GENCTL_PID_P = bind_add_param('PID_P', 5, 0.1)
GENCTL_PID_I = bind_add_param('PID_I', 6, 0.1)
-- maximum I contribution
GENCTL_PID_IMAX = bind_add_param('PID_IMAX', 7, 1.0)
-- RCn_OPTION value for 3 position switch
GENCTL_RC_FUNC = bind_add_param('RC_FUNC', 8, 300)
-- output servo channel that we will control
GENCTL_THR_CHAN = bind_add_param('THR_CHAN', 9, 0)
-- battery index to monitor, 0 is first battery
GENCTL_BAT_IDX = bind_add_param('BAT_IDX', 10, 0)
-- target voltage
GENCTL_VOLT_TARG = bind_add_param('VOLT_TARG', 11, 0)
-- maximum slew rate in percent/second for throttle change
GENCTL_SLEW_RATE = bind_add_param('SLEW_RATE', 12, 100)
local MAV_SEVERITY_INFO = 6
local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_EMERGENCY = 0
local switch = nil
local thr_pwm = GENCTL_PWM_MIN:get()
local last_switch_pos = nil
-- constrain a value between limits
local function constrain(v, vmin, vmax)
if v < vmin then
v = vmin
end
if v > vmax then
v = vmax
end
return v
end
-- a PI controller implemented as a Lua object
local function PI_controller(kP,kI,iMax,min,max)
-- the new instance. You can put public variables inside this self
-- declaration if you want to
local self = {}
-- private fields as locals
local _kP = kP
local _kI = kI
local _iMax = iMax
local _min = min
local _max = max
local _last_t = nil
local _I = 0
local _P = 0
local _total = 0
local _counter = 0
local _target = 0
local _current = 0
-- update the controller.
function self.update(target, current)
local now = millis():tofloat() * 0.001
if not _last_t then
_last_t = now
end
local dt = now - _last_t
_last_t = now
local err = target - current
_counter = _counter + 1
local P = _kP:get() * err
if ((_total < _max and _total > _min) or (_total >= _max and err < 0) or (_total <= _min and err > 0)) then
_I = _I + _kI:get() * err * dt
end
if _iMax:get() > 0 then
_I = constrain(_I, -_iMax:get(), iMax:get())
end
local I = _I
local ret = P + I
_target = target
_current = current
_P = P
_total = ret
return ret
end
-- reset integrator to an initial value
function self.reset(integrator)
_I = integrator
end
-- log the controller internals
function self.log(name)
-- allow for an external addition to total
logger.write(name,'Targ,Curr,P,I,Total','fffff',_target,_current,_P,_I,_total)
end
-- return the instance
return self
end
local thr_PI = PI_controller(GENCTL_PID_P, GENCTL_PID_I, GENCTL_PID_IMAX, 0, 1)
local last_pwm = GENCTL_PWM_MIN:get()
function update()
if switch == nil then
switch = rc:find_channel_for_option(math.floor(GENCTL_RC_FUNC:get()))
end
if switch == nil or GENCTL_ENABLE:get() < 1 then
-- nothing to do
return
end
local sw_pos = switch:get_aux_switch_pos()
if sw_pos ~= last_switch_pos then
if sw_pos == 0 then
gcs:send_text(MAV_SEVERITY_INFO,"GenCtl: off")
elseif sw_pos == 1 then
gcs:send_text(MAV_SEVERITY_INFO,"GenCtl: idle")
else
gcs:send_text(MAV_SEVERITY_INFO,"GenCtl: run")
end
last_switch_pos = sw_pos
end
if sw_pos == 0 then
-- force low throttle
thr_pwm = GENCTL_PWM_MIN:get()
thr_PI.reset(0)
elseif sw_pos == 1 then
-- force idle
thr_pwm = GENCTL_PWM_IDLE:get()
thr_PI.reset(0)
else
local bat_volt = battery:voltage_resting_estimate(GENCTL_BAT_IDX:get())
local thr_out = thr_PI.update(GENCTL_VOLT_TARG:get(), bat_volt)
thr_out = constrain(thr_out, 0, 1)
thr_pwm = GENCTL_PWM_IDLE:get() + thr_out * (GENCTL_PWM_MAX:get() - GENCTL_PWM_IDLE:get())
thr_PI.log("GENC")
end
local max_change = GENCTL_SLEW_RATE:get() * (GENCTL_PWM_MAX:get() - GENCTL_PWM_MIN:get()) * 0.01 / UPDATE_RATE_HZ
thr_pwm = constrain(thr_pwm, last_pwm - max_change, last_pwm + max_change)
last_pwm = thr_pwm
if GENCTL_THR_CHAN:get() > 0 then
SRV_Channels:set_output_pwm_chan(GENCTL_THR_CHAN:get()-1, math.floor(thr_pwm))
end
end
-- wrapper around update(). This calls update() at 10Hz,
-- 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_EMERGENCY, "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
return
end
return protected_wrapper, 1000/UPDATE_RATE_HZ
end
gcs:send_text(MAV_SEVERITY_INFO,"Loaded gen_control.lua")
-- start running update loop
return protected_wrapper()