From 2125e2479d018cbf64b39e97dfe89afad8ae66f8 Mon Sep 17 00:00:00 2001
From: les <les>
Date: Sat, 16 Oct 2010 17:54:00 +0000
Subject: [PATCH] Added AHRS boot loader SPI protocol

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1965 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/AHRS/Makefile                          | 113 +++++++-------
 flight/Bootloaders/AHRS/ahrs_slave_test.c     |  38 +++++
 flight/Bootloaders/AHRS/ahrs_spi_program.c    |  89 +++++++++++
 .../AHRS/ahrs_spi_program_master.c            | 137 +++++++++++++++++
 .../Bootloaders/AHRS/ahrs_spi_program_slave.c | 142 ++++++++++++++++++
 flight/Bootloaders/AHRS/inc/ahrs_bl.h         |  43 ++++--
 .../AHRS}/inc/ahrs_spi_program.h              |  25 ++-
 .../AHRS}/inc/ahrs_spi_program_master.h       |  34 ++++-
 .../AHRS}/inc/ahrs_spi_program_slave.h        |   4 +
 flight/Libraries/ahrs_spi_comm.c              |  24 ++-
 flight/Libraries/ahrs_spi_program_master.c    |  50 ------
 flight/Libraries/ahrs_spi_program_slave.c     |  36 -----
 flight/OpenPilot/Makefile                     |   7 +
 13 files changed, 571 insertions(+), 171 deletions(-)
 create mode 100644 flight/Bootloaders/AHRS/ahrs_slave_test.c
 create mode 100644 flight/Bootloaders/AHRS/ahrs_spi_program.c
 create mode 100644 flight/Bootloaders/AHRS/ahrs_spi_program_master.c
 create mode 100644 flight/Bootloaders/AHRS/ahrs_spi_program_slave.c
 rename flight/{Libraries => Bootloaders/AHRS}/inc/ahrs_spi_program.h (60%)
 rename flight/{Libraries => Bootloaders/AHRS}/inc/ahrs_spi_program_master.h (66%)
 rename flight/{Libraries => Bootloaders/AHRS}/inc/ahrs_spi_program_slave.h (90%)
 delete mode 100644 flight/Libraries/ahrs_spi_program_master.c
 delete mode 100644 flight/Libraries/ahrs_spi_program_slave.c

diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile
index 13457c59e..bbd8d4eda 100644
--- a/flight/AHRS/Makefile
+++ b/flight/AHRS/Makefile
@@ -1,24 +1,24 @@
  #####
  # Project: OpenPilot AHRS
- #    
- #  
+ #
+ #
  # Makefile for OpenPilot AHRS 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 
+ #
+ #
+ # 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 
+ #
+ # 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., 
+ #
+ # 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
  #####
 
@@ -42,7 +42,7 @@ endif
 
 FLASH_TOOL = OPENOCD
 
-# YES enables -mthumb option to flags for source-files listed 
+# YES enables -mthumb option to flags for source-files listed
 # in SRC and CPPSRC
 USE_THUMB_MODE = YES
 
@@ -85,7 +85,9 @@ CMSISDIR  = $(STMLIBDIR)/CMSIS/Core/CM3
 OPDIR = ../OpenPilot
 OPUAVOBJ = $(OPDIR)/UAVObjects
 OPUAVOBJINC = $(OPUAVOBJ)/inc
-OPSYSINC = $(OPDIR)/System/inc
+OPSYSINC = $(OPDIR)/System/inc
+BOOT = ../Bootloaders/AHRS
+BOOTINC = $(BOOT)/inc
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
@@ -97,7 +99,9 @@ SRC += ahrs_adc.c
 SRC += ahrs_timer.c
 SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
 SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
-SRC += $(FLIGHTLIB)/ahrs_spi_program_slave.c
+SRC += $(BOOT)/ahrs_spi_program_slave.c
+SRC += $(BOOT)/ahrs_slave_test.c
+SRC += $(BOOT)/ahrs_spi_program.c
 SRC += insgps.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
 
@@ -142,16 +146,16 @@ SRC += $(STMSPDSRCDIR)/misc.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 = 
+SRCARM =
 
 # List C++ source files here.
 # use file-extension .cpp for C++-files (not .C)
-CPPSRC = 
+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 = 
+CPPSRCARM =
 
 # List Assembler source files here.
 # Make them always end in a capital .S. Files ending in a lowercase .s
@@ -163,7 +167,7 @@ CPPSRCARM =
 ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL).S
 
 # List Assembler source files here which must be assembled in ARM-Mode..
-ASRCARM = 
+ASRCARM =
 
 # List any extra directories to look for include files here.
 #    Each directory must be seperated by a space.
@@ -176,17 +180,18 @@ EXTRAINCDIRS  += $(PIOSBOARDS)
 EXTRAINCDIRS  += $(STMSPDINCDIR)
 EXTRAINCDIRS  += $(CMSISDIR)
 EXTRAINCDIRS  += $(AHRSINC)
