diff --git a/Makefile b/Makefile
index cf19370..d3e0e74 100644
--- a/Makefile
+++ b/Makefile
@@ -58,7 +58,7 @@ Q := @
 NULL := 2>/dev/null
 endif
 
-all: directories images
+all: directories images Test
 Debug:images
 Release: images
 cleanDebug:clean
diff --git a/include/functions.h b/include/functions.h
index f761487..d08f8c1 100644
--- a/include/functions.h
+++ b/include/functions.h
@@ -29,3 +29,17 @@ static void DisableAll()
    DigIo::ch2ena_out.Clear();
    DigIo::ch3ena_out.Clear();
 }
+
+static void CalcTotals()
+{
+   s32fp totalCurrent = Param::Get(Param::c1idc) + Param::Get(Param::c2idc) + Param::Get(Param::c3idc);
+
+   Param::SetFixed(Param::idc, totalCurrent);
+   s32fp u1 = Param::Get(Param::c1udc);
+   s32fp u2 = Param::Get(Param::c2udc);
+   s32fp u3 = Param::Get(Param::c3udc);
+
+   s32fp udcmax = MAX(u1, MAX(u2, u3));
+   Param::SetFixed(Param::udc, udcmax);
+}
+
diff --git a/src/main.cpp b/src/main.cpp
index 43ee6b9..c004832 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -355,18 +355,6 @@ static void CalcEnable()
    Param::SetInt(Param::enable, enable);
 }
 
-static void CalcTotals()
-{
-   s32fp totalCurrent = Param::Get(Param::c1idc) + Param::Get(Param::c2idc) + Param::Get(Param::c3idc);
-   Param::SetFixed(Param::idc, totalCurrent);
-   s32fp u1 = Param::Get(Param::c1udc);
-   s32fp u2 = Param::Get(Param::c2udc);
-   s32fp u3 = Param::Get(Param::c3udc);
-
-   s32fp udcmax = MAX(u1, MAX(u2, u3));
-   Param::SetFixed(Param::udc, udcmax);
-}
-
 static void ResetValuesInOffMode()
 {
    if (Param::GetInt(Param::state) == OFF)
diff --git a/test/test.c b/test/test.c
index a69ebf9..eeaff39 100644
--- a/test/test.c
+++ b/test/test.c
@@ -5,6 +5,7 @@
 #include "test_can.h"
 #include "test_io.h"
 #include "test_state.h"
+#include "test_logic.h"
 
 extern void Param::Change(Param::PARAM_NUM ParamNum){ };
 
@@ -18,6 +19,7 @@ int main() {
       (TestCase){"test_is_evse_input", test_is_evse_input},
       (TestCase){"test_check_unplugged", test_check_unplugged},
       (TestCase){"test_disable_all", test_disable_all},
+      (TestCase){"test_calc_totals", test_calc_totals},
       // Example:
       //(TestCase){"Your tests name", function_name_without_brackets}
       //...
diff --git a/test/test.out b/test/test.out
index 0cd2e3e..02696a3 100755
Binary files a/test/test.out and b/test/test.out differ
diff --git a/test/test_logic.h b/test/test_logic.h
new file mode 100644
index 0000000..f4e2b8e
--- /dev/null
+++ b/test/test_logic.h
@@ -0,0 +1,29 @@
+#ifndef TEST_LOGIC_H
+#define TEST_LOGIC_H
+#endif
+#include <assert.h>
+#include "test_common.h"
+
+void test_calc_totals()
+{
+
+  // simple idc addition test
+  Param::SetInt(Param::c1idc, 12);
+  Param::SetInt(Param::c2idc, 13);
+  Param::SetInt(Param::c3idc, 11);
+
+  CalcTotals();
+
+  s32fp idc = Param::Get(Param::idc);
+  assert(36 == FP_TOINT(idc));
+
+  // udcmax test
+  Param::SetInt(Param::c1udc, 365);
+  Param::SetInt(Param::c2udc, 364);
+  Param::SetInt(Param::c3udc, 366);
+
+  CalcTotals();
+
+  s32fp udcmax = Param::Get(Param::udc);
+  assert(366 == FP_TOINT(udcmax));
+}