From dde80bb900df135cde30209b538f2311b2a94383 Mon Sep 17 00:00:00 2001
From: rusefillc <sdfsdfqsf2334234234>
Date: Thu, 28 Oct 2021 23:35:53 -0400
Subject: [PATCH] lua pid class #3411

---
 firmware/controllers/lua/lua_hooks.cpp  | 78 +++++++++++++++++++++++++
 unit_tests/tests/lua/test_lua_hooks.cpp | 38 ++++++++++++
 2 files changed, 116 insertions(+)

diff --git a/firmware/controllers/lua/lua_hooks.cpp b/firmware/controllers/lua/lua_hooks.cpp
index 3f406e58d4..5a162e9273 100644
--- a/firmware/controllers/lua/lua_hooks.cpp
+++ b/firmware/controllers/lua/lua_hooks.cpp
@@ -374,6 +374,71 @@ struct LuaSensor : public StoredValueSensor {
 	void showInfo(const char*) const {}
 };
 
+struct LuaPid {
+	LuaPid
+	()
+// todo	(float kp, float ki, float kd, float min, float max)
+		: m_pid(&m_params)
+	{
+		m_params.pFactor = 0;
+		m_params.iFactor = 0;
+		m_params.dFactor = 0;
+
+		m_params.offset = 0;
+		m_params.periodMs = 0;
+		m_params.minValue = 0;
+		m_params.maxValue = 0;
+
+		m_lastUpdate.reset();
+	}
+
+	float get(float input) {
+#if EFI_UNIT_TEST
+		extern int timeNowUs;
+		// this is how we avoid zero dt
+		timeNowUs += 1000;
+#endif
+		float dt = m_lastUpdate.getElapsedSecondsAndReset(getTimeNowNt());
+
+		return m_pid.getOutput(target, input, dt);
+	}
+
+	void setTarget(float value) {
+		target = value;
+	}
+
+	void setP(float value) {
+		m_params.pFactor = value;
+	}
+
+	void setI(float value) {
+		m_params.iFactor = value;
+	}
+
+	void setD(float value) {
+		m_params.dFactor = value;
+	}
+
+	void setMinValue(float value) {
+		m_params.minValue = value;
+	}
+
+	void setMaxValue(float value) {
+		m_params.maxValue = value;
+	}
+
+	void reset() {
+		m_pid.reset();
+	}
+
+private:
+	Pid m_pid;
+	Timer m_lastUpdate;
+	pid_s m_params;
+	// ugly as hell, a way to move forward while we wait for https://github.com/gengyong/luaaa/issues/7
+	float target;
+};
+
 void configureRusefiLuaHooks(lua_State* l) {
 
 	LuaClass<Timer> luaTimer(l, "Timer");
@@ -388,6 +453,19 @@ void configureRusefiLuaHooks(lua_State* l) {
 		.fun("set", &LuaSensor::set)
 		.fun("invalidate", &LuaSensor::invalidate);
 
+	LuaClass<LuaPid> luaPid(l, "Pid");
+	luaPid
+		.ctor()
+		.fun("get", &LuaPid::get)
+		.fun("setTarget", &LuaPid::setTarget)
+		.fun("setP", &LuaPid::setP)
+		.fun("setI", &LuaPid::setI)
+		.fun("setD", &LuaPid::setD)
+		.fun("setMinValue", &LuaPid::setMinValue)
+		.fun("setMaxValue", &LuaPid::setMaxValue)
+		.fun("reset", &LuaPid::reset)
+		;
+
 	lua_register(l, "print", lua_efi_print);
 	lua_register(l, "readPin", lua_readpin);
 	lua_register(l, "getAuxAnalog", lua_getAuxAnalog);
diff --git a/unit_tests/tests/lua/test_lua_hooks.cpp b/unit_tests/tests/lua/test_lua_hooks.cpp
index 99031e5410..0e2ffd8503 100644
--- a/unit_tests/tests/lua/test_lua_hooks.cpp
+++ b/unit_tests/tests/lua/test_lua_hooks.cpp
@@ -130,3 +130,41 @@ TEST(LuaHooks, LuaSensor) {
 	// Ensure that the sensor got unregistered on teardown of the Lua interpreter
 	EXPECT_FALSE(Sensor::hasSensor(SensorType::Clt));
 }
+
+static const char* pidTest = R"(
+function testFunc()
+	local pid = Pid.new()
+	pid:setP(0.5)
+	pid:setMinValue(-10)
+    pid:setMaxValue(10)
+
+	pid:setTarget(3)
+
+	-- delta is -4, output -2
+	if pid:get(7) ~= -2 then
+		return 1
+	end
+
+	pid:setTarget(4)
+	-- delta is 6, output 3
+	if pid:get(-2) ~= 3 then
+		return 2
+	end
+
+	pid:setTarget(0)
+	-- test clamping
+	if pid:get(100) ~= -10 then
+		return 3
+	end
+
+	if pid:get(-100) ~= 10 then
+		return 4
+	end
+
+	return 0
+end
+)";
+
+TEST(LuaHooks, LuaPid) {
+	EXPECT_EQ(testLuaReturnsNumber(pidTest), 0);
+}