diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 830245572..e8b5742e3 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -447,9 +447,9 @@ void print_ahrs_raw()
 	valid_data_buffer = PIOS_ADC_GetRawBuffer();
 
 	if (total_conversion_blocks != previous_conversion + 1)
-		PIOS_LED_On(LED1);	// not keeping up
+		PIOS_LED_On(PIOS_LED_HEARTBEAT);	// not keeping up
 	else
-		PIOS_LED_Off(LED1);
+		PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 	previous_conversion = total_conversion_blocks;
 	
 	
@@ -463,9 +463,9 @@ void print_ahrs_raw()
 			    PIOS_ADC_NUM_PINS *
 			    sizeof(valid_data_buffer[0]));
 	if (result == 0)
-		PIOS_LED_Off(LED1);
+		PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 	else {
-		PIOS_LED_On(LED1);
+		PIOS_LED_On(PIOS_LED_HEARTBEAT);
 	}	
 }
 #endif
@@ -490,7 +490,7 @@ void print_ahrs_raw()
 		// wait for new raw samples
 		while (previous_conversion == total_conversion_blocks);
 		if ((previous_conversion + 1) != total_conversion_blocks)
-			PIOS_LED_On(LED1);                              // we are not keeping up
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);                              // we are not keeping up
 		previous_conversion = total_conversion_blocks;
 
 		// fetch the buffer address for the new samples
@@ -534,9 +534,9 @@ void print_ahrs_raw()
 		result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0]));
 
 		if (result != 0)
-			PIOS_LED_On(LED1);          // all data not sent
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);          // all data not sent
 		else
-			PIOS_LED_Off(LED1);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 	}
 #endif
 
@@ -616,7 +616,7 @@ for all data to be up to date before doing anything*/
 
 		// Alive signal
 		if (((total_conversion_blocks % 100) & 0xFFFE) == 0)
-			PIOS_LED_Toggle(LED1);
+			PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 
 		// Delay for valid data
 
diff --git a/flight/AHRS/pios_board.c b/flight/AHRS/pios_board.c
index aa5b20855..cbd99a32a 100644
--- a/flight/AHRS/pios_board.c
+++ b/flight/AHRS/pios_board.c
@@ -25,6 +25,29 @@
 
 #include <pios.h>
 
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_3,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_SPI)
 
 #include <pios_spi_priv.h>
@@ -365,7 +388,13 @@ void PIOS_Board_Init(void) {
 	/* Brings up System using CMSIS functions, enables the LEDs. */
 	PIOS_SYS_Init();
 
-	PIOS_LED_On(LED1);
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
+#if defined(PIOS_LED_HEARTBEAT)
+	PIOS_LED_On(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
 
 	/* Delay system */
 	PIOS_DELAY_Init();
diff --git a/flight/Bootloaders/AHRS/ahrs_slave_test.c b/flight/Bootloaders/AHRS/ahrs_slave_test.c
index 4ef55b975..d26957f1c 100644
--- a/flight/Bootloaders/AHRS/ahrs_slave_test.c
+++ b/flight/Bootloaders/AHRS/ahrs_slave_test.c
@@ -16,7 +16,7 @@ bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size) {
 	PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Wrote %d bytes to %d\r\n",
 			size, offset);
 	memcpy(buf, buffer, size);
-	PIOS_LED_Toggle(LED1);
+	PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 	return (true);
 }
 
@@ -28,7 +28,7 @@ bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size) {
 	PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Read %d bytes from %d\r\n",
 			size, offset);
 	memcpy(buffer, buf, size);
-	PIOS_LED_Toggle(LED1);
+	PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 	return (true);
 }
 
diff --git a/flight/Bootloaders/AHRS/main.c b/flight/Bootloaders/AHRS/main.c
index b284e6365..756d12566 100644
--- a/flight/Bootloaders/AHRS/main.c
+++ b/flight/Bootloaders/AHRS/main.c
@@ -79,12 +79,7 @@ int main() {
 			break;
 		}
 	}