-EXTRAINCDIRS  += $(OPUAVOBJINC)
+EXTRAINCDIRS  += $(OPUAVOBJINC)
+EXTRAINCDIRS  += $(BOOTINC)
 
 # 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_LIBDIRS =
 
 # Extra Libraries
 #    Each library-name must be seperated by a space.
-#    i.e. to link with libxyz.a, libabc.a and libefsl.a: 
+#    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
@@ -195,7 +200,7 @@ EXTRA_LIBS =
 # Path to Linker-Scripts
 LINKERSCRIPTPATH = $(PIOSSTM32F10X)
 
-# Optimization level, can be [0, 1, 2, 3, s]. 
+# 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.)
 
@@ -207,7 +212,7 @@ CFLAGS += -Os
 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, 
+#  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
@@ -216,7 +221,7 @@ LOADFORMAT = both
 # Debugging format.
 DEBUGF = dwarf-2
 
-# Place project-specific -D (define) and/or 
+# Place project-specific -D (define) and/or
 # -U options for C here.
 CDEFS = -DSTM32F10X_$(MODEL)
 CDEFS += -DUSE_STDPERIPH_DRIVER
@@ -225,7 +230,7 @@ CDEFS += -DIN_AHRS
 ifeq ($(USE_BOOTLOADER), YES)
 CDEFS += -DUSE_BOOTLOADER
 endif
-# Place project-specific -D and/or -U options for 
+# Place project-specific -D and/or -U options for
 # Assembler with preprocessor here.
 #ADEFS = -DUSE_IRQ_ASM_WRAPPER
 ADEFS = -D__ASSEMBLY__
@@ -258,7 +263,7 @@ CFLAGS += -mcpu=$(MCU) -mthumb
 CFLAGS += $(CDEFS)
 CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
 
-CFLAGS += -mapcs-frame 
+CFLAGS += -mapcs-frame
 CFLAGS += -fomit-frame-pointer
 ifeq ($(CODE_SOURCERY), YES)
 CFLAGS += -fpromote-loop-indices
@@ -271,7 +276,7 @@ CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basen
 CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
 
 # flags only for C
-#CONLYFLAGS += -Wnested-externs 
+#CONLYFLAGS += -Wnested-externs
 CONLYFLAGS += $(CSTANDARD)
 
 # Assembler flags.
@@ -293,7 +298,7 @@ LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
 LDFLAGS += -lc
 LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
 LDFLAGS += $(MATH_LIB)
-LDFLAGS += -lc -lgcc 
+LDFLAGS += -lc -lgcc
 
 # Set linker-script name depending on selected submodel name
 LDFLAGS +=-T$(LINKERSCRIPTPATH)/link_stm32f10x_$(BOOT_MODEL).ld
@@ -320,10 +325,10 @@ OOCD_CL+=-c init
 OOCD_CL+=-c targets
 # commands to prepare flash-write
 OOCD_CL+= -c "reset halt"
-# flash erase 
+# flash erase
 OOCD_CL+=-c "stm32x mass_erase 0"
 # flash-write
-OOCD_CL+=-c "flash write_image $(OOCD_LOADFILE)" 
+OOCD_CL+=-c "flash write_image $(OOCD_LOADFILE)"
 # Verify
 OOCD_CL+=-c "verify_image $(OOCD_LOADFILE)"
 # reset target
@@ -351,7 +356,7 @@ REMOVE  = $(REMOVE_CMD) -f
 MSG_ERRORS_NONE = Errors: none
 MSG_BEGIN = "-------- begin (mode: $(RUN_MODE)) --------"
 MSG_END = --------  end  --------
-MSG_SIZE_BEFORE = Size before: 
+MSG_SIZE_BEFORE = Size before:
 MSG_SIZE_AFTER = Size after build:
 MSG_LOAD_FILE = Creating load file:
 MSG_EXTENDED_LISTING = Creating Extended Listing/Disassembly:
@@ -382,7 +387,7 @@ LSTFILES   = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
 DEPFILES   = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
 
 elf: $(OUTDIR)/$(TARGET).elf
-lss: $(OUTDIR)/$(TARGET).lss 
+lss: $(OUTDIR)/$(TARGET).lss
 sym: $(OUTDIR)/$(TARGET).sym
 hex: $(OUTDIR)/$(TARGET).hex
 bin: $(OUTDIR)/$(TARGET).bin
@@ -393,13 +398,13 @@ all: begin gccversion build sizeafter finished end
 
 ifeq ($(LOADFORMAT),ihex)
 build: elf hex lss sym
-else 
+else
 ifeq ($(LOADFORMAT),binary)
 build: elf bin lss sym
-else 
+else
 ifeq ($(LOADFORMAT),both)
 build: elf hex bin lss sym
-else 
+else
 $(error "$(MSG_FORMATERROR) $(FORMAT)")
 endif
 endif
