diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index e189588..af3c098 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -57,12 +57,12 @@ 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
 
+float currentConsumed=0; //Ah
+
 
 
 #define PIN_START A9
@@ -292,14 +292,6 @@ void loop() {
   if (newData3) {
     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;
-    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;
@@ -314,6 +306,14 @@ void loop() {
   if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers    
     last_send=loopmillis;
     sendCMD();    
+
+    //Update speed and trip
+    float _meanRPM=FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas;
+    meanSpeedms=_meanRPM*wheelcircumference/60.0;   // Units: 1/min * m / 60s
+    trip+=abs(meanSpeedms)* (SENDPERIOD/1000.0);
+
+    //mah consumed
+    currentConsumed += filtered_currentAll* (SENDPERIOD/1000.0)/3600.0; //amp hours
   }
 
   //If needed write log to serial port
@@ -358,7 +358,7 @@ void writeLogHeader(HardwareSerial &SerialRef) {
   SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_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,speed,trip");
+  SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed");
 }
 
 void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
@@ -388,7 +388,9 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
   SerialRef.print(throttle); SerialRef.print(",");
   SerialRef.print(brake); SerialRef.print(",");
   SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
-  SerialRef.print(trip); SerialRef.println(); //in m
+  SerialRef.print(trip); SerialRef.print(","); //in m
+  SerialRef.print(currentConsumed); SerialRef.println(); //in Ah (Amphours)
+  
 }
 
 void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)