diff --git a/Makefile b/Makefile
index fd8ed8aa4..72f46c89c 100644
--- a/Makefile
+++ b/Makefile
@@ -525,12 +525,13 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
 all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS)))
 endef
 
-ALL_BOARDS := coptercontrol pipxtreme revolution
+ALL_BOARDS := coptercontrol pipxtreme revolution osd
 
 # Friendly names of each board (used to find source tree)
 coptercontrol_friendly := CopterControl
 pipxtreme_friendly     := PipXtreme
 revolution_friendly    := Revolution
+osd_friendly    := OSD
 
 # Start out assuming that we'll build fw, bl and bu for all boards
 FW_BOARDS  := $(ALL_BOARDS)
diff --git a/flight/Bootloaders/OSD/Makefile b/flight/Bootloaders/OSD/Makefile
new file mode 100644
index 000000000..b058a4c2c
--- /dev/null
+++ b/flight/Bootloaders/OSD/Makefile
@@ -0,0 +1,395 @@
+ #####
+ # Project: OpenPilot INS_BOOTLOADER
+ #    
+ #  
+ # Makefile for OpenPilot INS project
+ #
+ # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
+ #    
+ # 
+ # 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
+ #####
+
+WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
+TOP      := $(realpath $(WHEREAMI)/../../../)
+include $(TOP)/make/firmware-defs.mk
+include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
+
+# Target file name (without extension).
+TARGET := bl_$(BOARD_NAME)
+
+# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
+OUTDIR := $(TOP)/build/$(TARGET)
+
+# Set developer code and compile options
+# Set to YES for debugging
+DEBUG ?= NO
+
+# Set to YES when using Code Sourcery toolchain
+CODE_SOURCERY ?= NO
+
+ifeq ($(CODE_SOURCERY), YES)
+REMOVE_CMD = cs-rm
+else
+REMOVE_CMD = rm
+endif
+
+FLASH_TOOL = OPENOCD
+
+# Paths
+REVO_BL = ./
+REVO_BLINC = $(REVO_BL)/inc
+PIOS = ../../PiOS
+PIOSINC = $(PIOS)/inc
+FLIGHTLIB = ../../Libraries
+FLIGHTLIBINC = ../../Libraries/inc
+PIOSSTM32F4XX = $(PIOS)/STM32F4xx
+PIOSCOMMON = $(PIOS)/Common
+PIOSBOARDS = $(PIOS)/Boards
+PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
+APPLIBDIR = $(PIOSSTM32F4XX)/Libraries
+STMLIBDIR = $(APPLIBDIR)
+STMSPDDIR = $(STMLIBDIR)/STM32F4xx_StdPeriph_Driver
+STMSPDSRCDIR = $(STMSPDDIR)/src
+STMSPDINCDIR = $(STMSPDDIR)/inc
+OPDIR = ../OpenPilot
+OPUAVOBJ = ../UAVObjects
+OPUAVOBJINC = $(OPUAVOBJ)/inc
+OPSYSINC = $(OPDIR)/System/inc
+BOOT = ../Bootloaders/Revolution
+BOOTINC = $(BOOT)/inc
+HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
+
+OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
+
+# List C source files here. (C dependencies are automatically generated.)
+# use file-extension c for "c-only"-files
+
+## BOOTLOADER:
+SRC += main.c
+SRC += pios_board.c
+SRC += pios_usb_board_data.c
+SRC += bl_fsm.c
+
+## PIOS Hardware (STM32F4xx)
+include $(PIOS)/STM32F4xx/library.mk
+
+## Library files
+SRC += $(FLIGHTLIB)/fifo_buffer.c
+
+# PIOS Hardware (Common)
+#SRC += $(PIOSCOMMON)/pios_com.c
+SRC += $(PIOSCOMMON)/pios_board_info.c
+SRC += $(PIOSCOMMON)/pios_com_msg.c
+SRC += $(PIOSCOMMON)/printf-stdarg.c
+SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
+
+# List C source files here which must be compiled in ARM-Mode (no -mthumb).
+# use file-extension c for "c-only"-files
+## just for testing, timer.c could be compiled in thumb-mode too
+SRCARM = 
+
+# List C++ source files here.
+# use file-extension .cpp for C++-files (not .C)
+CPPSRC = 
+
+# List C++ source files here which must be compiled in ARM-Mode.
+# use file-extension .cpp for C++-files (not .C)
+#CPPSRCARM = $(TARGET).cpp
+CPPSRCARM = 
+
+# List any extra directories to look for include files here.
+#    Each directory must be seperated by a space.
+EXTRAINCDIRS  += $(PIOS)
+EXTRAINCDIRS  += $(PIOSINC)
+EXTRAINCDIRS  += $(FLIGHTLIBINC)
+EXTRAINCDIRS  += $(PIOSSTM34FXX)
+EXTRAINCDIRS  += $(PIOSCOMMON)
+EXTRAINCDIRS  += $(PIOSBOARDS)
+EXTRAINCDIRS  += $(STMSPDINCDIR)
+EXTRAINCDIRS  += $(CMSISDIR)
+EXTRAINCDIRS  += $(REVO_BLINC)
+EXTRAINCDIRS  += $(BOOTINC)
+EXTRAINCDIRS  += $(HWDEFSINC)
+
+# List any extra directories to look for library files here.
+# Also add directories where the linker should search for
+# includes from linker-script to the list
+#     Each directory must be seperated by a space.
+EXTRA_LIBDIRS = 
+
+# Extra Libraries
+#    Each library-name must be seperated by a space.
+#    i.e. to link with libxyz.a, libabc.a and libefsl.a: 
+#    EXTRA_LIBS = xyz abc efsl
+# for newlib-lpc (file: libnewlibc-lpc.a):
+#    EXTRA_LIBS = newlib-lpc
+EXTRA_LIBS =
+
+# Path to Linker-Scripts
+LINKERSCRIPTPATH = $(PIOSSTM32FXX)
+
+# Optimization level, can be [0, 1, 2, 3, s]. 
+# 0 = turn off optimization. s = optimize for size.
+# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
+
+ifeq ($(DEBUG),YES)
+OPT = 0
+else
+OPT = s
+endif
+
+# Output format. (can be ihex or binary or both)
+#  binary to create a load-image in raw-binary format i.e. for SAM-BA, 
+#  ihex to create a load-image in Intel hex format
+#LOADFORMAT = ihex
+#LOADFORMAT = binary
+LOADFORMAT = both
+
+# Debugging format.
+DEBUGF = dwarf-2
+
+# Place project-specific -D (define) and/or 
+# -U options for C here.
+CDEFS = -DSTM32F10X_$(MODEL)
+CDEFS += -DUSE_STDPERIPH_DRIVER
+CDEFS += -DUSE_$(BOARD)
+
+# Provide (only) the bootloader with board-specific defines
+BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
+BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
+BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
+BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
+BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
+BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
+BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
+
+# Place project-specific -D and/or -U options for 
+# Assembler with preprocessor here.
+#ADEFS = -DUSE_IRQ_ASM_WRAPPER
+ADEFS = -D__ASSEMBLY__
+
+# Compiler flag to set the C Standard level.
+# c89   - "ANSI" C
+# gnu89 - c89 plus GCC extensions
+# c99   - ISO C99 standard (not yet fully implemented)
+# gnu99 - c99 plus GCC extensions
+CSTANDARD = -std=gnu99
+
+#-----
+
+# Compiler flags.
+
+#  -g*:          generate debugging information
+#  -O*:          optimization level
+#  -f...:        tuning, see GCC manual and avr-libc documentation
+#  -Wall...:     warning level
+#  -Wa,...:      tell GCC to pass this to the assembler.
+#    -adhlns...: create assembler listing
+#
+# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
+
+ifeq ($(DEBUG),YES)
+CFLAGS = 
+endif
+
+# This is not the best place for these.  Really should abstract out
+# to the board file or something
+CFLAGS += -DSTM32F4XX
+CFLAGS += -DMEM_SIZE=1024000000
+
+CFLAGS += -g$(DEBUGF)
+CFLAGS += -O$(OPT)
+ifeq ($(DEBUG),NO)
+CFLAGS += -fdata-sections -ffunction-sections
+endif
+CFLAGS += -mcpu=$(MCU)
+CFLAGS += $(CDEFS)
+CFLAGS += $(BLONLY_CDEFS)
+CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
+
+CFLAGS += -mapcs-frame 
+CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
+CFLAGS += -fpromote-loop-indices
+endif
+
+CFLAGS += -Wall
+#CFLAGS += -Werror
+CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
+# Compiler flags to generate dependency files:
+CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
+
+# flags only for C
+#CONLYFLAGS += -Wnested-externs 
+CONLYFLAGS += $(CSTANDARD)
+
+# Assembler flags.
+#  -Wa,...:    tell GCC to pass this to the assembler.
+#  -ahlns:     create listing
+ASFLAGS  = -mcpu=$(MCU) -I. -x assembler-with-cpp
+ASFLAGS += $(ADEFS)
+ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
+ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
+
+MATH_LIB = -lm
+
+# Linker flags.
+#  -Wl,...:     tell GCC to pass this to linker.
+#    -Map:      create map file
+#    --cref:    add cross reference to  map file
+LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
+ifeq ($(DEBUG),NO)
+LDFLAGS += -Wl,-static
+endif
+LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
+LDFLAGS += -lc
+LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
+LDFLAGS += $(MATH_LIB)
+LDFLAGS += -lc -lgcc 
+
+#Linker scripts                                                                                                                                                                                                              
+LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_BL))
+
+# Define programs and commands.
+REMOVE  = $(REMOVE_CMD) -f
+
+# List of all source files.
+ALLSRC     = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
+# List of all source files without directory and file-extension.
+ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
+
+# Define all object files.
+ALLOBJ     = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
+
+# Define all listing files (used for make clean).
+LSTFILES   = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
+# Define all depedency-files (used for make clean).
+DEPFILES   = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
+
+# Default target.
+all: build
+
+ifeq ($(LOADFORMAT),ihex)
+build: elf hex sym
+else 
+ifeq ($(LOADFORMAT),binary)
+build: elf bin sym
+else 
+ifeq ($(LOADFORMAT),both)
+build: elf hex bin sym
+else 
+$(error "$(MSG_FORMATERROR) $(FORMAT)")
+endif
+endif
+endif
+
+# Link: create ELF output file from object files.
+$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
+
+# Assemble: create object files from assembler source files.
+$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src)))) 
+
+# Assemble: create object files from assembler source files. ARM-only
+$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C source files.
+$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C source files. ARM-only
+$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C++ source files.
+$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src)))) 
+
+# Compile: create object files from C++ source files. ARM-only
+$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src)))) 
+
+# Compile: create assembler files from C source files. ARM/Thumb
+$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
+
+# Compile: create assembler files from C source files. ARM only
+$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
+
+$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
+
+$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
+
+# Add jtag targets (program and wipe)
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
+
+.PHONY: elf lss sym hex bin bino
+elf: $(OUTDIR)/$(TARGET).elf
+lss: $(OUTDIR)/$(TARGET).lss 
+sym: $(OUTDIR)/$(TARGET).sym
+hex: $(OUTDIR)/$(TARGET).hex
+bin: $(OUTDIR)/$(TARGET).bin
+bino: $(OUTDIR)/$(TARGET).bin.o
+
+# Display sizes of sections.
+$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
+
+# Generate Doxygen documents
+docs:
+	doxygen  $(DOXYGENDIR)/doxygen.cfg
+
+# Install: install binary file with prefix/suffix into install directory
+install: $(OUTDIR)/$(TARGET).bin
+ifneq ($(INSTALL_DIR),)
+	@echo $(MSG_INSTALLING) $(call toprel, $<)
+	$(V1) mkdir -p $(INSTALL_DIR)
+	$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).bin
+else
+	$(error INSTALL_DIR must be specified for $@)
+endif
+
+# Target: clean project.
+clean: clean_list
+
+clean_list :
+	@echo $(MSG_CLEANING)
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
+	$(V1) $(REMOVE) $(ALLOBJ)
+	$(V1) $(REMOVE) $(LSTFILES)
+	$(V1) $(REMOVE) $(DEPFILES)
+	$(V1) $(REMOVE) $(SRC:.c=.s)
+	$(V1) $(REMOVE) $(SRCARM:.c=.s)
+	$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
+	$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
+
+
+# Create output files directory
+# all known MS Windows OS define the ComSpec environment variable
+ifdef ComSpec
+$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
+else
+$(shell mkdir -p $(OUTDIR) 2>/dev/null)
+endif
+
+# Include the dependency files.
+ifdef ComSpec
+-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
+else
+-include $(shell mkdir -p $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
+endif
+
+# Listing of phony targets.
+.PHONY : all build clean clean_list install
diff --git a/flight/Bootloaders/OSD/bl_fsm.c b/flight/Bootloaders/OSD/bl_fsm.c
new file mode 100644
index 000000000..10e26a46e
--- /dev/null
+++ b/flight/Bootloaders/OSD/bl_fsm.c
@@ -0,0 +1,591 @@
+#include <stdint.h>		/* uint*_t */
+#include <stddef.h>		/* NULL */
+
+#include "bl_fsm.h"
+
+#include "pios_opahrs_proto.h"
+
+#include "pios.h"
+
+struct lfsm_context {
+	enum lfsm_state curr_state;
+	enum opahrs_msg_link_state link_state;
+	enum opahrs_msg_type user_payload_type;
+	uint32_t user_payload_len;
+
+	uint32_t errors;
+
+	uint8_t * rx;
+	uint8_t * tx;
+
+	uint8_t * link_rx;
+	uint8_t * link_tx;
+
+	uint8_t * user_rx;
+	uint8_t * user_tx;
+
+	struct lfsm_link_stats stats;
+};
+
+static struct lfsm_context context = { 0 };
+
+static void lfsm_update_link_tx(struct lfsm_context * context);
+static void lfsm_init_rx(struct lfsm_context * context);
+
+static uint32_t PIOS_SPI_OP;
+void lfsm_attach(uint32_t spi_id) {
+	PIOS_SPI_OP = spi_id;
+}
+
+/*
+ *
+ * Link Finite State Machine
+ *
+ */
+
+struct lfsm_transition {
+	void (*entry_fn)(struct lfsm_context * context);
+	enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS];
+};
+
+static void go_faulted(struct lfsm_context * context);
+static void go_stopped(struct lfsm_context * context);
+static void go_stopping(struct lfsm_context * context);
+static void go_inactive(struct lfsm_context * context);
+static void go_user_busy(struct lfsm_context * context);
+static void go_user_busy_rx_pending(struct lfsm_context * context);
+static void go_user_busy_tx_pending(struct lfsm_context * context);
+static void go_user_busy_rxtx_pending(struct lfsm_context * context);
+static void go_user_rx_pending(struct lfsm_context * context);
+static void go_user_tx_pending(struct lfsm_context * context);
+static void go_user_rxtx_pending(struct lfsm_context * context);
+static void go_user_rx_active(struct lfsm_context * context);
+static void go_user_tx_active(struct lfsm_context * context);
+static void go_user_rxtx_active(struct lfsm_context * context);
+
+const static struct lfsm_transition lfsm_transitions[LFSM_STATE_NUM_STATES] = {
+		[LFSM_STATE_FAULTED] = {
+			.entry_fn = go_faulted,
+		}, [LFSM_STATE_STOPPED] = {
+			.entry_fn = go_stopped,
+			.next_state = {
+				[LFSM_EVENT_INIT_LINK] = LFSM_STATE_INACTIVE,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
+			},
+		}, [LFSM_STATE_STOPPING] = {
+			.entry_fn = go_stopping,
+			.next_state = {
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_STOPPED,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_STOPPED,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
+			},
+		}, [LFSM_STATE_INACTIVE] = {
+			.entry_fn = go_inactive,
+			.next_state = {
+				[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
+				[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
+				[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
+			},
+		}, [LFSM_STATE_USER_BUSY] = {
+			.entry_fn = go_user_busy,
+			.next_state = {
+				[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
+				[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
+				[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
+				[LFSM_EVENT_USER_DONE] = LFSM_STATE_INACTIVE,
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY,
+			},
+		}, [LFSM_STATE_USER_BUSY_RX_PENDING] = {
+			.entry_fn = go_user_busy_rx_pending,
+			.next_state = {
+				[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
+				[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING,
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING,
+			},
+		}, [LFSM_STATE_USER_BUSY_TX_PENDING] = {
+			.entry_fn = go_user_busy_tx_pending,
+			.next_state = {
+				[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
+				[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING,
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING,
+			},
+		}, [LFSM_STATE_USER_BUSY_RXTX_PENDING] = {
+			.entry_fn = go_user_busy_rxtx_pending,
+			.next_state = {
+				[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING,
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
+			},
+		}, [LFSM_STATE_USER_RX_PENDING] = {
+			.entry_fn = go_user_rx_pending,
+			.next_state = {
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RX_ACTIVE,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
+			},
+		}, [LFSM_STATE_USER_TX_PENDING] = {
+			.entry_fn = go_user_tx_pending,
+			.next_state = {
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_TX_ACTIVE,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_TX_ACTIVE,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE,
+			},
+		}, [LFSM_STATE_USER_RXTX_PENDING] = {
+			.entry_fn = go_user_rxtx_pending,
+			.next_state = {
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE,
+			},
+		}, [LFSM_STATE_USER_RX_ACTIVE] = {
+			.entry_fn = go_user_rx_active,
+			.next_state = {
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
+			},
+		}, [LFSM_STATE_USER_TX_ACTIVE] = {
+			.entry_fn = go_user_tx_active,
+			.next_state = {
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
+			},
+		}, [LFSM_STATE_USER_RXTX_ACTIVE] = {
+			.entry_fn = go_user_rxtx_active,
+			.next_state = {
+				[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
+				[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
+				[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
+			},
+		}, };
+
+/*
+ * FSM State Entry Functions
+ */
+
+static void go_faulted(struct lfsm_context * context) {
+	PIOS_DEBUG_Assert(0);
+}
+
+static void go_stopped(struct lfsm_context * context) {
+#if 0
+	PIOS_SPI_Stop(PIOS_SPI_OP);
+#endif
+}
+
+static void go_stopping(struct lfsm_context * context) {
+	context->link_tx = NULL;
+	context->tx = NULL;
+}
+
+static void go_inactive(struct lfsm_context * context) {
+	context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE;
+	lfsm_update_link_tx(context);
+
+	context->user_rx = NULL;
+	context->user_tx = NULL;
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_busy(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_rx);
+
+	context->user_rx = NULL;
+	context->user_tx = NULL;
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	lfsm_update_link_tx(context);
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_busy_rx_pending(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_rx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	lfsm_update_link_tx(context);
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_busy_tx_pending(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_tx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	lfsm_update_link_tx(context);
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_busy_rxtx_pending(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_rx); PIOS_DEBUG_Assert(context->user_tx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	lfsm_update_link_tx(context);
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_rx_pending(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_rx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	lfsm_update_link_tx(context);
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_tx_pending(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_tx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	lfsm_update_link_tx(context);
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_rxtx_pending(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_rx); PIOS_DEBUG_Assert(context->user_tx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	lfsm_update_link_tx(context);
+
+	context->rx = context->link_rx;
+	context->tx = context->link_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_rx_active(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_rx);
+
+	context->rx = context->user_rx;
+	context->tx = context->link_tx;
+	context->link_state = OPAHRS_MSG_LINK_STATE_READY;
+
+	lfsm_update_link_tx(context);
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_tx_active(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_tx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
+	context->rx = context->link_rx;
+	context->tx = context->user_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+static void go_user_rxtx_active(struct lfsm_context * context) {
+	/* Sanity checks */
+	PIOS_DEBUG_Assert(context->user_rx); PIOS_DEBUG_Assert(context->user_tx);
+
+	context->link_state = OPAHRS_MSG_LINK_STATE_READY;
+	context->rx = context->user_rx;
+	context->tx = context->user_tx;
+
+	lfsm_init_rx(context);
+	PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
+			context->user_payload_len, lfsm_irq_callback);
+}
+
+/*
+ *
+ * Misc Helper Functions
+ *
+ */
+
+static void lfsm_update_link_tx_v0(struct opahrs_msg_v0 * msg,
+		enum opahrs_msg_link_state state, uint16_t errors) {
+	opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
+
+	msg->payload.link.state = state;
+	msg->payload.link.errors = errors;
+}
+
+static void lfsm_update_link_tx_v1(struct opahrs_msg_v1 * msg,
+		enum opahrs_msg_link_state state, uint16_t errors) {
+	opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
+
+	msg->payload.link.state = state;
+	msg->payload.link.errors = errors;
+}
+
+static void lfsm_update_link_tx(struct lfsm_context * context) {
+	PIOS_DEBUG_Assert(context->link_tx);
+
+	switch (context->user_payload_type) {
+	case OPAHRS_MSG_TYPE_USER_V0:
+		lfsm_update_link_tx_v0((struct opahrs_msg_v0 *) context->link_tx,
+				context->link_state, context->errors);
+		break;
+	case OPAHRS_MSG_TYPE_USER_V1:
+		lfsm_update_link_tx_v1((struct opahrs_msg_v1 *) context->link_tx,
+				context->link_state, context->errors);
+		break;
+	case OPAHRS_MSG_TYPE_LINK:
+		PIOS_DEBUG_Assert(0);
+	}
+}
+
+static void lfsm_init_rx(struct lfsm_context * context) {
+	PIOS_DEBUG_Assert(context->rx);
+
+	switch (context->user_payload_type) {
+	case OPAHRS_MSG_TYPE_USER_V0:
+		opahrs_msg_v0_init_rx((struct opahrs_msg_v0 *) context->rx);
+		break;
+	case OPAHRS_MSG_TYPE_USER_V1:
+		opahrs_msg_v1_init_rx((struct opahrs_msg_v1 *) context->rx);
+		break;
+	case OPAHRS_MSG_TYPE_LINK:
+		PIOS_DEBUG_Assert(0);
+	}
+}
+
+/*
+ *
+ * External API
+ *
+ */
+
+void lfsm_inject_event(enum lfsm_event event) {
+	PIOS_IRQ_Disable();
+
+	/*
+	 * Move to the next state
+	 *
+	 * This is done prior to calling the new state's entry function to
+	 * guarantee that the entry function never depends on the previous
+	 * state.  This way, it cannot ever know what the previous state was.
+	 */
+	context.curr_state = lfsm_transitions[context.curr_state].next_state[event];
+
+	/* Call the entry function (if any) for the next state. */
+	if (lfsm_transitions[context.curr_state].entry_fn) {
+		lfsm_transitions[context.curr_state].entry_fn(&context);
+	}
+	PIOS_IRQ_Enable();
+}
+
+void lfsm_init(void) {
+	context.curr_state = LFSM_STATE_STOPPED;
+	go_stopped(&context);
+}
+
+void lfsm_set_link_proto_v0(struct opahrs_msg_v0 * link_tx,
+		struct opahrs_msg_v0 * link_rx) {
+	PIOS_DEBUG_Assert(link_tx);
+
+	context.link_tx = (uint8_t *) link_tx;
+	context.link_rx = (uint8_t *) link_rx;
+	context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0;
+	context.user_payload_len = sizeof(*link_tx);
+
+	lfsm_update_link_tx_v0(link_tx, context.link_state, context.errors);
+
+	lfsm_inject_event(LFSM_EVENT_INIT_LINK);
+}
+
+void lfsm_set_link_proto_v1(struct opahrs_msg_v1 * link_tx,
+		struct opahrs_msg_v1 * link_rx) {
+	PIOS_DEBUG_Assert(link_tx);
+
+	context.link_tx = (uint8_t *) link_tx;
+	context.link_rx = (uint8_t *) link_rx;
+	context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1;
+	context.user_payload_len = sizeof(*link_tx);
+
+	lfsm_update_link_tx_v1(link_tx, context.link_state, context.errors);
+
+	lfsm_inject_event(LFSM_EVENT_INIT_LINK);
+}
+
+void lfsm_user_set_tx_v0(struct opahrs_msg_v0 * user_tx) {
+	PIOS_DEBUG_Assert(user_tx);
+
+	PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
+	context.user_tx = (uint8_t *) user_tx;
+
+	lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
+}
+
+void lfsm_user_set_rx_v0(struct opahrs_msg_v0 * user_rx) {
+	PIOS_DEBUG_Assert(user_rx); PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
+
+	context.user_rx = (uint8_t *) user_rx;
+
+	lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
+}
+
+void lfsm_user_set_tx_v1(struct opahrs_msg_v1 * user_tx) {
+	PIOS_DEBUG_Assert(user_tx); PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
+
+	context.user_tx = (uint8_t *) user_tx;
+
+	lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
+}
+
+void lfsm_user_set_rx_v1(struct opahrs_msg_v1 * user_rx) {
+	PIOS_DEBUG_Assert(user_rx); PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
+
+	context.user_rx = (uint8_t *) user_rx;
+
+	lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
+}
+
+void lfsm_user_done(void) {
+	lfsm_inject_event(LFSM_EVENT_USER_DONE);
+}
+
+void lfsm_stop(void) {
+	lfsm_inject_event(LFSM_EVENT_STOP);
+}
+
+void lfsm_get_link_stats(struct lfsm_link_stats * stats) {
+	PIOS_DEBUG_Assert(stats);
+
+	*stats = context.stats;
+}
+
+enum lfsm_state lfsm_get_state(void) {
+	return context.curr_state;
+}
+
+/*
+ *
+ * ISR Callback
+ *
+ */
+
+void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val) {
+	if (!crc_ok) {
+		context.stats.rx_badcrc++;
+		lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
+		return;
+	}
+
+	if (!context.rx) {
+		/* No way to know what we just received, assume invalid */
+		lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
+		return;
+	}
+
+	/* Recover the head and tail pointers from the message */
+	struct opahrs_msg_link_head * head = 0;
+	struct opahrs_msg_link_tail * tail = 0;
+
+	switch (context.user_payload_type) {
+	case OPAHRS_MSG_TYPE_USER_V0:
+		head = &((struct opahrs_msg_v0 *) context.rx)->head;
+		tail = &((struct opahrs_msg_v0 *) context.rx)->tail;
+		break;
+	case OPAHRS_MSG_TYPE_USER_V1:
+		head = &((struct opahrs_msg_v1 *) context.rx)->head;
+		tail = &((struct opahrs_msg_v1 *) context.rx)->tail;
+		break;
+	case OPAHRS_MSG_TYPE_LINK:
+		/* Should never be rx'ing before the link protocol version is known */
+		PIOS_DEBUG_Assert(0);
+		break;
+	}
+
+	/* Check for bad magic */
+	if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) || (tail->magic
+			!= OPAHRS_MSG_MAGIC_TAIL)) {
+		if (head->magic != OPAHRS_MSG_MAGIC_HEAD) {
+			context.stats.rx_badmagic_head++;
+		}
+		if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) {
+			context.stats.rx_badmagic_tail++;
+		}
+		lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
+		return;
+	}
+
+	/* Good magic, find out what type of payload we've got */
+	switch (head->type) {
+	case OPAHRS_MSG_TYPE_LINK:
+		context.stats.rx_link++;
+		lfsm_inject_event(LFSM_EVENT_RX_LINK);
+		break;
+	case OPAHRS_MSG_TYPE_USER_V0:
+	case OPAHRS_MSG_TYPE_USER_V1:
+		if (head->type == context.user_payload_type) {
+			context.stats.rx_user++;
+			lfsm_inject_event(LFSM_EVENT_RX_USER);
+		} else {
+			/* Mismatched user payload type */
+			context.stats.rx_badver++;
+			lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
+		}
+		break;
+	default:
+		/* Unidentifiable payload type */
+		context.stats.rx_badtype++;
+		lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
+	}
+}
diff --git a/flight/Bootloaders/OSD/inc/bl_fsm.h b/flight/Bootloaders/OSD/inc/bl_fsm.h
new file mode 100644
index 000000000..7c443edb7
--- /dev/null
+++ b/flight/Bootloaders/OSD/inc/bl_fsm.h
@@ -0,0 +1,97 @@
+/**
+ ******************************************************************************
+ *
+ * @file       ahrs_fsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief     
+ * @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 BL_FSM_H
+#define BL_FSM_H
+
+#include "pios_opahrs_proto.h"
+
+enum lfsm_state {
+	LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */
+	LFSM_STATE_STOPPED,
+	LFSM_STATE_STOPPING,
+	LFSM_STATE_INACTIVE,
+	LFSM_STATE_USER_BUSY,
+	LFSM_STATE_USER_BUSY_RX_PENDING,
+	LFSM_STATE_USER_BUSY_TX_PENDING,
+	LFSM_STATE_USER_BUSY_RXTX_PENDING,
+	LFSM_STATE_USER_RX_PENDING,
+	LFSM_STATE_USER_TX_PENDING,
+	LFSM_STATE_USER_RXTX_PENDING,
+	LFSM_STATE_USER_RX_ACTIVE,
+	LFSM_STATE_USER_TX_ACTIVE,
+	LFSM_STATE_USER_RXTX_ACTIVE,
+
+	LFSM_STATE_NUM_STATES
+/* Must be last */
+};
+
+enum lfsm_event {
+	LFSM_EVENT_INIT_LINK,
+	LFSM_EVENT_STOP,
+	LFSM_EVENT_USER_SET_RX,
+	LFSM_EVENT_USER_SET_TX,
+	LFSM_EVENT_USER_DONE,
+	LFSM_EVENT_RX_LINK,
+	LFSM_EVENT_RX_USER,
+	LFSM_EVENT_RX_UNKNOWN,
+
+	LFSM_EVENT_NUM_EVENTS
+/* Must be last */
+};
+
+struct lfsm_link_stats {
+	uint32_t rx_badcrc;
+	uint32_t rx_badmagic_head;
+	uint32_t rx_badmagic_tail;
+	uint32_t rx_link;
+	uint32_t rx_user;
+	uint32_t tx_user;
+	uint32_t rx_badtype;
+	uint32_t rx_badver;
+};
+
+extern void lfsm_attach(uint32_t spi_id);
+extern void lfsm_init(void);
+extern void lfsm_inject_event(enum lfsm_event event);
+
+extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val);
+
+extern void lfsm_get_link_stats(struct lfsm_link_stats * stats);
+extern enum lfsm_state lfsm_get_state(void);
+
+extern void lfsm_set_link_proto_v0(struct opahrs_msg_v0 * link_tx,
+		struct opahrs_msg_v0 * link_rx);
+extern void lfsm_user_set_rx_v0(struct opahrs_msg_v0 * user_rx);
+extern void lfsm_user_set_tx_v0(struct opahrs_msg_v0 * user_tx);
+
+extern void lfsm_set_link_proto_v1(struct opahrs_msg_v1 * link_tx,
+		struct opahrs_msg_v1 * link_rx);
+extern void lfsm_user_set_rx_v1(struct opahrs_msg_v1 * user_rx);
+extern void lfsm_user_set_tx_v1(struct opahrs_msg_v1 * user_tx);
+
+extern void lfsm_user_done(void);
+
+#endif	/* BL_FSM_H */
diff --git a/flight/Bootloaders/OSD/inc/osd_bl.h b/flight/Bootloaders/OSD/inc/osd_bl.h
new file mode 100644
index 000000000..d96ba3bf6
--- /dev/null
+++ b/flight/Bootloaders/OSD/inc/osd_bl.h
@@ -0,0 +1,53 @@
+/**
+ ******************************************************************************
+ *
+ * @file       ahrs_bl.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Main AHRS_BL header.
+ * @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 INS_BL_H
+#define INS_BL_H
+
+
+/* PIOS Includes */
+#include <pios.h>
+
+/** Start programming
+returns: true if FLASH erased and ready to program
+*/
+bool StartProgramming(void);
+
+/** Write a block to FLASH
+buffer contains the data to be written
+returns: true if FLASH programmed correctly
+*/
+bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size);
+
+/** Read a block from FLASH
+returns: true if FLASH read correctly.
+Buffer is set to the read data
+*/
+bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size);
+
+
+
+#endif /* AHRS_BL_H */
diff --git a/flight/Bootloaders/OSD/inc/pios_config.h b/flight/Bootloaders/OSD/inc/pios_config.h
new file mode 100644
index 000000000..469ea9035
--- /dev/null
+++ b/flight/Bootloaders/OSD/inc/pios_config.h
@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_config.h  
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      PiOS configuration header. 
+ *                 - Central compile time config for the project.
+ * @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_CONFIG_H
+#define PIOS_CONFIG_H
+
+/* Enable/Disable PiOS Modules */
+#define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_IRQ
+#define PIOS_INCLUDE_LED
+#define PIOS_INCLUDE_SPI
+#define PIOS_INCLUDE_SYS
+#define PIOS_INCLUDE_IAP
+#define PIOS_INCLUDE_USB
+#define PIOS_INCLUDE_USB_HID
+#define PIOS_INCLUDE_COM_MSG
+//#define PIOS_INCLUDE_BL_HELPER
+//#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
+
+#endif /* PIOS_CONFIG_H */
diff --git a/flight/Bootloaders/OSD/inc/pios_usb_board_data.h b/flight/Bootloaders/OSD/inc/pios_usb_board_data.h
new file mode 100644
index 000000000..8fe467f4f
--- /dev/null
+++ b/flight/Bootloaders/OSD/inc/pios_usb_board_data.h
@@ -0,0 +1,51 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_USB_BOARD Board specific USB definitions
+ * @brief Board specific USB definitions
+ * @{
+ *
+ * @file       pios_usb_board_data.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Board specific USB 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_USB_BOARD_DATA_H
+#define PIOS_USB_BOARD_DATA_H
+
+#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
+
+#define PIOS_USB_BOARD_EP_NUM 2
+
+#include "pios_usb_defs.h" 	/* struct usb_* */
+
+#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD
+#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_BL)
+
+/*
+ * The bootloader uses a simplified report structure
+ *   BL: <REPORT_ID><DATA>...<DATA>
+ *   FW: <REPORT_ID><LENGTH><DATA>...<DATA>
+ * This define changes the behaviour in pios_usb_hid.c
+ */
+#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
+
+#endif	/* PIOS_USB_BOARD_DATA_H */
diff --git a/flight/Bootloaders/OSD/main.c b/flight/Bootloaders/OSD/main.c
new file mode 100644
index 000000000..6e0c0e18d
--- /dev/null
+++ b/flight/Bootloaders/OSD/main.c
@@ -0,0 +1,90 @@
+/**
+ ******************************************************************************
+ * @addtogroup AHRS BOOTLOADER
+ * @brief The AHRS Modules perform
+ *
+ * @{ 
+ * @addtogroup AHRS_BOOTLOADER_Main
+ * @brief Main function which does the hardware dependent stuff
+ * @{ 
+ *
+ *
+ * @file       main.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief
+ * @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
+ */
+
+/* OpenPilot Includes */
+#include "osd_bl.h"
+#include <pios_board_info.h>
+#include "bl_fsm.h"		/* lfsm_state */
+//#include "stm32f2xx_flash.h"
+
+extern void PIOS_Board_Init(void);
+
+#define NSS_HOLD_STATE       ((GPIOB->IDR & GPIO_Pin_12) ? 0 : 1)
+enum bootloader_status boot_status;
+/* Private typedef -----------------------------------------------------------*/
+typedef void
+(*pFunction)(void);
+pFunction Jump_To_Application;
+uint32_t JumpAddress;
+/* Function Prototypes */
+//void
+//process_spi_request(void);
+void
+jump_to_app();
+uint32_t Fw_crc;
+/**
+ * @brief Bootloader Main function
+ */
+int main() {
+	/* Brings up System using CMSIS functions, enables the LEDs. */
+	PIOS_SYS_Init();
+	PIOS_Board_Init();
+
+	jump_to_app();
+	return 0;
+}
+
+
+void jump_to_app() {
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+
+	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;
+		/* Initialize user application's Stack Pointer */
+		__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
+		Jump_To_Application();
+	} else {
+		boot_status = jump_failed;
+		return;
+	}
+}
diff --git a/flight/Bootloaders/OSD/pios_board.c b/flight/Bootloaders/OSD/pios_board.c
new file mode 100644
index 000000000..a89f83103
--- /dev/null
+++ b/flight/Bootloaders/OSD/pios_board.c
@@ -0,0 +1,70 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_board.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Defines board specific static initializers for hardware for the AHRS board.
+ * @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
+ */
+
+#include "board_hw_defs.c"
+
+#include <pios_board_info.h>
+#include <pios.h>
+
+uint32_t pios_com_telem_usb_id;
+
+static bool board_init_complete = false;
+void PIOS_Board_Init() {
+	if (board_init_complete) {
+		return;
+	}
+
+	/* Delay system */
+	PIOS_DELAY_Init();
+	
+	PIOS_LED_Init(&pios_led_cfg);
+
+
+#if 0 && defined(PIOS_INCLUDE_USB)
+	/* Initialize board specific USB data */
+	PIOS_USB_BOARD_DATA_Init();
+
+	/* Activate the HID-only USB configuration */
+	PIOS_USB_DESC_HID_ONLY_Init();
+
+	uint32_t pios_usb_id;
+	PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
+
+#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
+	uint32_t pios_usb_hid_id;
+	if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
+		PIOS_Assert(0);
+	}
+	if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
+		PIOS_Assert(0);
+	}
+#endif	/* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
+
+	PIOS_USBHOOK_Activate();
+
+#endif	/* PIOS_INCLUDE_USB */
+
+	board_init_complete = true;
+}
diff --git a/flight/Bootloaders/OSD/pios_usb_board_data.c b/flight/Bootloaders/OSD/pios_usb_board_data.c
new file mode 100644
index 000000000..2af604ca4
--- /dev/null
+++ b/flight/Bootloaders/OSD/pios_usb_board_data.c
@@ -0,0 +1,113 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_USB_BOARD Board specific USB definitions
+ * @brief Board specific USB definitions
+ * @{
+ *
+ * @file       pios_usb_board_data.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Board specific USB 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
+ */
+
+#include "pios_usb_board_data.h" /* struct usb_*, USB_* */
+#include "pios_sys.h"		 /* PIOS_SYS_SerialNumberGet */
+#include "pios_usbhook.h"	 /* PIOS_USBHOOK_* */
+
+static const uint8_t usb_product_id[8] = {
+	sizeof(usb_product_id),
+	USB_DESC_TYPE_STRING,
+	'O', 0,
+	'S', 0,
+	'D', 0,
+};
+
+static uint8_t usb_serial_number[52] = {
+	sizeof(usb_serial_number),
+	USB_DESC_TYPE_STRING,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0,
+	0, 0
+};
+
+static const struct usb_string_langid usb_lang_id = {
+	.bLength = sizeof(usb_lang_id),
+	.bDescriptorType = USB_DESC_TYPE_STRING,
+	.bLangID = htousbs(USB_LANGID_ENGLISH_UK),
+};
+
+static const uint8_t usb_vendor_id[28] = {
+	sizeof(usb_vendor_id),
+	USB_DESC_TYPE_STRING,
+	'o', 0,
+	'p', 0,
+	'e', 0,
+	'n', 0,
+	'p', 0,
+	'i', 0,
+	'l', 0,
+	'o', 0,
+	't', 0,
+	'.', 0,
+	'o', 0,
+	'r', 0,
+	'g', 0
+};
+
+int32_t PIOS_USB_BOARD_DATA_Init(void)
+{
+	/* Load device serial number into serial number string */
+	uint8_t sn[25];
+	PIOS_SYS_SerialNumberGet((char *)sn);
+	for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) {
+		usb_serial_number[2 + 2 * i] = sn[i];
+	}
+
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));
+
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id));
+	PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id));
+
+	return 0;
+}
diff --git a/flight/OSD/Makefile b/flight/OSD/Makefile
index 0acf8f4f3..c24ce075a 100644
--- a/flight/OSD/Makefile
+++ b/flight/OSD/Makefile
@@ -49,7 +49,7 @@ endif
 FLASH_TOOL = OPENOCD
 
 # List of modules to include
-MODULES = Osd/osdgen Osd/osdinput GPS Telemetry #FirmwareIAP
+MODULES = Osd/osdgen Osd/osdinput GPS Telemetry FirmwareIAP
 
 
 # Paths
diff --git a/flight/OSD/System/inc/pios_config.h b/flight/OSD/System/inc/pios_config.h
index d1a0bd12a..d0e882e46 100644
--- a/flight/OSD/System/inc/pios_config.h
+++ b/flight/OSD/System/inc/pios_config.h
@@ -39,6 +39,8 @@
 #define PIOS_INCLUDE_USART
 #define PIOS_INCLUDE_COM
 #define PIOS_INCLUDE_FREERTOS
+#define PIOS_INCLUDE_BL_HELPER
+
 #define PIOS_INCLUDE_GPIO
 #define PIOS_INCLUDE_EXTI
 #define PIOS_INCLUDE_USB
diff --git a/flight/PiOS/STM32F4xx/link_STM32F4xx_OP_memory.ld b/flight/PiOS/STM32F4xx/link_STM32F4xx_OP_memory.ld
index eb5927ce4..d6082e917 100644
--- a/flight/PiOS/STM32F4xx/link_STM32F4xx_OP_memory.ld
+++ b/flight/PiOS/STM32F4xx/link_STM32F4xx_OP_memory.ld
@@ -1,7 +1,7 @@
 MEMORY
 {
 	BD_INFO (r)   : ORIGIN = 0x08008000 - 0x80, LENGTH =            0x000080
-	FLASH (rx)    : ORIGIN = 0x08000000,        LENGTH = 0x100000 - 0x008000
+	FLASH (rx)    : ORIGIN = 0x08008000,        LENGTH = 0x100000 - 0x008000
 	SRAM (rwx)    : ORIGIN = 0x20000000,        LENGTH = 0x020000
 	CCSRAM (rw)   : ORIGIN = 0x10000000,        LENGTH = 0x010000	
 }
diff --git a/flight/board_hw_defs/osd/board_hw_defs.c b/flight/board_hw_defs/osd/board_hw_defs.c
new file mode 100644
index 000000000..b536f3579
--- /dev/null
+++ b/flight/board_hw_defs/osd/board_hw_defs.c
@@ -0,0 +1,311 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotSystem OpenPilot System
+ * @{
+ * @addtogroup OpenPilotCore OpenPilot Core
+ * @{
+ *
+ * @file       board_hw_defs.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Defines board specific static initializers for hardware for the OpenPilot board.
+ * @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
+ */
+#include <pios_config.h>
+
+#if defined(PIOS_INCLUDE_LED)
+
+#include <pios_led_priv.h>
+static const struct pios_led pios_leds[] = {
+	[PIOS_LED_HEARTBEAT] = {
+		.pin = {
+			.gpio = GPIOD,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_13,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+				.GPIO_Mode  = GPIO_Mode_OUT,
+				.GPIO_OType = GPIO_OType_PP,
+				.GPIO_PuPd = GPIO_PuPd_UP
+			},
+		},
+	},
+	[PIOS_LED_ALARM] = {
+		.pin = {
+			.gpio = GPIOD,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_12,
+				.GPIO_Speed = GPIO_Speed_50MHz,
+				.GPIO_Mode  = GPIO_Mode_OUT,
+				.GPIO_OType = GPIO_OType_PP,
+				.GPIO_PuPd = GPIO_PuPd_UP
+			},
+		},
+	},
+};
+
+static const struct pios_led_cfg pios_led_cfg = {
+	.leds     = pios_leds,
+	.num_leds = NELEMENTS(pios_leds),
+};
+
+#endif	/* PIOS_INCLUDE_LED */
+
+#include <pios_usart_priv.h>
+
+#if defined(PIOS_INCLUDE_GPS)
+/*
+ * GPS USART
+ */
+static const struct pios_usart_cfg pios_usart_gps_cfg = {
+	.regs = USART1,
+	.remap = GPIO_AF_USART1,
+	.init = {
+		.USART_BaudRate = 38400,
+		.USART_WordLength = USART_WordLength_8b,
+		.USART_Parity = USART_Parity_No,
+		.USART_StopBits = USART_StopBits_1,
+		.USART_HardwareFlowControl =
+		USART_HardwareFlowControl_None,
+		.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = USART1_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_10,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_9,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
+#endif /* PIOS_INCLUDE_GPS */
+
+#ifdef PIOS_INCLUDE_COM_AUX
+/*
+ * AUX USART
+ */
+static const struct pios_usart_cfg pios_usart_aux_cfg = {
+	.regs = USART1,
+	.remap = GPIO_AF_USART1,
+	.init = {
+		.USART_BaudRate = 230400,
+		.USART_WordLength = USART_WordLength_8b,
+		.USART_Parity = USART_Parity_No,
+		.USART_StopBits = USART_StopBits_1,
+		.USART_HardwareFlowControl =
+		USART_HardwareFlowControl_None,
+		.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = USART1_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_10,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_9,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
+#endif /* PIOS_COM_AUX */
+
+#ifdef PIOS_INCLUDE_COM_TELEM
+/*
+ * Telemetry on main USART
+ */
+static const struct pios_usart_cfg pios_usart_telem_main_cfg = {
+	.regs = USART6,
+	.remap = GPIO_AF_USART6,
+	.init = {
+		.USART_BaudRate = 57600,
+		.USART_WordLength = USART_WordLength_8b,
+		.USART_Parity = USART_Parity_No,
+		.USART_StopBits = USART_StopBits_1,
+		.USART_HardwareFlowControl =
+		USART_HardwareFlowControl_None,
+		.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = USART6_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_7,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_6,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
+#endif /* PIOS_COM_TELEM */
+
+
+
+#if defined(PIOS_INCLUDE_COM)
+
+#include <pios_com_priv.h>
+
+#endif /* PIOS_INCLUDE_COM */
+
+
+#if defined(PIOS_INCLUDE_RTC)
+/*
+ * Realtime Clock (RTC)
+ */
+#include <pios_rtc_priv.h>
+
+void PIOS_RTC_IRQ_Handler (void);
+void RTC_WKUP_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
+static const struct pios_rtc_cfg pios_rtc_main_cfg = {
+	.clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1
+	// For some reason it's acting like crystal is 16 Mhz.  This clock is then divided
+	// by another 16 to give a nominal 62.5 khz clock
+	.prescaler = 100, // Every 100 cycles gives 625 Hz
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = RTC_WKUP_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+void PIOS_RTC_IRQ_Handler (void)
+{
+	PIOS_RTC_irq_handler ();
+}
+
+#endif
+
+
+#if defined(PIOS_INCLUDE_USB)
+#include "pios_usb_priv.h"
+
+static const struct pios_usb_cfg pios_usb_main_cfg = {
+	.irq = {
+		.init    = {
+			.NVIC_IRQChannel                   = OTG_FS_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
+			.NVIC_IRQChannelSubPriority        = 3,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+	.vsense = {
+		.gpio = GPIOD,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_11,
+			.GPIO_Speed = GPIO_Speed_25MHz,
+			.GPIO_Mode  = GPIO_Mode_IN,
+			.GPIO_OType = GPIO_OType_OD,
+		},
+	}
+};
+
+#include "pios_usb_board_data_priv.h"
+#include "pios_usb_desc_hid_cdc_priv.h"
+#include "pios_usb_desc_hid_only_priv.h"
+#include "pios_usbhook.h"
+
+#endif	/* PIOS_INCLUDE_USB */
+
+#if defined(PIOS_INCLUDE_COM_MSG)
+
+#include <pios_com_msg_priv.h>
+
+#endif /* PIOS_INCLUDE_COM_MSG */
+
+#if defined(PIOS_INCLUDE_USB_HID)
+#include <pios_usb_hid_priv.h>
+
+const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
+	.data_if = 0,
+	.data_rx_ep = 1,
+	.data_tx_ep = 1,
+};
+#endif /* PIOS_INCLUDE_USB_HID */
+
+#if defined(PIOS_INCLUDE_USB_CDC)
+#include <pios_usb_cdc_priv.h>
+
+const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = {
+	.ctrl_if = 1,
+	.ctrl_tx_ep = 2,
+
+	.data_if = 2,
+	.data_rx_ep = 3,
+	.data_tx_ep = 3,
+};
+#endif	/* PIOS_INCLUDE_USB_CDC */
diff --git a/make/boards/osd/board-info.mk b/make/boards/osd/board-info.mk
index bcfe990b5..768670c34 100644
--- a/make/boards/osd/board-info.mk
+++ b/make/boards/osd/board-info.mk
@@ -12,10 +12,10 @@ MODEL_SUFFIX        :=
 OPENOCD_CONFIG      := stm32f4xx.cfg
 
 # Note: These must match the values in link_$(BOARD)_memory.ld
-#BL_BANK_BASE        := 0x08000000  # Start of bootloader flash
-#BL_BANK_SIZE        := 0x00008000  # Should include BD_INFO region
-FW_BANK_BASE        := 0x08000000  # Start of firmware flash
-FW_BANK_SIZE        := 0x0001E000  # Should include FW_DESC_SIZE
+BL_BANK_BASE        := 0x08000000  # Start of bootloader flash
+BL_BANK_SIZE        := 0x00008000  # Should include BD_INFO region
+FW_BANK_BASE        := 0x08008000  # Start of firmware flash
+FW_BANK_SIZE        := 0x000F8000  # Should include FW_DESC_SIZE
 
 FW_DESC_SIZE        := 0x00000064