@@ -428,9 +433,9 @@ sizeafter:
 #	@if [ -f  $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
 	@echo $(MSG_SIZE_AFTER)
 	$(ELFSIZE)
-	
+
 # Display compiler version information.
-gccversion : 
+gccversion :
 	@$(CC) --version
 #	@echo $(ALLOBJ)
 
@@ -454,7 +459,7 @@ endif
 ##	@echo
 	@echo $(MSG_LOAD_FILE) $@
 	$(OBJCOPY) -O ihex $< $@
-	
+
 # Create final output file (.bin) from ELF output file.
 %.bin: %.elf
 ##	@echo
@@ -491,18 +496,18 @@ define ASSEMBLE_TEMPLATE
 $(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
 ##	@echo
 	@echo $(MSG_ASSEMBLING) $$< "->" $$@
-	$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@ 
+	$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
 endef
-$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src)))) 
+$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
 
 # Assemble: create object files from assembler source files. ARM-only
 define ASSEMBLE_ARM_TEMPLATE
 $(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
 ##	@echo
 	@echo $(MSG_ASSEMBLING_ARM) $$< "->" $$@
-	$(CC) -c $$(ASFLAGS) $$< -o $$@ 
+	$(CC) -c $$(ASFLAGS) $$< -o $$@
 endef
-$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src)))) 
+$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
 
 
 # Compile: create object files from C source files.
@@ -510,18 +515,18 @@ define COMPILE_C_TEMPLATE
 $(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
 ##	@echo
 	@echo $(MSG_COMPILING) $$< "->" $$@
-	$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ 
+	$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
 endef
-$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src)))) 
+$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
 
 # Compile: create object files from C source files. ARM-only
 define COMPILE_C_ARM_TEMPLATE
 $(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
 ##	@echo
 	@echo $(MSG_COMPILING_ARM) $$< "->" $$@
-	$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ 
+	$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
 endef
-$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src)))) 
+$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
 
 
 # Compile: create object files from C++ source files.
@@ -529,18 +534,18 @@ define COMPILE_CPP_TEMPLATE
 $(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
 ##	@echo
 	@echo $(MSG_COMPILINGCPP) $$< "->" $$@
-	$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@ 
+	$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
 endef
-$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src)))) 
+$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
 
 # Compile: create object files from C++ source files. ARM-only
 define COMPILE_CPP_ARM_TEMPLATE
 $(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
 ##	@echo
 	@echo $(MSG_COMPILINGCPP_ARM) $$< "->" $$@
-	$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@ 
+	$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
 endef
-$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src)))) 
+$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
 
 
 # Compile: create assembler files from C source files. ARM/Thumb
@@ -556,7 +561,7 @@ $(SRCARM:.c=.s) : %.s : %.c
 # Generate Doxygen documents
 docs:
 	doxygen  $(DOXYGENDIR)/doxygen.cfg
-	
+
 # Target: clean project.
 clean: begin clean_list finished end
 
