diff --git a/conf/modules/actuators_faulhaber.xml b/conf/modules/actuators_faulhaber.xml
index 60e9147943..a06f51912c 100644
--- a/conf/modules/actuators_faulhaber.xml
+++ b/conf/modules/actuators_faulhaber.xml
@@ -13,7 +13,7 @@
   <settings>
     <dl_settings>
       <dl_settings NAME="Faulhaber">
-        <dl_setting var="faulhaber.mode" min="0" step="1" max="3" shortname="mode" values="INIT|VELOCITY|ERROR" module="modules/actuators/actuators_faulhaber" handler="SetMode"/>
+        <dl_setting var="faulhaber.mode" min="0" step="1" max="3" shortname="mode" values="INIT|VELOCITY|ERROR|REQ_ERROR|RESET_ERROR" module="modules/actuators/actuators_faulhaber" handler="SetMode"/>
         <dl_setting var="faulhaber.p_gain" min="0" step="0.001" max="1"/>
         <dl_setting var="faulhaber.max_velocity" min="0" step="100" max="100000"/>
         <dl_setting var="faulhaber.setpoint_position" min="0" step="1000" max="3600000"/>
@@ -40,12 +40,15 @@
     <define name="$(FAULHABER_DEV_UPPER)_BAUD" value="B115200"/>
     <file name="actuators_faulhaber.c"/>
     <test>
+        <define name="USE_UART0"/>
         <define name="USE_UART4"/>
         <define name="PERIODIC_FREQUENCY" value="500"/>
         <define name="FAULHABER_DEV" value="uart4"/>
         <define name="get_servo_min_FAULHABER" value=""/>
         <define name="get_servo_max_FAULHABER" value=""/>
         <define name="get_servo_idx_FAULHABER" value=""/>
+        <define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
+        <define name="DOWNLINK_DEVICE" value="uart0"/>
     </test>
   </makefile>
 </module>
diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.c b/sw/airborne/modules/actuators/actuators_faulhaber.c
index 7783434725..c585e91b21 100644
--- a/sw/airborne/modules/actuators/actuators_faulhaber.c
+++ b/sw/airborne/modules/actuators/actuators_faulhaber.c
@@ -39,7 +39,7 @@
 
 /* Maximum velocity for the position controller */
 #ifndef FAULHABER_MAX_VELOCITY
-#define FAULHABER_MAX_VELOCITY 14000
+#define FAULHABER_MAX_VELOCITY 12000
 #endif
 
 static struct uart_periph *faulhaber_dev = &(FAULHABER_DEV);
@@ -59,7 +59,7 @@ static struct faulhaber_parser_t faulhaber_p;
 struct faulhaber_t faulhaber;
 
 #define Polynom 0xD5
-uint8_t faulhaber_crc8(uint8_t u8Byte, uint8_t u8CRC)
+static uint8_t faulhaber_crc8(uint8_t u8Byte, uint8_t u8CRC)
 {
   uint8_t i;
   u8CRC = u8CRC ^ u8Byte;
@@ -272,7 +272,7 @@ void actuators_faulhaber_periodic(void)
           if(get_sys_time_float() - last_time < 0.01)
             break;
           
-          // Set to position mode
+          // Set to velocity mode
           static uint8_t data[] = { 0x60, 0x60, 0x00, 0x03 }; // Set 0x6060.00 to 0x03: Velocity mode
           faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
           last_time = get_sys_time_float();
@@ -327,6 +327,67 @@ void actuators_faulhaber_periodic(void)
           break;
         }
       }
+      break;
+    }
+
+    /* FH_MODE_REQ_ERR */
+    case FH_MODE_REQ_ERR: {
+      switch(faulhaber.state) {
+        case 0: {
+          // Request the status code
+          uint8_t data[] = { 0x20, 0x23, 0x00}; // Get 0x2320.00: Get the error code
+          faulhaber_send_command(faulhaber_dev, 0x01, data, 3);
+          faulhaber.state++;
+          break;
+        }
+        case 1: {
+          // Request the status code
+          uint8_t data[] = { 0x21, 0x23, 0x00}; // Get 0x2321.00: Get the error mask
+          faulhaber_send_command(faulhaber_dev, 0x01, data, 3);
+          faulhaber.state++;
+          break;
+        }
+        case 2: {
+          // Request the status code
+          uint8_t data[] = { 0x01, 0x10, 0x00}; // Get 0x1001.00: Get the error mask
+          faulhaber_send_command(faulhaber_dev, 0x01, data, 3);
+          faulhaber.state++;
+          break;
+          
+        }
+        default: {
+          // Do nothing and stop requesting
+          break;
+        }
+      }
+      break;
+    }
+
+    /* FH_MODE_RESET_ERR */
+    case FH_MODE_RESET_ERR: {
+      switch(faulhaber.state) {
+        case 0: {
+          static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0E, 0x00}; // Set 0x6040.00 to 0x000E: Try to start
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          break;
+        }
+        case 1: {
+          static uint8_t data[] = { 0x60, 0x60, 0x00, 0x03 }; // Set 0x6060.00 to 0x03: Velocity mode
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          faulhaber.state++;
+          break;
+        }
+        default: {
+          // Enable operation
+          static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state = 0;
+          faulhaber.mode = FH_MODE_VELOCITY;
+          break;
+        }
+      }
+      break;
     }
 
     /* Do nothing */
@@ -351,6 +412,10 @@ static void faulhaber_parse_msg(struct faulhaber_parser_t *p)
     // Send ABI message
     AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_FAULHABER_ID, &feedback, 1);
   }
+  // Write requests
+  else if(p->cmd_code == 0x02) {
+    // Do nothing
+  }
   // Parse the statuscode message
   else if(p->cmd_code == 0x05) {
     uint16_t status_code = p->data[0] | (p->data[1] << 8);
@@ -366,6 +431,25 @@ static void faulhaber_parse_msg(struct faulhaber_parser_t *p)
     // Target reached
     if(!faulhaber.target_reached && status_code&0x400)
       faulhaber.target_reached = true;
+    
+    // If the drive got disabled
+    if(!(status_code&0x0001) || !(status_code&0x0002) || !(status_code&0x0004)) {
+      char error_msg[250];
+      int rc = snprintf(error_msg, 200, "[FH]%d %04X", p->cmd_code, status_code);
+      DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, error_msg);
+    }
+  }
+  else {
+    // Unknown message
+    char error_msg[250];
+    int rc = snprintf(error_msg, 200, "[FH]%d ", p->cmd_code);
+    for(int i = 0; i < p->data_length; i++) {
+      rc += snprintf(error_msg + rc, 200 - rc, "%02X", p->data[i]);
+    }
+
+    if (rc > 0) {
+      DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, error_msg);
+    }
   }
 }
 
diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.h b/sw/airborne/modules/actuators/actuators_faulhaber.h
index c0cc8aaf4e..164b1bff95 100644
--- a/sw/airborne/modules/actuators/actuators_faulhaber.h
+++ b/sw/airborne/modules/actuators/actuators_faulhaber.h
@@ -32,7 +32,9 @@
 enum faulhaber_modes_t {
   FH_MODE_INIT,
   FH_MODE_VELOCITY,
-  FH_MODE_ERROR
+  FH_MODE_ERROR,
+  FH_MODE_REQ_ERR,
+  FH_MODE_RESET_ERR,
 };
 
 struct faulhaber_t {