diff --git a/conf/autopilot/lisa_test_progs.makefile b/conf/autopilot/lisa_test_progs.makefile
index ff30b14cc6..4bd773ad43 100644
--- a/conf/autopilot/lisa_test_progs.makefile
+++ b/conf/autopilot/lisa_test_progs.makefile
@@ -921,5 +921,5 @@ test_csc_servo.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
 #test_csc_servo.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
 test_csc_servo.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./10.)'
 test_csc_servo.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_csc_servo.CFLAGS += -DUSE_CAN1
+test_csc_servo.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ
 test_csc_servo.srcs += can.c $(SRC_ARCH)/can_hw.c
diff --git a/sw/airborne/lisa/test_csc_servo.c b/sw/airborne/lisa/test_csc_servo.c
index 234d5b7856..a1572cb2b9 100644
--- a/sw/airborne/lisa/test_csc_servo.c
+++ b/sw/airborne/lisa/test_csc_servo.c
@@ -35,6 +35,11 @@ uint16_t servos[4];
 int main(void) {
   main_init();
 
+  servos[0] = 1;
+  servos[1] = 2;
+  servos[2] = 3;
+  servos[3] = 4;
+
   while(1) {
 	  if (sys_time_periodic())
 		  main_periodic_task();
@@ -51,15 +56,11 @@ static inline void main_init( void ) {
 }
 
 static inline void main_periodic_task( void ) {
-
-	servos[0] = 1;
-	servos[1] = 2;
-	servos[2] = 3;
-	servos[3] = 4;
+	servos[0]++;
 
 	can_transmit(1, 0, (uint8_t *)servos, 8);
 
-	LED_TOGGLE(2);
+	LED_TOGGLE(0);
 
 	LED_PERIODIC();
 }
diff --git a/sw/airborne/stm32/can_hw.c b/sw/airborne/stm32/can_hw.c
index b49d47b225..f1d5d80d5c 100644
--- a/sw/airborne/stm32/can_hw.c
+++ b/sw/airborne/stm32/can_hw.c
@@ -41,6 +41,7 @@
 #define GPIO_Pin_CAN_TX GPIO_Pin_12
 
 CanTxMsg can_tx_msg;
+CanRxMsg can_rx_msg;
 RCC_ClocksTypeDef rcc_clocks;
 
 void can_hw_init(void)
@@ -96,7 +97,7 @@ void can_hw_init(void)
 	can.CAN_SJW = CAN_SJW_1tq;
 	can.CAN_BS1 = CAN_BS1_3tq;
 	can.CAN_BS2 = CAN_BS2_5tq;
-	can.CAN_Prescaler = 4;
+	can.CAN_Prescaler = 11;
 	CAN_Init(CAN1, &can);
 
 	/* CAN filter init */
@@ -136,3 +137,30 @@ int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len)
 
 	return 0;
 }
+
+void usb_lp_can1_rx0_irq_handler(void)
+{
+	CAN_Receive(CAN1, CAN_FIFO0, &can_rx_msg);
+	//LED_TOGGLE(3);
+
+	if((can_rx_msg.Data[0] & 0x01) == 0x01){
+		LED_ON(4);
+	}else{
+		LED_OFF(4);
+	}
+	if((can_rx_msg.Data[0] & 0x02) == 0x02){
+		LED_ON(5);
+	}else{
+		LED_OFF(5);
+	}
+	if((can_rx_msg.Data[0] & 0x04) == 0x04){
+		LED_ON(6);
+	}else{
+		LED_OFF(6);
+	}
+	if((can_rx_msg.Data[0] & 0x08) == 0x08){
+		LED_ON(7);
+	}else{
+		LED_OFF(7);
+	}
+}
diff --git a/sw/airborne/stm32/stm32_vector_table.c b/sw/airborne/stm32/stm32_vector_table.c
index 6e94879b25..faa857af79 100644
--- a/sw/airborne/stm32/stm32_vector_table.c
+++ b/sw/airborne/stm32/stm32_vector_table.c
@@ -131,7 +131,7 @@ extern void usb_hp_can1_tx_irq_handler(void);
 
 #ifdef USE_USB_LP_CAN1_RX0_IRQ
 extern void usb_lp_can1_rx0_irq_handler(void);
-#define USB_LP_CAN1_RX0_IRQ_HANDLER usb_lp_can_rx0_irq_handler
+#define USB_LP_CAN1_RX0_IRQ_HANDLER usb_lp_can1_rx0_irq_handler
 #else
 #define USB_LP_CAN1_RX0_IRQ_HANDLER null_handler
 #endif