diff --git a/flight/Bootloaders/AHRS/ahrs_slave_test.c b/flight/Bootloaders/AHRS/ahrs_slave_test.c
new file mode 100644
index 000000000..54dc702f1
--- /dev/null
+++ b/flight/Bootloaders/AHRS/ahrs_slave_test.c
@@ -0,0 +1,38 @@
+#include "ahrs_bl.h"
+#include "ahrs_spi_program.h"
+
+uint8_t buf[256];
+
+bool StartProgramming(void)
+{
+	PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Started programming\r\n");
+	return(true);
+}
+
+bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size)
+{
+	if(size > SPI_MAX_PROGRAM_DATA_SIZE)
+	{
+PIOS_COM_SendFormattedString(PIOS_COM_AUX,"oversize: %d\r\n", size);
+		return(false);
+	}
+	PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Wrote %d bytes to %d\r\n", size, offset);
+	memcpy(buf,buffer,size);
+	PIOS_LED_Toggle(LED1);
+	return(true);
+}
+
+bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size)
+{
+	if(size > SPI_MAX_PROGRAM_DATA_SIZE)
+	{
+PIOS_COM_SendFormattedString(PIOS_COM_AUX,"oversize: %d\r\n", size);
+		return(false);
+	}
+	PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Read %d bytes from %d\r\n", size, offset);
+	memcpy(buffer,buf,size);
+	PIOS_LED_Toggle(LED1);
+	return(true);
+}
+
+
diff --git a/flight/Bootloaders/AHRS/ahrs_spi_program.c b/flight/Bootloaders/AHRS/ahrs_spi_program.c
new file mode 100644
index 000000000..22d48b1a8
--- /dev/null
+++ b/flight/Bootloaders/AHRS/ahrs_spi_program.c
@@ -0,0 +1,89 @@
+#include <stdint.h>
+#include "ahrs_spi_program.h"
+
+// Static CRC polynomial table
+static uint32_t crcTable[256] =
+{
+	0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA,
+	0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
+	0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
+	0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
+	0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
+	0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
+	0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
+	0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
+	0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
+	0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
+	0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
+	0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
+	0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
+	0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
+	0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
+	0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
+
+	0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
+	0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
+	0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
+	0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
+	0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
+	0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
+	0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
+	0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
+	0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
+	0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
+	0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
+	0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
+	0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
+	0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
+	0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
+	0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
+
+	0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
+	0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
+	0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
+	0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
+	0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
+	0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
+	0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
+	0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
+	0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
+	0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
+	0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
+	0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
+	0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
+	0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
+	0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
+	0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
+
+	0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
+	0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
+	0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
+	0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
+	0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
+	0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
+	0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
+	0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
+	0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
+	0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
+	0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
+	0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
+	0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
+	0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
+	0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
+	0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D,
+};
+
+/**generate CRC32 from a program packet
+this is slightly overkill but we want to be sure
+*/
+uint32_t GenerateCRC (AhrsProgramPacket * packet)
+{
+	uint8_t * ptr = (uint8_t *)packet;
+	int size = ((int)&packet->crc) - (int)packet;
+	uint32_t crc = 0xFFFFFFFF;
+	for(int ct=0; ct< size; ct++)
+	{
+		crc = ((crc) >> 8) ^ crcTable[(*ptr++) ^ ((crc) & 0x000000FF)];
+	}
+	return(~crc);
+}
diff --git a/flight/Bootloaders/AHRS/ahrs_spi_program_master.c b/flight/Bootloaders/AHRS/ahrs_spi_program_master.c
new file mode 100644
index 000000000..48d2f0a90
--- /dev/null
+++ b/flight/Bootloaders/AHRS/ahrs_spi_program_master.c
@@ -0,0 +1,137 @@
+/**
+ ******************************************************************************
+ *
+ * @file       ahrs_spi_program_master.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      AHRS programming over SPI link - master(OpenPilot) end.
+ *
+ * @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 "openpilot.h"
+#include "ahrs_spi_program_master.h"
+#include "ahrs_spi_program.h"
+#include "pios_spi.h"
+
+PROGERR TransferPacket(AhrsProgramPacket *txBuf, AhrsProgramPacket *rxBuf);
+
+#define MAX_CONNECT_TRIES 500 //half a second
+
+bool AhrsProgramConnect(void)
+{
+	AhrsProgramPacket rxBuf;
+	AhrsProgramPacket txBuf;
+	memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
+	memcpy(&txBuf,SPI_PROGRAM_REQUEST,SPI_PROGRAM_REQUEST_LENGTH);
+	for(int ct = 0; ct < MAX_CONNECT_TRIES; ct++) {
+		PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
+		uint32_t res = PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *) &txBuf,
+											  (uint8_t *) & rxBuf, SPI_PROGRAM_REQUEST_LENGTH +1, NULL);
+		PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
+		if(res == 0 && memcmp(&rxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
+			return (true);
+		}
+
+		vTaskDelay(1 / portTICK_RATE_MS);
+	}
+	return (false);
+}
+
+
+PROGERR AhrsProgramWrite(uint32_t address, void * data, uint32_t size)
+{
+	AhrsProgramPacket rxBuf;
+	AhrsProgramPacket txBuf;
+	memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
+	memcpy(txBuf.data,data,size);
+	txBuf.size = size;
+	txBuf.type = PROGRAM_WRITE;
+	txBuf.address = address;
+	PROGERR ret = TransferPacket(&txBuf, &rxBuf);
+	if(ret != PROGRAM_ERR_OK) {
+		return(ret);
+	}
+	return(PROGRAM_ERR_OK);
+}
+
+PROGERR AhrsProgramRead(uint32_t address, void * data, uint32_t size)
+{
+	AhrsProgramPacket rxBuf;
+	AhrsProgramPacket txBuf;
+	memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
+	txBuf.size = size;
+	txBuf.type = PROGRAM_READ;
+	txBuf.address = address;
+	PROGERR ret = TransferPacket(&txBuf, &rxBuf);
+	if(ret != PROGRAM_ERR_OK) {
+		return(ret);
+	}
+	memcpy(data, rxBuf.data, size);
+	return(PROGRAM_ERR_OK);
+}
+
+PROGERR AhrsProgramReboot(void)
+{
+	AhrsProgramPacket rxBuf;
+	AhrsProgramPacket txBuf;
+	memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
+	txBuf.type = PROGRAM_REBOOT;
+	memcpy(txBuf.data,REBOOT_CONFIRMATION,REBOOT_CONFIRMATION_LENGTH);
+	PROGERR ret = TransferPacket(&txBuf, &rxBuf);
+	//If AHRS has rebooted we will get comms errors
+	if(ret == PROGRAM_ERR_LINK) {
+		return(PROGRAM_ERR_OK);
+	}
+	return(PROGRAM_ERR_FUNCTION);
+}
+
+PROGERR TransferPacket(AhrsProgramPacket *txBuf, AhrsProgramPacket *rxBuf)
+{
+	static uint32_t pktId = 0;
+	pktId++;
+	txBuf->packetId = pktId;
+	txBuf->crc = GenerateCRC(txBuf);
+	int ct = 0;
+	for(; ct < MAX_CONNECT_TRIES; ct++) {
+		PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
+		uint32_t res = PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *) txBuf,
+											  (uint8_t *) rxBuf, sizeof(AhrsProgramPacket), NULL);
+		PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
+		if(res == 0) {
+			if(rxBuf->type != PROGRAM_NULL &&
+					rxBuf->crc == GenerateCRC(rxBuf) &&
+					rxBuf->packetId == pktId) {
+				break;
+			}
+		}
+
+		vTaskDelay(1 / portTICK_RATE_MS);
+	}
+	if(ct == MAX_CONNECT_TRIES) {
+		return (PROGRAM_ERR_LINK);
+	}
+	if(rxBuf->type  != PROGRAM_ACK) {
+		return(PROGRAM_ERR_FUNCTION);
+	}
+	return(PROGRAM_ERR_OK);
+}
+
+
+
+
diff --git a/flight/Bootloaders/AHRS/ahrs_spi_program_slave.c b/flight/Bootloaders/AHRS/ahrs_spi_program_slave.c
new file mode 100644
index 000000000..1501ce4ac
--- /dev/null
+++ b/flight/Bootloaders/AHRS/ahrs_spi_program_slave.c
@@ -0,0 +1,142 @@
+/**
+ ******************************************************************************
+ *
+ * @file       ahrs_spi_program_slave.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      AHRS programming over SPI link - slave(AHRS) end.
+ *
+ * @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 <stdint.h>
+#include <string.h>
+
+#include "pios_opahrs_proto.h"
+#include "pios_spi.h"
+#include "STM32103CB_AHRS.h"
+
+#include "ahrs_bl.h"
+#include "ahrs_spi_program_slave.h"
+#include "ahrs_spi_program.h"
+
+static AhrsProgramPacket txBuf;
+static AhrsProgramPacket rxBuf;
+static bool done = false;
+
+static void ProcessPacket();
+
+#define WAIT_IF_RECEIVING() while(!(GPIOB->IDR & GPIO_Pin_12)){}; //NSS must be high
+
+//Number of crc failures to allow before giving up
+#define PROGRAM_PACKET_TRIES 4
+
+void AhrsProgramReceive(void)
+{
+	done = false;
+	memset(&txBuf,0,sizeof(AhrsProgramPacket));
+	//wait for a program request
+	int count = PROGRAM_PACKET_TRIES;
+	while(1) {
+		WAIT_IF_RECEIVING();
+		while((PIOS_SPI_Busy(PIOS_SPI_OP) != 0)){};
+		memset(&rxBuf,'a',sizeof(AhrsProgramPacket));
+		int32_t res = PIOS_SPI_TransferBlock(PIOS_SPI_OP, NULL, (uint8_t*) &rxBuf,
+											 SPI_PROGRAM_REQUEST_LENGTH + 1, NULL);
+
+		if(res == 0 &&
+				memcmp(&rxBuf, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
+			break;
+		}
+		if(count-- == 0) {
+			return;
+		}
+	}
+
+	if(!StartProgramming()) {
+		//Couldn't erase FLASH. Nothing we can do.
+		return;
+	}
+
+	//send ack
+	memcpy(&txBuf,SPI_PROGRAM_ACK,SPI_PROGRAM_REQUEST_LENGTH);
+	WAIT_IF_RECEIVING();
+	while(0 != PIOS_SPI_TransferBlock(PIOS_SPI_OP,(uint8_t*) &txBuf, NULL,
+									  SPI_PROGRAM_REQUEST_LENGTH + 1, NULL)) {};
+
+	txBuf.type = PROGRAM_NULL;
+
+	while(!done) {
+		WAIT_IF_RECEIVING();
+		if(0 == PIOS_SPI_TransferBlock(PIOS_SPI_OP,(uint8_t*) &txBuf,
+									   (uint8_t*) &rxBuf, sizeof(AhrsProgramPacket), NULL)) {
+
+			uint32_t crc = GenerateCRC(&rxBuf);
+			if(crc != rxBuf.crc ||
+				txBuf.packetId == rxBuf.packetId) {
+				continue;
+			}
+			ProcessPacket();
+			txBuf.packetId = rxBuf.packetId;
+			txBuf.crc = GenerateCRC(&txBuf);
+		}
+	}
+}
+
+
+
+
+void ProcessPacket()
+{
+	switch(rxBuf.type) {
+	case PROGRAM_NULL:
+		txBuf.type = PROGRAM_NULL;
+		break;
+
+	case PROGRAM_WRITE:
+		if(WriteData(rxBuf.address, rxBuf.data, rxBuf.size)) {
+			txBuf.type = PROGRAM_ACK;
+			txBuf.size = rxBuf.size;
+		} else {
+			txBuf.type = PROGRAM_ERR;
+		}
+		break;
+
+	case PROGRAM_READ:
+		if(ReadData(rxBuf.address, txBuf.data, rxBuf.size)) {
+			txBuf.type = PROGRAM_ACK;
+			txBuf.size = rxBuf.size;
+		} else {
+			txBuf.type = PROGRAM_ERR;
+		}
+		break;
+
+	case PROGRAM_REBOOT:
+		if(0 == memcmp(rxBuf.data,REBOOT_CONFIRMATION,REBOOT_CONFIRMATION_LENGTH)) {
+			done = true;
+			txBuf.type = PROGRAM_ACK;
+		}else{
+			txBuf.type = PROGRAM_ERR;
+		}
+		break;
+
+	default:
+		txBuf.type = PROGRAM_ERR;
+	}
+}
diff --git a/flight/Bootloaders/AHRS/inc/ahrs_bl.h b/flight/Bootloaders/AHRS/inc/ahrs_bl.h
index 16935676d..6314d827b 100644
--- a/flight/Bootloaders/AHRS/inc/ahrs_bl.h
+++ b/flight/Bootloaders/AHRS/inc/ahrs_bl.h
@@ -7,19 +7,19 @@
  * @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 
+/*
+ * 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 
+ *
+ * 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., 
+ *
+ * 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
  */
 