-	//while(TRUE)
-	//	{
-	//		PIOS_LED_Toggle(LED1);
-	//		PIOS_DELAY_WaitmS(1000);
-	//	}
-	//GO_dfu = TRUE;
+
 	PIOS_IAP_Init();
 	GO_dfu = GO_dfu | PIOS_IAP_CheckRequest();// OR with app boot request
 	if (GO_dfu == FALSE) {
@@ -97,7 +92,7 @@ int main() {
 	PIOS_Board_Init();
 	boot_status = idle;
 	Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
-	PIOS_LED_On(LED1);
+	PIOS_LED_On(PIOS_LED_HEARTBEAT);
 	while (1) {
 		process_spi_request();
 	}
@@ -150,7 +145,7 @@ void process_spi_request(void) {
 		Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
 		lfsm_user_set_tx_v0(&user_tx_v0);
 		boot_status = idle;
-		PIOS_LED_Off(LED1);
+		PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
 		break;
 	case OPAHRS_MSG_V0_REQ_RESET:
@@ -158,7 +153,6 @@ void process_spi_request(void) {
 		PIOS_SYS_Reset();
 		break;
 	case OPAHRS_MSG_V0_REQ_VERSIONS:
-		//PIOS_LED_On(LED1);
 		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS);
 		user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION;
 		user_tx_v0.payload.user.v.rsp.versions.hw_version = (BOARD_TYPE << 8)
@@ -191,7 +185,7 @@ void process_spi_request(void) {
 		lfsm_user_set_tx_v0(&user_tx_v0);
 		break;
 	case OPAHRS_MSG_V0_REQ_FWUP_DATA:
-		PIOS_LED_On(LED1);
+		PIOS_LED_On(PIOS_LED_HEARTBEAT);
 		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
 		if (!(user_rx_v0.payload.user.v.req.fwup_data.adress
 				< bdinfo->fw_base)) {
@@ -209,7 +203,7 @@ void process_spi_request(void) {
 		} else {
 			boot_status = outside_dev_capabilities;
 		}
-		PIOS_LED_Off(LED1);
+		PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
 		lfsm_user_set_tx_v0(&user_tx_v0);
 		break;
@@ -227,10 +221,10 @@ void process_spi_request(void) {
 		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
 		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
 		lfsm_user_set_tx_v0(&user_tx_v0);
-		PIOS_LED_On(LED1);
+		PIOS_LED_On(PIOS_LED_HEARTBEAT);
 		if (PIOS_BL_HELPER_FLASH_Start() == TRUE) {
 			boot_status = started;
-			PIOS_LED_Off(LED1);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else {
 			boot_status = start_failed;
 			break;
@@ -254,15 +248,13 @@ void process_spi_request(void) {
 void jump_to_app() {
 	const struct pios_board_info * bdinfo = &pios_board_info_blob;
 
-	PIOS_LED_On(LED1);
+	PIOS_LED_On(PIOS_LED_HEARTBEAT);
 	if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
 		FLASH_Lock();
 		RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
 		RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
 		RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
 		RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
-		//_SetCNTR(0); // clear interrupt mask
-		//_SetISTR(0); // clear all requests
 
 		JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
 		Jump_To_Application = (pFunction) JumpAddress;
diff --git a/flight/Bootloaders/AHRS/pios_board.c b/flight/Bootloaders/AHRS/pios_board.c
index 756da9288..b779be4e3 100644
--- a/flight/Bootloaders/AHRS/pios_board.c
+++ b/flight/Bootloaders/AHRS/pios_board.c
@@ -25,6 +25,29 @@
 
 #include <pios.h>
 
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_3,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_SPI)
 
 #include <pios_spi_priv.h>
@@ -138,6 +161,10 @@ void PIOS_Board_Init() {
 		return;
 	}
 
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
 	/* Set up the SPI interface to the OP board */
 	if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
 		PIOS_DEBUG_Assert(0);
diff --git a/flight/Bootloaders/BootloaderUpdater/main.c b/flight/Bootloaders/BootloaderUpdater/main.c
index 9b1691dca..195a25b33 100644
--- a/flight/Bootloaders/BootloaderUpdater/main.c
+++ b/flight/Bootloaders/BootloaderUpdater/main.c
@@ -50,14 +50,14 @@ int main() {
 
 	PIOS_SYS_Init();
 	PIOS_Board_Init();
-	PIOS_LED_On(LED1);
+	PIOS_LED_On(PIOS_LED_HEARTBEAT);
 	PIOS_DELAY_WaitmS(3000);
-	PIOS_LED_Off(LED1);
+	PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 
 	/// Self overwrite check
 	uint32_t base_address = SCB->VTOR;
 	if ((0x08000000 + embedded_image_size) > base_address)
-		error(LED1);
+		error(PIOS_LED_HEARTBEAT);
 	///
 	FLASH_Unlock();
 
@@ -82,7 +82,7 @@ int main() {
 	}
 
 	if (fail == true)
-		error(LED1);
+		error(PIOS_LED_HEARTBEAT);
 
 
 	///
@@ -90,7 +90,7 @@ int main() {
 	/// Bootloader programing
 	for (uint32_t offset = 0; offset < embedded_image_size/sizeof(uint32_t); ++offset) {
 		bool result = false;
-		PIOS_LED_Toggle(LED1);
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 		for (uint8_t retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
 			if (result == false) {
 				result = (FLASH_ProgramWord(0x08000000 + (offset * 4), embedded_image_start[offset])
@@ -98,13 +98,13 @@ int main() {
 			}
 		}
 		if (result == false)
-			error(LED1);
+			error(PIOS_LED_HEARTBEAT);
 	}
 	///
 	for (uint8_t x = 0; x < 3; ++x) {
-			PIOS_LED_On(LED1);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			PIOS_DELAY_WaitmS(1000);
-			PIOS_LED_Off(LED1);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 			PIOS_DELAY_WaitmS(1000);
 	}
 
diff --git a/flight/Bootloaders/CopterControl/main.c b/flight/Bootloaders/CopterControl/main.c
index 7a61ae9e9..17733bbef 100644
--- a/flight/Bootloaders/CopterControl/main.c
+++ b/flight/Bootloaders/CopterControl/main.c
@@ -69,7 +69,6 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
 uint8_t processRX();
 void jump_to_app();
 
-#define BLUE LED1
 int main() {
 	PIOS_SYS_Init();
 	if (BSL_HOLD_STATE == 0)
@@ -112,7 +111,7 @@ int main() {
 		case DFUidle:
 			period1 = 5000;
 			sweep_steps1 = 100;
-			PIOS_LED_Off(BLUE);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		case uploading:
@@ -124,12 +123,12 @@ int main() {
 		case downloading:
 			period1 = 2500;
 			sweep_steps1 = 50;
-			PIOS_LED_Off(BLUE);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		case BLidle:
 			period1 = 0;
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		default://error
@@ -141,19 +140,19 @@ int main() {
 
 		if (period1 != 0) {
 			if (LedPWM(period1, sweep_steps1, stopwatch))
-				PIOS_LED_On(BLUE);
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			else
-				PIOS_LED_Off(BLUE);
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 
 		if (period2 != 0) {
 			if (LedPWM(period2, sweep_steps2, stopwatch))
-				PIOS_LED_On(BLUE);
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			else
-				PIOS_LED_Off(BLUE);
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else
-			PIOS_LED_Off(BLUE);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 
 		if (stopwatch > 50 * 1000 * 1000)
 			stopwatch = 0;
diff --git a/flight/Bootloaders/CopterControl/pios_board.c b/flight/Bootloaders/CopterControl/pios_board.c
index a70537fcc..451a92871 100644
--- a/flight/Bootloaders/CopterControl/pios_board.c
+++ b/flight/Bootloaders/CopterControl/pios_board.c
@@ -25,7 +25,28 @@
 
 #include <pios.h>
 
-// ***********************************************************************************
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_6,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
 
 #if defined(PIOS_INCLUDE_COM_MSG)
 
@@ -33,8 +54,6 @@
 
 #endif /* PIOS_INCLUDE_COM_MSG */
 
-// ***********************************************************************************
-
 #if defined(PIOS_INCLUDE_USB)
 #include "pios_usb_priv.h"
 
@@ -90,6 +109,10 @@ void PIOS_Board_Init(void) {
 	/* Initialize the PiOS library */
 	PIOS_GPIO_Init();
 
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_USB)
 	/* Initialize board specific USB data */
 	PIOS_USB_BOARD_DATA_Init();
diff --git a/flight/Bootloaders/OpenPilot/main.c b/flight/Bootloaders/OpenPilot/main.c
index 3a06c9751..da39e2521 100644
--- a/flight/Bootloaders/OpenPilot/main.c
+++ b/flight/Bootloaders/OpenPilot/main.c
@@ -98,8 +98,6 @@ uint8_t processRX();
 void jump_to_app();
 uint32_t sspTimeSource();
 
-#define BLUE LED1
-#define RED	LED2
 #define LED_PWM_TIMER	TIM6
 int main() {
 	PIOS_SYS_Init();
@@ -159,7 +157,7 @@ int main() {
 		case DFUidle:
 			period1 = 50;
 			sweep_steps1 = 100;
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 			period2 = 0;
 			break;
 		case uploading:
@@ -171,12 +169,12 @@ int main() {
 		case downloading:
 			period1 = 25;
 			sweep_steps1 = 50;
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 			period2 = 0;
 			break;
 		case BLidle:
 			period1 = 0;
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		default://error
@@ -188,19 +186,19 @@ int main() {
 
 		if (period1 != 0) {
 			if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER)))
-				PIOS_LED_On(BLUE);
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			else
-				PIOS_LED_Off(BLUE);
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 
 		if (period2 != 0) {
 			if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER)))
-				PIOS_LED_On(RED);
+				PIOS_LED_On(PIOS_LED_ALARM);
 			else
-				PIOS_LED_Off(RED);
+				PIOS_LED_Off(PIOS_LED_ALARM);
 		} else
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 
 		if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100)
 			STOPWATCH_Reset(LED_PWM_TIMER);
diff --git a/flight/Bootloaders/OpenPilot/pios_board.c b/flight/Bootloaders/OpenPilot/pios_board.c
index d1ba74d8b..43101aa03 100644
--- a/flight/Bootloaders/OpenPilot/pios_board.c
+++ b/flight/Bootloaders/OpenPilot/pios_board.c
@@ -29,6 +29,39 @@
 //#include <openpilot.h>
 //#include <uavobjectsinit.h>
 
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_12,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+	[PIOS_LED_ALARM] = {
+		.pin = {
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_13,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_SPI)
 
 #include <pios_spi_priv.h>
@@ -273,6 +306,10 @@ void PIOS_Board_Init(void) {
 
 	PIOS_GPIO_Init();
 
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_USB)
 	/* Initialize board specific USB data */
 	PIOS_USB_BOARD_DATA_Init();
diff --git a/flight/Bootloaders/PipXtreme/main.c b/flight/Bootloaders/PipXtreme/main.c
index 602193f5c..908589178 100644
--- a/flight/Bootloaders/PipXtreme/main.c
+++ b/flight/Bootloaders/PipXtreme/main.c
@@ -69,8 +69,6 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
 uint8_t processRX();
 void jump_to_app();
 
-#define BLUE LED1
-#define RED	LED4
 int main() {
 	PIOS_SYS_Init();
 	if (BSL_HOLD_STATE == 0)
@@ -113,7 +111,7 @@ int main() {
 		case DFUidle:
 			period1 = 5000;
 			sweep_steps1 = 100;
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 			period2 = 0;
 			break;
 		case uploading:
@@ -125,12 +123,12 @@ int main() {
 		case downloading:
 			period1 = 2500;
 			sweep_steps1 = 50;
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 			period2 = 0;
 			break;
 		case BLidle:
 			period1 = 0;
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		default://error
@@ -142,19 +140,19 @@ int main() {
 
 		if (period1 != 0) {
 			if (LedPWM(period1, sweep_steps1, stopwatch))
-				PIOS_LED_On(BLUE);
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			else
-				PIOS_LED_Off(BLUE);
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 
 		if (period2 != 0) {
 			if (LedPWM(period2, sweep_steps2, stopwatch))
-				PIOS_LED_On(RED);
+				PIOS_LED_On(PIOS_LED_ALARM);
 			else
-				PIOS_LED_Off(RED);
+				PIOS_LED_Off(PIOS_LED_ALARM);
 		} else
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 
 		if (stopwatch > 50 * 1000 * 1000)
 			stopwatch = 0;
diff --git a/flight/Bootloaders/PipXtreme/pios_board.c b/flight/Bootloaders/PipXtreme/pios_board.c
index a70537fcc..60ec1db76 100644
--- a/flight/Bootloaders/PipXtreme/pios_board.c
+++ b/flight/Bootloaders/PipXtreme/pios_board.c
@@ -25,7 +25,58 @@
 
 #include <pios.h>
 
-// ***********************************************************************************
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_USB] = {
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_3,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+	[PIOS_LED_LINK] = {
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_5,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+	[PIOS_LED_RX] = {
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_6,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+	[PIOS_LED_TX] = {
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_7,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
 
 #if defined(PIOS_INCLUDE_COM_MSG)
 
@@ -33,8 +84,6 @@
 
 #endif /* PIOS_INCLUDE_COM_MSG */
 
-// ***********************************************************************************
-
 #if defined(PIOS_INCLUDE_USB)
 #include "pios_usb_priv.h"
 
diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c
index a7cd909d9..3c64141ec 100644
--- a/flight/CopterControl/System/coptercontrol.c
+++ b/flight/CopterControl/System/coptercontrol.c
@@ -72,7 +72,9 @@ int main()
 
 #ifdef ERASE_FLASH
 	PIOS_Flash_W25X_EraseChip();
-	PIOS_LED_Off(LED1);
+#if defined(PIOS_LED_HEARTBEAT)
+	PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
 	while (1) ;
 #endif
 
@@ -88,13 +90,14 @@ int main()
 	/* If all is well we will never reach here as the scheduler will now be running. */
 
 	/* Do some indication to user that something bad just happened */
-	PIOS_LED_Off(LED1);
 	while (1) {
-		PIOS_LED_Toggle(LED1);
+#if defined(PIOS_LED_HEARTBEAT)
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
 		PIOS_DELAY_WaitmS(100);
 	}
 
-    return 0;
+	return 0;
 }
 
 /**
diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c
index a7197768d..afac2dff0 100644
--- a/flight/CopterControl/System/pios_board.c
+++ b/flight/CopterControl/System/pios_board.c
@@ -34,6 +34,29 @@
 #include <manualcontrolsettings.h>
 #include <gcsreceiver.h>
 
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_6,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_SPI)
 
 #include <pios_spi_priv.h>
@@ -1058,6 +1081,15 @@ void PIOS_Board_Init(void) {
 	EventDispatcherInitialize();
 	UAVObjInitialize();
 
+#if defined(PIOS_INCLUDE_RTC)
+	/* Initialize the real-time clock and its associated tick */
+	PIOS_RTC_Init(&pios_rtc_main_cfg);
+#endif
+
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
 	HwSettingsInitialize();
 
 #ifndef ERASE_FLASH
@@ -1080,11 +1112,6 @@ void PIOS_Board_Init(void) {
 		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
 	}
 
-#if defined(PIOS_INCLUDE_RTC)
-	/* Initialize the real-time clock and its associated tick */
-	PIOS_RTC_Init(&pios_rtc_main_cfg);
-#endif
-
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
diff --git a/flight/INS/ins.c b/flight/INS/ins.c
index 92224a239..2fe538acc 100644
--- a/flight/INS/ins.c
+++ b/flight/INS/ins.c
@@ -94,18 +94,18 @@ void blink(int led, int times)
 void test_accel()
 {
 	if(PIOS_BMA180_Test())
-		blink(LED1, 1);
+		blink(PIOS_LED_HEARTBEAT, 1);
 	else
-		blink(LED2, 1);
+		blink(PIOS_LED_ALARM, 1);
 }
 
 #if defined (PIOS_INCLUDE_HMC5883)
 void test_mag()
 {
 	if(PIOS_HMC5883_Test())
-		blink(LED1, 2);
+		blink(PIOS_LED_HEARTBEAT, 2);
 	else
-		blink(LED2, 2);
+		blink(PIOS_LED_ALARM, 2);
 }
 #endif
 
@@ -113,9 +113,9 @@ void test_mag()
 void test_pressure()
 {
 	if(PIOS_BMP085_Test())
-		blink(LED1, 3);
+		blink(PIOS_LED_HEARTBEAT, 3);
 	else
-		blink(LED2, 3);
+		blink(PIOS_LED_ALARM, 3);
 }
 #endif
 
@@ -123,9 +123,9 @@ void test_pressure()
 void test_imu()
 {
 	if(PIOS_IMU3000_Test())
-		blink(LED1, 4);
+		blink(PIOS_LED_HEARTBEAT, 4);
 	else
-		blink(LED2, 4);
+		blink(PIOS_LED_ALARM, 4);
 }
 #endif
 
diff --git a/flight/INS/pios_board.c b/flight/INS/pios_board.c
index ffbf27c45..f1ef66d8e 100644
--- a/flight/INS/pios_board.c
+++ b/flight/INS/pios_board.c
@@ -31,6 +31,39 @@
 
 #include <pios.h>
 
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_3,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+	[PIOS_LED_ALARM] = {
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_2,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_SPI)
 
 #include <pios_spi_priv.h>
@@ -520,6 +553,10 @@ void PIOS_Board_Init(void) {
 	/* Brings up System using CMSIS functions, enables the LEDs. */
 	PIOS_SYS_Init();
 
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
 	/* Delay system */
 	PIOS_DELAY_Init();
 
diff --git a/flight/Modules/FirmwareIAP/firmwareiap.c b/flight/Modules/FirmwareIAP/firmwareiap.c
index 21678397e..aaf2622bd 100644
--- a/flight/Modules/FirmwareIAP/firmwareiap.c
+++ b/flight/Modules/FirmwareIAP/firmwareiap.c
@@ -230,11 +230,14 @@ static uint32_t get_time(void)
  * Executed by event dispatcher callback to reset INS before resetting OP 
  */
 static void resetTask(UAVObjEvent * ev)
-{		
-	PIOS_LED_Toggle(LED1);
-#if (PIOS_LED_NUM > 1)
-	PIOS_LED_Toggle(LED2);
-#endif
+{
+#if defined (PIOS_LED_HEARTBEAT)
+	PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
+
+#if defined (PIOS_LED_ALARM)
+	PIOS_LED_Toggle(PIOS_LED_ALARM);
+#endif	/* PIOS_LED_ALARM */
 	
 	if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) {
 		lastResetSysTime = xTaskGetTickCount();
diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c
index 7e476f686..cae6af590 100644
--- a/flight/Modules/System/systemmod.c
+++ b/flight/Modules/System/systemmod.c
@@ -177,16 +177,18 @@ static void systemTask(void *parameters)
 #endif
 
 		// Flash the heartbeat LED
-		PIOS_LED_Toggle(LED1);
+#if defined(PIOS_LED_HEARTBEAT)
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
 
 		// Turn on the error LED if an alarm is set
-#if (PIOS_LED_NUM > 1)
+#if defined (PIOS_LED_ALARM)
 		if (AlarmsHasWarnings()) {
-			PIOS_LED_On(LED2);
+			PIOS_LED_On(PIOS_LED_ALARM);
 		} else {
-			PIOS_LED_Off(LED2);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 		}
-#endif
+#endif	/* PIOS_LED_ALARM */
 
 		FlightStatusData flightStatus;
 		FlightStatusGet(&flightStatus);
diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c
index c90f5fd81..affe65926 100644
--- a/flight/OpenPilot/System/openpilot.c
+++ b/flight/OpenPilot/System/openpilot.c
@@ -110,13 +110,12 @@ int main()
 	/* If all is well we will never reach here as the scheduler will now be running. */
 
 	/* Do some indication to user that something bad just happened */
-	PIOS_LED_Off(LED1); \
-	for(;;) { \
-		PIOS_LED_Toggle(LED1); \
-		PIOS_DELAY_WaitmS(100); \
-	};
-
-	return 0;
+	while (1) {
+#if defined(PIOS_LED_HEARTBEAT)
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
+		PIOS_DELAY_WaitmS(100);
+	}
 }
 
 
diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c
index 0fb873f0a..16da82cdd 100644
--- a/flight/OpenPilot/System/pios_board.c
+++ b/flight/OpenPilot/System/pios_board.c
@@ -36,6 +36,39 @@
 //#define I2C_DEBUG_PIN			0
 //#define USART_GPS_DEBUG_PIN		1
 
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_12,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+	[PIOS_LED_ALARM] = {
+		.pin = {
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_13,
+				.GPIO_Mode  = GPIO_Mode_Out_PP,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
+
 #if defined(PIOS_INCLUDE_SPI)
 
 #include <pios_spi_priv.h>
@@ -1128,6 +1161,15 @@ void PIOS_Board_Init(void) {
 	EventDispatcherInitialize();
 	UAVObjInitialize();
 
+#if defined(PIOS_INCLUDE_RTC)
+	/* Initialize the real-time clock and its associated tick */
+	PIOS_RTC_Init(&pios_rtc_main_cfg);
+#endif
+
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
 	HwSettingsInitialize();
 
 	PIOS_WDG_Init();
@@ -1146,11 +1188,6 @@ void PIOS_Board_Init(void) {
 		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
 	}
 
-#if defined(PIOS_INCLUDE_RTC)
-	/* Initialize the real-time clock and its associated tick */
-	PIOS_RTC_Init(&pios_rtc_main_cfg);
-#endif
-
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
diff --git a/flight/PiOS/Boards/STM32103CB_AHRS.h b/flight/PiOS/Boards/STM32103CB_AHRS.h
index 26d88fce1..1852d60fb 100644
--- a/flight/PiOS/Boards/STM32103CB_AHRS.h
+++ b/flight/PiOS/Boards/STM32103CB_AHRS.h
@@ -71,13 +71,7 @@ TIM8  |           |           |           |
 //------------------------
 // PIOS_LED
 //------------------------
-#define PIOS_LED_LED1_GPIO_PORT			GPIOA
-#define PIOS_LED_LED1_GPIO_PIN			GPIO_Pin_3
-#define PIOS_LED_LED1_GPIO_CLK			RCC_APB2Periph_GPIOA
-#define PIOS_LED_NUM				1
-#define PIOS_LED_PORTS				{ PIOS_LED_LED1_GPIO_PORT }
-#define PIOS_LED_PINS				{ PIOS_LED_LED1_GPIO_PIN }
-#define PIOS_LED_CLKS				{ PIOS_LED_LED1_GPIO_CLK }
+#define PIOS_LED_HEARTBEAT	0
 
 //-------------------------
 // System Settings
diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h
index 7773b9092..05214876f 100644
--- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h
+++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h
@@ -83,13 +83,7 @@ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
 //------------------------
 // PIOS_LED
 //------------------------
-#define PIOS_LED_LED1_GPIO_PORT			GPIOA
-#define PIOS_LED_LED1_GPIO_PIN			GPIO_Pin_6
-#define PIOS_LED_LED1_GPIO_CLK			RCC_APB2Periph_GPIOA
-#define PIOS_LED_NUM				1
-#define PIOS_LED_PORTS				{ PIOS_LED_LED1_GPIO_PORT }
-#define PIOS_LED_PINS				{ PIOS_LED_LED1_GPIO_PIN }
-#define PIOS_LED_CLKS				{ PIOS_LED_LED1_GPIO_CLK }
+#define PIOS_LED_HEARTBEAT	0
 
 //-------------------------
 // System Settings
diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
index b918ead67..8d04e05a4 100644
--- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
+++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
@@ -80,42 +80,29 @@ TIM4  |                     STOPWATCH                    |
 // *****************************************************************
 // PIOS_LED
 
-#define PIOS_LED_LED1_GPIO_PORT			GPIOA					// USB Activity LED
-#define PIOS_LED_LED1_GPIO_PIN			GPIO_Pin_3
-#define PIOS_LED_LED1_GPIO_CLK			RCC_APB2Periph_GPIOA
+#define PIOS_LED_USB	0
+#define PIOS_LED_LINK	1
+#define PIOS_LED_RX	2
+#define PIOS_LED_TX	3
 
-#define PIOS_LED_LED2_GPIO_PORT			GPIOB					// LINK LED
-#define PIOS_LED_LED2_GPIO_PIN			GPIO_Pin_5
-#define PIOS_LED_LED2_GPIO_CLK			RCC_APB2Periph_GPIOB
+#define PIOS_LED_HEARTBEAT PIOS_LED_USB
+#define PIOS_LED_ALARM PIOS_LED_TX
 
-#define PIOS_LED_LED3_GPIO_PORT			GPIOB					// RX LED
-#define PIOS_LED_LED3_GPIO_PIN			GPIO_Pin_6
-#define PIOS_LED_LED3_GPIO_CLK			RCC_APB2Periph_GPIOB
+#define USB_LED_ON					PIOS_LED_On(PIOS_LED_USB)
+#define USB_LED_OFF					PIOS_LED_Off(PIOS_LED_USB)
+#define USB_LED_TOGGLE					PIOS_LED_Toggle(PIOS_LED_USB)
 
-#define PIOS_LED_LED4_GPIO_PORT			GPIOB					// TX LED
-#define PIOS_LED_LED4_GPIO_PIN			GPIO_Pin_7
-#define PIOS_LED_LED4_GPIO_CLK			RCC_APB2Periph_GPIOB
+#define LINK_LED_ON					PIOS_LED_On(PIOS_LED_LINK)
+#define LINK_LED_OFF					PIOS_LED_Off(PIOS_LED_LINK)
+#define LINK_LED_TOGGLE					PIOS_LED_Toggle(PIOS_LED_LINK)
 
-#define PIOS_LED_NUM					4
-#define PIOS_LED_PORTS					{ PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT, PIOS_LED_LED3_GPIO_PORT, PIOS_LED_LED4_GPIO_PORT }
-#define PIOS_LED_PINS					{ PIOS_LED_LED1_GPIO_PIN,  PIOS_LED_LED2_GPIO_PIN,  PIOS_LED_LED3_GPIO_PIN,  PIOS_LED_LED4_GPIO_PIN }
-#define PIOS_LED_CLKS					{ PIOS_LED_LED1_GPIO_CLK,  PIOS_LED_LED2_GPIO_CLK,  PIOS_LED_LED3_GPIO_CLK,  PIOS_LED_LED4_GPIO_CLK }
+#define RX_LED_ON					PIOS_LED_On(PIOS_LED_RX)
+#define RX_LED_OFF					PIOS_LED_Off(PIOS_LED_RX)
+#define RX_LED_TOGGLE					PIOS_LED_Toggle(PIOS_LED_RX)
 
-#define USB_LED_ON						PIOS_LED_On(LED1)
-#define USB_LED_OFF						PIOS_LED_Off(LED1)
-#define USB_LED_TOGGLE					PIOS_LED_Toggle(LED1)
-
-#define LINK_LED_ON						PIOS_LED_On(LED2)
-#define LINK_LED_OFF					PIOS_LED_Off(LED2)
-#define LINK_LED_TOGGLE					PIOS_LED_Toggle(LED2)
-
-#define RX_LED_ON						PIOS_LED_On(LED3)
-#define RX_LED_OFF						PIOS_LED_Off(LED3)
-#define RX_LED_TOGGLE					PIOS_LED_Toggle(LED3)
-
-#define TX_LED_ON						PIOS_LED_On(LED4)
-#define TX_LED_OFF						PIOS_LED_Off(LED4)
-#define TX_LED_TOGGLE					PIOS_LED_Toggle(LED4)
+#define TX_LED_ON					PIOS_LED_On(PIOS_LED_TX)
+#define TX_LED_OFF					PIOS_LED_Off(PIOS_LED_TX)
+#define TX_LED_TOGGLE					PIOS_LED_Toggle(PIOS_LED_TX)
 
 // *****************************************************************
 // Timer interrupt
diff --git a/flight/PiOS/Boards/STM3210E_INS.h b/flight/PiOS/Boards/STM3210E_INS.h
index 37ca4abe3..77283a5e4 100644
--- a/flight/PiOS/Boards/STM3210E_INS.h
+++ b/flight/PiOS/Boards/STM3210E_INS.h
@@ -76,16 +76,8 @@ TIM8  |           |           |           |
 //------------------------
 // PIOS_LED
 //------------------------
-#define PIOS_LED_LED1_GPIO_PORT                 GPIOA
-#define PIOS_LED_LED1_GPIO_PIN                  GPIO_Pin_3
-#define PIOS_LED_LED1_GPIO_CLK                  RCC_APB2Periph_GPIOA
-#define PIOS_LED_LED2_GPIO_PORT                 GPIOA
-#define PIOS_LED_LED2_GPIO_PIN                  GPIO_Pin_2
-#define PIOS_LED_LED2_GPIO_CLK                  RCC_APB2Periph_GPIOA
-#define PIOS_LED_NUM                            2
-#define PIOS_LED_PORTS                          { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT }
-#define PIOS_LED_PINS                           { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN }
-#define PIOS_LED_CLKS                           { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK }
+#define PIOS_LED_HEARTBEAT	0
+#define PIOS_LED_ALARM		1
 
 //------------------------
 // PIOS_SPI
diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h
index 7099234c6..4b5067888 100644
--- a/flight/PiOS/Boards/STM3210E_OP.h
+++ b/flight/PiOS/Boards/STM3210E_OP.h
@@ -91,16 +91,8 @@ TIM8  | Servo 5   | Servo 6   | Servo 7   | Servo 8
 //------------------------
 // PIOS_LED
 //------------------------
-#define PIOS_LED_LED1_GPIO_PORT                 GPIOC
-#define PIOS_LED_LED1_GPIO_PIN                  GPIO_Pin_12
-#define PIOS_LED_LED1_GPIO_CLK                  RCC_APB2Periph_GPIOC
-#define PIOS_LED_LED2_GPIO_PORT                 GPIOC
-#define PIOS_LED_LED2_GPIO_PIN                  GPIO_Pin_13
-#define PIOS_LED_LED2_GPIO_CLK                  RCC_APB2Periph_GPIOC
-#define PIOS_LED_NUM                            2
-#define PIOS_LED_PORTS                          { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT }
-#define PIOS_LED_PINS                           { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN }
-#define PIOS_LED_CLKS                           { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK }
+#define PIOS_LED_HEARTBEAT	0
+#define PIOS_LED_ALARM		1
 
 //------------------------
 // PIOS_SPI
diff --git a/flight/PiOS/Common/pios_flashfs_objlist.c b/flight/PiOS/Common/pios_flashfs_objlist.c
index e02683657..b4322ab78 100644
--- a/flight/PiOS/Common/pios_flashfs_objlist.c
+++ b/flight/PiOS/Common/pios_flashfs_objlist.c
@@ -81,7 +81,9 @@ int32_t PIOS_FLASHFS_Init()
 		if(object_table_magic != OBJECT_TABLE_MAGIC) {
 			if(magic_fail_count++ > MAX_BADMAGIC) {
 				PIOS_FLASHFS_ClearObjectTableHeader();
-				PIOS_LED_Toggle(LED1);
+#if defined(PIOS_LED_HEARTBEAT)
+				PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
 				magic_fail_count = 0;
 				magic_good = true;
 			} else {
diff --git a/flight/PiOS/STM32F10x/pios_led.c b/flight/PiOS/STM32F10x/pios_led.c
index 76b5bcccf..5db5cc884 100644
--- a/flight/PiOS/STM32F10x/pios_led.c
+++ b/flight/PiOS/STM32F10x/pios_led.c
@@ -33,57 +33,107 @@
 
 #if defined(PIOS_INCLUDE_LED)
 
-/* Private Function Prototypes */
+#include <pios_led_priv.h>
 
-/* Local Variables */
-static GPIO_TypeDef *LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS;
-static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS;
-static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS;
+const static struct pios_led_cfg * led_cfg;
 
 /**
 * Initialises all the LED's
 */
-void PIOS_LED_Init(void)
+int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg)
 {
-	GPIO_InitTypeDef GPIO_InitStructure;
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
-	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+	PIOS_Assert(cfg);
 
-	for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) {
-		RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE);
-		GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum];
-		GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure);
+	/* Store away the config in a global used by API functions */
+	led_cfg = cfg;
 
-		/* LED's Off */
-		LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum];
+	for (uint8_t i = 0; i < cfg->num_leds; i++) {
+		const struct pios_led * led = &(cfg->leds[i]);
+
+		/* Enable the peripheral clock for the GPIO */
+		switch ((uint32_t)led->pin.gpio) {
+		case (uint32_t) GPIOA:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
+			break;
+		case (uint32_t) GPIOB:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+			break;
+		case (uint32_t) GPIOC:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+			break;
+		default:
+			PIOS_Assert(0);
+			break;
+		}
+
+		if (led->remap) {
+			GPIO_PinRemapConfig(led->remap, ENABLE);
+		}
+
+		GPIO_Init(led->pin.gpio, &led->pin.init);
+
+		PIOS_LED_Off(i);
 	}
+
+	return 0;
 }
 
 /**
 * Turn on LED
-* \param[in] LED LED Name (LED1, LED2)
+* \param[in] LED LED id
 */
-void PIOS_LED_On(LedTypeDef LED)
+void PIOS_LED_On(uint32_t led_id)
 {
-	LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED];
+	PIOS_Assert(led_cfg);
+
+	if (led_id >= led_cfg->num_leds) {
+		/* LED index out of range */
+		return;
+	}
+
+	const struct pios_led * led = &(led_cfg->leds[led_id]);
+
+	GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
 }
 
 /**
 * Turn off LED
-* \param[in] LED LED Name (LED1, LED2)
+* \param[in] LED LED id
 */
-void PIOS_LED_Off(LedTypeDef LED)
+void PIOS_LED_Off(uint32_t led_id)
 {
-	LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED];
+	PIOS_Assert(led_cfg);
+
+	if (led_id >= led_cfg->num_leds) {
+		/* LED index out of range */
+		return;
+	}
+
+	const struct pios_led * led = &(led_cfg->leds[led_id]);
+
+	GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin);
 }
 
 /**
 * Toggle LED on/off
-* \param[in] LED LED Name (LED1, LED2)
+* \param[in] LED LED id
 */
-void PIOS_LED_Toggle(LedTypeDef LED)
+void PIOS_LED_Toggle(uint32_t led_id)
 {
-	LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED];
+	PIOS_Assert(led_cfg);
+
+	if (led_id >= led_cfg->num_leds) {
+		/* LED index out of range */
+		return;
+	}
+
+	const struct pios_led * led = &(led_cfg->leds[led_id]);
+
+	if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) {
+		PIOS_LED_On(led_id);
+	} else {
+		PIOS_LED_Off(led_id);
+	}
 }
 
 #endif
diff --git a/flight/PiOS/STM32F10x/pios_sys.c b/flight/PiOS/STM32F10x/pios_sys.c
index cc35194e7..632be2426 100644
--- a/flight/PiOS/STM32F10x/pios_sys.c
+++ b/flight/PiOS/STM32F10x/pios_sys.c
@@ -77,10 +77,6 @@ void PIOS_SYS_Init(void)
 	/* Initialise Basic NVIC */
 	NVIC_Configuration();
 
-#if defined(PIOS_INCLUDE_LED)
-	/* Initialise LEDs */
-	PIOS_LED_Init();
-#endif
 }
 
 /**
@@ -105,16 +101,12 @@ int32_t PIOS_SYS_Reset(void)
 	PIOS_IRQ_Disable();
 
 	// turn off all board LEDs
-#if (PIOS_LED_NUM == 1)
-	PIOS_LED_Off(LED1);
-#elif (PIOS_LED_NUM == 2)
-	PIOS_LED_Off(LED1);
-	PIOS_LED_Off(LED2);
-#endif
-
-	/* Reset STM32 */
-	//RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */
-	//RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */
+#if defined(PIOS_LED_HEARTBEAT)
+	PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
+#if defined(PIOS_LED_ALARM)
+	PIOS_LED_Off(PIOS_LED_ALARM);
+#endif	/* PIOS_LED_ALARM */
 
 	RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
 	RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
@@ -210,13 +202,21 @@ void assert_failed(uint8_t * file, uint32_t line)
 	/* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */
 
 	/* Setup the LEDs to Alternate */
-	PIOS_LED_On(LED1);
-	PIOS_LED_Off(LED2);
+#if defined(PIOS_LED_HEARTBEAT)
+	PIOS_LED_On(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
+#if defined(PIOS_LED_ALARM)
+	PIOS_LED_Off(PIOS_LED_ALARM);
+#endif	/* PIOS_LED_ALARM */
 
 	/* Infinite loop */
 	while (1) {
-		PIOS_LED_Toggle(LED1);
-		PIOS_LED_Toggle(LED2);
+#if defined(PIOS_LED_HEARTBEAT)
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
+#if defined(PIOS_LED_ALARM)
+		PIOS_LED_Toggle(PIOS_LED_ALARM);
+#endif	/* PIOS_LED_ALARM */
 		for (int i = 0; i < 1000000; i++) ;
 	}
 }
diff --git a/flight/PiOS/inc/pios_led.h b/flight/PiOS/inc/pios_led.h
index c18ea3524..2506b75cf 100644
--- a/flight/PiOS/inc/pios_led.h
+++ b/flight/PiOS/inc/pios_led.h
@@ -30,23 +30,9 @@
 #ifndef PIOS_LED_H
 #define PIOS_LED_H
 
-/* Type Definitions */
-#if (PIOS_LED_NUM == 1)
-typedef enum { LED1 = 0 } LedTypeDef;
-#elif (PIOS_LED_NUM == 2)
-typedef enum { LED1 = 0, LED2 = 1 } LedTypeDef;
-#elif (PIOS_LED_NUM == 3)
-typedef enum { LED1 = 0, LED2 = 1, LED3 = 2 } LedTypeDef;
-#elif (PIOS_LED_NUM == 4)
-typedef enum { LED1 = 0, LED2 = 1, LED3 = 2, LED4 = 3 } LedTypeDef;
-#else
-#error PIOS_LED_NUM not defined
-#endif
-
 /* Public Functions */
-extern void PIOS_LED_Init(void);
-extern void PIOS_LED_On(LedTypeDef LED);
-extern void PIOS_LED_Off(LedTypeDef LED);
-extern void PIOS_LED_Toggle(LedTypeDef LED);
+extern void PIOS_LED_On(uint32_t led_id);
+extern void PIOS_LED_Off(uint32_t led_id);
+extern void PIOS_LED_Toggle(uint32_t led_id);
 
 #endif /* PIOS_LED_H */
diff --git a/flight/PiOS/inc/pios_led_priv.h b/flight/PiOS/inc/pios_led_priv.h
new file mode 100644
index 000000000..20f09eafc
--- /dev/null
+++ b/flight/PiOS/inc/pios_led_priv.h
@@ -0,0 +1,54 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup   PIOS_LED LED Functions
+ * @brief PIOS interface for LEDs
+ * @{
+ *
+ * @file       pios_led_priv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      LED private definitions.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * 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; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * 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, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_LED_PRIV_H
+#define PIOS_LED_PRIV_H
+
+#include <pios.h>
+#include <pios_stm32.h>
+
+struct pios_led {
+	struct stm32_gpio pin;
+	uint32_t remap;
+};
+
+struct pios_led_cfg {
+	const struct pios_led * leds;
+	uint8_t num_leds;
+};
+
+extern int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg);
+
+#endif /* PIOS_LED_PRIV_H */
+
+/**
+  * @}
+  * @}
+  */