diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 154791d..43f24e3 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -57,6 +57,14 @@ unsigned long brake_ok_time=0;
 bool error_throttle_outofrange=false;
 bool error_brake_outofrange=false;
 
+unsigned long last_feedback=0; //time millis at last feedback receive
+
+float meanSpeedms=0;
+float trip=0; //trip distance in meters
+float wheelcircumference=0.5278; //wheel diameter in m. 8.4cm radius -> 0.084m*2*Pi
+
+
+
 #define PIN_START A9
 #define PIN_LED_START 2 //Enginge start led
 
@@ -285,6 +293,13 @@ void loop() {
     updateMotorparams(motorparamsRear,FeedbackRear);
   }
 
+  if (newData2 || newData3) { //when new data arrived from one controller
+    float _meanRPM=FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas;
+    float meanSpeedms=_meanRPM*wheelcircumference/60.0;   // Units: 1/min * m / 60s
+    trip+=abs(meanSpeedms)* (millis()-last_feedback) / 1000.0;    
+    last_feedback=millis();
+  }
+
   
   if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
     last_adcread=loopmillis;
@@ -341,9 +356,9 @@ float filterMedian(int16_t* values) {
 void writeLogHeader(HardwareSerial &SerialRef) {
   SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
   SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
-  SerialRef.print("speed_FrontL,speed_FrontR,speed_RearL,speed_RearR,");
+  SerialRef.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
   SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
-  SerialRef.println("currentAll,throttle,brake");
+  SerialRef.println("currentAll,throttle,brake,speed,trip");
 }
 
 void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
@@ -371,7 +386,9 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
 
   SerialRef.print(currentAll,3); SerialRef.print(","); //invert. positive current is drive current
   SerialRef.print(throttle); SerialRef.print(",");
-  SerialRef.print(brake); SerialRef.println();
+  SerialRef.print(brake); SerialRef.print(",");
+  SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
+  SerialRef.print(trip); SerialRef.println(); //in m
 }
 
 void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)