From 06ef5aed1423267feac2a4c273f7573b4e6e47ae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 12 May 2020 20:54:46 +1000
Subject: [PATCH] AP_Scripting: added an example of OOP programming

very useful pattern for more complex scripts
---
 .../AP_Scripting/examples/OOP_example.lua     | 135 ++++++++++++++++++
 1 file changed, 135 insertions(+)
 create mode 100644 libraries/AP_Scripting/examples/OOP_example.lua

diff --git a/libraries/AP_Scripting/examples/OOP_example.lua b/libraries/AP_Scripting/examples/OOP_example.lua
new file mode 100644
index 0000000000000..feb376123188c
--- /dev/null
+++ b/libraries/AP_Scripting/examples/OOP_example.lua
@@ -0,0 +1,135 @@
+-- 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()