diff --git a/.vscode/settings.json b/.vscode/settings.json
index 0e356f0bb1ff1c0753da7dce8a1e1468c780fbeb..659a6316bd75ac99b9a6019dab07525e71159181 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,6 +1,7 @@
 {
     "files.associations": {
         "controller.h": "c",
-        "typeinfo": "c"
+        "typeinfo": "c",
+        "attitude_controller.h": "c"
     }
 }
\ No newline at end of file
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c
index 20fb6886eb8ee449a0dcafa981f5e3b676b7cdbb..8de5d6748d89381ff07969b4fc7268af1e162531 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c
@@ -182,13 +182,23 @@ static uint8_t usbd_cf_Setup(void *pdev , USB_SETUP_REQ  *req)
   } else if(command == 0x02){
     //transition to DFU bootloader mode
 
-    // arm-none-eabi-gcc 4.9.0 does not correctly inline this
-    //     //     // MSP function, so we write it out explicitly here.
+    //shutdown any tasks running
+    RCC_DeInit();
+    SysTick->CTRL = 0;  //reset the Systick timer
+    SysTick->LOAD = 0;
+    SysTick->VAL  = 0;
+
+    __set_PRIMASK(1);   //disable interrupts
+
+    SYSCFG_MemoryRemapConfig(SYSCFG_MemoryRemap_SystemFlash);
+
+    //set main stack pointer to its default value
     __set_MSP(*((uint32_t*) 0x00000000));
-    //__ASM volatile ("movs r3, #0\nldr r3, [r3, #0]\nMSR msp, r3\n" : : : "r3", "sp");
 
+    //jump to boot loader (start of system memory plus 4)
     ((void (*)(void)) *((uint32_t*) 0x00000004))();
 
+    while(1);
 
   }