@@ -29,6 +29,25 @@
 
 
 /* PIOS Includes */
-#include <pios.h>
+#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/Libraries/inc/ahrs_spi_program.h b/flight/Bootloaders/AHRS/inc/ahrs_spi_program.h
similarity index 60%
rename from flight/Libraries/inc/ahrs_spi_program.h
rename to flight/Bootloaders/AHRS/inc/ahrs_spi_program.h
index 18360358e..dcb285416 100644
--- a/flight/Libraries/inc/ahrs_spi_program.h
+++ b/flight/Bootloaders/AHRS/inc/ahrs_spi_program.h
@@ -17,34 +17,43 @@ Note crc is a CRC32 as the CRC8 used in hardware can be fooled.
    Time out if we waited too long.
 3) Compare ack packet with transmitted packet. The data
    should be the bitwise inverse of the data transmitted.
+   packetId should correspond to the transmitted packet.
 4) repeat for next packet until finished
-5) Repeat using verify packets with data all zeros
+5) Repeat using verify packets
    Returned data should be exactly as read from memory
 
 In the slave:
 1) Wait for an AhrsProgramPacket
 2) Check CRC then write to memory
-3) Bitwise invert data
-4) Transmit packet.
-5) repeat until we receive a verify packet
-6) verify until we receive a reboot packet
+3) Bitwise invert data and add it to the return packet
+4) Copy packetId from received packet
+5) Transmit packet.
+6) repeat until we receive a read packet
+7) read memory as requested until we receive a reboot packet
 Reboot packets had better have some sort of magic number in the data,
 just to be absolutely sure
 
 */
 
