diff --git a/firmware/can.cpp b/firmware/can.cpp
index f4862da..ed9bed0 100644
--- a/firmware/can.cpp
+++ b/firmware/can.cpp
@@ -3,6 +3,8 @@
 
 #include "can_helper.h"
 #include "heater_control.h"
+#include "lambda_conversion.h"
+#include "sampling.h"
 
 static const CANConfig canConfig500 =
 {
@@ -10,9 +12,25 @@ static const CANConfig canConfig500 =
     CAN_BTR_SJW(0) | CAN_BTR_BRP(5)  | CAN_BTR_TS1(12) | CAN_BTR_TS2(1) | CAN_BTR_LBKM,
 };
 
+static THD_WORKING_AREA(waCanTxThread, 256);
+void CanTxThread(void*)
+{
+    while(1)
+    {
+        float esr = GetSensorInternalResistance();
+        float lambda = GetLambda();
+
+        SendCanData(lambda, esr);
+        SendEmulatedAemXseries(lambda, 0);
+
+        chThdSleepMilliseconds(10);
+    }
+}
+
 void InitCan()
 {
     canStart(&CAND1, &canConfig500);
+    chThdCreateStatic(waCanTxThread, sizeof(waCanTxThread), NORMALPRIO, CanTxThread, nullptr);
 }
 
 struct StandardDataFrame
diff --git a/firmware/main.cpp b/firmware/main.cpp
index 593a290..be9db94 100644
--- a/firmware/main.cpp
+++ b/firmware/main.cpp
@@ -7,7 +7,6 @@
 #include "pump_control.h"
 #include "pump_dac.h"
 #include "sampling.h"
-#include "lambda_conversion.h"
 
 static const UARTConfig uartCfg =
 {
@@ -44,13 +43,10 @@ int main() {
 
     InitCan();
 
-    while(1) {
-        float esr = GetSensorInternalResistance();
-        float lambda = GetLambda();
-
-        SendCanData(lambda, esr);
-        SendEmulatedAemXseries(lambda, 0);
-
-        chThdSleepMilliseconds(10);
+    while(true)
+    {
+        palTogglePad(GPIOB, 6);
+        chThdSleepMilliseconds(400);
     }
+
 }