diff --git a/Bench-Data-Firmware/src/main.cpp b/Bench-Data-Firmware/src/main.cpp
index 3525cd9..f0ff53d 100644
--- a/Bench-Data-Firmware/src/main.cpp
+++ b/Bench-Data-Firmware/src/main.cpp
@@ -10,6 +10,54 @@ volatile bool new_rpm = false;
 const unsigned long deltat_lambda = 100000;
 unsigned long last_lambda = 0;
 
+typedef uint8_t data_type;
+
+enum{
+  SPEED = 0,
+  LAMBDA = 1,
+};
+
+#pragma pack(push,1)
+typedef struct {
+  uint16_t sync;
+  data_type type;
+  uint32_t timestamp;
+  float data;
+  uint16_t crc;
+} data_packet;
+#pragma pack(pop)
+
+uint16_t crc16_ccitt(const uint8_t *data, size_t length)
+{
+    uint16_t crc = 0xFFFF;
+
+    for (size_t i = 0; i < length; i++) {
+        crc ^= (uint16_t)data[i] << 8;
+
+        for (uint8_t bit = 0; bit < 8; bit++) {
+            if (crc & 0x8000) {
+                crc = (crc << 1) ^ 0x1021;
+            } else {
+                crc <<= 1;
+            }
+        }
+    }
+
+    return crc;
+}
+
+data_packet pack(data_type type,uint32_t timestamp, float data){
+  data_packet output{};
+  
+  output.sync = 0xBEEF;
+  output.type = type;
+  output.timestamp = timestamp;
+  output.data = data;
+  output.crc = crc16_ccitt(&output.type, sizeof(data_type)+sizeof(uint32_t)+sizeof(float));
+  return output;
+}
+
+
 void interrupt(){
   unsigned long now = micros();
   timestamp2 = timestamp1;
@@ -31,8 +79,10 @@ void loop() {
     unsigned long deltat = timestamp1 - timestamp2;
     float rpmhz = 500000.0 / deltat;
     new_rpm = false;
-    Serial.print("Speed: ");
-    Serial.println(rpmhz);
+    data_packet speed_packet = pack(SPEED,timestamp1,rpmhz);
+    Serial.write((uint8_t*)&speed_packet, sizeof(data_packet));
+    //Serial.print("Speed: ");
+    //Serial.println(rpmhz);
   }
   unsigned long now = micros();
   if (now > last_lambda + deltat_lambda){
@@ -40,10 +90,12 @@ void loop() {
     float voltage = output / 204.6;
     float lambda = 0.56030161*voltage + 0.43969839;
     last_lambda = now;
-    Serial.print("Voltage: ");
-    Serial.println(voltage);
-    Serial.print("Lambda:");
-    Serial.println(lambda);
+    data_packet lambda_packet = pack(LAMBDA,now,lambda);
+    Serial.write((uint8_t*)&lambda_packet,sizeof(lambda_packet));
+    //Serial.print("Voltage: ");
+    //Serial.println(voltage);
+    //Serial.print("Lambda:");
+    //Serial.println(lambda);
   }
 
 }