ardupilot/libraries/AP_Scripting/examples/OOP_example.lua

136 lines
3.2 KiB
Lua

-- this is an example of how to do object oriented programming in Lua
function constrain(v, minv, maxv)
-- constrain a value between two limits
if v < minv then
return minv
end
if v > maxv then
return maxv
end
return v
end
--[[
a PI controller with feed-forward implemented as a Lua object, using
closure style object
--]]
local function PIFF(kFF,kP,kI,iMax)
-- the new instance. You can put public variables inside this self
-- declaration if you want to
local self = {}
-- private fields as locals
local _kFF = kFF
local _kP = kP or 0.0
local _kI = kI or 0.0
local _kD = kD or 0.0
local _iMax = iMax
local _last_t = nil
local _log_data = {}
local _I = 0
local _counter = 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 FF = _kFF * target
local P = _kP * err
_I = _I + _kI * err * dt
if _iMax then
_I = constrain(_I, -_iMax, _iMax)
end
local I = _I
local ret = FF + P + I
_log_data = { target, current, FF, P, I, ret }
return ret
end
-- log the controller internals
function self.log(name)
logger.write(name,'Targ,Curr,FF,P,I,Total','ffffff',table.unpack(_log_data))
end
-- return the instance
return self
end
--[[
another example of a PIFF controller as an object, this time using
metatables. Using metatables uses less memory and object creation is
faster, but access to variables is slower
--]]
local PIFF2 = {}
PIFF2.__index = PIFF2
function PIFF2.new(kFF,kP,kI,iMax)
-- the new instance. You can put public variables inside this self
-- declaration if you want to
local self = setmetatable({},PIFF2)
self.kFF = kFF
self.kP = kP
self.kI = kI
self.iMax = iMax
self.last_t = nil
self.log_data = {}
self.I = 0
self.counter = 0
return self
end
function PIFF2.update(self, target, current)
local now = millis():tofloat() * 0.001
if not self.last_t then
self.last_t = now
end
local dt = now - self.last_t
self.last_t = now
local err = target - current
self.counter = self.counter + 1
local FF = self.kFF * target
local P = self.kP * err
self.I = self.I + self.kI * err * dt
if self.iMax then
self.I = constrain(self.I, -self.iMax, self.iMax)
end
local ret = FF + P + self.I
self.log_data = { target, current, FF, P, self.I, ret }
return ret
end
function PIFF2.log(self, name)
logger.write(name,'Targ,Curr,FF,P,I,Total','ffffff',table.unpack(self.log_data))
end
--[[
declare two PI controllers, using one of each style. Note the use of new() for the metatables style
--]]
local PI_elevator = PIFF(1.1, 0.0, 0.0, 20.0)
local PI_rudder = PIFF2.new(1.1, 0.0, 0.0, 20.0)
function test()
-- note the different syntax for the two varients
elevator = PI_elevator.update(1.0, 0.5)
rudder = PI_rudder:update(2.0, 0.7)
PI_elevator.log("PEL")
PI_rudder:log("PRD")
gcs:send_text(0, "tick: " .. tostring(millis()))
return test, 500
end
return test()