diff --git a/src/drivers/boards/px4fmu-v5/led.c b/src/drivers/boards/px4fmu-v5/led.c
index 5b5019a151..3ded48f7d0 100644
--- a/src/drivers/boards/px4fmu-v5/led.c
+++ b/src/drivers/boards/px4fmu-v5/led.c
@@ -79,10 +79,10 @@ static uint32_t g_ledmap[] = {
 
 #  define xlat(p) (p)
 static uint32_t g_ledmap[] = {
-	GPIO_nLED_BLUE,    // Indexed by LED_BLUE
-	GPIO_nLED_RED,     // Indexed by LED_RED, LED_AMBER
-	GPIO_nSAFETY_SWITCH_LED_OUT,  // Indexed by LED_SAFETY
-	GPIO_nLED_GREEN,   // Indexed by LED_GREEN
+	GPIO_nLED_BLUE,                     // Indexed by LED_BLUE
+	GPIO_nLED_RED,                      // Indexed by LED_RED, LED_AMBER
+	GPIO_nSAFETY_SWITCH_LED_OUT_INIT,   // Indexed by LED_SAFETY (defaulted to an input)
+	GPIO_nLED_GREEN,                    // Indexed by LED_GREEN
 };
 
 #endif
@@ -90,6 +90,10 @@ static uint32_t g_ledmap[] = {
 __EXPORT void led_init(void)
 {
 	/* Configure LED GPIOs for output */
+	if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
+		g_ledmap[2] = GPIO_nSAFETY_SWITCH_LED_OUT;
+	}
+
 	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
 		stm32_configgpio(g_ledmap[l]);
 	}