From f243f1608cd094b7d3bb7eae2bdcce26f9f4c060 Mon Sep 17 00:00:00 2001
From: Andrey <rusefillc@gmail.com>
Date: Tue, 6 Sep 2022 15:12:43 -0400
Subject: [PATCH] B6

---
 firmware/config/engines/vw_b6.cpp     |  6 ++-
 firmware/controllers/lua/lua_lib.h    | 20 +++++++
 unit_tests/tests/lua/test_lua_vag.cpp | 76 +++++++++++++++++++++++++--
 3 files changed, 96 insertions(+), 6 deletions(-)

diff --git a/firmware/config/engines/vw_b6.cpp b/firmware/config/engines/vw_b6.cpp
index 711bcdbbed..efa655d6da 100644
--- a/firmware/config/engines/vw_b6.cpp
+++ b/firmware/config/engines/vw_b6.cpp
@@ -159,12 +159,14 @@ void setProteusVwPassatB6() {
 
 	strncpy(config->luaScript, R"(
 AIRBAG = 0x050
-MOTOR_1 = 0x280
-MOTOR_3 = 0x380
 GRA = 0x388
 TCU_1 = 0x440
 TCU_2 = 0x540
 BRAKE_2 = 0x5A0
+
+
+MOTOR_1 = 0x280
+MOTOR_3 = 0x380
 MOTOR_INFO = 0x580
 MOTOR_5 = 0x480
 MOTOR_6 = 0x488
diff --git a/firmware/controllers/lua/lua_lib.h b/firmware/controllers/lua/lua_lib.h
index b935fa3aeb..d90f6b6f41 100644
--- a/firmware/controllers/lua/lua_lib.h
+++ b/firmware/controllers/lua/lua_lib.h
@@ -5,6 +5,9 @@
 #define ARRAY_EQUALS "function equals(data1, data2) \
  \
   local index = 1 \
+  if data1 == nil then \
+     return -666 \
+  end \
   while data1[index] ~= nil do \
 	if math.floor(data1[index]) ~= math.floor(data2[index]) then \
        return -1 - index \
@@ -98,4 +101,21 @@ function getBitRange(data, bitIndex, bitWidth) \
 end \
 "
 
+#define SET_BIT_RANGE_LSB " \
+function setBitRange(data, totalBitIndex, bitWidth, value) \
+	local byteIndex = totalBitIndex >> 3 \
+	local bitInByteIndex = totalBitIndex - byteIndex * 8 \
+	if (bitInByteIndex + bitWidth > 8) then \
+		bitsToHandleNow = 8 - bitInByteIndex \
+		setBitRange(data, totalBitIndex + bitsToHandleNow, bitWidth - bitsToHandleNow, value >> bitsToHandleNow) \
+		bitWidth = bitsToHandleNow; \
+	end \
+	mask = (1 << bitWidth) - 1 \
+	data[1 + byteIndex] = data[1 + byteIndex] & (~(mask << bitInByteIndex)) \
+	maskedValue = value & mask \
+	shiftedValue = maskedValue << bitInByteIndex \
+	data[1 + byteIndex] = data[1 + byteIndex] | shiftedValue \
+end \
+"
+
 
diff --git a/unit_tests/tests/lua/test_lua_vag.cpp b/unit_tests/tests/lua/test_lua_vag.cpp
index c100782642..c37969d0ea 100644
--- a/unit_tests/tests/lua/test_lua_vag.cpp
+++ b/unit_tests/tests/lua/test_lua_vag.cpp
@@ -131,25 +131,27 @@ TEST(LuaVag, unpackMotor1_torq_req) {
     EXPECT_NEAR_M3(testLuaReturnsNumberOrNil(realdata).value_or(0), 21.84);
 }
 
-#define realMotor3Packet "\ndata = { 0x00, 0x62, 0xFA, 0x00, 0x22, 0x00, 0x00, 0xFA}\n "
+#define realMotor3Packet "\ndata = { 0x00, 0x62, 0xFA, 0xDA, 0x22, 0x00, 0x00, 0xFA}\n "
 
 TEST(LuaVag, packMotor3) {
-	const char* script = PRINT_ARRAY ARRAY_EQUALS SET_TWO_BYTES R"(
+	const char* script = SET_BIT_RANGE_LSB PRINT_ARRAY ARRAY_EQUALS SET_TWO_BYTES R"(
 
 	function testFunc()
 		tps = 100
 		iat = 25.5
+		desired_wheel_torque = 284.7
 
 		canMotor3 = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
 
  		canMotor3[2] = (iat + 48) / 0.75
 		canMotor3[3] = tps / 0.4
-		canMotor3[5] = 0x22
+		canMotor3[5] = 0x20
+		setBitRange(canMotor3, 24, 12, math.floor(desired_wheel_torque / 0.39))
  		canMotor3[8] = tps / 0.4
 
 		print(arrayToString(canMotor3))
 
-		expected = { 0x00, 0x62, 0xFA, 0x00, 0x22, 0x00, 0x00, 0xFA }
+		expected = { 0x00, 0x62, 0xFA, 0xDA, 0x22, 0x00, 0x00, 0xFA }
 		return equals(canMotor3, expected)
 	end
 	)";
@@ -180,6 +182,61 @@ TEST(LuaVag, unpackMotor3_pps) {
     EXPECT_NEAR_M3(testLuaReturnsNumberOrNil(script).value_or(0), 100);
 }
 
+TEST(LuaVag, setBitRange) {
+	{
+		const char* script = PRINT_ARRAY ARRAY_EQUALS SET_BIT_RANGE_LSB R"(
+
+		function testFunc()
+			data = { 0x34, 0x56, 0x00 }
+			setBitRange(data, 4, 8, 0xAB)
+
+			print(arrayToString(data))
+
+			expected = { 0xB4, 0x5A, 0x00 }
+			return equals(data, expected)
+		end
+		)";
+
+	    EXPECT_NEAR_M3(testLuaReturnsNumberOrNil(script).value_or(0), 0);
+	}
+
+	{
+		const char* script = PRINT_ARRAY ARRAY_EQUALS SET_BIT_RANGE_LSB R"(
+
+		function testFunc()
+			data = { 0x00, 0x00, 0x00 }
+			setBitRange(data, 5, 9, 0x1FF)
+
+			print(arrayToString(data))
+
+			expected = { 0xE0, 0x3F, 0x00 }
+			return equals(data, expected)
+		end
+		)";
+
+	    EXPECT_NEAR_M3(testLuaReturnsNumberOrNil(script).value_or(0), 0);
+	}
+
+
+	{
+		const char* script = PRINT_ARRAY ARRAY_EQUALS SET_BIT_RANGE_LSB R"(
+
+		function testFunc()
+			data = { 0xFF, 0xFF, 0x00 }
+			setBitRange(data, 5, 9, 0x0)
+
+			print(arrayToString(data))
+
+			expected = { 0x1F, 0xC0, 0x00 }
+			return equals(data, expected)
+		end
+		)";
+
+	    EXPECT_NEAR_M3(testLuaReturnsNumberOrNil(script).value_or(0), 0);
+	}
+
+}
+
 TEST(LuaVag, unpackMotor3_iat) {
 	const char* script = 	GET_BIT_RANGE_LSB	realMotor3Packet	R"(
 	function testFunc()
@@ -191,6 +248,17 @@ TEST(LuaVag, unpackMotor3_iat) {
     EXPECT_NEAR_M3(testLuaReturnsNumberOrNil(script).value_or(0), 25.5);
 }
 
+TEST(LuaVag, unpackMotor3_desired_wheel_torque) {
+	const char* script = 	GET_BIT_RANGE_LSB	realMotor3Packet	R"(
+	function testFunc()
+		desired_wheel_torque = getBitRange(data, 24, 12) * 0.39
+		return desired_wheel_torque
+	end
+	)";
+
+    EXPECT_NEAR_M3(testLuaReturnsNumberOrNil(script).value_or(0), 284.7);
+}
+
 #define realMotor6Packet "\ndata = { 0x3D, 0x54, 0x69, 0x7E, 0xFE, 0xFF, 0xFF, 0x80}\n "
 
 TEST(LuaVag, unpackMotor6_actual_torq) {