-typedef enum { PROGRAM_NULL, PROGRAM_DATA, PROGRAM_ACK, PROGRAM_VERIFY, PROGRAM_REBOOT } ProgramType;
-#define SPI_MAX_PROGRAM_DATA_SIZE 256	//max 256 bytes per packet
+typedef enum { PROGRAM_NULL, PROGRAM_WRITE, PROGRAM_READ, PROGRAM_ACK, PROGRAM_REBOOT, PROGRAM_ERR} ProgramType;
+#define SPI_MAX_PROGRAM_DATA_SIZE (14 * 4)	//USB comms uses 14x 32 bit words
+
+#define REBOOT_CONFIRMATION "AHRS REBOOT"
+#define REBOOT_CONFIRMATION_LENGTH 11
 
 /** Proposed program packet defintion
 */
 
 typedef struct {
 	ProgramType type;
+	uint32_t packetId;   //Transmission packet ID
 	uint32_t address;	//base address to place data
-	uint32_t size;		//Size of data
+	uint32_t size;		//Size of data (0 to SPI_MAX_PROGRAM_DATA_SIZE)
 	uint8_t data[SPI_MAX_PROGRAM_DATA_SIZE];
 	uint32_t crc;		//CRC32 - hardware CRC8 can be fooled
+	uint8_t dummy; //for some reason comms trashes the last byte sent
 } AhrsProgramPacket;
 
+uint32_t GenerateCRC (AhrsProgramPacket * packet);
+
 #endif
