diff --git a/crazyflie/test/.gitignore b/crazyflie/test/.gitignore
new file mode 100644
index 000000000..567609b12
--- /dev/null
+++ b/crazyflie/test/.gitignore
@@ -0,0 +1 @@
+build/
diff --git a/crazyflie/test/Makefile b/crazyflie/test/Makefile
new file mode 100644
index 000000000..bd2940ff7
--- /dev/null
+++ b/crazyflie/test/Makefile
@@ -0,0 +1,23 @@
+SKETCH = test
+
+FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX
+
+PORT = /dev/ttyACM0
+
+BIN = build/$(SKETCH).ino.bin
+
+$(BIN): $(SKETCH).ino *.cpp *.h
+ arduino-cli compile --fqbn $(FQBN) --build-path=$(PWD)/build $(SKETCH).ino
+
+flash: $(BIN)
+ ./usb-bootloader.py
+ dfu-util -d 0483:df11 -a 0 -s 0x08004000:leave -D $(BIN)
+
+clean:
+ rm -rf build
+
+edit:
+ vim $(SKETCH).ino
+
+listen:
+ miniterm.py $(PORT) 115200
diff --git a/crazyflie/test/bootloader.cpp b/crazyflie/test/bootloader.cpp
new file mode 100755
index 000000000..119becf1d
--- /dev/null
+++ b/crazyflie/test/bootloader.cpp
@@ -0,0 +1,88 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ * @file bootloader.c
+ * Functions to handle transitioning from the firmware to bootloader (DFU) mode on startup
+ *
+ */
+
+#include
+
+#include "bootloader.h"
+
+#if 0
+// bootloader code based from micropython machine_bootloader function
+
+// STM32H7 has ECC and writes to RAM must be 64-bit so they are fully committed
+// to actual SRAM before a system reset occurs.
+#define BL_STATE_PTR ((uint64_t *) SRAM2_BASE) //start of 16kb SRAM bank in stm32f405
+#define BL_STATE_KEY (0x5a5) //arbitrary bit pattern used as a marker
+#define BL_STATE_KEY_MASK (0xfff)
+#define BL_STATE_KEY_SHIFT (32)
+#define BL_STATE_INVALID (0)
+#define BL_STATE_VALID(reg, addr) ((uint64_t)(reg) | ((uint64_t)((addr) | BL_STATE_KEY)) << BL_STATE_KEY_SHIFT)
+#define BL_STATE_GET_REG(s) ((s) & 0xffffffff)
+#define BL_STATE_GET_KEY(s) (((s) >> BL_STATE_KEY_SHIFT) & BL_STATE_KEY_MASK)
+#define BL_STATE_GET_ADDR(s) (((s) >> BL_STATE_KEY_SHIFT) & ~BL_STATE_KEY_MASK)
+
+
+/**
+ * @brief Branch directly to the bootloader address, setting the
+ * stack pointer and destination address first.
+ * Based from the micropython machine_bootloader function.
+ *
+ * @param r0 The register to utilize
+ * @param bl_addr The bootloader address to jump to
+ */
+static void branch_to_bootloader(uint32_t r0, uint32_t bl_addr){
+ __asm volatile (
+ "ldr r2, [r1, #0]\n" // get address of stack pointer
+ "msr msp, r2\n" // get stack pointer
+ "ldr r2, [r1, #4]\n" // get address of destination
+ "bx r2\n" // branch to bootloader
+ );
+ //unreachable code
+ while(1);
+}
+
+void check_enter_bootloader(){
+ uint64_t bl_state = *BL_STATE_PTR;
+ //set to invalid for next boot
+ *BL_STATE_PTR = BL_STATE_INVALID;
+
+ if(BL_STATE_GET_KEY(bl_state) == BL_STATE_KEY && (RCC->CSR & RCC_CSR_SFTRSTF)){
+ //if botloader data valid and was just reset with NVIC_SystemReset
+
+ //remap memory to system flash
+ SYSCFG_MemoryRemapConfig(SYSCFG_MemoryRemap_SystemFlash);
+ branch_to_bootloader(BL_STATE_GET_REG(bl_state), BL_STATE_GET_ADDR(bl_state));
+ }
+}
+
+void enter_bootloader(uint32_t r0, uint32_t bl_addr){
+ //set bootloader state values
+ *BL_STATE_PTR = BL_STATE_VALID(r0, bl_addr);
+
+ NVIC_SystemReset();
+}
+#endif
diff --git a/crazyflie/test/bootloader.h b/crazyflie/test/bootloader.h
new file mode 100755
index 000000000..c22689bbc
--- /dev/null
+++ b/crazyflie/test/bootloader.h
@@ -0,0 +1,56 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ * @file bootloader.h
+ * Functions to handle transitioning from the firmware to bootloader (DFU) mode on startup
+ *
+ */
+
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Check if memory is set after software
+ * reboot indicating we need to branch to the bootloader.
+ * Run every time on startup.
+ */
+void check_enter_bootloader();
+
+/**
+ * @brief Initiate the procedure to reboot into the bootloader.
+ *
+ * This function does not return, instead setting a flag to
+ * jump to the bootloader on the next start and
+ * issuing a software reset.
+ *
+ * @param r0 The register to utilize when jumping
+ * @param bl_addr The bootloader address to jump to
+ */
+void enter_bootloader(uint32_t r0, uint32_t bl_addr);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/crazyflie/test/test.ino b/crazyflie/test/test.ino
new file mode 100644
index 000000000..6d0ac66f7
--- /dev/null
+++ b/crazyflie/test/test.ino
@@ -0,0 +1,15 @@
+#include "bootloader.h"
+
+void setup()
+{
+ Serial.begin(115200);
+}
+
+void loop()
+{
+ static uint32_t count = 0;
+
+ Serial.println(count++);
+
+ delay(1000);
+}
diff --git a/crazyflie/test/usb-bootloader.py b/crazyflie/test/usb-bootloader.py
new file mode 100755
index 000000000..5afdb1745
--- /dev/null
+++ b/crazyflie/test/usb-bootloader.py
@@ -0,0 +1,23 @@
+#!/usr/bin/python3
+
+#send a USB request to the crazyflie, causing it to enter DFU mode (bootloader) automatically
+#flash new firmware using 'make flash_dfu'
+
+from time import sleep
+import usb
+import usb.core
+
+#find connected crazyflie, usb vendor and product id = 0483:5740
+dev = usb.core.find(idVendor=0x0483, idProduct=0x5740)
+if dev is None:
+ raise ValueError('Could not find any USB connected crazyflie')
+
+#send signal to enter bootloader
+try:
+ dev.ctrl_transfer(bmRequestType=usb.TYPE_VENDOR,
+ bRequest=0x01, wValue=0x01, wIndex=2)
+except IOError:
+ #io error expected because the crazyflie will not respond to USB request as it resets into the bootloader
+ #TODO usbd_cf_Setup function in firmware needs to return USBD_OK before rebooting to fix this
+ #sleep to allow time for crazyflie to get into DFU mode
+ sleep(0.5)