diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 5ec6c4a..985087f 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -13,16 +13,15 @@
 const uint16_t calib_throttle_min = 350;
 const uint16_t calib_throttle_max = 810;
 
-int16_t out_speedFL=0;
-int16_t out_speedFR=0;
-int16_t out_speedRL=0;
-int16_t out_speedRR=0;
+
 unsigned long last_send = 0;
 unsigned long last_receive = 0;
 
 float avg_currentL=0;
 float avg_currentR=0;
 
+int16_t cmd_send=0;
+
 
 // Global variables for serial communication
 typedef struct{
@@ -44,6 +43,7 @@ typedef struct{
 } SerialCommand;
 SerialCommand CommandFront;
 
+
 typedef struct{
    uint16_t start;
    int16_t  cmd1;
@@ -52,7 +52,7 @@ typedef struct{
    int16_t  speedR_meas;
    int16_t  batVoltage;
    int16_t  boardTemp;
-   int16_t  curL_DC;
+   int16_t  curL_DC; //negative values are current drawn. positive values mean generated current
    int16_t  curR_DC;
    uint16_t cmdLed;
    uint16_t  checksum;
@@ -60,10 +60,24 @@ typedef struct{
 SerialFeedback FeedbackFront;
 SerialFeedback NewFeedbackFront;
 
+#define CURRENT_FILTER_SIZE 100 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
+#define CURRENT_MEANVALUECOUNT 10 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used
+typedef struct{
+  int16_t curL_DC[CURRENT_FILTER_SIZE] = {0};
+  int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
+  uint8_t cur_pos=0;
+  int16_t cmdL=0;
+  int16_t cmdR=0;
+} MotorParameter;
+MotorParameter motorparamsFront;
+
 
 void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
 bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
 
+int sort_desc(const void *cmp1, const void *cmp2);
+float filterMedian(int16_t* values);
+
 void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
 {
   // Create command
@@ -139,7 +153,6 @@ bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
   sread.incomingBytePrev = sread.incomingByte;
 
   return _result; //new data was available
-
 }
 
   // ########################## SETUP ##########################
@@ -161,43 +174,74 @@ unsigned long loopmillis;
 void loop() {
   loopmillis=millis(); //read millis for this cycle
 
-  bool newData=false;
-  newData=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
+  bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
   
   
   //Serial.print("fo="); Serial.println(count);
   //count++;
 
-  if (newData) {
-    float _current = (FeedbackFront.curL_DC+FeedbackFront.curR_DC)/2.0 / 50; 
-    
-    /*
-    Serial.print(FeedbackFront.curL_DC); Serial.print(", ");
-    Serial.print(FeedbackFront.curR_DC); Serial.print(", ");
-
-    Serial.print(", mean="); Serial.print(_current);
-    Serial.print(", ");
-    Serial.print(FeedbackFront.cmd1); Serial.print(", ");
-    Serial.print(FeedbackFront.cmd2); Serial.print(", ");
-    Serial.println();
-    */
-
-    #define CURRENTFILTER 0.95
-    avg_currentL=avg_currentL*CURRENTFILTER+FeedbackFront.curL_DC*(1-CURRENTFILTER);
-    avg_currentR=avg_currentR*CURRENTFILTER+FeedbackFront.curR_DC*(1-CURRENTFILTER);
+  if (newData2) {
+    motorparamsFront.cur_pos++;
+    motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
+    motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
+    motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
   }
 
   if (loopmillis - last_send > SENDPERIOD) {
     last_send=loopmillis;
 
     uint16_t throttle_raw = analogRead(PIN_THROTTLE);
-    //Serial.print("Analog: "); Serial.print(throttle_raw);
-    out_speedFR=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000)));
-    
-    //Serial.print(", Send: "); Serial.println(out_speedFL);
-    SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2);   
+    int16_t throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
+       
 
-    Serial.print(out_speedFL); Serial.print(", "); Serial.print(out_speedFR); Serial.print(", "); Serial.print(avg_currentR); Serial.print(", "); Serial.println(avg_currentL);
+    float freewheel_current=0.1;
+    float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
+    float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0;
+    float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0;
+
+    float filtered_currentAll=(filtered_curFL+filtered_curFR)/2.0; //mean current of all motors
+
+    if (throttle_pos>=motorparamsFront.cmdR) { //accelerating
+      cmd_send = throttle_pos; //if throttle higher than apply throttle directly
+    }else{ //freewheeling or braking
+      if (-filtered_currentAll>freewheel_current) { //breaking current high enough
+        cmd_send-= max(0, (-filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor
+      }
+      cmd_send-=1; //reduce slowly anyways
+      cmd_send=constrain(cmd_send,0,1000);
+    }
+
+    //apply throttle command to all motors
+    motorparamsFront.cmdL=cmd_send;
+    motorparamsFront.cmdR=cmd_send;
+
+    SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
+
+    Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println();
     
   }
+}
+
+int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
+{
+  float a = *((float *)cmp1);
+  float b = *((float *)cmp2);
+  return a > b ? -1 : (a < b ? 1 : 0);
+}
+
+float filterMedian(int16_t* values) {
+  float copied_values[CURRENT_FILTER_SIZE];
+  for(int i=0;i<CURRENT_FILTER_SIZE;i++) {
+    copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
+  }
+  float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
+  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
+
+  float mean=copied_values[CURRENT_FILTER_SIZE/2];
+  for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
+    mean+=copied_values[CURRENT_FILTER_SIZE/2-i]+copied_values[CURRENT_FILTER_SIZE/2+i]; //add two values around center
+  }
+  mean/=(1+CURRENT_MEANVALUECOUNT*2);
+
+  return mean;
 }
\ No newline at end of file