diff --git a/flight/Libraries/inc/ahrs_spi_program_master.h b/flight/Bootloaders/AHRS/inc/ahrs_spi_program_master.h
similarity index 66%
rename from flight/Libraries/inc/ahrs_spi_program_master.h
rename to flight/Bootloaders/AHRS/inc/ahrs_spi_program_master.h
index 2a97a1797..25bc7412d 100644
--- a/flight/Libraries/inc/ahrs_spi_program_master.h
+++ b/flight/Bootloaders/AHRS/inc/ahrs_spi_program_master.h
@@ -24,13 +24,41 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
- #ifndef AHRS_PROGRAM_MASTER_H
+#ifndef AHRS_PROGRAM_MASTER_H
 #define AHRS_PROGRAM_MASTER_H
 
+typedef enum {PROGRAM_ERR_OK, //OK
+	PROGRAM_ERR_LINK, //comms error
+	PROGRAM_ERR_FUNCTION, //function failed
+} PROGERR;
+
+
 /** Connect to AHRS and request programming mode
-* returns: 0 if connected, -1 if failed.
+* returns: false if failed.
 */
-uint32_t AhrsProgramConnect(void);
+bool AhrsProgramConnect(void);
+
+/** Write data to AHRS
+* size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE
+* returns: error status
+*/
+
+PROGERR AhrsProgramWrite(uint32_t address, void * data, uint32_t size);
+
+/** Read data from AHRS
+* size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE
+* returns: error status
+*/
+
+PROGERR AhrsProgramRead(uint32_t address, void * data, uint32_t size);
+
+
+/** reboot AHRS
+* returns: error status
+*/
+
+PROGERR AhrsProgramReboot(void);
+
 
 //TODO: Implement programming protocol
 
diff --git a/flight/Libraries/inc/ahrs_spi_program_slave.h b/flight/Bootloaders/AHRS/inc/ahrs_spi_program_slave.h
similarity index 90%
rename from flight/Libraries/inc/ahrs_spi_program_slave.h
rename to flight/Bootloaders/AHRS/inc/ahrs_spi_program_slave.h
index 2bdd7e60e..c74342968 100644
--- a/flight/Libraries/inc/ahrs_spi_program_slave.h
+++ b/flight/Bootloaders/AHRS/inc/ahrs_spi_program_slave.h
@@ -27,5 +27,9 @@
  #ifndef AHRS_SPI_PROGRAM_SLAVE_H
 #define AHRS_SPI_PROGRAM_SLAVE_H
 
+/** Check if OpenPilot is trying to program AHRS
+* If so, it will program the FLASH then return
+* If not it just returns.
+*/
 void AhrsProgramReceive(void);
 #endif //AHRS_PROGRAM_SLAVE_H
diff --git a/flight/Libraries/ahrs_spi_comm.c b/flight/Libraries/ahrs_spi_comm.c
index 58a10ca10..8b0a506a6 100644
--- a/flight/Libraries/ahrs_spi_comm.c
+++ b/flight/Libraries/ahrs_spi_comm.c
@@ -23,7 +23,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-
+#include <pios.h>
 #include "ahrs_spi_comm.h"
 #include "ahrs_spi_program.h"
 
@@ -49,7 +49,7 @@ CRC8 does not always catch noise from cross-coupling between data lines.
 #endif
 
 //packet types
-typedef enum { COMMS_NULL, COMMS_OBJECT, COMMS_PROGRAM } COMMSCOMMAND;
+typedef enum { COMMS_NULL, COMMS_OBJECT } COMMSCOMMAND;
 
 //The maximum number of objects that can be updated in one cycle.
 //Currently the link is capable of sending 3 packets per cycle but 2 is enough
@@ -78,6 +78,7 @@ typedef struct {
 	union {			//allow for expansion to other packet types.
 		ObjectPacketData objects[MAX_UPDATE_OBJECTS];
 	};
+	uint8_t dummy; //For some reason comms trashes the last byte
 } CommsDataPacket;
 
 static void FillObjectPacket();
@@ -125,9 +126,11 @@ static bool callbackPending[MAX_AHRS_OBJECTS];
 static uint8_t linkOk = false;
 static int okCount = MIN_OK_FRAMES;
 static int emptyCount = MIN_EMPTY_OBJECTS;
+static bool programReceive = false;
 
 void AhrsInitComms(void)
 {
+	programReceive = false;
 	AhrsInitHandles();
 	memset(objCallbacks, 0, sizeof(AhrsEventCallback) * MAX_AHRS_OBJECTS);
 	memset(callbackPending, 0, sizeof(bool) * MAX_AHRS_OBJECTS);
@@ -237,9 +240,6 @@ void HandleRxPacket()
 		HandleObjectPacket();
 		break;
 
-	case COMMS_PROGRAM:	//TODO: programming protocol
-		break;
-
 	default:
 		txPacket.status.invalidPacket++;
 	}
@@ -313,7 +313,6 @@ void CommsCallback(uint8_t crc_ok, uint8_t crc_val)
 	}
 
 
