diff --git a/make/source.mk b/make/source.mk
index 7e0b332e01..569695a259 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -1,6 +1,7 @@
 COMMON_SRC = \
             build/build_config.c \
             build/debug.c \
+            build/debug_pin.c \
             build/version.c \
             $(TARGET_DIR_SRC) \
             main.c \
diff --git a/src/main/build/debug_pin.c b/src/main/build/debug_pin.c
new file mode 100644
index 0000000000..80409ee963
--- /dev/null
+++ b/src/main/build/debug_pin.c
@@ -0,0 +1,88 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_DEBUG_PIN
+
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+
+typedef struct dbgPin_s {
+    ioTag_t tag;
+    GPIO_TypeDef *gpio;
+    uint32_t setBSRR;
+    uint32_t resetBSRR;
+} dbgPin_t;
+
+dbgPin_t dbgPins[] = {
+    { .tag = IO_TAG(NONE) },
+};
+
+void dbgPinInit(void)
+{
+    for (unsigned i = 0; i < ARRAYLEN(dbgPins); i++) {
+        dbgPin_t *dbgPin = &dbgPins[i];
+        IO_t io = IOGetByTag(dbgPin->tag);
+        if (!io) {
+            continue;
+        }
+        IOConfigGPIO(io, IOCFG_OUT_PP);
+        dbgPin->gpio = IO_GPIO(io);
+        int pinSrc = IO_GPIO_PinSource(io);
+        dbgPin->setBSRR = (1 << pinSrc);
+        dbgPin->resetBSRR = (1 << (pinSrc + 16));
+    }
+}
+
+void dbgPinHi(int index)
+{
+    if ((unsigned)index >= ARRAYLEN(dbgPins)) {
+        return;
+    }
+
+    dbgPin_t *dbgPin = &dbgPins[index];
+    if (dbgPin->gpio) {
+#if defined(STM32F7)
+        dbgPin->gpio->BSRR = dbgPin->setBSRR;
+#else
+        dbgPin->gpio->BSRRL = dbgPin->setBSRR;
+#endif
+    }
+}
+
+void dbgPinLo(int index)
+{
+    if ((unsigned)index > ARRAYLEN(dbgPins)) {
+        return;
+    }
+
+    dbgPin_t *dbgPin = &dbgPins[index];
+
+    if (dbgPins->gpio) {
+#if defined(STM32F7)
+        dbgPin->gpio->BSRR = dbgPin->resetBSRR;
+#else
+        dbgPin->gpio->BSRRL = dbgPin->resetBSRR;
+#endif
+    }
+}
+
+#endif
diff --git a/src/main/build/debug_pin.h b/src/main/build/debug_pin.h
new file mode 100644
index 0000000000..36fb9663db
--- /dev/null
+++ b/src/main/build/debug_pin.h
@@ -0,0 +1,25 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+void dbgPinInit(void);
+void dbgPinHi(int index);
+void dbgPinLo(int index);
diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c
index b44e032114..072306437e 100644
--- a/src/main/drivers/dshot_bitbang.c
+++ b/src/main/drivers/dshot_bitbang.c
@@ -46,10 +46,13 @@
 
 #include "pg/motor.h"
 
-//TODO: Change these to be only used if USE_DEBUG_PIN is not defined once the debug_pin functionality has been merged
+#if defined(USE_DEBUG_PIN)
+#include "build/debug_pin.h"
+#else
 #define dbgPinInit()
 #define dbgPinHi(x)
 #define dbgPinLo(x)
+#endif
 
 FAST_RAM_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS];  // TIM1 or TIM8
 FAST_RAM_ZERO_INIT int usedMotorPacers = 0;
diff --git a/src/main/drivers/dshot_bitbang_stdperiph.c b/src/main/drivers/dshot_bitbang_stdperiph.c
index c282a67525..78749effc6 100644
--- a/src/main/drivers/dshot_bitbang_stdperiph.c
+++ b/src/main/drivers/dshot_bitbang_stdperiph.c
@@ -44,10 +44,13 @@
 
 #include "pg/motor.h"
 
-//TODO: Change these to be only used if USE_DEBUG_PIN is not defined once the debug_pin functionality has been merged
+#if defined(USE_DEBUG_PIN)
+#include "build/debug_pin.h"
+#else
 #define dbgPinInit()
 #define dbgPinHi(x)
 #define dbgPinLo(x)
+#endif
 
 void bbGpioSetup(bbMotor_t *bbMotor)
 {
diff --git a/src/main/target/NUCLEOF722/target.h b/src/main/target/NUCLEOF722/target.h
index 61eb2db4fa..5c64a8d84c 100644
--- a/src/main/target/NUCLEOF722/target.h
+++ b/src/main/target/NUCLEOF722/target.h
@@ -24,6 +24,8 @@
 
 #define USBD_PRODUCT_STRING "NucleoF722"
 
+#define USE_DEBUG_PIN
+
 //#define USE_ESC_TELEMETRY
 
 #define LED0_PIN   PB7  // blue