diff --git a/controller/controller.ino b/controller/controller.ino
index 51555be..367bbde 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -22,6 +22,7 @@
 #define ADC_CALIB_THROTTLE_MAX 3120 //maximum adc value that should correspond to full speed
 
 #define PIN_STARTLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor
+uint8_t startled=0;
 #define PIN_STARTBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed
 #define STARTBUTTON_DOWN digitalRead(PIN_STARTBUTTON)
 
@@ -35,6 +36,9 @@
 #define MODESWITCH_DOWN !digitalRead(PIN_MODESWITCH)
 #define PIN_MODELED_GREEN PA12
 #define PIN_MODELED_RED PA11
+uint8_t modeled_green=0;
+uint8_t modeled_red=0;
+long last_ledupdate=0;
 
 #define PIN_RELAISFRONT PB14 //connected to relais which presses the powerbutton of the hoverboard for the front wheels
 #define PIN_RELAISREAR PB15 //connected to relais which presses the powerbutton of the hoverboard for the rear wheels
@@ -198,6 +202,8 @@ void loop() {
     Serial.print("currentmode="); Serial.println(modeToString(currentmode));
     last_currentmode=currentmode;
   }
+
+  ledUpdate();
   
   modeloops();
 
@@ -546,7 +552,59 @@ String modeToString(uint8_t m) {
 }
 
 
+void ledUpdate () {
+  #define LEDUPDATETIME 20
+  #define FASTERRORBLINKDELAY 200 //period for startled to blink on error
+  if (loopmillis - last_ledupdate >= LEDUPDATETIME) {
+    last_ledupdate=loopmillis;
+    // ## StartLed ##
+    uint8_t _ledbrightness;
+    switch (currentmode) { //modeLed for different currentmodes
+      case booting: //Startled dimmed
+        startled=127;
+        break;
+      case idle: //Breathing Startled
+        _ledbrightness=uint8_t( (loopmillis/10)%(512) );
+        startled=_ledbrightness<=255 ? _ledbrightness : (512)-_ledbrightness; //reverse if >255 to go down again
+        break;
+      case on: //Startled on
+        startled=255;
+        break;
+      case error: //Startled blink
+        startled=(loopmillis/FASTERRORBLINKDELAY)%2==0 ? 0 : 255; // Blink led
+        break;
+      case off: //Startled off
+        startled=0; 
+        break;
+    }
 
+    // ## ModeLed ##
+    if (currentmode!=requestmode) { //ongoing modechange
+      modeled_green=0; modeled_red=0; //ModeLed=Off
+    }else{
+      switch (currentmode) { //modeLed for different currentmodes
+        case booting: 
+          modeled_green=255; modeled_red=127; //ModeLed=Lime
+          break;
+        case idle: 
+          modeled_green=255; modeled_red=255; //ModeLed=Yellow
+          break;
+        case on:
+          modeled_green=255; modeled_red=0; //ModeLed=Green
+          break;
+        case error:
+          modeled_green=0; modeled_red=255; //ModeLed=Red
+          break;
+        case off:
+          modeled_green=255; modeled_red=127; //ModeLed=Lime
+          break;
+      }
+    }
+    analogWrite(PIN_MODELED_GREEN, map(modeled_green, 0, 255, 0, 65535));
+    analogWrite(PIN_MODELED_RED, map(modeled_red, 0, 255, 0, 65535));
+    analogWrite(PIN_STARTLED, map(startled, 0, 255, 0, 65535));
+  }
+}
 
 
 
diff --git a/other/filtertest/filtertest.pde b/other/filtertest/filtertest.pde
index 65ebe20..831c103 100644
--- a/other/filtertest/filtertest.pde
+++ b/other/filtertest/filtertest.pde
@@ -183,9 +183,10 @@ void draw() {
     SoutSpeed.setValue(outSpeed);
   }
   
-  
-  //fill(inputValue*1.0/maxvalue*255);
-  //rect(0,0,width,100);  
+  //Breathing led test
+  int _b=int((loopmillis/10)%(255*2));
+  fill( _b<255 ? _b : (255*2)-_b );
+  rect(0,0,width,10);  
   
   
 }