-	rxPacket.magicNumber = 0;
 	if (crc_ok) {
 		if (!linkOk && okCount > 0) {
 			okCount--;
@@ -326,8 +325,11 @@ void CommsCallback(uint8_t crc_ok, uint8_t crc_val)
 		HandleRxPacket();
 	} else {
 #ifdef IN_AHRS			//AHRS - do we neeed to enter program mode?
-		if (memcmp(&rxPacket, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
-			AhrsProgramReceive();
+		if (memcmp(&rxPacket, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH) == 0)
+		{
+			rxPacket.magicNumber = 0;
+			programReceive = true; //flag it to be executed in program space
+			return;
 		}
 #endif
 		txPacket.status.crcErrors++;
@@ -339,6 +341,7 @@ void CommsCallback(uint8_t crc_ok, uint8_t crc_val)
 			}
 		}
 	}
+	rxPacket.magicNumber = 0;
 #ifdef IN_AHRS
 	/*queue next frame
 	   If PIOS_SPI_TransferBlock() fails for any reason, comms will stop working.
@@ -367,6 +370,11 @@ void PollEvents(void)
 #ifdef IN_AHRS
 void AhrsPoll()
 {
+	if(programReceive)
+	{
+		AhrsProgramReceive();
+		programReceive = false;
+	}
 	PollEvents();
 	if (PIOS_SPI_Busy(PIOS_SPI_OP) != 0) {	//Everything is working correctly
 		return;
diff --git a/flight/Libraries/ahrs_spi_program_master.c b/flight/Libraries/ahrs_spi_program_master.c
deleted file mode 100644
index 3664ef7bd..000000000
--- a/flight/Libraries/ahrs_spi_program_master.c
+++ /dev/null
@@ -1,50 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       ahrs_spi_program_master.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      AHRS programming over SPI link - master(OpenPilot) end.
- *
- * @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 "ahrs_program_master.h"
-#include "ahrs_program.h"
-#include "pios_spi.h"
-
-char connectTxBuf[SPI_PROGRAM_REQUEST_LENGTH] = { SPI_PROGRAM_REQUEST };
-
-char connectRxBuf[SPI_PROGRAM_REQUEST_LENGTH];
-#define MAX_CONNECT_TRIES 10
-
-uint32_t AhrsProgramConnect(void)
-{
-	memset(connectRxBuf, 0, SPI_PROGRAM_REQUEST_LENGTH);
-	for (int ct = 0; ct < MAX_CONNECT_TRIES; ct++) {
-		uint32_t res = PIOS_SPI_TransferBlock(PIOS_SPI_OP, (uint8_t *) & connectTxBuf,
-						      (uint8_t *) & connectRxBuf, SPI_PROGRAM_REQUEST_LENGTH, NULL);
-		if (res == 0 && memcmp(connectRxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
-			return (0);
-		}
-
-		vTaskDelay(1 / portTICK_RATE_MS);
-	}
-	return (-1);
-}
diff --git a/flight/Libraries/ahrs_spi_program_slave.c b/flight/Libraries/ahrs_spi_program_slave.c
deleted file mode 100644
index 7fde0011f..000000000
--- a/flight/Libraries/ahrs_spi_program_slave.c
+++ /dev/null
@@ -1,36 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       ahrs_spi_program_slave.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      AHRS programming over SPI link - slave(AHRS) end.
- *
- * @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 <stdint.h>
-#include "ahrs_spi_program_slave.h"
-#include "ahrs_spi_program.h"
-
-//TODO:Implement programming protocol
-void AhrsProgramReceive(void)
-{
-
-}
diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile
index 4f6a76f31..2e9ecf2a8 100644
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@@ -117,6 +117,8 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS
 RTOSSRCDIR = $(RTOSDIR)/Source
 RTOSINCDIR = $(RTOSSRCDIR)/include
 DOXYGENDIR = ../Doc/Doxygen
+AHRSBOOTLOADER = ../Bootloaders/AHRS/
+AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
@@ -268,6 +270,10 @@ SRC += $(RTOSSRCDIR)/portable/MemMang/heap_2.c
 SRC += $(DOSFSDIR)/dosfs.c
 SRC += $(DOSFSDIR)/dfs_sdcard.c
 
+## AHRS boot loader comms
+SRC += $(AHRSBOOTLOADER)/ahrs_spi_program_master.c
+SRC += $(AHRSBOOTLOADER)/ahrs_spi_program.c
+
 ## Mass Storage Device
 #SRC += $(MSDDIR)/msd.c
 #SRC += $(MSDDIR)/msd_bot.c
@@ -324,6 +330,7 @@ EXTRAINCDIRS  += $(MSDDIR)
 EXTRAINCDIRS  += $(RTOSINCDIR)
 EXTRAINCDIRS  += $(APPLIBDIR)
 EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
+EXTRAINCDIRS  += $(AHRSBOOTLOADERINC)
 
 EXTRAINCDIRS += ${foreach MOD, ${MODULES}, Modules/${MOD}/inc} ${OPMODULEDIR}/System/inc