Merge branch 'next'

.gitignore

@@ -66,3 +66,5 @@ GPATH
 GRTAGS
 GSYMS
 GTAGS
+/.cproject
+/.project

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "overo"]
+	path = overo
+	url = git@github.com:peabody124/op_overo.git

HISTORY.txt

@@ -1,5 +1,56 @@
 Short summary of changes.  For a complete list see the git log.
 
+2012-07-10
+On Windows the installation mode was changed from per-user to per-machine
+(for all users) installation.  It is recommended to completely uninstall
+previous version before installing new one to remove per-user installed
+files.  Per-machine installation requires elevated (administrator) previleges
+during install.  But since the same rights are now required to install
+optional CDC driver (virtual communication port), it was deemed acceptable.
+
+2012-06-04
+AeroSimRC support merged into next
+
+2012-05-26
+VirtualFlybar which allows a more aggressive flight mode than rate mode
+support.  Also PiroCompensation added.
+
+2012-05-26
+Revert some UI changes that didn't work consistently between OSX and Windows.
+
+2012-05-24
+Merged the updated firmware for the PipXtreme, thanks to Brian for a lot of
+work on this.
+
+2012-05-04
+Support for CC3D.  This involved changes to various things such as the sensors
+being split from AttitudeRaw to Accels,Gyros,Magnetometer.  A single firmware
+image fw_coptercontrol will run on both CC and CC3D.  When compiling the
+bootloader one must set the HW_REVISION to the appropriate value.  0x01 is for
+CC and 0x02 is for CC3D.  If the wrong bootloader is installed the firmware
+will not run.
+
+2012-05-02
+Reduction in the memory usage due to the UAVObject metadata.  Now the update
+periods are using a smaller data type and the various flags relating to access
+controls and update modes are stored in a bitfield.  The UAVObjectBrowser has
+not been updated to allow these modes to be easily changed.
+
+2012-03-31
+Support for ground vehicle configuration has been added to the the GCS.
+
+2012-02-14
+New QML based system to allow more flexible UI.  Upgraded stabilization
+configuration.
+
+2012-01-02
+CC FW now supports USB Virtual Com Port (VCP/CDC) in addition to the original HID interface
+New ComUsbBridge module can bridge any serial port to the USB CDC port
+CC FW now detects repeated faults during init and boots with default hwsettings
+
+2012-01-02
+Added new camera stabilization features: AxisLock mode and LPF.
+
 2011-12-10
 Merged a change that sorts the UAVO fields based on size.  Because this changes
 all of the objects, erase all existing flash files based on this.

Makefile

@@ -68,10 +68,11 @@ help:
 	@echo "   Here is a summary of the available targets:"
 	@echo
 	@echo "   [Tool Installers]"
-	@echo "     qt_sdk_install       - Install the QT v4.6.2 tools"
+	@echo "     qt_sdk_install       - Install the QT v4.7.3 tools"
 	@echo "     arm_sdk_install      - Install the Code Sourcery ARM gcc toolchain"
 	@echo "     openocd_install      - Install the OpenOCD JTAG daemon"
 	@echo "     stm32flash_install   - Install the stm32flash tool for unbricking boards"
+	@echo "     dfuutil_install      - Install the dfu-util tool for unbricking F4-based boards"
 	@echo
 	@echo "   [Big Hammer]"
 	@echo "     all                  - Generate UAVObjects, build openpilot firmware and gcs"
@@ -113,9 +114,6 @@ help:
 	@echo "                            supported boards are ($(BL_BOARDS))"
 	@echo
 	@echo "   [Simulation]"
-	@echo "     sim_posix            - Build OpenPilot simulation firmware for"
-	@echo "                            a POSIX compatible system (Linux, Mac OS X, ...)"
-	@echo "     sim_posix_clean      - Delete all build output for the POSIX simulation"
 	@echo "     sim_win32            - Build OpenPilot simulation firmware for"
 	@echo "                            Windows using mingw and msys"
 	@echo "     sim_win32_clean      - Delete all build output for the win32 simulation"
@@ -160,20 +158,29 @@ $(BUILD_DIR):
 ###############################################################
 
 # Set up QT toolchain
-QT_SDK_DIR := $(TOOLS_DIR)/qtsdk-2010.02
+QT_SDK_DIR := $(TOOLS_DIR)/qtsdk-v1.2
 
 .PHONY: qt_sdk_install
-qt_sdk_install: QT_SDK_URL  := http://get.qt.nokia.com/qtsdk/qt-sdk-linux-x86-opensource-2010.02.bin
+qt_sdk_install: QT_SDK_URL  := http://www.developer.nokia.com/dp?uri=http://sw.nokia.com/id/8ea74da4-fec1-4277-8b26-c58cc82e204b/Qt_SDK_Lin32_offline
-qt_sdk_install: QT_SDK_FILE := $(notdir $(QT_SDK_URL))
+qt_sdk_install: QT_SDK_FILE := Qt_SDK_Lin32_offline_v1_2_en.run
+#qt_sdk_install: QT_SDK_URL  := http://www.developer.nokia.com/dp?uri=http://sw.nokia.com/id/c365bbf5-c2b9-4dda-9c1f-34b2c8d07785/Qt_SDK_Lin32_offline_v1_1_2
+#qt_sdk_install: QT_SDK_FILE := Qt_SDK_Lin32_offline_v1_1_2_en.run
 # order-only prereq on directory existance:
 qt_sdk_install : | $(DL_DIR) $(TOOLS_DIR)
 qt_sdk_install: qt_sdk_clean
         # download the source only if it's newer than what we already have
-	$(V1) wget -N -P "$(DL_DIR)" "$(QT_SDK_URL)"
+	$(V1) wget -N --content-disposition -P "$(DL_DIR)" "$(QT_SDK_URL)"
+        # tell the user exactly which path they should select in the GUI
+	$(V1) echo "*** NOTE NOTE NOTE ***"
+	$(V1) echo "*"
+	$(V1) echo "*  In the GUI, please use exactly this path as the installation path:"
+	$(V1) echo "*        $(QT_SDK_DIR)"
+	$(V1) echo "*"
+	$(V1) echo "*** NOTE NOTE NOTE ***"
 
         #installer is an executable, make it executable and run it
 	$(V1) chmod u+x "$(DL_DIR)/$(QT_SDK_FILE)"
-	"$(DL_DIR)/$(QT_SDK_FILE)" --installdir "$(QT_SDK_DIR)"
+	$(V1) "$(DL_DIR)/$(QT_SDK_FILE)" -style cleanlooks
 
 .PHONY: qt_sdk_clean
 qt_sdk_clean:
@@ -199,43 +206,180 @@ arm_sdk_clean:
 	$(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR)
 
 # Set up openocd tools
-OPENOCD_DIR := $(TOOLS_DIR)/openocd
+OPENOCD_DIR       := $(TOOLS_DIR)/openocd
+OPENOCD_WIN_DIR   := $(TOOLS_DIR)/openocd_win
+OPENOCD_BUILD_DIR := $(DL_DIR)/openocd-build
 
 .PHONY: openocd_install
+openocd_install: | $(DL_DIR) $(TOOLS_DIR)
 openocd_install: OPENOCD_URL  := http://sourceforge.net/projects/openocd/files/openocd/0.5.0/openocd-0.5.0.tar.bz2/download
 openocd_install: OPENOCD_FILE := openocd-0.5.0.tar.bz2
-# order-only prereq on directory existance:
-openocd_install: | $(DL_DIR) $(TOOLS_DIR)
 openocd_install: openocd_clean
         # download the source only if it's newer than what we already have
 	$(V1) wget -N -P "$(DL_DIR)" --trust-server-name "$(OPENOCD_URL)"
 
         # extract the source
-	$(V1) [ ! -d "$(DL_DIR)/openocd-build" ] || $(RM) -r "$(DL_DIR)/openocd-build"
+	$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -r "$(OPENOCD_BUILD_DIR)"
-	$(V1) mkdir -p "$(DL_DIR)/openocd-build"
+	$(V1) mkdir -p "$(OPENOCD_BUILD_DIR)"
-	$(V1) tar -C $(DL_DIR)/openocd-build -xjf "$(DL_DIR)/$(OPENOCD_FILE)"
+	$(V1) tar -C $(OPENOCD_BUILD_DIR) -xjf "$(DL_DIR)/$(OPENOCD_FILE)"
 
         # build and install
 	$(V1) mkdir -p "$(OPENOCD_DIR)"
 	$(V1) ( \
-	  cd $(DL_DIR)/openocd-build/openocd-0.5.0 ; \
+	  cd $(OPENOCD_BUILD_DIR)/openocd-0.5.0 ; \
-	  ./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi --enable-buspirate; \
+	  ./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi ; \
+	  $(MAKE) --silent ; \
+	  $(MAKE) --silent install ; \
+	)
+
+        # delete the extracted source when we're done
+	$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
+
+.PHONY: ftd2xx_install
+
+FTD2XX_DIR := $(DL_DIR)/ftd2xx
+
+ftd2xx_install: | $(DL_DIR)
+ftd2xx_install: FTD2XX_URL  := http://www.ftdichip.com/Drivers/CDM/Beta/CDM20817.zip
+ftd2xx_install: FTD2XX_FILE := CDM20817.zip
+ftd2xx_install: ftd2xx_clean
+        # download the file only if it's newer than what we already have
+	$(V0) @echo " DOWNLOAD     $(FTD2XX_URL)"
+	$(V1) wget -q -N -P "$(DL_DIR)" "$(FTD2XX_URL)"
+
+        # extract the source
+	$(V0) @echo " EXTRACT      $(FTD2XX_FILE) -> $(FTD2XX_DIR)"
+	$(V1) mkdir -p "$(FTD2XX_DIR)"
+	$(V1) unzip -q -d "$(FTD2XX_DIR)" "$(DL_DIR)/$(FTD2XX_FILE)"
+
+.PHONY: ftd2xx_clean
+ftd2xx_clean:
+	$(V0) @echo " CLEAN        $(FTD2XX_DIR)"
+	$(V1) [ ! -d "$(FTD2XX_DIR)" ] || $(RM) -r "$(FTD2XX_DIR)"
+
+.PHONY: ftd2xx_install
+
+LIBUSB_WIN_DIR := $(DL_DIR)/libusb-win32-bin-1.2.6.0
+
+libusb_win_install: | $(DL_DIR)
+libusb_win_install: LIBUSB_WIN_URL  := http://sourceforge.net/projects/libusb-win32/files/libusb-win32-releases/1.2.6.0/libusb-win32-bin-1.2.6.0.zip/download
+libusb_win_install: LIBUSB_WIN_FILE := libusb-win32-bin-1.2.6.0.zip
+libusb_win_install: libusb_win_clean
+        # download the file only if it's newer than what we already have
+	$(V0) @echo " DOWNLOAD     $(LIBUSB_WIN_URL)"
+	$(V1) wget -q -N -P "$(DL_DIR)" --trust-server-name "$(LIBUSB_WIN_URL)"
+
+        # extract the source
+	$(V0) @echo " EXTRACT      $(LIBUSB_WIN_FILE) -> $(LIBUSB_WIN_DIR)"
+	$(V1) mkdir -p "$(LIBUSB_WIN_DIR)"
+	$(V1) unzip -q -d "$(DL_DIR)" "$(DL_DIR)/$(LIBUSB_WIN_FILE)"
+
+        # fixup .h file needed by openocd build
+	$(V0) @echo " FIXUP        $(LIBUSB_WIN_DIR)"
+	$(V1) ln -s "$(LIBUSB_WIN_DIR)/include/lusb0_usb.h" "$(LIBUSB_WIN_DIR)/include/usb.h"
+
+.PHONY: libusb_win_clean
+libusb_win_clean:
+	$(V0) @echo " CLEAN        $(LIBUSB_WIN_DIR)"
+	$(V1) [ ! -d "$(LIBUSB_WIN_DIR)" ] || $(RM) -r "$(LIBUSB_WIN_DIR)"
+
+.PHONY: openocd_git_win_install
+
+openocd_git_win_install: | $(DL_DIR) $(TOOLS_DIR)
+openocd_git_win_install: OPENOCD_URL  := git://openocd.git.sourceforge.net/gitroot/openocd/openocd
+openocd_git_win_install: OPENOCD_REV  := f1c0133321c8fcadadd10bba5537c0a634eb183b
+openocd_git_win_install: openocd_win_clean libusb_win_install ftd2xx_install
+        # download the source
+	$(V0) @echo " DOWNLOAD     $(OPENOCD_URL) @ $(OPENOCD_REV)"
+	$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
+	$(V1) mkdir -p "$(OPENOCD_BUILD_DIR)"
+	$(V1) git clone --no-checkout $(OPENOCD_URL) "$(DL_DIR)/openocd-build"
+	$(V1) ( \
+	  cd $(OPENOCD_BUILD_DIR) ; \
+	  git checkout -q $(OPENOCD_REV) ; \
+	)
+
+        # apply patches
+	$(V0) @echo " PATCH        $(OPENOCD_BUILD_DIR)"
+	$(V1) ( \
+	  cd $(OPENOCD_BUILD_DIR) ; \
+	  git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0001-armv7m-remove-dummy-FP-regs-for-new-gdb.patch ; \
+	  git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0002-rtos-add-stm32_stlink-to-FreeRTOS-targets.patch ; \
+	)
+
+        # build and install
+	$(V0) @echo " BUILD        $(OPENOCD_WIN_DIR)"
+	$(V1) mkdir -p "$(OPENOCD_WIN_DIR)"
+	$(V1) ( \
+	  cd $(OPENOCD_BUILD_DIR) ; \
+	  ./bootstrap ; \
+	  ./configure --enable-maintainer-mode --prefix="$(OPENOCD_WIN_DIR)" \
+		--build=i686-pc-linux-gnu --host=i586-mingw32msvc \
+		CPPFLAGS=-I$(LIBUSB_WIN_DIR)/include \
+		LDFLAGS=-L$(LIBUSB_WIN_DIR)/lib/gcc \
+		--enable-ft2232_ftd2xx --with-ftd2xx-win32-zipdir=$(FTD2XX_DIR) \
+		--disable-werror \
+		--enable-stlink ; \
 	  $(MAKE) ; \
 	  $(MAKE) install ; \
 	)
 
         # delete the extracted source when we're done
-	$(V1) [ ! -d "$(DL_DIR)/openocd-build" ] || $(RM) -r "$(DL_DIR)/openocd-build"
+	$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
+
+.PHONY: openocd_win_clean
+openocd_win_clean:
+	$(V0) @echo " CLEAN        $(OPENOCD_WIN_DIR)"
+	$(V1) [ ! -d "$(OPENOCD_WIN_DIR)" ] || $(RM) -r "$(OPENOCD_WIN_DIR)"
+
+.PHONY: openocd_git_install
+
+openocd_git_install: | $(DL_DIR) $(TOOLS_DIR)
+openocd_git_install: OPENOCD_URL  := git://openocd.git.sourceforge.net/gitroot/openocd/openocd
+openocd_git_install: OPENOCD_REV  := f1c0133321c8fcadadd10bba5537c0a634eb183b
+openocd_git_install: openocd_clean
+        # download the source
+	$(V0) @echo " DOWNLOAD     $(OPENOCD_URL) @ $(OPENOCD_REV)"
+	$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
+	$(V1) mkdir -p "$(OPENOCD_BUILD_DIR)"
+	$(V1) git clone --no-checkout $(OPENOCD_URL) "$(OPENOCD_BUILD_DIR)"
+	$(V1) ( \
+	  cd $(OPENOCD_BUILD_DIR) ; \
+	  git checkout -q $(OPENOCD_REV) ; \
+	)
+
+        # apply patches
+	$(V0) @echo " PATCH        $(OPENOCD_DIR)"
+	$(V1) ( \
+	  cd $(OPENOCD_BUILD_DIR) ; \
+	  git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0001-armv7m-remove-dummy-FP-regs-for-new-gdb.patch ; \
+	  git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0002-rtos-add-stm32_stlink-to-FreeRTOS-targets.patch ; \
+	)
+
+        # build and install
+	$(V0) @echo " BUILD        $(OPENOCD_DIR)"
+	$(V1) mkdir -p "$(OPENOCD_DIR)"
+	$(V1) ( \
+	  cd $(OPENOCD_BUILD_DIR) ; \
+	  ./bootstrap ; \
+	  ./configure --enable-maintainer-mode --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi --enable-buspirate --enable-stlink ; \
+	  $(MAKE) ; \
+	  $(MAKE) install ; \
+	)
+
+        # delete the extracted source when we're done
+	$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
 
 .PHONY: openocd_clean
 openocd_clean:
+	$(V0) @echo " CLEAN        $(OPENOCD_DIR)"
 	$(V1) [ ! -d "$(OPENOCD_DIR)" ] || $(RM) -r "$(OPENOCD_DIR)"
 
 STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash
 
 .PHONY: stm32flash_install
 stm32flash_install: STM32FLASH_URL := http://stm32flash.googlecode.com/svn/trunk
-stm32flash_install: STM32FLASH_REV := 52
+stm32flash_install: STM32FLASH_REV := 61
 stm32flash_install: stm32flash_clean
         # download the source
 	$(V0) @echo " DOWNLOAD     $(STM32FLASH_URL) @ r$(STM32FLASH_REV)"
@@ -250,6 +394,38 @@ stm32flash_clean:
 	$(V0) @echo " CLEAN        $(STM32FLASH_DIR)"
 	$(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -r "$(STM32FLASH_DIR)"
 
+DFUUTIL_DIR := $(TOOLS_DIR)/dfu-util
+
+.PHONY: dfuutil_install
+dfuutil_install: DFUUTIL_URL  := http://dfu-util.gnumonks.org/releases/dfu-util-0.5.tar.gz
+dfuutil_install: DFUUTIL_FILE := $(notdir $(DFUUTIL_URL))
+dfuutil_install: | $(DL_DIR) $(TOOLS_DIR)
+dfuutil_install: dfuutil_clean
+        # download the source
+	$(V0) @echo " DOWNLOAD     $(DFUUTIL_URL)"
+	$(V1) wget -N -P "$(DL_DIR)" "$(DFUUTIL_URL)"
+
+        # extract the source
+	$(V0) @echo " EXTRACT      $(DFUUTIL_FILE)"
+	$(V1) [ ! -d "$(DL_DIR)/dfuutil-build" ] || $(RM) -r "$(DL_DIR)/dfuutil-build"
+	$(V1) mkdir -p "$(DL_DIR)/dfuutil-build"
+	$(V1) tar -C $(DL_DIR)/dfuutil-build -xf "$(DL_DIR)/$(DFUUTIL_FILE)"
+
+        # build
+	$(V0) @echo " BUILD        $(DFUUTIL_DIR)"
+	$(V1) mkdir -p "$(DFUUTIL_DIR)"
+	$(V1) ( \
+	  cd $(DL_DIR)/dfuutil-build/dfu-util-0.5 ; \
+	  ./configure --prefix="$(DFUUTIL_DIR)" ; \
+	  $(MAKE) ; \
+	  $(MAKE) install ; \
+	)
+
+.PHONY: dfuutil_clean
+dfuutil_clean:
+	$(V0) @echo " CLEAN        $(DFUUTIL_DIR)"
+	$(V1) [ ! -d "$(DFUUTIL_DIR)" ] || $(RM) -r "$(DFUUTIL_DIR)"
+
 ##############################
 #
 # Set up paths to tools
@@ -257,7 +433,7 @@ stm32flash_clean:
 ##############################
 
 ifeq ($(shell [ -d "$(QT_SDK_DIR)" ] && echo "exists"), exists)
-  QMAKE=$(QT_SDK_DIR)/qt/bin/qmake
+  QMAKE=$(QT_SDK_DIR)/Desktop/Qt/4.8.0/gcc/bin/qmake
 else
   # not installed, hope it's in the path...
   QMAKE=qmake
@@ -312,7 +488,7 @@ uavobjgenerator:
 	  $(MAKE) --no-print-directory -w ; \
 	)
 
-UAVOBJ_TARGETS := gcs flight python matlab java
+UAVOBJ_TARGETS := gcs flight python matlab java wireshark
 .PHONY:uavobjects
 uavobjects:  $(addprefix uavobjects_, $(UAVOBJ_TARGETS))
 
@@ -342,6 +518,7 @@ uavobjects_clean:
 
 # $(1) = Canonical board name all in lower case (e.g. coptercontrol)
 # $(2) = Name of board used in source tree (e.g. CopterControl)
+# $(3) = Short name for board (e.g CC)
 define FW_TEMPLATE
 .PHONY: $(1) fw_$(1)
 $(1): fw_$(1)_opfw
@@ -352,6 +529,8 @@ fw_$(1)_%: uavobjects_flight
 	$(V1) cd $(ROOT_DIR)/flight/$(2) && \
 		$$(MAKE) -r --no-print-directory \
 		BOARD_NAME=$(1) \
+		BOARD_SHORT_NAME=$(3) \
+		BUILD_TYPE=fw \
 		TCHAIN_PREFIX="$(ARM_SDK_PREFIX)" \
 		REMOVE_CMD="$(RM)" OOCD_EXE="$(OPENOCD)" \
 		$$*
@@ -375,6 +554,8 @@ bl_$(1)_%:
 	$(V1) cd $(ROOT_DIR)/flight/Bootloaders/$(2) && \
 		$$(MAKE) -r --no-print-directory \
 		BOARD_NAME=$(1) \
+		BOARD_SHORT_NAME=$(3) \
+		BUILD_TYPE=bl \
 		TCHAIN_PREFIX="$(ARM_SDK_PREFIX)" \
 		REMOVE_CMD="$(RM)" OOCD_EXE="$(OPENOCD)" \
 		$$*
@@ -409,6 +590,8 @@ bu_$(1)_%: bl_$(1)_bino
 	$(V1) cd $(ROOT_DIR)/flight/Bootloaders/BootloaderUpdater && \
 		$$(MAKE) -r --no-print-directory \
 		BOARD_NAME=$(1) \
+		BOARD_SHORT_NAME=$(3) \
+		BUILD_TYPE=bu \
 		TCHAIN_PREFIX="$(ARM_SDK_PREFIX)" \
 		REMOVE_CMD="$(RM)" OOCD_EXE="$(OPENOCD)" \
 		$$*
@@ -419,42 +602,105 @@ bu_$(1)_clean:
 	$(V1) $(RM) -fr $(BUILD_DIR)/bu_$(1)
 endef
 
+# $(1) = Canonical board name all in lower case (e.g. coptercontrol)
+define EF_TEMPLATE
+.PHONY: ef_$(1)
+ef_$(1): ef_$(1)_bin
+
+ef_$(1)_%: bl_$(1)_bin fw_$(1)_opfw
+	$(V1) mkdir -p $(BUILD_DIR)/ef_$(1)/dep
+	$(V1) cd $(ROOT_DIR)/flight/EntireFlash && \
+		$$(MAKE) -r --no-print-directory \
+		BOARD_NAME=$(1) \
+		BOARD_SHORT_NAME=$(3) \
+		BUILD_TYPE=ef \
+		TCHAIN_PREFIX="$(ARM_SDK_PREFIX)" \
+		DFU_CMD="$(DFUUTIL_DIR)/bin/dfu-util" \
+		$$*
+
+.PHONY: ef_$(1)_clean
+ef_$(1)_clean:
+	$(V0) @echo " CLEAN      $$@"
+	$(V1) $(RM) -fr $(BUILD_DIR)/ef_$(1)
+endef
+
+# When building any of the "all_*" targets, tell all sub makefiles to display
+# additional details on each line of output to describe which build and target
+# that each line applies to.
+ifneq ($(strip $(filter all_%,$(MAKECMDGOALS))),)
+export ENABLE_MSG_EXTRA := yes
+endif
+
+# When building more than one goal in a single make invocation, also
+# enable the extra context for each output line
+ifneq ($(word 2,$(MAKECMDGOALS)),)
+export ENABLE_MSG_EXTRA := yes
+endif
+
 # $(1) = Canonical board name all in lower case (e.g. coptercontrol)
 define BOARD_PHONY_TEMPLATE
 .PHONY: all_$(1)
 all_$(1): $$(filter fw_$(1), $$(FW_TARGETS))
 all_$(1): $$(filter bl_$(1), $$(BL_TARGETS))
 all_$(1): $$(filter bu_$(1), $$(BU_TARGETS))
+all_$(1): $$(filter ef_$(1), $$(EF_TARGETS))
 
 .PHONY: all_$(1)_clean
 all_$(1)_clean: $$(addsuffix _clean, $$(filter fw_$(1), $$(FW_TARGETS)))
 all_$(1)_clean: $$(addsuffix _clean, $$(filter bl_$(1), $$(BL_TARGETS)))
 all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
+all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS)))
 endef
 
-ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins
+ALL_BOARDS := coptercontrol pipxtreme simposix
+
+# SimPosix only builds on Linux so drop it from the list for
+# all other platforms.
+ifneq ($(UNAME), Linux)
+ALL_BOARDS  := $(filter-out simposix, $(ALL_BOARDS))
+endif
 
 # Friendly names of each board (used to find source tree)
-openpilot_friendly     := OpenPilot
 coptercontrol_friendly := CopterControl
 pipxtreme_friendly     := PipXtreme
-ins_friendly           := INS
+revolution_friendly    := Revolution
-ahrs_friendly          := AHRS
+simposix_friendly      := SimPosix
+
+# SimPosix only builds on Linux so drop it from the list for
+# all other platforms.
+ifneq ($(UNAME), Linux)
+ALL_BOARDS  := $(filter-out simposix, $(ALL_BOARDS))
+endif
+
+# Short hames of each board (used to display board name in parallel builds)
+coptercontrol_short    := 'cc  '
+pipxtreme_short        := 'pipx'
+revolution_short       := 'revo'
+simposix_short         := 'posx'
+osd_short              := 'osd '
 
 # Start out assuming that we'll build fw, bl and bu for all boards
 FW_BOARDS  := $(ALL_BOARDS)
 BL_BOARDS  := $(ALL_BOARDS)
 BU_BOARDS  := $(ALL_BOARDS)
+EF_BOARDS  := $(ALL_BOARDS)
 
 # FIXME: The INS build doesn't have a bootloader or bootloader
 #        updater yet so we need to filter them out to prevent errors.
-BL_BOARDS  := $(filter-out ins, $(ALL_BOARDS))
+BL_BOARDS  := $(filter-out ins, $(BL_BOARDS))
-BU_BOARDS  := $(filter-out ins, $(ALL_BOARDS))
+BU_BOARDS  := $(filter-out ins, $(BU_BOARDS))
+
+# SimPosix doesn't have a BL, BU or EF target so we need to
+# filter them out to prevent errors on the all_flight target.
+BL_BOARDS  := $(filter-out simposix, $(BL_BOARDS))
+BU_BOARDS  := $(filter-out simposix, $(BU_BOARDS))
+EF_BOARDS  := $(filter-out simposix, $(EF_BOARDS))
 
 # Generate the targets for whatever boards are left in each list
 FW_TARGETS := $(addprefix fw_, $(FW_BOARDS))
 BL_TARGETS := $(addprefix bl_, $(BL_BOARDS))
 BU_TARGETS := $(addprefix bu_, $(BU_BOARDS))
+EF_TARGETS := $(addprefix ef_, $(EF_BOARDS))
 
 .PHONY: all_fw all_fw_clean
 all_fw:        $(addsuffix _opfw,  $(FW_TARGETS))
@@ -468,29 +714,28 @@ all_bl_clean:  $(addsuffix _clean, $(BL_TARGETS))
 all_bu:        $(addsuffix _opfw,  $(BU_TARGETS))
 all_bu_clean:  $(addsuffix _clean, $(BU_TARGETS))
 
+.PHONY: all_ef all_ef_clean
+all_ef:        $(EF_TARGETS)
+all_ef_clean:  $(addsuffix _clean, $(EF_TARGETS))
+
 .PHONY: all_flight all_flight_clean
-all_flight:       all_fw all_bl all_bu
+all_flight:       all_fw all_bl all_bu all_ef
-all_flight_clean: all_fw_clean all_bl_clean all_bu_clean
+all_flight_clean: all_fw_clean all_bl_clean all_bu_clean all_ef_clean
 
 # Expand the groups of targets for each board
 $(foreach board, $(ALL_BOARDS), $(eval $(call BOARD_PHONY_TEMPLATE,$(board))))
 
 # Expand the bootloader updater rules
-$(foreach board, $(ALL_BOARDS), $(eval $(call BU_TEMPLATE,$(board),$($(board)_friendly))))
+$(foreach board, $(ALL_BOARDS), $(eval $(call BU_TEMPLATE,$(board),$($(board)_friendly),$($(board)_short))))
 
 # Expand the firmware rules
-$(foreach board, $(ALL_BOARDS), $(eval $(call FW_TEMPLATE,$(board),$($(board)_friendly))))
+$(foreach board, $(ALL_BOARDS), $(eval $(call FW_TEMPLATE,$(board),$($(board)_friendly),$($(board)_short))))
 
 # Expand the bootloader rules
-$(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_friendly))))
+$(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_friendly),$($(board)_short))))
 
-.PHONY: sim_posix
+# Expand the entire-flash rules
-sim_posix: sim_posix_elf
+$(foreach board, $(ALL_BOARDS), $(eval $(call EF_TEMPLATE,$(board),$($(board)_friendly),$($(board)_short))))
-
-sim_posix_%: uavobjects_flight
-	$(V1) mkdir -p $(BUILD_DIR)/sitl_posix
-	$(V1) $(MAKE) --no-print-directory \
-		-C $(ROOT_DIR)/flight/OpenPilot --file=$(ROOT_DIR)/flight/OpenPilot/Makefile.posix $*
 
 .PHONY: sim_win32
 sim_win32: sim_win32_exe

flight/AHRS/ahrs.c

@@ -1,1265 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup AHRS AHRS
- * @brief The AHRS Modules perform
- *
- * @{
- * @addtogroup AHRS_Main
- * @brief Main function which does the hardware dependent stuff
- * @{
- *
- *
- * @file       ahrs.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      INSGPS Test Program
- * @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 "ahrs.h"
-#include <pios_board_info.h>
-#include "pios.h"
-#include "ahrs_timer.h"
-#include "ahrs_spi_comm.h"
-#include "insgps.h"
-#include "CoordinateConversions.h"
-#include <stdbool.h>
-#include "fifo_buffer.h"
-
-#define DEG_TO_RAD         (M_PI / 180.0)
-#define RAD_TO_DEG         (180.0 / M_PI)
-
-#define INSGPS_GPS_TIMEOUT 2   /* 2 seconds triggers reinit of position */
-#define INSGPS_GPS_MINSAT  6   /* 2 seconds triggers reinit of position */
-#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates    */
-#define INSGPS_MAGLEN      1000
-#define INSGPS_MAGTOL      0.5 /* error in magnetic vector length to use  */
-
-#define GYRO_OOB(x) ((x > (1000 * DEG_TO_RAD)) || (x < (-1000 * DEG_TO_RAD)))
-#define ACCEL_OOB(x) (((x > 12*9.81) || (x < -12*9.81)))
-#define ISNAN(x) (x != x)
-// down-sampled data index
-#define ACCEL_RAW_X_IDX			2
-#define ACCEL_RAW_Y_IDX			0
-#define ACCEL_RAW_Z_IDX			4
-#define GYRO_RAW_X_IDX			1
-#define GYRO_RAW_Y_IDX			3
-#define GYRO_RAW_Z_IDX			5
-#define GYRO_TEMP_RAW_XY_IDX	6
-#define GYRO_TEMP_RAW_Z_IDX		7
-#define MAG_RAW_X_IDX			1
-#define MAG_RAW_Y_IDX			0
-#define MAG_RAW_Z_IDX			2
-
-// For debugging the raw sensors
-//#define DUMP_RAW
-//#define DUMP_EKF
-//#define PIP_DUMP_RAW
-
-volatile int8_t ahrs_algorithm;
-
-/* INS functions */
-void ins_outdoor_update();
-void ins_indoor_update();
-void simple_update();
-
-/* Data accessors */
-void adc_callback(float *);
-bool get_accel_gyro_data();
-void process_mag_data();
-void reset_values();
-void calibrate_sensors(void);
-
-/* Communication functions */
-void send_calibration(void);
-void send_attitude(void);
-void send_velocity(void);
-void send_position(void);
-void homelocation_callback(AhrsObjHandle obj);
-void altitude_callback(AhrsObjHandle obj);
-void calibration_callback(AhrsObjHandle obj);
-void gps_callback(AhrsObjHandle obj);
-void settings_callback(AhrsObjHandle obj);
-void affine_rotate(float scale[3][4], float rotation[3]);
-void calibration(float result[3], float scale[3][4], float arg[3]);
-
-/* Bootloader related functions and var*/
-void firmwareiapobj_callback(AhrsObjHandle obj);
-volatile uint8_t reset_count=0;
-
-/**
- * @addtogroup AHRS_Global_Data AHRS Global Data
- * @{
- * Public data.  Used by both EKF and the sender
- */
-
-//! Contains the data from the mag sensor chip
-struct mag_sensor mag_data;
-
-//! Contains the data from the accelerometer
-struct accel_sensor  accel_data;
-
-//! Contains the data from the gyro
-struct gyro_sensor gyro_data;
-
-//! Conains the current estimate of the attitude
-struct attitude_solution attitude_data;
-
-//! Contains data from the altitude sensor
-struct altitude_sensor altitude_data;
-
-//! Contains data from the GPS (via the SPI link)
-struct gps_sensor gps_data;
-
-//! The oversampling rate, ekf is 2k / this
-uint8_t adc_oversampling = 15;
-
-//! Offset correction of barometric alt, to match gps data
-static float baro_offset = 0;
-
-static float mag_len = 0;
-
-typedef enum { AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING } states;
-volatile int32_t ekf_too_slow;
-volatile int32_t total_conversion_blocks;
-
-//! Buffer to allow ADC to run a bit faster than EKF
-t_fifo_buffer adc_fifo_buffer;
-
-
-/**
- * @}
- */
-
-/* INS functions */
-
-/**
- * @brief Update the EKF when in outdoor mode.  The primary difference is using the GPS values.
- */
-void ins_outdoor_update() 
-{
-	float gyro[3], accel[3], vel[3];
-	static uint32_t last_gps_time = 0;
-	uint16_t sensors;
-		
-	// format data for INS algo
-	gyro[0] = gyro_data.filtered.x;
-	gyro[1] = gyro_data.filtered.y;
-	gyro[2] = gyro_data.filtered.z;
-	accel[0] = accel_data.filtered.x,
-	accel[1] = accel_data.filtered.y,
-	accel[2] = accel_data.filtered.z,
-		
-	INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
-	attitude_data.quaternion.q1 = Nav.q[0];
-	attitude_data.quaternion.q2 = Nav.q[1];
-	attitude_data.quaternion.q3 = Nav.q[2];
-	attitude_data.quaternion.q4 = Nav.q[3];			
-	send_attitude();  // get message out quickly
-	send_velocity();
-	send_position();
-	INSCovariancePrediction(1 / (float)EKF_RATE);
-		
-	sensors = 0;
-	
-	/* 
-	 * Detect if greater than certain time since last gps update and if so
-	 * reset EKF to that position since probably drifted too far for safe
-	 * update 
-	 */
-	uint32_t this_gps_time = timer_count();
-	float gps_delay;
-	
-	if (this_gps_time < last_gps_time)
-		gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
-	else 
-		gps_delay = (this_gps_time - last_gps_time) / timer_rate();
-	last_gps_time = this_gps_time;
-	
-	if (gps_data.updated)
-	{
-		vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
-		vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
-		vel[2] = 0;
-				
-		if(gps_delay > INSGPS_GPS_TIMEOUT)
-			INSPosVelReset(gps_data.NED,vel); // position stale, reset		
-		else  {
-			sensors |= HORIZ_SENSORS | POS_SENSORS;						
-		}
-			
-		/* 
-		 * When using gps need to make sure that barometer is brought into NED frame   
-		 * we should try and see if the altitude from the home location is good enough 
-		 * to use for the offset but for now starting with this conservative filter    
-		 */
-		if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
-			baro_offset = gps_data.NED[2] + altitude_data.altitude;
-		} else {
-			/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
-			baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
-		}
-		gps_data.updated = false;
-	} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
-		vel[0] = 0;
-		vel[1] = 0;
-		vel[2] = 0;
-		sensors |= VERT_SENSORS | HORIZ_SENSORS;
-	}
-	
-	if(mag_data.updated) {
-		sensors |= MAG_SENSORS;
-		mag_data.updated = false;
-	}
-	
-	if(altitude_data.updated) {
-		sensors |= BARO_SENSOR;
-		altitude_data.updated = false;
-	}
-		
-	/* 
-	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher 
-	 * although probably should occur within INS itself 
-	 */
-	INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);	
-}
-
-/**
- * @brief Update the EKF when in indoor mode
- */
-void ins_indoor_update()
-{
-	float gyro[3], accel[3], vel[3];
-	static uint32_t last_indoor_time = 0;
-	uint16_t sensors = 0;
-
-	// format data for INS algo
-	gyro[0] = gyro_data.filtered.x;
-	gyro[1] = gyro_data.filtered.y;
-	gyro[2] = gyro_data.filtered.z;
-	accel[0] = accel_data.filtered.x,
-	accel[1] = accel_data.filtered.y,
-	accel[2] = accel_data.filtered.z,
-	
-	INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
-	attitude_data.quaternion.q1 = Nav.q[0];
-	attitude_data.quaternion.q2 = Nav.q[1];
-	attitude_data.quaternion.q3 = Nav.q[2];
-	attitude_data.quaternion.q4 = Nav.q[3];			
-	send_attitude();  // get message out quickly
-	send_velocity();
-	send_position();
-	INSCovariancePrediction(1 / (float)EKF_RATE);
-	
-	/* Indoors, update with zero position and velocity and high covariance */
-	vel[0] = 0;
-	vel[1] = 0;
-	vel[2] = 0;
-	
-	uint32_t this_indoor_time = timer_count();
-	float indoor_delay;
-	
-	/*
-	 * Detect if greater than certain time since last gps update and if so
-	 * reset EKF to that position since probably drifted too far for safe
-	 * update
-	 */
-	if (this_indoor_time < last_indoor_time)
-		indoor_delay = ((0xFFFF - last_indoor_time) - this_indoor_time) / timer_rate();
-	else 
-		indoor_delay = (this_indoor_time - last_indoor_time) / timer_rate();
-	last_indoor_time = this_indoor_time;
-		
-	if(indoor_delay > INSGPS_GPS_TIMEOUT)
-		INSPosVelReset(vel,vel);
-	else 
-		sensors = HORIZ_SENSORS | VERT_SENSORS;
-
-	if(mag_data.updated && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
-		sensors |= MAG_SENSORS;
-		mag_data.updated = false;
-	}
-	
-	if(altitude_data.updated) {
-		sensors |= BARO_SENSOR;
-		altitude_data.updated = false;
-	}
-	
-	/* 
-	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher 
-	 * although probably should occur within INS itself 
-	 */
-	INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS);	
-}
-
-/**
- * @brief Initialize the EKF assuming stationary
- */
-void ins_init_algorithm()
-{
-	float Rbe[3][3], q[4], accels[3], rpy[3], mag;
-	float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
-	bool using_mags, using_gps;
-	
-	INSGPSInit();
-	
-	HomeLocationData home;
-	HomeLocationGet(&home);
-	
-	accels[0]=accel_data.filtered.x;
-	accels[1]=accel_data.filtered.y;
-	accels[2]=accel_data.filtered.z;
-	
-	using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
-	using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0);  /* only use mags when valid home location */
-	
-	using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0);
-
-	/* Block till a data update */
-	get_accel_gyro_data();
-
-	/* Ensure we get mag data in a timely manner */
-	uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec
-	while(using_mags && !mag_data.updated && fail_count--) {
-		get_accel_gyro_data();
-		AhrsPoll();
-	}
-	using_mags &= mag_data.updated;		
-	
-	if (using_mags) {
-		/* Spin waiting for mag data */
-		while(!mag_data.updated) {
-			get_accel_gyro_data();
-			AhrsPoll();
-		}
-		mag_data.updated = false;
-
-		RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
-		R2Quaternion(Rbe,q);
-		if (using_gps)
-			INSSetState(gps_data.NED, zeros, q, zeros, zeros);
-		else
-			INSSetState(zeros, zeros, q, zeros, zeros);
-	} else {		
-		// assume yaw = 0
-		mag = VectorMagnitude(accels);
-		rpy[1] = asinf(-accels[0]/mag);
-		rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
-		rpy[2] = 0;
-		RPY2Quaternion(rpy,q);
-		if (using_gps)
-			INSSetState(gps_data.NED, zeros, q, zeros, zeros);
-		else
-			INSSetState(zeros, zeros, q, zeros, zeros);
-	}
-	
-	INSResetP(Pdiag);
-	
-	// TODO: include initial estimate of gyro bias?
-}
-
-/**
- * @brief Simple update using just mag and accel.  Yaw biased and big attitude changes.
- */
-void simple_update() {
-	float q[4];
-	float rpy[3];
-	/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
-	/* Very simple computation of the heading and attitude from accel. */
-	rpy[2] = atan2((mag_data.raw.axis[MAG_RAW_Y_IDX]), (-1 * mag_data.raw.axis[MAG_RAW_X_IDX])) * RAD_TO_DEG;
-	rpy[1] = atan2(accel_data.filtered.x, accel_data.filtered.z) * RAD_TO_DEG;
-	rpy[0] = atan2(accel_data.filtered.y, accel_data.filtered.z) * RAD_TO_DEG;
-	
-	RPY2Quaternion(rpy, q);
-	attitude_data.quaternion.q1 = q[0];
-	attitude_data.quaternion.q2 = q[1];
-	attitude_data.quaternion.q3 = q[2];
-	attitude_data.quaternion.q4 = q[3];
-	send_attitude();	
-}
-
-/**
- * @brief Output all the important inputs and states of the ekf through serial port 
- */
-#ifdef DUMP_EKF
-
-extern float **P, *X;	// covariance matrix and state vector
-
-void print_ekf_binary() 
-{
-	uint16_t states = ins_get_num_states();
-	uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
-	// Dump raw buffer
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16);                                                         // framing header (1:16)
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks));  // dump block number (17:20)
-	
-	PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3);                     // accel data (21:32)
-	PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3);                      // gyro data (33:44)
-	
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1);                                       // mag update (45)
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4);                                 // mag data (46:57)
-	
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data));                                // gps data (58:85)
-	
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * states);                                             // X (86:149)
-	for(uint8_t i = 0; i < states; i++) 
-		PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &((*P)[i + i * states]), 4);                         // diag(P) (150:213)
-	
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4);                                 // BaroAlt (214:217)
-	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4);                                            // baro_offset (218:221)
-}
-#else
-void print_ekf_binary() {}
-#endif
- 
-/**
- * @brief Debugging function to output all the ADC samples 
- */
-#if defined(DUMP_RAW)
-void print_ahrs_raw() 
-{
-	int result;
-	static int previous_conversion = 0;
-	int16_t * valid_data_buffer;
-	
-	uint8_t framing[16] =
-	{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
-		15 };
-	
-	get_accel_gyro_data();
-	
-	valid_data_buffer = PIOS_ADC_GetRawBuffer();
-
-	if (total_conversion_blocks != previous_conversion + 1)
-		PIOS_LED_On(LED1);	// not keeping up
-	else
-		PIOS_LED_Off(LED1);
-	previous_conversion = total_conversion_blocks;
-	
-	
-	// Dump raw buffer
-	result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, &framing[0], 16);	// framing header
-	result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks));	// dump block number
-	result +=
-	PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX,
-			    (uint8_t *) & valid_data_buffer[0],
-			    adc_oversampling *
-			    PIOS_ADC_NUM_PINS *
-			    sizeof(valid_data_buffer[0]));
-	if (result == 0)
-		PIOS_LED_Off(LED1);
-	else {
-		PIOS_LED_On(LED1);
-	}	
-}
-#endif
-
-#if defined(PIP_DUMP_RAW)
-
-	#define MAX_OVERSAMPLING	PIOS_ADC_MAX_OVERSAMPLING
-
-	void print_ahrs_raw()
-	{
-		int16_t accel_x[MAX_OVERSAMPLING], accel_y[MAX_OVERSAMPLING], accel_z[MAX_OVERSAMPLING];
-		int16_t  gyro_x[MAX_OVERSAMPLING],  gyro_y[MAX_OVERSAMPLING],  gyro_z[MAX_OVERSAMPLING];
-		#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
-			int16_t mag[3];
-			int16_t mag_x, mag_y, mag_z;
-		#endif
-
-		static int previous_conversion = 0;
-
-		uint8_t framing[3] = {0xD2, 0x73, 0x00};
-
-		// wait for new raw samples
-		while (previous_conversion == total_conversion_blocks);
-		if ((previous_conversion + 1) != total_conversion_blocks)
-			PIOS_LED_On(LED1);                              // we are not keeping up
-		previous_conversion = total_conversion_blocks;
-
-		// fetch the buffer address for the new samples
-		int16_t *valid_data_buffer = PIOS_ADC_GetRawBuffer();
-
-		// fetch number of raw samples in the buffer (per channel)
-		int over_sampling = PIOS_ADC_GetOverSampling();
-
-		framing[2] = over_sampling;
-
-		// copy the raw samples into their own buffers
-		for (uint16_t i = 0, j = 0; i < over_sampling; i++, j += PIOS_ADC_NUM_CHANNELS)
-		{
-			accel_x[i] = valid_data_buffer[ACCEL_RAW_X_IDX + j];
-			accel_y[i] = valid_data_buffer[ACCEL_RAW_Y_IDX + j];
-			accel_z[i] = valid_data_buffer[ACCEL_RAW_Z_IDX + j];
-
-			gyro_x[i] = valid_data_buffer[GYRO_RAW_X_IDX + j];
-			gyro_y[i] = valid_data_buffer[GYRO_RAW_Y_IDX + j];
-			gyro_z[i] = valid_data_buffer[GYRO_RAW_Z_IDX + j];
-		}
-
-		#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
-			if (PIOS_HMC5843_NewDataAvailable())
-			{
-				PIOS_HMC5843_ReadMag(mag);
-
-				mag_x = mag[MAG_RAW_X_IDX];
-				mag_y = mag[MAG_RAW_Y_IDX];
-				mag_z = mag[MAG_RAW_Z_IDX];
-			}
-		#endif
-
-		// send the raw samples
-		int result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, framing, sizeof(framing));
-		result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_x, over_sampling * sizeof(accel_x[0]));
-		result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_y, over_sampling * sizeof(accel_y[0]));
-		result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_z, over_sampling * sizeof(accel_z[0]));
-		result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_x, over_sampling * sizeof(gyro_x[0]));
-		result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_y, over_sampling * sizeof(gyro_y[0]));
-		result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0]));
-
-		if (result != 0)
-			PIOS_LED_On(LED1);          // all data not sent
-		else
-			PIOS_LED_Off(LED1);
-	}
-#endif
-
-extern void PIOS_Board_Init(void);
-
-/**
- * @brief AHRS Main function
- */
-int main()
-{
-	gps_data.quality = -1;
-	uint32_t up_time_real = 0;
-	uint32_t up_time = 0;
-	uint32_t last_up_time = 0;
-	static int8_t last_ahrs_algorithm;
-	uint32_t last_counter_idle_start = 0;
-	uint32_t last_counter_idle_end = 0;
-	uint32_t idle_counts = 0;
-	uint32_t running_counts = 0;
-	uint32_t counter_val = 0;
-	ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE;
-
-	reset_values();
-
-	PIOS_Board_Init();
-
-#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
-	// Get 3 ID bytes
-	strcpy((char *)mag_data.id, "ZZZ");
-	PIOS_HMC5843_ReadID(mag_data.id);
-#endif
-	
-	while(!AhrsLinkReady()) {
-		AhrsPoll();
-	}
-/* we didn't connect the callbacks before because we have to wait
-for all data to be up to date before doing anything*/
-	
-	AHRSCalibrationConnectCallback(calibration_callback);
-	GPSPositionConnectCallback(gps_callback);
-	BaroAltitudeConnectCallback(altitude_callback);
-	AHRSSettingsConnectCallback(settings_callback);
-	HomeLocationConnectCallback(homelocation_callback);
-	FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
-
-	calibration_callback(AHRSCalibrationHandle()); //force an update
-
-#if defined(DUMP_RAW) || defined(PIP_DUMP_RAW)
-	while (1) {
-		AhrsPoll();
-		print_ahrs_raw();
-	}
-#endif
-
-	timer_start();
-
-	/******************* Main EKF loop ****************************/
-	while(1) {
-
-
-		AhrsPoll();
-		AhrsStatusData status;
-		AhrsStatusGet(&status);
-		status.CPULoad = ((float)running_counts /
-						  (float)(idle_counts + running_counts)) * 100;
-		status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
-		status.RunningTimePerCyle = running_counts / (timer_rate() / 10000);
-		status.DroppedUpdates = ekf_too_slow;
-		up_time = timer_count();
-		if(up_time >= last_up_time) // normal condition
-			up_time_real += ((up_time - last_up_time) * 1000) / timer_rate();
-		else
-			up_time_real += ((0xFFFF - last_up_time + up_time) * 1000) / timer_rate(); 
-		last_up_time = up_time;
-		status.RunningTime = up_time_real;
-		AhrsStatusSet(&status);
-
-		// Alive signal
-		if (((total_conversion_blocks % 100) & 0xFFFE) == 0)
-			PIOS_LED_Toggle(LED1);
-
-		// Delay for valid data
-
-		counter_val = timer_count();
-		running_counts = counter_val - last_counter_idle_end;
-		last_counter_idle_start = counter_val;
-
-		// This function blocks till data avilable
-		get_accel_gyro_data();
-
-		// Get any mag data available
-		process_mag_data();
-
-		counter_val = timer_count();
-		idle_counts = counter_val - last_counter_idle_start;
-		last_counter_idle_end = counter_val;
-		
-		
-		if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
-		   ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) ||
-		   ACCEL_OOB(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
-		   GYRO_OOB(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z)) {	
-			// If any values are NaN or huge don't update
-//TODO: add field to ahrs status to track number of these events
-			continue;
-		}
-		
-		print_ekf_binary();
-
-		/* If algorithm changed reinit.  This could go in callback but wouldn't be synchronous */
-		if (ahrs_algorithm != last_ahrs_algorithm)
-			ins_init_algorithm();
-		last_ahrs_algorithm = ahrs_algorithm;
-		
-		switch(ahrs_algorithm) {
-			case AHRSSETTINGS_ALGORITHM_SIMPLE:
-				simple_update();
-				break;
-			case AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
-				ins_outdoor_update();
-				break;
-			case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR:
-			case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
-				ins_indoor_update();
-				break;
-		}
-	}
-
-	return 0;
-}
-
-/**
- * @brief Get the accel and gyro data from whichever source when available
- *
- * This function will act as the HAL for the new INS sensors
- */
-bool get_accel_gyro_data() 
-{
-	float accel[6];
-	float gyro[6];
-	uint16_t spin_count = 1;
-	while(fifoBuf_getUsed(&adc_fifo_buffer) < (sizeof(accel) + sizeof(gyro))) {
-		if(spin_count++ == 0) 
-			AhrsPoll();
-	}
-	
-	fifoBuf_getData(&adc_fifo_buffer, &accel[0], sizeof(float) * 3);
-	fifoBuf_getData(&adc_fifo_buffer, &gyro[0], sizeof(float) * 3);	
-	fifoBuf_getData(&adc_fifo_buffer, &accel[3], sizeof(float) * 3);
-	fifoBuf_getData(&adc_fifo_buffer, &gyro[3], sizeof(float) * 3);	
-	
-	accel_data.filtered.x = (accel[0] + accel[3]) / 2;
-	accel_data.filtered.y = (accel[1] + accel[4]) / 2;
-	accel_data.filtered.z = (accel[2] + accel[5]) / 2;
-	gyro_data.filtered.x = (gyro[0] + gyro[3]) / 2;
-	gyro_data.filtered.y = (gyro[1] + gyro[4]) / 2;
-	gyro_data.filtered.z = (gyro[2] + gyro[5]) / 2;
-	return true;
-}
-
-/**
- * @brief Perform calibration of a 3-axis field sensor using an affine transformation
- * matrix.
- *
- * Computes result = scale * arg.
- *
- * @param result[out] The three-axis resultant field.
- * @param scale[in] The 4x4 affine transformation matrix.  The fourth row is implicitly
- * 	[0 0 0 1]
- * @param arg[in] The 3-axis input field.  The 'w' component is implicitly 1.
- */
-void calibration(float result[3], float scale[3][4], float arg[3])
-{
-	for (int row = 0; row < 3; ++row) {
-		result[row] = 0.0f;
-		int col;
-		for (col = 0; col < 3; ++col) {
-			result[row] += scale[row][col] * arg[col];
-		}
-		// fourth column: arg has an implicit w value of 1.0f.
-		result[row] += scale[row][col];
-	}
-}
-
-/**
- * @brief Scale an affine transformation matrix by a rotation, defined by a
- * rotation vector.  scale <- rotation * scale
- *
- * @param scale[in,out] The affine transformation matrix to be rotated
- * @param rotation[in] The rotation vector defining the rotation
- */
-void affine_rotate(float scale[3][4], float rotation[3])
-{
-	// Rotate the scale factor matrix in-place
-	float rmatrix[3][3];
-	Rv2Rot(rotation, rmatrix);
-
-	float ret[3][4];
-	for (int row = 0; row < 3; ++row) {
-		for (int col = 0; col < 4; ++col) {
-			ret[row][col] = 0.0f;
-			for (int i = 0; i < 3; ++i) {
-				ret[row][col] += rmatrix[row][i] * scale[i][col];
-			}
-		}
-	}
-	// copy output to argument
-	for (int row = 0; row < 3; ++row) {
-		for (int col = 0; col < 4; ++col) {
-			scale[row][col] = ret[row][col];
-		}
-	}
-}
-
-/**
- * @brief Downsample the analog data
- * @return none
- *
- * Tried to make as much of the filtering fixed point when possible.  Need to account
- * for offset for each sample before the multiplication if filter not a boxcar.  Could
- * precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET.  Puts data into global
- * data structures @ref accel_data and @ref gyro_data.
- *
- * The accel_data values are converted into a coordinate system where X is forwards along
- * the fuselage, Y is along right the wing, and Z is down.
- */
-void adc_callback(float * downsampled_data)
-{	
-	AHRSSettingsData settings;
-	AHRSSettingsGet(&settings);
-
-#if 0
-	// Get the Y data.  Third byte in.  Convert to m/s
-	float accel_filtered[3];
-	accel_filtered[1] = 0;
-	for (i = 0; i < adc_oversampling; i++)
-		accel_filtered[1] += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
-	accel_filtered[1] /= (float) fir_coeffs[adc_oversampling];
-	// Get the X data which projects forward/backwards.  Fifth byte in.  Convert to m/s
-	accel_filtered[0] = 0;
-	for (i = 0; i < adc_oversampling; i++)
-		accel_filtered[0] += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
-	accel_filtered[0] /= (float) fir_coeffs[adc_oversampling];
-
-	// Get the Z data.  Third byte in.  Convert to m/s
-	accel_filtered[2] = 0;
-	for (i = 0; i < adc_oversampling; i++)
-		accel_filtered[2] += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
-	accel_filtered[2] /= (float) fir_coeffs[adc_oversampling];
-
-	float accel_scaled[3];
-	calibration(accel_scaled, accel_data.calibration.scale, accel_filtered);
-	accel_data.filtered.x = accel_scaled[0];
-	accel_data.filtered.y = accel_scaled[1];
-	accel_data.filtered.z = accel_scaled[2];
-#else
-	float accel[3], gyro[3];
-
-	float raw_accel[3] = {
-			downsampled_data[ACCEL_RAW_X_IDX],
-			downsampled_data[ACCEL_RAW_Y_IDX],
-			-downsampled_data[ACCEL_RAW_Z_IDX]
-	};
-
-	// Accel data is (y,x,z) in first third and fifth byte.  Convert to m/s^2
-	calibration(accel, accel_data.calibration.scale, raw_accel);
-
-	// Gyro data is (x,y,z) in second, fifth and seventh byte.  Convert to rad/s
-	gyro[0] = ( ( downsampled_data[GYRO_RAW_X_IDX] + gyro_data.calibration.tempcompfactor[0] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
-	gyro[1] = ( ( downsampled_data[GYRO_RAW_Y_IDX] + gyro_data.calibration.tempcompfactor[1] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
-	gyro[2] = ( ( downsampled_data[GYRO_RAW_Z_IDX] + gyro_data.calibration.tempcompfactor[2] * downsampled_data[GYRO_TEMP_RAW_Z_IDX]  ) * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
-
-#endif
-#if 0
-	static float gravity_tracking[3] = {0,0,0};
-	const float tau = 0.9999;
-	gravity_tracking[0] = tau * gravity_tracking[0]  + (1-tau) * accel[0];
-	gravity_tracking[1] = tau * gravity_tracking[1] + (1-tau) * accel[1];
-	gravity_tracking[2] = tau * gravity_tracking[2] + (1-tau) * (accel[2] + 9.81);
-	if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
-		accel[0] -= gravity_tracking[0];
-		accel[1] -= gravity_tracking[1];
-		accel[2] -= gravity_tracking[2];
-	}
-#endif
-	if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
-		fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
-		fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
-	} else {
-		ekf_too_slow++;
-	}
-	
-	AttitudeRawData raw;
-
-	raw.gyrotemp[0] = downsampled_data[GYRO_TEMP_RAW_XY_IDX];
-	raw.gyrotemp[1] = downsampled_data[GYRO_TEMP_RAW_Z_IDX];
-
-	raw.gyros[0] = gyro[0] * RAD_TO_DEG;
-	raw.gyros[1] = gyro[1] * RAD_TO_DEG;
-	raw.gyros[2] = gyro[2] * RAD_TO_DEG;
-
-	raw.accels[0] = accel[0];
-	raw.accels[1] = accel[1];
-	raw.accels[2] = accel[2];
-
-	raw.magnetometers[0] = mag_data.scaled.axis[0];
-	raw.magnetometers[1] = mag_data.scaled.axis[1];
-	raw.magnetometers[2] = mag_data.scaled.axis[2];
-
-	if (settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE)
-	{
-		raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
-		raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
-		raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
-
-		raw.accels[0] -= Nav.accel_bias[0];
-		raw.accels[1] -= Nav.accel_bias[1];
-		raw.accels[2] -= Nav.accel_bias[2];
-	}
-
-	AttitudeRawSet(&raw);
-	
-	total_conversion_blocks++;
-}
-
-#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
-/**
- * @brief Get the mag data from the I2C sensor and load into structure
- * @return none
- * 
- * This function also considers if the home location is set and has a valid
- * magnetic field before updating the mag data to prevent data being used that
- * cannot be interpreted.  In addition the mag data is not used for the first
- * five seconds to allow the filter to start to converge 
- */
-void process_mag_data() 
-{
-	// Get magnetic readings
-	// For now don't use mags until the magnetic field is set AND until 5 seconds
-	// after initialization otherwise it seems to have problems
-	// TODO: Follow up this initialization issue
-	HomeLocationData home;
-	HomeLocationGet(&home);
-	if (PIOS_HMC5843_NewDataAvailable()) {
-		PIOS_HMC5843_ReadMag(mag_data.raw.axis);
-
-		// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
-		mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
-		mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
-		mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
-	
-		// Only use if magnetic length reasonable
-		float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
-
-		mag_data.updated =  (home.Set == HOMELOCATION_SET_TRUE) && 
-			((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) &&
-			((mag_data.raw.axis[MAG_RAW_X_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Y_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Z_IDX] != 0)) &&
-			((Blen < mag_len * (1 + INSGPS_MAGTOL)) && (Blen > mag_len * (1 - INSGPS_MAGTOL)));
-	}		
-}
-#else
-void process_mag_data() { }
-#endif
-
-/**
- * @brief Assumes board is not moving computes biases and variances of sensors
- * @returns None
- *
- * All data is stored in global structures.  This function should be called from OP when
- * aircraft is in stable state and then the data stored to SD card.
- *
- * After this function the bias for each sensor will be the mean value.  This doesn't make
- * sense for the z accel so make sure 6 point calibration is also run and those values set
- * after these read.
- */
-#define NBIAS 100
-#define NVAR  500
-void calibrate_sensors()
-{
-	int i,j;
-	float accel_bias[3] = {0, 0, 0};
-	float gyro_bias[3] = {0, 0, 0};
-	float mag_bias[3] = {0, 0, 0};
-
-
-	for (i = 0, j = 0; i < NBIAS; i++) {
-
-		get_accel_gyro_data();
-		
-		gyro_bias[0] += gyro_data.filtered.x / NBIAS;
-		gyro_bias[1] += gyro_data.filtered.y / NBIAS;
-		gyro_bias[2] += gyro_data.filtered.z / NBIAS;
-		accel_bias[0] += accel_data.filtered.x / NBIAS;
-		accel_bias[1] += accel_data.filtered.y / NBIAS;
-		accel_bias[2] += accel_data.filtered.z / NBIAS;
-
-#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
-		if(PIOS_HMC5843_NewDataAvailable()) {
-			j ++;
-			PIOS_HMC5843_ReadMag(mag_data.raw.axis);
-			mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
-			mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
-			mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
-			mag_bias[0] += mag_data.scaled.axis[0];
-			mag_bias[1] += mag_data.scaled.axis[1];
-			mag_bias[2] += mag_data.scaled.axis[2];
-		}
-#endif
-
-	}
-	mag_bias[0] /= j;
-	mag_bias[1] /= j;
-	mag_bias[2] /= j;
-
-	gyro_data.calibration.variance[0] = 0;
-	gyro_data.calibration.variance[1] = 0;
-	gyro_data.calibration.variance[2] = 0;
-	mag_data.calibration.variance[0] = 0;
-	mag_data.calibration.variance[1] = 0;
-	mag_data.calibration.variance[2] = 0;
-	accel_data.calibration.variance[0] = 0;
-	accel_data.calibration.variance[1] = 0;
-	accel_data.calibration.variance[2] = 0;
-
-	for (i = 0, j = 0; j < NVAR; j++) {
-		get_accel_gyro_data();
-
-		gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
-		gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
-		gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
-		accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
-		accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
-		accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
-
-#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
-		if(PIOS_HMC5843_NewDataAvailable()) {
-			j ++;
-			PIOS_HMC5843_ReadMag(mag_data.raw.axis);
-			mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
-			mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
-			mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
-			mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
-			mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
-			mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
-		}
-#endif
-
-	}
-
-	mag_data.calibration.variance[0] /= j;
-	mag_data.calibration.variance[1] /= j;
-	mag_data.calibration.variance[2] /= j;
-
-	gyro_data.calibration.bias[0] -= gyro_bias[0];
-	gyro_data.calibration.bias[1] -= gyro_bias[1];
-	gyro_data.calibration.bias[2] -= gyro_bias[2];
-}
-
-/**
- * @brief Populate fields with initial values
- */
-void reset_values()
-{
-	accel_data.calibration.scale[0][1] = 0;
-	accel_data.calibration.scale[1][0] = 0;
-	accel_data.calibration.scale[0][2] = 0;
-	accel_data.calibration.scale[2][0] = 0;
-	accel_data.calibration.scale[1][2] = 0;
-	accel_data.calibration.scale[2][1] = 0;
-
-	accel_data.calibration.scale[0][0] = 0.0359;
-	accel_data.calibration.scale[1][1] = 0.0359;
-	accel_data.calibration.scale[2][2] = 0.0359;
-	accel_data.calibration.scale[0][3] = -73.5;
-	accel_data.calibration.scale[1][3] = -73.5;
-	accel_data.calibration.scale[2][3] = -73.5;
-
-	accel_data.calibration.variance[0] = 1e-4;
-	accel_data.calibration.variance[1] = 1e-4;
-	accel_data.calibration.variance[2] = 1e-4;
-
-	gyro_data.calibration.scale[0] = -0.014;
-	gyro_data.calibration.scale[1] = 0.014;
-	gyro_data.calibration.scale[2] = -0.014;
-	gyro_data.calibration.bias[0] = -24;
-	gyro_data.calibration.bias[1] = -24;
-	gyro_data.calibration.bias[2] = -24;
-	gyro_data.calibration.variance[0] = 1;
-	gyro_data.calibration.variance[1] = 1;
-	gyro_data.calibration.variance[2] = 1;
-	mag_data.calibration.scale[0] = 1;
-	mag_data.calibration.scale[1] = 1;
-	mag_data.calibration.scale[2] = 1;
-	mag_data.calibration.bias[0] = 0;
-	mag_data.calibration.bias[1] = 0;
-	mag_data.calibration.bias[2] = 0;
-	mag_data.calibration.variance[0] = 50;
-	mag_data.calibration.variance[1] = 50;
-	mag_data.calibration.variance[2] = 50;
-}
-
-
-void send_attitude(void)
-{
-	AttitudeActualData attitude;
-	AHRSSettingsData settings;
-	AHRSSettingsGet(&settings);
-	
-	attitude.q1 = attitude_data.quaternion.q1;
-	attitude.q2 = attitude_data.quaternion.q2;
-	attitude.q3 = attitude_data.quaternion.q3;
-	attitude.q4 = attitude_data.quaternion.q4;
-	float rpy[3];
-	Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
-	attitude.Roll = rpy[0] + settings.RollBias;
-	attitude.Pitch = rpy[1] + settings.PitchBias;
-	attitude.Yaw = rpy[2] + settings.YawBias;
-	if(attitude.Yaw > 360)
-		attitude.Yaw -= 360;
-	AttitudeActualSet(&attitude);
-}
-
-void send_velocity(void)
-{
-	VelocityActualData velocityActual;
-	VelocityActualGet(&velocityActual);
-	
-	// convert into cm
-	velocityActual.North = Nav.Vel[0] * 100;
-	velocityActual.East = Nav.Vel[1] * 100;
-	velocityActual.Down = Nav.Vel[2] * 100;	
-	
-	VelocityActualSet(&velocityActual);
-}
-
-void send_position(void)
-{
-	PositionActualData positionActual;
-	PositionActualGet(&positionActual);
-	
-	// convert into cm
-	positionActual.North = Nav.Pos[0] * 100;
-	positionActual.East = Nav.Pos[1] * 100;
-	positionActual.Down = Nav.Pos[2] * 100;	
-	
-	PositionActualSet(&positionActual);
-}
-
-void send_calibration(void)
-{
-	AHRSCalibrationData cal;
-	AHRSCalibrationGet(&cal);
-	for(int ct=0; ct<3; ct++)
-	{
-		cal.accel_var[ct] = accel_data.calibration.variance[ct];
-		cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
-		cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
-		cal.mag_var[ct] = mag_data.calibration.variance[ct];
-	}
-	cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
-	AHRSCalibrationSet(&cal);
-}
-
-/**
- * @brief AHRS calibration callback
- *
- * Called when the OP board sets the calibration
- */
-void calibration_callback(AhrsObjHandle obj)
-{
-	AHRSCalibrationData cal;
-	AHRSCalibrationGet(&cal);
-	if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
-
-		accel_data.calibration.scale[0][1] = cal.accel_ortho[0];
-		accel_data.calibration.scale[1][0] = cal.accel_ortho[0];
-
-		accel_data.calibration.scale[0][2] = cal.accel_ortho[1];
-		accel_data.calibration.scale[2][0] = cal.accel_ortho[1];
-
-		accel_data.calibration.scale[1][2] = cal.accel_ortho[2];
-		accel_data.calibration.scale[2][1] = cal.accel_ortho[2];
-
-#if 0
-		// TODO: Enable after v1.0 feature freeze.
-		float rotation[3] = { cal.accel_rotation[0],
-				cal.accel_rotation[1],
-				cal.accel_rotation[2],
-		};
-
-		affine_rotate(accel_data.calibration.scale, rotation);
-#endif
-
-		for(int ct=0; ct<3; ct++)
-		{
-			accel_data.calibration.scale[ct][ct] = cal.accel_scale[ct];
-			accel_data.calibration.scale[ct][3] = cal.accel_bias[ct];
-			accel_data.calibration.variance[ct] = cal.accel_var[ct];
-
-			gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
-			gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
-			gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
-#if 1
-			gyro_data.calibration.tempcompfactor[ct] = cal.gyro_tempcompfactor[ct];
-#endif
-			mag_data.calibration.bias[ct] = cal.mag_bias[ct];
-			mag_data.calibration.scale[ct] = cal.mag_scale[ct];
-			mag_data.calibration.variance[ct] = cal.mag_var[ct];
-		}
-		// Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to 
-		// one in code
-		float mag_var[3] = {mag_data.calibration.variance[0] / INSGPS_MAGLEN / INSGPS_MAGLEN, 
-			mag_data.calibration.variance[1] / INSGPS_MAGLEN / INSGPS_MAGLEN, 
-			mag_data.calibration.variance[2] / INSGPS_MAGLEN / INSGPS_MAGLEN};
-		INSSetMagVar(mag_var);
-		INSSetAccelVar(accel_data.calibration.variance);
-		INSSetGyroVar(gyro_data.calibration.variance);
-	}
-	else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE) {
-		calibrate_sensors();
-		send_calibration();
-	}
-	
-	INSSetPosVelVar(cal.pos_var, cal.vel_var);
-
-}
-
-void gps_callback(AhrsObjHandle obj)
-{
-	GPSPositionData pos;
-	GPSPositionGet(&pos);
-	HomeLocationData home;
-	HomeLocationGet(&home);
-
-	// convert from cm back to meters
-	double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
-	// put in local NED frame
-	double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
-	LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
-
-	gps_data.heading = pos.Heading;
-	gps_data.groundspeed = pos.Groundspeed;
-	gps_data.quality = 1;  /* currently unused */
-	gps_data.updated = true;
-
-	// if poor don't use this update
-	if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
-	   (pos.Satellites < INSGPS_GPS_MINSAT) || 
-	   (pos.PDOP >= INSGPS_GPS_MINPDOP) || 
-	   (home.Set == FALSE) ||
-	   ((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
-	{
-		gps_data.quality = 0;
-		gps_data.updated = false;
-	}
-
-}
-
-void altitude_callback(AhrsObjHandle obj)
-{
-	BaroAltitudeData alt;
-	BaroAltitudeGet(&alt);
-	altitude_data.altitude = alt.Altitude;
-	altitude_data.updated = true;
-}
-
-void settings_callback(AhrsObjHandle obj)
-{
-	AHRSSettingsData settings;
-	AHRSSettingsGet(&settings);
-
-	ahrs_algorithm = settings.Algorithm;
-	
-	if(settings.Downsampling != adc_oversampling) {
-		adc_oversampling = settings.Downsampling;
-		PIOS_ADC_Config(adc_oversampling);				
-	}	
-}
-
-void homelocation_callback(AhrsObjHandle obj) 
-{
-	HomeLocationData data;
-	HomeLocationGet(&data);
-
-	mag_len = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2));
-	float Be[3] = {data.Be[0] / mag_len, data.Be[1] / mag_len, data.Be[2] / mag_len};
-	
-	INSSetMagNorth(Be);
-}
-
-void firmwareiapobj_callback(AhrsObjHandle obj) 
-{
-	const struct pios_board_info * bdinfo = &pios_board_info_blob;
-
-	FirmwareIAPObjData firmwareIAPObj;
-	FirmwareIAPObjGet(&firmwareIAPObj);
-	if(firmwareIAPObj.ArmReset==0)
-		reset_count=0;
-	if(firmwareIAPObj.ArmReset==1)
-	{
-
-		if((firmwareIAPObj.BoardType==bdinfo->board_type) || (firmwareIAPObj.BoardType==0xFF))
-		{
-
-			++reset_count;
-			if(reset_count>2)
-			{
-				PIOS_IAP_SetRequest1();
-				PIOS_IAP_SetRequest2();
-				PIOS_SYS_Reset();
-			}
-		}
-	}
-	else if(firmwareIAPObj.BoardType==bdinfo->board_type && firmwareIAPObj.crc!=PIOS_BL_HELPER_CRC_Memory_Calc())
-	{
-		PIOS_BL_HELPER_FLASH_Read_Description(firmwareIAPObj.Description,bdinfo->desc_size);
-		firmwareIAPObj.crc=PIOS_BL_HELPER_CRC_Memory_Calc();
-		firmwareIAPObj.BoardRevision=bdinfo->board_rev;
-		FirmwareIAPObjSet(&firmwareIAPObj);
-	}
-}
-
-/**
- * @}
- */
-

flight/AHRS/inc/ahrs.h

@@ -1,115 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       ahrs.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Main AHRS 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 AHRS_H
-#define AHRS_H
-
-/* PIOS Includes */
-#include <pios.h>
-
-struct mag_sensor {
-	uint8_t id[4];
-	uint8_t updated;
-	struct {
-		int16_t axis[3];
-	} raw;
-	struct {
-		float axis[3];
-	} scaled;
-	struct {
-		float bias[3];
-		float scale[3];
-		float variance[3];
-	} calibration;
-};
-
-//! Contains the data from the accelerometer
-struct accel_sensor {
-	struct {
-		uint16_t x;
-		uint16_t y;
-		uint16_t z;
-	} raw;
-	struct {
-		float x;
-		float y;
-		float z;
-	} filtered;
-	struct {
-		float scale[3][4];
-		float variance[3];
-	} calibration;
-};
-
-//! Contains the data from the gyro
-struct gyro_sensor {
-	struct {
-		uint16_t x;
-		uint16_t y;
-		uint16_t z;
-	} raw;
-	struct {
-		float x;
-		float y;
-		float z;
-	} filtered;
-	struct {
-		float bias[3];
-		float scale[3];
-		float variance[3];
-		float tempcompfactor[3];
-	} calibration;
-	struct {
-		uint16_t xy;
-		uint16_t z;
-	} temp;
-};
-
-//! Conains the current estimate of the attitude
-struct attitude_solution {
-	struct {
-		float q1;
-		float q2;
-		float q3;
-		float q4;
-	} quaternion;
-};
-
-//! Contains data from the altitude sensor
-struct altitude_sensor {
-	float altitude;
-	bool updated;
-};
-
-//! Contains data from the GPS (via the SPI link)
-struct gps_sensor {
-	float NED[3];
-	float heading;
-	float groundspeed;
-	float quality;
-	bool updated;
-};
-
-#endif /* AHRS_H */

flight/AHRS/inc/ahrs_fsm.h

@@ -1,94 +0,0 @@
- /**
- ******************************************************************************
- *
- * @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 AHRS_FSM_H
-#define AHRS_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_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 /* AHRS_FSM_H */

flight/AHRS/pios_board.c

@@ -1,428 +0,0 @@
-/**
- ******************************************************************************
- *
- * @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 <pios.h>
-
-#if defined(PIOS_INCLUDE_SPI)
-
-#include <pios_spi_priv.h>
-
-/* OP Interface
- * 
- * NOTE: Leave this declared as const data so that it ends up in the 
- * .rodata section (ie. Flash) rather than in the .bss section (RAM).
- */
-void PIOS_SPI_op_irq_handler(void);
-void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
-void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
-static const struct pios_spi_cfg pios_spi_op_cfg = {
-	.regs = SPI2,
-	.init = {
-		 .SPI_Mode = SPI_Mode_Slave,
-		 .SPI_Direction = SPI_Direction_2Lines_FullDuplex,
-		 .SPI_DataSize = SPI_DataSize_8b,
-		 .SPI_NSS = SPI_NSS_Hard,
-		 .SPI_FirstBit = SPI_FirstBit_MSB,
-		 .SPI_CRCPolynomial = 7,
-		 .SPI_CPOL = SPI_CPOL_High,
-		 .SPI_CPHA = SPI_CPHA_2Edge,
-		 },
-	.use_crc = TRUE,
-	.dma = {
-		.ahb_clk = RCC_AHBPeriph_DMA1,
-
-		.irq = {
-			.flags =
-			(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
-			 DMA1_FLAG_GL4),
-			.init = {
-				 .NVIC_IRQChannel = DMA1_Channel4_IRQn,
-				 .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-				 .NVIC_IRQChannelSubPriority = 0,
-				 .NVIC_IRQChannelCmd = ENABLE,
-				 },
-			},
-
-		.rx = {
-		       .channel = DMA1_Channel4,
-		       .init = {
-				.DMA_PeripheralBaseAddr =
-				(uint32_t) & (SPI2->DR),
-				.DMA_DIR = DMA_DIR_PeripheralSRC,
-				.DMA_PeripheralInc =
-				DMA_PeripheralInc_Disable,
-				.DMA_MemoryInc = DMA_MemoryInc_Enable,
-				.DMA_PeripheralDataSize =
-				DMA_PeripheralDataSize_Byte,
-				.DMA_MemoryDataSize =
-				DMA_MemoryDataSize_Byte,
-				.DMA_Mode = DMA_Mode_Normal,
-				.DMA_Priority = DMA_Priority_Medium,
-				.DMA_M2M = DMA_M2M_Disable,
-				},
-		       },
-		.tx = {
-		       .channel = DMA1_Channel5,
-		       .init = {
-				.DMA_PeripheralBaseAddr =
-				(uint32_t) & (SPI2->DR),
-				.DMA_DIR = DMA_DIR_PeripheralDST,
-				.DMA_PeripheralInc =
-				DMA_PeripheralInc_Disable,
-				.DMA_MemoryInc = DMA_MemoryInc_Enable,
-				.DMA_PeripheralDataSize =
-				DMA_PeripheralDataSize_Byte,
-				.DMA_MemoryDataSize =
-				DMA_MemoryDataSize_Byte,
-				.DMA_Mode = DMA_Mode_Normal,
-				.DMA_Priority = DMA_Priority_Medium,
-				.DMA_M2M = DMA_M2M_Disable,
-				},
-		       },
-		},
-	.ssel = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_12,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_IN_FLOATING,
-			  },
-		 },
-	.sclk = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_13,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_IN_FLOATING,
-			  },
-		 },
-	.miso = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_14,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_AF_PP,
-			  },
-		 },
-	.mosi = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_15,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_IN_FLOATING,
-			  },
-		 },
-};
-
-uint32_t pios_spi_op_id;
-void PIOS_SPI_op_irq_handler(void)
-{
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_SPI_IRQ_Handler(pios_spi_op_id);
-}
-
-#endif /* PIOS_INCLUDE_SPI */
-
-/*
- * ADC system
- */
-#include "pios_adc_priv.h"
-extern void PIOS_ADC_handler(void);
-void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
-// Remap the ADC DMA handler to this one
-static const struct pios_adc_cfg pios_adc_cfg = {
-	.dma = {
-		.ahb_clk  = RCC_AHBPeriph_DMA1,
-		.irq = {
-			.flags   = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
-			.init    = {
-				.NVIC_IRQChannel                   = DMA1_Channel1_IRQn,
-				.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-				.NVIC_IRQChannelSubPriority        = 0,
-				.NVIC_IRQChannelCmd                = ENABLE,
-			},
-		},
-		.rx = {
-			.channel = DMA1_Channel1,
-			.init    = {
-				.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR,
-				.DMA_DIR                = DMA_DIR_PeripheralSRC,
-				.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-				.DMA_MemoryInc          = DMA_MemoryInc_Enable,
-				.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
-				.DMA_MemoryDataSize     = DMA_MemoryDataSize_Word,
-				.DMA_Mode               = DMA_Mode_Circular,
-				.DMA_Priority           = DMA_Priority_High,
-				.DMA_M2M                = DMA_M2M_Disable,
-			},
-		}
-	}, 
-	.half_flag = DMA1_IT_HT1,
-	.full_flag = DMA1_IT_TC1,
-};
-
-struct pios_adc_dev pios_adc_devs[] = {
-	{
-		.cfg = &pios_adc_cfg,
-		.callback_function = NULL,
-	},
-};
-
-uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
-
-void PIOS_ADC_handler() {
-	PIOS_ADC_DMA_Handler();
-}
-
-
-#if defined(PIOS_INCLUDE_USART)
-#include <pios_usart_priv.h>
-
-/*
- * AUX USART
- */
-static const struct pios_usart_cfg pios_usart_aux_cfg = {
-	.regs = USART3,
-	.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 = USART3_IRQn,
-			 .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-			 .NVIC_IRQChannelSubPriority = 0,
-			 .NVIC_IRQChannelCmd = ENABLE,
-			 },
-		},
-	.rx = {
-	       .gpio = GPIOB,
-	       .init = {
-			.GPIO_Pin = GPIO_Pin_11,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode = GPIO_Mode_IPU,
-			},
-	       },
-	.tx = {
-	       .gpio = GPIOB,
-	       .init = {
-			.GPIO_Pin = GPIO_Pin_10,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode = GPIO_Mode_AF_PP,
-			},
-	       },
-};
-
-#endif /* PIOS_INCLUDE_USART */
-
-#if defined(PIOS_INCLUDE_COM)
-
-#include <pios_com_priv.h>
-
-#define PIOS_COM_AUX_TX_BUF_LEN 192
-static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
-
-#endif /* PIOS_INCLUDE_COM */
-
-#if defined(PIOS_INCLUDE_I2C)
-
-#include <pios_i2c_priv.h>
-
-/*
- * I2C Adapters
- */
-
-void PIOS_I2C_main_adapter_ev_irq_handler(void);
-void PIOS_I2C_main_adapter_er_irq_handler(void);
-void I2C1_EV_IRQHandler()
-    __attribute__ ((alias("PIOS_I2C_main_adapter_ev_irq_handler")));
-void I2C1_ER_IRQHandler()
-    __attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler")));
-
-static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
-	.regs = I2C1,
-	.init = {
-		 .I2C_Mode = I2C_Mode_I2C,
-		 .I2C_OwnAddress1 = 0,
-		 .I2C_Ack = I2C_Ack_Enable,
-		 .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-		 .I2C_DutyCycle = I2C_DutyCycle_2,
-		 .I2C_ClockSpeed = 200000,	/* bits/s */
-		 },
-	.transfer_timeout_ms = 50,
-	.scl = {
-		.gpio = GPIOB,
-		.init = {
-			 .GPIO_Pin = GPIO_Pin_6,
-			 .GPIO_Speed = GPIO_Speed_10MHz,
-			 .GPIO_Mode = GPIO_Mode_AF_OD,
-			 },
-		},
-	.sda = {
-		.gpio = GPIOB,
-		.init = {
-			 .GPIO_Pin = GPIO_Pin_7,
-			 .GPIO_Speed = GPIO_Speed_10MHz,
-			 .GPIO_Mode = GPIO_Mode_AF_OD,
-			 },
-		},
-	.event = {
-		  .flags = 0,	/* FIXME: check this */
-		  .init = {
-			   .NVIC_IRQChannel = I2C1_EV_IRQn,
-			   .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-			   .NVIC_IRQChannelSubPriority = 0,
-			   .NVIC_IRQChannelCmd = ENABLE,
-			   },
-		  },
-	.error = {
-		  .flags = 0,	/* FIXME: check this */
-		  .init = {
-			   .NVIC_IRQChannel = I2C1_ER_IRQn,
-			   .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-			   .NVIC_IRQChannelSubPriority = 0,
-			   .NVIC_IRQChannelCmd = ENABLE,
-			   },
-		  },
-};
-
-uint32_t pios_i2c_main_adapter_id;
-void PIOS_I2C_main_adapter_ev_irq_handler(void)
-{
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id);
-}
-
-void PIOS_I2C_main_adapter_er_irq_handler(void)
-{
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id);
-}
-
-#endif /* PIOS_INCLUDE_I2C */
-
-#if defined(PIOS_ENABLE_DEBUG_PINS)
-
-static const struct stm32_gpio pios_debug_pins[] = {
-  {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_11,
-      .GPIO_Speed = GPIO_Speed_50MHz,
-      .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-    },
-  },
-  {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_10,
-      .GPIO_Speed = GPIO_Speed_50MHz,
-      .GPIO_Mode  = GPIO_Mode_Out_PP,
-    },
-  },
-};
-
-#endif /* PIOS_ENABLE_DEBUG_PINS */
-
-extern const struct pios_com_driver pios_usart_com_driver;
-
-uint32_t pios_com_aux_id;
-uint8_t adc_fifo_buf[sizeof(float) * 6 * 4] __attribute__ ((aligned(4)));    // align to 32-bit to try and provide speed improvement
-
-/**
- * PIOS_Board_Init()
- * initializes all the core subsystems on this specific hardware
- * called from System/openpilot.c
- */
-void PIOS_Board_Init(void) {
-	/* Brings up System using CMSIS functions, enables the LEDs. */
-	PIOS_SYS_Init();
-
-	PIOS_LED_On(LED1);
-
-	/* Delay system */
-	PIOS_DELAY_Init();
-
-	/* Communication system */
-#if !defined(PIOS_ENABLE_DEBUG_PINS)
-#if defined(PIOS_INCLUDE_COM)
-	{
-		uint32_t pios_usart_aux_id;
-		if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
-			PIOS_DEBUG_Assert(0);
-		}
-		if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
-				  NULL, 0,
-				  pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
-			PIOS_DEBUG_Assert(0);
-		}
-	}
-#endif	/* PIOS_INCLUDE_COM */
-#endif
-
-	/* IAP System Setup */
-	PIOS_IAP_Init();
-
-	/* ADC system */
-	PIOS_ADC_Init();
-	extern uint8_t adc_oversampling;
-	PIOS_ADC_Config(adc_oversampling);
-	extern void adc_callback(float *);
-	PIOS_ADC_SetCallback(adc_callback);
-
-	/* ADC buffer */
-	extern t_fifo_buffer adc_fifo_buffer;
-	fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf));
-
-	/* Setup the Accelerometer FS (Full-Scale) GPIO */
-	PIOS_GPIO_Enable(0);
-	SET_ACCEL_6G;
-
-#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
-	/* Magnetic sensor system */
-	if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-	PIOS_HMC5843_Init();
-#endif
-
-#if defined(PIOS_INCLUDE_SPI)
-#include "ahrs_spi_comm.h"
-	AhrsInitComms();
-
-	/* Set up the SPI interface to the OP board */
-	if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-
-	AhrsConnect(pios_spi_op_id);
-#endif
-}
-

flight/AHRS/test.c

@@ -1,37 +0,0 @@
-#include "inc/insgps.h"
-#include "stdio.h"
-#include "math.h"
-
-extern struct NavStruct Nav;
-extern float X[13];
-
-int main()
-{
-	float gyro[3] = { 2.47, -0.25, 7.71 }, accel[3] = {
-	-1.02, 0.70, -10.11}, dT = 0.04, mags[3] = {
-	-50, -180, -376};
-	float Pos[3] = { 0, 0, 0 }, Vel[3] = {
-	0, 0, 0}, BaroAlt = 2.66, Speed = 4.4, Heading = 0;
-	float yaw;
-	int i, j;
-
-	INSGPSInit();
-
-	for (i = 0; i < 10000000; i++) {
-		INSPrediction(gyro, accel, dT);
-		//MagCorrection(mags);
-		FullCorrection(mags, Pos, Vel, BaroAlt);
-		yaw =
-		    atan2((float)2 *
-			  (Nav.q[0] * Nav.q[3] + Nav.q[1] * Nav.q[2]),
-			  (float)(1 -
-				  2 * (Nav.q[2] * Nav.q[2] +
-				       Nav.q[3] * Nav.q[3]))) * 180 / M_PI;
-
-		printf("%0.3f ", yaw);
-		for (j = 0; j < 13; j++)
-			printf("%f ", X[j]);
-		printf("\r\n");
-	}
-	return 0;
-}

flight/Bootloaders/AHRS/ahrs_slave_test.c

@@ -1,34 +0,0 @@
-#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);
-}
-

flight/Bootloaders/AHRS/ahrs_spi_program.c

@@ -1,65 +0,0 @@
-#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);
-}

flight/Bootloaders/AHRS/ahrs_spi_program_master.c

@@ -1,131 +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 "openpilot.h"
-#include "ahrs_spi_program_master.h"
-#include "ahrs_spi_program.h"
-#include "pios_spi.h"
-
-PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf,
-		AhrsProgramPacket *rxBuf);
-
-#define MAX_CONNECT_TRIES 500 //half a second
-bool AhrsProgramConnect(uint32_t spi_id) {
-	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(spi_id, 0);
-		uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) &txBuf,
-				(uint8_t *) &rxBuf, SPI_PROGRAM_REQUEST_LENGTH + 1, NULL);
-		PIOS_SPI_RC_PinSet(spi_id, 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 spi_id, 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(spi_id, &txBuf, &rxBuf);
-	if (ret != PROGRAM_ERR_OK) {
-		return (ret);
-	}
-	return (PROGRAM_ERR_OK);
-}
-
-PROGERR AhrsProgramRead(uint32_t spi_id, 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(spi_id, &txBuf, &rxBuf);
-	if (ret != PROGRAM_ERR_OK) {
-		return (ret);
-	}
-	memcpy(data, rxBuf.data, size);
-	return (PROGRAM_ERR_OK);
-}
-
-PROGERR AhrsProgramReboot(uint32_t spi_id) {
-	AhrsProgramPacket rxBuf;
-	AhrsProgramPacket txBuf;
-	memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
-	txBuf.type = PROGRAM_REBOOT;
-	memcpy(txBuf.data, REBOOT_CONFIRMATION, REBOOT_CONFIRMATION_LENGTH);
-	PROGERR ret = TransferPacket(spi_id, &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(uint32_t spi_id, 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(spi_id, 0);
-		uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) txBuf,
-				(uint8_t *) rxBuf, sizeof(AhrsProgramPacket), NULL);
-		PIOS_SPI_RC_PinSet(spi_id, 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);
-}
-

flight/Bootloaders/AHRS/ahrs_spi_program_slave.c

@@ -1,137 +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 <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(uint32_t spi_id) {
-	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(spi_id) != 0)) {
-		};
-		memset(&rxBuf, 'a', sizeof(AhrsProgramPacket));
-		int32_t res = PIOS_SPI_TransferBlock(spi_id, 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(spi_id, (uint8_t*) &txBuf, NULL,
-			SPI_PROGRAM_REQUEST_LENGTH + 1, NULL)) {
-	};
-
-	txBuf.type = PROGRAM_NULL;
-
-	while (!done) {
-		WAIT_IF_RECEIVING();
-		if (0 == PIOS_SPI_TransferBlock(spi_id, (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;
-	}
-}

flight/Bootloaders/AHRS/bl_fsm.c

@@ -1,591 +0,0 @@
-#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);
-	}
-}

flight/Bootloaders/AHRS/inc/ahrs_spi_program.h

@@ -1,65 +0,0 @@
-#ifndef AHRS_SPI_PROGRAM_H
-#define AHRS_SPI_PROGRAM_H
-
-/* Special packets to enter programming mode.
- Note: these must both be SPI_PROGRAM_REQUEST_LENGTH long.
- Pad with spaces if needed.
- */
-#define SPI_PROGRAM_REQUEST "AHRS START PROGRAMMING  "
-#define SPI_PROGRAM_ACK     "AHRS PROGRAMMING STARTED"
-#define SPI_PROGRAM_REQUEST_LENGTH 24
-
-/**Proposed programming protocol:
- In the master:
- 1) Send a AhrsProgramPacket containing the relevant data.
- Note crc is a CRC32 as the CRC8 used in hardware can be fooled.
- 2) Keep sending PROGRAM_NULL packets and wait for an ack.
- 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
- 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 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_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 (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

flight/Bootloaders/AHRS/inc/ahrs_spi_program_master.h

@@ -1,66 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       ahrs_spi_program_master.h
- * @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
- */
-
-#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: false if failed.
- */
-bool AhrsProgramConnect(uint32_t spi_id);
-
-/** Write data to AHRS
- * size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE
- * returns: error status
- */
-
-PROGERR AhrsProgramWrite(uint32_t spi_id, 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 spi_id, uint32_t address, void * data,
-		uint32_t size);
-
-/** reboot AHRS
- * returns: error status
- */
-
-PROGERR AhrsProgramReboot(uint32_t spi_id);
-
-//TODO: Implement programming protocol
-
-#endif //AHRS_PROGRAM_MASTER_H

flight/Bootloaders/AHRS/inc/bl_fsm.h

@@ -1,97 +0,0 @@
-/**
- ******************************************************************************
- *
- * @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 */

flight/Bootloaders/AHRS/main.c

@@ -1,276 +0,0 @@
-/**
- ******************************************************************************
- * @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 "ahrs_bl.h"
-#include <pios_board_info.h>
-#include "pios_opahrs_proto.h"
-#include "bl_fsm.h"		/* lfsm_state */
-#include "stm32f10x_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();
-uint8_t jumpFW = FALSE;
-uint32_t Fw_crc;
-/**
- * @brief Bootloader Main function
- */
-int main() {
-
-	uint8_t GO_dfu = false;
-	/* Brings up System using CMSIS functions, enables the LEDs. */
-	PIOS_SYS_Init();
-	/* Enable Prefetch Buffer */
-	FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
-
-	/* Flash 2 wait state */
-	FLASH_SetLatency(FLASH_Latency_2);
-	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
-	/* Delay system */
-	PIOS_DELAY_Init();
-
-	for (uint32_t t = 0; t < 10000000; ++t) {
-		if (NSS_HOLD_STATE == 1)
-			GO_dfu = TRUE;
-		else {
-			GO_dfu = FALSE;
-			break;
-		}
-	}
-	//while(TRUE)
-	//	{
-	//		PIOS_LED_Toggle(LED1);
-	//		PIOS_DELAY_WaitmS(1000);
-	//	}
-	//GO_dfu = TRUE;
-	PIOS_IAP_Init();
-	GO_dfu = GO_dfu | PIOS_IAP_CheckRequest();// OR with app boot request
-	if (GO_dfu == FALSE) {
-		jump_to_app();
-	}
-	if (PIOS_IAP_CheckRequest()) {
-		PIOS_DELAY_WaitmS(1000);
-		PIOS_IAP_ClearRequest();
-	}
-	PIOS_Board_Init();
-	boot_status = idle;
-	Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
-	PIOS_LED_On(LED1);
-	while (1) {
-		process_spi_request();
-	}
-	return 0;
-}
-
-static struct opahrs_msg_v0 link_tx_v0;
-static struct opahrs_msg_v0 link_rx_v0;
-static struct opahrs_msg_v0 user_tx_v0;
-static struct opahrs_msg_v0 user_rx_v0;
-void process_spi_request(void) {
-	const struct pios_board_info * bdinfo = &pios_board_info_blob;
-	bool msg_to_process = FALSE;
-
-	PIOS_IRQ_Disable();
-	/* Figure out if we're in an interesting stable state */
-	switch (lfsm_get_state()) {
-	case LFSM_STATE_USER_BUSY:
-		msg_to_process = TRUE;
-		break;
-	case LFSM_STATE_INACTIVE:
-		/* Queue up a receive buffer */
-		lfsm_user_set_rx_v0(&user_rx_v0);
-		lfsm_user_done();
-		break;
-	case LFSM_STATE_STOPPED:
-		/* Get things going */
-		lfsm_set_link_proto_v0(&link_tx_v0, &link_rx_v0);
-		break;
-	default:
-		/* Not a stable state */
-		break;
-	}
-	PIOS_IRQ_Enable();
-
-	if (!msg_to_process) {
-		/* Nothing to do */
-		//PIOS_COM_SendFormattedString(PIOS_COM_AUX, ".");
-		return;
-	}
-
-	if (user_rx_v0.tail.magic != OPAHRS_MSG_MAGIC_TAIL) {
-		return;
-	}
-
-	switch (user_rx_v0.payload.user.t) {
-
-	case OPAHRS_MSG_V0_REQ_FWUP_VERIFY:
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
-		Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		boot_status = idle;
-		PIOS_LED_Off(LED1);
-		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
-		break;
-	case OPAHRS_MSG_V0_REQ_RESET:
-		PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.reset.reset_delay_in_ms);
-		PIOS_SYS_Reset();
-		break;
-	case OPAHRS_MSG_V0_REQ_VERSIONS:
-		//PIOS_LED_On(LED1);
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS);
-		user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION;
-		user_tx_v0.payload.user.v.rsp.versions.hw_version = (BOARD_TYPE << 8)
-				| BOARD_REVISION;
-		user_tx_v0.payload.user.v.rsp.versions.fw_crc = Fw_crc;
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		break;
-	case OPAHRS_MSG_V0_REQ_MEM_MAP:
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP);
-		user_tx_v0.payload.user.v.rsp.mem_map.density = bdinfo->hw_type;
-		user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE
-				| (BOARD_WRITABLE << 1));
-		user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory
-				= bdinfo->fw_size;
-		user_tx_v0.payload.user.v.rsp.mem_map.size_of_description
-				= bdinfo->desc_size;
-		user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code
-				= bdinfo->fw_base;
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		break;
-	case OPAHRS_MSG_V0_REQ_SERIAL:
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_SERIAL);
-		PIOS_SYS_SerialNumberGet(
-				(char *) &(user_tx_v0.payload.user.v.rsp.serial.serial_bcd));
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		break;
-	case OPAHRS_MSG_V0_REQ_FWUP_STATUS:
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
-		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		break;
-	case OPAHRS_MSG_V0_REQ_FWUP_DATA:
-		PIOS_LED_On(LED1);
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
-		if (!(user_rx_v0.payload.user.v.req.fwup_data.adress
-				< bdinfo->fw_base)) {
-			for (uint8_t x = 0; x
-					< user_rx_v0.payload.user.v.req.fwup_data.size; ++x) {
-				if (FLASH_ProgramWord(
-						(user_rx_v0.payload.user.v.req.fwup_data.adress
-								+ ((uint32_t)(x * 4))),
-						user_rx_v0.payload.user.v.req.fwup_data.data[x])
-						!= FLASH_COMPLETE) {
-					boot_status = write_error;
-					break;
-				}
-			}
-		} else {
-			boot_status = outside_dev_capabilities;
-		}
-		PIOS_LED_Off(LED1);
-		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		break;
-	case OPAHRS_MSG_V0_REQ_FWDN_DATA:
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWDN_DATA);
-		uint32_t adr = user_rx_v0.payload.user.v.req.fwdn_data.adress;
-		for (uint8_t x = 0; x < 4; ++x) {
-			user_tx_v0.payload.user.v.rsp.fw_dn.data[x]
-					= *PIOS_BL_HELPER_FLASH_If_Read(adr + x);
-		}
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		break;
-	case OPAHRS_MSG_V0_REQ_FWUP_START:
-		FLASH_Unlock();
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
-		user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
-		lfsm_user_set_tx_v0(&user_tx_v0);
-		PIOS_LED_On(LED1);
-		if (PIOS_BL_HELPER_FLASH_Start() == TRUE) {
-			boot_status = started;
-			PIOS_LED_Off(LED1);
-		} else {
-			boot_status = start_failed;
-			break;
-		}
-
-		break;
-	case OPAHRS_MSG_V0_REQ_BOOT:
-		PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.boot.boot_delay_in_ms);
-		FLASH_Lock();
-		jump_to_app();
-		break;
-	default:
-		break;
-	}
-
-	/* Finished processing the received message, requeue it */
-	lfsm_user_set_rx_v0(&user_rx_v0);
-	lfsm_user_done();
-	return;
-}
-void jump_to_app() {
-	const struct pios_board_info * bdinfo = &pios_board_info_blob;
-
-	PIOS_LED_On(LED1);
-	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;
-	}
-}

flight/Bootloaders/AHRS/pios_board.c

@@ -1,149 +0,0 @@
-/**
- ******************************************************************************
- *
- * @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 <pios.h>
-
-#if defined(PIOS_INCLUDE_SPI)
-
-#include <pios_spi_priv.h>
-
-/* OP Interface
- * 
- * NOTE: Leave this declared as const data so that it ends up in the 
- * .rodata section (ie. Flash) rather than in the .bss section (RAM).
- */
-void PIOS_SPI_op_irq_handler(void);
-void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler")));
-void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler")));
-static const struct pios_spi_cfg
-		pios_spi_op_cfg = {
-				.regs = SPI2,
-				.init = {
-					.SPI_Mode = SPI_Mode_Slave,
-					.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
-					.SPI_DataSize = SPI_DataSize_8b,
-					.SPI_NSS = SPI_NSS_Hard,
-					.SPI_FirstBit = SPI_FirstBit_MSB,
-					.SPI_CRCPolynomial = 7,
-					.SPI_CPOL = SPI_CPOL_High,
-					.SPI_CPHA = SPI_CPHA_2Edge,
-				},
-				.use_crc = TRUE,
-				.dma = {
-					.ahb_clk = RCC_AHBPeriph_DMA1,
-
-					.irq = {
-						.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
-						.init = {
-							.NVIC_IRQChannel = DMA1_Channel4_IRQn,
-							.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-							.NVIC_IRQChannelSubPriority = 0,
-							.NVIC_IRQChannelCmd = ENABLE,
-						},
-					},
-
-					.rx = {
-						.channel = DMA1_Channel4,
-						.init = {
-							.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
-							.DMA_DIR = DMA_DIR_PeripheralSRC,
-							.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
-							.DMA_MemoryInc = DMA_MemoryInc_Enable,
-							.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-							.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
-							.DMA_Mode = DMA_Mode_Normal,
-							.DMA_Priority = DMA_Priority_Medium,
-							.DMA_M2M = DMA_M2M_Disable,
-						},
-					},
-					.tx = {
-						.channel = DMA1_Channel5,
-						.init = {
-							.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
-							.DMA_DIR = DMA_DIR_PeripheralDST,
-							.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
-							.DMA_MemoryInc = DMA_MemoryInc_Enable,
-							.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-							.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
-							.DMA_Mode = DMA_Mode_Normal,
-							.DMA_Priority = DMA_Priority_Medium,
-							.DMA_M2M = DMA_M2M_Disable,
-						},
-					},
-				}, .ssel = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_12,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_IN_FLOATING,
-					},
-				}, .sclk = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_13,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_IN_FLOATING,
-					},
-				}, .miso = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_14,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_AF_PP,
-					},
-				}, .mosi = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_15,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_IN_FLOATING,
-					},
-				}, };
-
-uint32_t pios_spi_op_id;
-void PIOS_SPI_op_irq_handler(void) {
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_SPI_IRQ_Handler(pios_spi_op_id);
-}
-
-#endif /* PIOS_INCLUDE_SPI */
-
-#include "bl_fsm.h"		/* lfsm_* */
-
-static bool board_init_complete = false;
-void PIOS_Board_Init() {
-	if (board_init_complete) {
-		return;
-	}
-
-	/* Set up the SPI interface to the OP board */
-	if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-	lfsm_attach(pios_spi_op_id);
-	lfsm_init();
-
-	board_init_complete = true;
-}

flight/Bootloaders/BootloaderUpdater/Makefile

@@ -89,6 +89,7 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS
 RTOSSRCDIR = $(RTOSDIR)/Source
 RTOSINCDIR = $(RTOSSRCDIR)/include
 DOXYGENDIR = ../Doc/Doxygen
+HWDEFSINC = $(TOP)/flight/board_hw_defs/$(BOARD_NAME)
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
@@ -165,6 +166,7 @@ EXTRAINCDIRS  += $(MSDDIR)
 EXTRAINCDIRS  += $(RTOSINCDIR)
 EXTRAINCDIRS  += $(APPLIBDIR)
 EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
+EXTRAINCDIRS  += $(HWDEFSINC)
 
 
 

flight/Bootloaders/BootloaderUpdater/main.c

@@ -28,6 +28,7 @@
 /* Bootloader Includes */
 #include <pios.h>
 #include <stdbool.h>
+#include "pios_board_info.h"
 
 #define MAX_WRI_RETRYS 3
 /* Prototype of PIOS_Board_Init() function */
@@ -50,15 +51,40 @@ int main() {
 
 	PIOS_SYS_Init();
 	PIOS_Board_Init();
-	PIOS_LED_On(LED1);
+	PIOS_LED_On(PIOS_LED_HEARTBEAT);
 	PIOS_DELAY_WaitmS(3000);
-	PIOS_LED_Off(LED1);
+	PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 
 	/// Self overwrite check
 	uint32_t base_address = SCB->VTOR;
 	if ((0x08000000 + embedded_image_size) > base_address)
-		error(LED1);
+		error(PIOS_LED_HEARTBEAT);
 	///
+
+	/*
+	 * Make sure the bootloader we're carrying is for the same
+	 * board type and board revision as the one we're running on.
+	 *
+	 * Assume the bootloader in flash and the bootloader contained in
+	 * the updater both carry a board_info_blob at the end of the image.
+	 */
+
+	/* Calculate how far the board_info_blob is from the beginning of the bootloader */
+	uint32_t board_info_blob_offset = (uint32_t)&pios_board_info_blob - (uint32_t)0x08000000;
+
+	/* Use the same offset into our embedded bootloader image */
+	struct pios_board_info * new_board_info_blob = (struct pios_board_info *)
+		((uint32_t)embedded_image_start + board_info_blob_offset);
+
+	/* Compare the two board info blobs to make sure they're for the same HW revision */
+	if ((pios_board_info_blob.magic != new_board_info_blob->magic) ||
+		(pios_board_info_blob.board_type != new_board_info_blob->board_type) ||
+		(pios_board_info_blob.board_rev != new_board_info_blob->board_rev)) {
+		error(PIOS_LED_HEARTBEAT);
+	}
+
+	/* Embedded bootloader looks like it's the right one for this HW, proceed... */
+
 	FLASH_Unlock();
 
 	/// Bootloader memory space erase
@@ -82,7 +108,7 @@ int main() {
 	}
 
 	if (fail == true)
-		error(LED1);
+		error(PIOS_LED_HEARTBEAT);
 
 
 	///
@@ -90,7 +116,7 @@ int main() {
 	/// Bootloader programing
 	for (uint32_t offset = 0; offset < embedded_image_size/sizeof(uint32_t); ++offset) {
 		bool result = false;
-		PIOS_LED_Toggle(LED1);
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 		for (uint8_t retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
 			if (result == false) {
 				result = (FLASH_ProgramWord(0x08000000 + (offset * 4), embedded_image_start[offset])
@@ -98,13 +124,13 @@ int main() {
 			}
 		}
 		if (result == false)
-			error(LED1);
+			error(PIOS_LED_HEARTBEAT);
 	}
 	///
 	for (uint8_t x = 0; x < 3; ++x) {
-			PIOS_LED_On(LED1);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			PIOS_DELAY_WaitmS(1000);
-			PIOS_LED_Off(LED1);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 			PIOS_DELAY_WaitmS(1000);
 	}
 

flight/Bootloaders/BootloaderUpdater/pios_board.c

@@ -23,9 +23,20 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+/* Pull in the board-specific static HW definitions.
+ * Including .c files is a bit ugly but this allows all of
+ * the HW definitions to be const and static to limit their
+ * scope.  
+ *
+ * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
+ */
+#include "board_hw_defs.c"
+
 #include <pios.h>
 
 void PIOS_Board_Init(void) {
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+
 	/* Enable Prefetch Buffer */
 	FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
 
@@ -33,9 +44,15 @@ void PIOS_Board_Init(void) {
 	FLASH_SetLatency(FLASH_Latency_2);
 
 	/* Delay system */
-	PIOS_DELAY_Init();	
+	PIOS_DELAY_Init();
-	
+
+	/* LEDs */
+#if defined(PIOS_INCLUDE_LED)
+	const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
+	PIOS_Assert(led_cfg);
+	PIOS_LED_Init(led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
 	/* Initialize the PiOS library */
 	PIOS_GPIO_Init();
-
 }

flight/Bootloaders/CopterControl/Makefile

@@ -86,6 +86,7 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS
 RTOSSRCDIR = $(RTOSDIR)/Source
 RTOSINCDIR = $(RTOSSRCDIR)/include
 DOXYGENDIR = ../Doc/Doxygen
+HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
@@ -103,20 +104,22 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
 SRC += $(PIOSSTM32F10X)/pios_irq.c
 SRC += $(PIOSSTM32F10X)/pios_debug.c
 SRC += $(PIOSSTM32F10X)/pios_gpio.c
-
+SRC += $(PIOSSTM32F10X)/pios_iap.c
+SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
 
 # PIOS USB related files (seperated to make code maintenance more easy)
+SRC += $(PIOSSTM32F10X)/pios_usb.c
+SRC += $(PIOSSTM32F10X)/pios_usbhook.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_desc.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
+SRC += $(OPSYSTEM)/pios_usb_board_data.c
+SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
+SRC += $(PIOSCOMMON)/pios_usb_util.c
 
 ## PIOS Hardware (Common)
 SRC += $(PIOSCOMMON)/pios_board_info.c
-SRC += $(PIOSCOMMON)/pios_com.c
+SRC += $(PIOSCOMMON)/pios_com_msg.c
-SRC += $(PIOSCOMMON)/pios_bl_helper.c
-SRC += $(PIOSCOMMON)/pios_iap.c
 SRC += $(PIOSCOMMON)/printf-stdarg.c
 
 ## Libraries for flight calculations
@@ -197,8 +200,7 @@ EXTRAINCDIRS  += $(MSDDIR)
 EXTRAINCDIRS  += $(RTOSINCDIR)
 EXTRAINCDIRS  += $(APPLIBDIR)
 EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
-
+EXTRAINCDIRS  += $(HWDEFSINC)
-
 
 # List any extra directories to look for library files here.
 # Also add directories where the linker should search for
@@ -413,7 +415,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
 $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino
 elf: $(OUTDIR)/$(TARGET).elf

flight/Bootloaders/CopterControl/inc/pios_config.h

@@ -32,15 +32,16 @@
 #define PIOS_CONFIG_H
 #define PIOS_INCLUDE_BL_HELPER
 #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
-#define USB_HID
 /* Enable/Disable PiOS Modules */
 #define PIOS_INCLUDE_DELAY
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_SYS
+#define PIOS_INCLUDE_USB
 #define PIOS_INCLUDE_USB_HID
-#define PIOS_INCLUDE_COM
+#define PIOS_INCLUDE_COM_MSG
 #define PIOS_INCLUDE_GPIO
+#define PIOS_INCLUDE_IAP
 #define PIOS_NO_GPS
 
 #endif /* PIOS_CONFIG_H */

flight/Bootloaders/CopterControl/inc/pios_usb.h

@@ -1,85 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup   PIOS_USB USB Functions
- * @brief PIOS USB interface code
- * @{
- *
- * @file       pios_usb.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * 	       Parts by Thorsten Klose (tk@midibox.org)
- * @brief      USB functions 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 PIOS_USB_H
-#define PIOS_USB_H
-
-/* Local defines */
-/* Following settings allow to customise the USB device descriptor */
-#ifndef PIOS_USB_VENDOR_ID
-#define PIOS_USB_VENDOR_ID		0x20A0
-#endif
-
-#ifndef PIOS_USB_VENDOR_STR
-#define PIOS_USB_VENDOR_STR		"openpilot.org"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_STR
-#define PIOS_USB_PRODUCT_STR		"CopterControl"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_ID
-#define PIOS_USB_PRODUCT_ID		0x415B
-#endif
-
-#ifndef PIOS_USB_VERSION_ID
-#define PIOS_USB_VERSION_ID		0x0401	/* CopterControl (04) Bootloader (01) */
-#endif
-
-/* Internal defines which are used by PIOS USB HID (don't touch) */
-#define PIOS_USB_EP_NUM			2
-
-/* Buffer table base address */
-#define PIOS_USB_BTABLE_ADDRESS		0x000
-
-/* EP0 rx/tx buffer base address */
-#define PIOS_USB_ENDP0_RXADDR		0x040
-#define PIOS_USB_ENDP0_TXADDR		0x080
-
-/* EP1 Rx/Tx buffer base address for HID driver */
-#define PIOS_USB_ENDP1_TXADDR		0x0C0
-#define PIOS_USB_ENDP1_RXADDR		0x100
-
-/* Global Variables */
-extern void (*pEpInt_IN[7])(void);
-extern void (*pEpInt_OUT[7])(void);
-
-/* Public Functions */
-extern int32_t PIOS_USB_Init(uint32_t mode);
-extern int32_t PIOS_USB_IsInitialized(void);
-extern int32_t PIOS_USB_CableConnected(void);
-
-#endif /* PIOS_USB_H */
-
-/**
- * @}
- * @}
- */

flight/Bootloaders/CopterControl/inc/pios_usb_board_data.h

@@ -0,0 +1,52 @@
+/**
+ ******************************************************************************
+ * @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_COPTERCONTROL
+#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL)
+#define PIOS_USB_BOARD_SN_SUFFIX "+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 */

flight/Bootloaders/CopterControl/main.c

@@ -32,6 +32,7 @@
 #include "usb_lib.h"
 #include "pios_iap.h"
 #include "fifo_buffer.h"
+#include "pios_com_msg.h"
 /* Prototype of PIOS_Board_Init() function */
 extern void PIOS_Board_Init(void);
 extern void FLASH_Download();
@@ -62,22 +63,20 @@ uint8_t JumpToApp = FALSE;
 uint8_t GO_dfu = FALSE;
 uint8_t USB_connected = FALSE;
 uint8_t User_DFU_request = FALSE;
-static uint8_t mReceive_Buffer[64];
+static uint8_t mReceive_Buffer[63];
 /* Private function prototypes -----------------------------------------------*/
 uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
 uint8_t processRX();
 void jump_to_app();
 
-#define BLUE LED1
 int main() {
-	PIOS_SYS_Init();
+	PIOS_SYS_Init();	
-	if (BSL_HOLD_STATE == 0)
+	PIOS_Board_Init();
-		USB_connected = TRUE;
-
 	PIOS_IAP_Init();
+	
+	USB_connected = PIOS_USB_CableConnected(0);
 
 	if (PIOS_IAP_CheckRequest() == TRUE) {
-		PIOS_Board_Init();
 		PIOS_DELAY_WaitmS(1000);
 		User_DFU_request = TRUE;
 		PIOS_IAP_ClearRequest();
@@ -111,7 +110,7 @@ int main() {
 		case DFUidle:
 			period1 = 5000;
 			sweep_steps1 = 100;
-			PIOS_LED_Off(BLUE);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		case uploading:
@@ -123,12 +122,12 @@ int main() {
 		case downloading:
 			period1 = 2500;
 			sweep_steps1 = 50;
-			PIOS_LED_Off(BLUE);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		case BLidle:
 			period1 = 0;
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		default://error
@@ -140,19 +139,19 @@ int main() {
 
 		if (period1 != 0) {
 			if (LedPWM(period1, sweep_steps1, stopwatch))
-				PIOS_LED_On(BLUE);
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			else
-				PIOS_LED_Off(BLUE);
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 
 		if (period2 != 0) {
 			if (LedPWM(period2, sweep_steps2, stopwatch))
-				PIOS_LED_On(BLUE);
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			else
-				PIOS_LED_Off(BLUE);
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else
-			PIOS_LED_Off(BLUE);
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 
 		if (stopwatch > 50 * 1000 * 1000)
 			stopwatch = 0;
@@ -198,10 +197,8 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
 }
 
 uint8_t processRX() {
-	while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
+	if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
-		if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
+		processComand(mReceive_Buffer);
-			processComand(mReceive_Buffer);
-		}
 	}
 	return TRUE;
 }

flight/Bootloaders/CopterControl/op_dfu.c

@@ -30,6 +30,7 @@
 #include "pios.h"
 #include "op_dfu.h"
 #include "pios_bl_helper.h"
+#include "pios_com_msg.h"
 #include <pios_board_info.h>
 //programmable devices
 Device devicesTable[10];
@@ -294,6 +295,10 @@ void processComand(uint8_t *xReceive_Buffer) {
 		sendData(Buffer + 1, 63);
 		break;
 	case JumpFW:
+		if (Data == 0x5AFE) {
+			/* Force board into safe mode */
+			PIOS_IAP_WriteBootCount(0xFFFF);
+		}
 		FLASH_Lock();
 		JumpToApp = 1;
 		break;
@@ -442,9 +447,7 @@ uint32_t CalcFirmCRC() {
 
 }
 void sendData(uint8_t * buf, uint16_t size) {
-	PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, buf, size);
+	PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
-	if (DeviceState == downloading)
-		PIOS_DELAY_WaitmS(20);//this is an hack, we should check wtf is wrong with hid
 }
 
 bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) {

flight/Bootloaders/CopterControl/pios_board.c

@@ -23,41 +23,17 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+/* Pull in the board-specific static HW definitions.
+ * Including .c files is a bit ugly but this allows all of
+ * the HW definitions to be const and static to limit their
+ * scope.  
+ *
+ * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
+ */
+#include "board_hw_defs.c"
+#include <pios_board_info.h>
 #include <pios.h>
 
-// ***********************************************************************************
-
-#if defined(PIOS_INCLUDE_COM)
-
-#include <pios_com_priv.h>
-
-#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
-#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
-
-static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN];
-static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN];
-
-#endif /* PIOS_INCLUDE_COM */
-
-// ***********************************************************************************
-
-#if defined(PIOS_INCLUDE_USB_HID)
-#include "pios_usb_hid_priv.h"
-
-static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
-  .irq = {
-    .init    = {
-      .NVIC_IRQChannel                   = USB_LP_CAN1_RX0_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-};
-#endif	/* PIOS_INCLUDE_USB_HID */
-
-extern const struct pios_com_driver pios_usb_com_driver;
-
 uint32_t pios_com_telem_usb_id;
 
 /**
@@ -83,21 +59,49 @@ void PIOS_Board_Init(void) {
 	/* Initialize the PiOS library */
 	PIOS_GPIO_Init();
 
-#if defined(PIOS_INCLUDE_USB_HID)
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	
+#if defined(PIOS_INCLUDE_LED)
+	const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
+	PIOS_Assert(led_cfg);
+	PIOS_LED_Init(led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
+#if 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;
+	switch(bdinfo->board_rev) {
+		case BOARD_REVISION_CC:
+			PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
+			break;
+		case BOARD_REVISION_CC3D:
+			PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
+			break;
+		default:
+			PIOS_Assert(0);
+	}
+#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_main_cfg)) {
+	if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
 		PIOS_Assert(0);
 	}
-#if defined(PIOS_INCLUDE_COM)
+	if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
-	if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
-			  pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer),
-			  pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) {
 		PIOS_Assert(0);
 	}
-#endif	/* PIOS_INCLUDE_COM */
+#endif	/* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
-#endif	/* PIOS_INCLUDE_USB_HID */
+
+#endif	/* PIOS_INCLUDE_USB */
 
 	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
 
 	board_init_complete = true;
 }
+
+void PIOS_ADC_DMA_Handler()
+{
+}

flight/Bootloaders/CopterControl/pios_usb_board_data.c

@@ -0,0 +1,101 @@
+/**
+ ******************************************************************************
+ * @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_* */
+#include "pios_usb_util.h"	 /* PIOS_USB_UTIL_AsciiToUtf8 */
+
+static const uint8_t usb_product_id[28] = {
+	sizeof(usb_product_id),
+	USB_DESC_TYPE_STRING,
+	'C', 0,
+	'o', 0,
+	'p', 0,
+	't', 0,
+	'e', 0,
+	'r', 0,
+	'C', 0,
+	'o', 0,
+	'n', 0,
+	't', 0,
+	'r', 0,
+	'o', 0,
+	'l', 0,
+};
+
+static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
+	sizeof(usb_serial_number),
+	USB_DESC_TYPE_STRING,
+};
+
+static const struct usb_string_langid usb_lang_id = {
+	.bLength = sizeof(usb_lang_id),
+	.bDescriptorType = USB_DESC_TYPE_STRING,
+	.bLangID = htousbs(USB_LANGID_ENGLISH_US),
+};
+
+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[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
+	PIOS_SYS_SerialNumberGet((char *)sn);
+
+	/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
+	uint8_t * utf8 = &(usb_serial_number[2]);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
+
+	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;
+}

flight/Bootloaders/OpenPilot/Makefile

@@ -1,486 +0,0 @@
- #####
- # Project: OpenPilot
- #    
- #  
- # Makefile for OpenPilot project build PiOS and the AP.
- #
- # 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 to compile for debugging
-DEBUG ?= NO
-
-# Set to YES to use the Servo output pins for debugging via scope or logic analyser
-ENABLE_DEBUG_PINS ?= NO
-
-# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs  
-ENABLE_AUX_UART ?= NO
-
-# Set to YES when using Code Sourcery toolchain
-CODE_SOURCERY ?= NO
-
-# Remove command is different for Code Sourcery on Windows
-ifeq ($(CODE_SOURCERY), YES)
-REMOVE_CMD = cs-rm
-else
-REMOVE_CMD = rm
-endif
-
-FLASH_TOOL = OPENOCD
-
-# Paths
-OPSYSTEM = .
-OPSYSTEMINC = $(OPSYSTEM)/inc
-OPUAVTALK = ./UAVTalk
-OPUAVTALKINC = $(OPUAVTALK)/inc
-OPUAVOBJ = ./UAVObjects
-OPUAVOBJINC = $(OPUAVOBJ)/inc
-OPTESTS  = ./Tests
-OPMODULEDIR = ./Modules
-FLIGHTLIB = ../../Libraries
-FLIGHTLIBINC = ../../Libraries/inc
-PIOS = ../../PiOS
-PIOSINC = $(PIOS)/inc
-PIOSSTM32F10X = $(PIOS)/STM32F10x
-PIOSCOMMON = $(PIOS)/Common
-PIOSBOARDS = $(PIOS)/Boards
-APPLIBDIR = $(PIOSSTM32F10X)/Libraries
-STMLIBDIR = $(APPLIBDIR)
-STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
-STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
-STMSPDSRCDIR = $(STMSPDDIR)/src
-STMSPDINCDIR = $(STMSPDDIR)/inc
-STMUSBSRCDIR = $(STMUSBDIR)/src
-STMUSBINCDIR = $(STMUSBDIR)/inc
-CMSISDIR  = $(STMLIBDIR)/CMSIS/Core/CM3
-DOSFSDIR  = $(APPLIBDIR)/dosfs
-MSDDIR  = $(APPLIBDIR)/msd
-RTOSDIR = $(APPLIBDIR)/FreeRTOS
-RTOSSRCDIR = $(RTOSDIR)/Source
-RTOSINCDIR = $(RTOSSRCDIR)/include
-DOXYGENDIR = ../Doc/Doxygen
-
-# List C source files here. (C dependencies are automatically generated.)
-# use file-extension c for "c-only"-files
-
-## OPENPILOT_BL CORE:
-SRC += $(OPSYSTEM)/main.c
-SRC += $(OPSYSTEM)/pios_board.c
-SRC += $(OPSYSTEM)/op_dfu.c
-SRC += $(FLIGHTLIB)/stopwatch.c
-SRC += $(OPSYSTEM)/ssp.c
-
-
-## PIOS Hardware (STM32F10x)
-SRC += $(PIOSSTM32F10X)/pios_sys.c
-SRC += $(PIOSSTM32F10X)/pios_led.c
-SRC += $(PIOSSTM32F10X)/pios_delay.c
-SRC += $(PIOSSTM32F10X)/pios_usart.c
-SRC += $(PIOSSTM32F10X)/pios_irq.c
-SRC += $(PIOSSTM32F10X)/pios_spi.c
-SRC += $(PIOSSTM32F10X)/pios_debug.c
-SRC += $(PIOSSTM32F10X)/pios_gpio.c
-
-# PIOS USB related files (seperated to make code maintenance more easy)
-SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_desc.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
-
-## PIOS Hardware (Common)
-SRC += $(PIOSCOMMON)/pios_board_info.c
-SRC += $(PIOSCOMMON)/pios_com.c
-SRC += $(PIOSCOMMON)/pios_opahrs_v0.c
-SRC += $(PIOSCOMMON)/pios_bl_helper.c
-SRC += $(PIOSCOMMON)/pios_iap.c
-SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
-SRC += $(PIOSCOMMON)/printf-stdarg.c
-
-## Libraries for flight calculations
-SRC += $(FLIGHTLIB)/fifo_buffer.c
-
-## CMSIS for STM32
-SRC += $(CMSISDIR)/core_cm3.c
-SRC += $(CMSISDIR)/system_stm32f10x.c
-
-## Used parts of the STM-Library
-SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
-SRC += $(STMSPDSRCDIR)/misc.c
-
-## STM32 USB Library
-SRC += $(STMUSBSRCDIR)/usb_core.c
-SRC += $(STMUSBSRCDIR)/usb_init.c
-SRC += $(STMUSBSRCDIR)/usb_int.c
-SRC += $(STMUSBSRCDIR)/usb_mem.c
-SRC += $(STMUSBSRCDIR)/usb_regs.c
-SRC += $(STMUSBSRCDIR)/usb_sil.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 Assembler source files here.
-# Make them always end in a capital .S. Files ending in a lowercase .s
-# will not be considered source files but generated files (assembler
-# output from the compiler), and will be deleted upon "make clean"!
-# Even though the DOS/Win* filesystem matches both .s and .S the same,
-# it will preserve the spelling of the filenames, and gcc itself does
-# care about how the name is spelled on its command-line.
-ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
-
-# List Assembler source files here which must be assembled in ARM-Mode..
-ASRCARM = 
-
-# List any extra directories to look for include files here.
-#    Each directory must be seperated by a space.
-EXTRAINCDIRS  =  $(OPSYSTEM)
-EXTRAINCDIRS  += $(OPSYSTEMINC)
-EXTRAINCDIRS  += $(OPUAVTALK)
-EXTRAINCDIRS  += $(OPUAVTALKINC)
-EXTRAINCDIRS  += $(OPUAVOBJ)
-EXTRAINCDIRS  += $(OPUAVOBJINC)
-EXTRAINCDIRS  += $(PIOS)
-EXTRAINCDIRS  += $(PIOSINC)
-EXTRAINCDIRS  += $(FLIGHTLIBINC)
-EXTRAINCDIRS  += $(PIOSSTM32F10X)
-EXTRAINCDIRS  += $(PIOSCOMMON)
-EXTRAINCDIRS  += $(PIOSBOARDS)
-EXTRAINCDIRS  += $(STMSPDINCDIR)
-EXTRAINCDIRS  += $(STMUSBINCDIR)
-EXTRAINCDIRS  += $(CMSISDIR)
-EXTRAINCDIRS  += $(DOSFSDIR)
-EXTRAINCDIRS  += $(MSDDIR)
-EXTRAINCDIRS  += $(RTOSINCDIR)
-EXTRAINCDIRS  += $(APPLIBDIR)
-EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
-
-
-
-# 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 = $(PIOSSTM32F10X)
-
-# 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)
-ifeq ($(ENABLE_DEBUG_PINS), YES)
-CDEFS += -DPIOS_ENABLE_DEBUG_PINS
-endif
-ifeq ($(ENABLE_AUX_UART), YES)
-CDEFS += -DPIOS_ENABLE_AUX_UART
-endif
-
-# 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 = -DDEBUG
-endif
-
-CFLAGS += -g$(DEBUGF)
-CFLAGS += -O$(OPT)
-ifeq ($(DEBUG),NO)
-CFLAGS += -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 
-
-# Set linker-script name depending on selected submodel name
-LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
-LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_BL_sections.ld
-
-# 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
-
-${OUTDIR}/InitMods.c: Makefile
-	@echo $(MSG_MODINIT) $(call toprel, $@)
-	@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c 
-	@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote}  >> ${OUTDIR}/InitMods.c
-	@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
-	@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote}  >> ${OUTDIR}/InitMods.c
-	@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
-
-# 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
-
-# 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

flight/Bootloaders/OpenPilot/inc/pios_usb.h

@@ -1,85 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup   PIOS_USB USB Functions
- * @brief PIOS USB interface code
- * @{
- *
- * @file       pios_usb.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * 	       Parts by Thorsten Klose (tk@midibox.org)
- * @brief      USB functions 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 PIOS_USB_H
-#define PIOS_USB_H
-
-/* Local defines */
-/* Following settings allow to customise the USB device descriptor */
-#ifndef PIOS_USB_VENDOR_ID
-#define PIOS_USB_VENDOR_ID		0x20A0
-#endif
-
-#ifndef PIOS_USB_VENDOR_STR
-#define PIOS_USB_VENDOR_STR		"openpilot.org"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_STR
-#define PIOS_USB_PRODUCT_STR		"OpenPilot"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_ID
-#define PIOS_USB_PRODUCT_ID		0x415A
-#endif
-
-#ifndef PIOS_USB_VERSION_ID
-#define PIOS_USB_VERSION_ID		0x0101	/* OpenPilot (01) Bootloader (01) */
-#endif
-
-/* Internal defines which are used by PIOS USB HID (don't touch) */
-#define PIOS_USB_EP_NUM			2
-
-/* Buffer table base address */
-#define PIOS_USB_BTABLE_ADDRESS		0x000
-
-/* EP0 rx/tx buffer base address */
-#define PIOS_USB_ENDP0_RXADDR		0x040
-#define PIOS_USB_ENDP0_TXADDR		0x080
-
-/* EP1 Rx/Tx buffer base address for HID driver */
-#define PIOS_USB_ENDP1_TXADDR		0x0C0
-#define PIOS_USB_ENDP1_RXADDR		0x100
-
-/* Global Variables */
-extern void (*pEpInt_IN[7])(void);
-extern void (*pEpInt_OUT[7])(void);
-
-/* Public Functions */
-extern int32_t PIOS_USB_Init(uint32_t mode);
-extern int32_t PIOS_USB_IsInitialized(void);
-extern int32_t PIOS_USB_CableConnected(void);
-
-#endif /* PIOS_USB_H */
-
-/**
- * @}
- * @}
- */

flight/Bootloaders/OpenPilot/inc/ssp.h

@@ -1,117 +0,0 @@
-/*******************************************************************
- *
- *	NAME: ssp.h
- *
- *
- *******************************************************************/
-#ifndef SSP_H
-#define SSP_H
-/** INCLUDE FILES **/
-#include <stdint.h>
-
-/** LOCAL DEFINITIONS **/
-#ifndef TRUE
-#define TRUE	1
-#endif
-
-#ifndef FALSE
-#define FALSE	0
-#endif
-
-#define SSP_TX_IDLE       	0   // not expecting a ACK packet (no current transmissions in progress)
-#define SSP_TX_WAITING    	1   // waiting for a valid ACK to arrive
-#define SSP_TX_TIMEOUT    	2   // failed to receive a valid ACK in the timeout period, after retrying.
-#define SSP_TX_ACKED      	3   // valid ACK received before timeout period.
-#define SSP_TX_BUFOVERRUN 	4   // amount of data to send execeds the transmission buffer sizeof
-#define SSP_TX_BUSY       	5   // Attempted to start a transmission while a transmission was already in  progress.
-//#define SSP_TX_FAIL    - failure...
-
-#define	SSP_RX_IDLE			0
-#define SSP_RX_RECEIVING 	1
-#define SSP_RX_COMPLETE		2
-
-// types of packet that can be received
-#define SSP_RX_DATA       	5
-#define SSP_RX_ACK        	6
-#define SSP_RX_SYNCH      	7
-
-typedef enum decodeState_ {
-	decode_len1_e = 0,
-	decode_seqNo_e,
-	decode_data_e,
-	decode_crc1_e,
-	decode_crc2_e,
-	decode_idle_e
-} DecodeState_t;
-
-typedef enum ReceiveState {
-	state_escaped_e = 0, state_unescaped_e
-} ReceiveState_t;
-
-typedef struct {
-	uint8_t *pbuff;
-	uint16_t length;
-	uint16_t crc;
-	uint8_t seqNo;
-} Packet_t;
-
-typedef struct {
-
-	uint8_t *rxBuf; // Buffer used to store rcv data
-	uint16_t rxBufSize; // rcv buffer size.
-	uint8_t *txBuf; // Length of data in buffer
-	uint16_t txBufSize; // CRC for data in Packet buff
-	uint16_t max_retry; // Maximum number of retrys for a single transmit.
-	int32_t timeoutLen; //  how long to wait for each retry to succeed
-	void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received
-	int16_t (*pfSerialRead)(void); // function to call to read a byte from serial hardware
-	void (*pfSerialWrite)( uint8_t); // function used to write a byte to serial hardware for transmission
-	uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point
-} PortConfig_t;
-
-typedef struct Port_tag {
-	void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received
-	int16_t (*pfSerialRead)(void); // function to read a character from the serial input stream
-	void (*pfSerialWrite)( uint8_t); // function to write a byte to be sent out the serial port
-	uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point
-	uint8_t retryCount; // how many times have we tried to transmit the 'send' packet
-	uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet
-	int32_t timeoutLen; // how long to wait for each retry to succeed
-	int32_t timeout; // current timeout. when 'time' reaches this point we have timed out
-	uint8_t txSeqNo; // current 'send' packet sequence number
-	uint16_t rxBufPos; //  current buffer position in the receive packet
-	uint16_t rxBufLen; // number of 'data' bytes in the buffer
-	uint8_t rxSeqNo; // current 'receive' packet number
-	uint16_t rxBufSize; // size of the receive buffer.
-	uint16_t txBufSize; // size of the transmit buffer.
-	uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed.
-	uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received.
-	uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host
-	// this is required when switching from the application to the bootloader
-	// and vice-versa. This fixes the firwmare download timeout.
-	// when this flag is set to true, the next time we send a packet we will first
-	// send a synchronize packet.
-	ReceiveState_t InputState;
-	DecodeState_t DecodeState;
-	uint16_t SendState;
-	uint16_t crc;
-	uint32_t RxError;
-	uint32_t TxError;
-	uint16_t flags;
-} Port_t;
-
-/** Public Data **/
-
-/** PUBLIC FUNCTIONS **/
-int16_t ssp_ReceiveProcess(Port_t *thisport);
-int16_t ssp_SendProcess(Port_t *thisport);
-uint16_t ssp_SendString(Port_t *thisport, char *str);
-int16_t ssp_SendData(Port_t *thisport, const uint8_t * data,
-		const uint16_t length);
-void ssp_Init(Port_t *thisport, const PortConfig_t* const info);
-int16_t ssp_ReceiveByte(Port_t *thisport);
-uint16_t ssp_Synchronise(Port_t *thisport);
-
-/** EXTERNAL FUNCTIONS **/
-
-#endif

flight/Bootloaders/OpenPilot/main.c

@@ -1,295 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotBL OpenPilot BootLoader
- * @brief These files contain the code to the OpenPilot MB Bootloader.
- *
- * @{
- * @file       main.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      This is the file with the main function of the OpenPilot BootLoader
- * @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
- */
-/* Bootloader Includes */
-#include <pios.h>
-#include <pios_board_info.h>
-#include "pios_opahrs.h"
-#include "stopwatch.h"
-#include "op_dfu.h"
-#include "usb_lib.h"
-#include "pios_iap.h"
-#include "ssp.h"
-#include "fifo_buffer.h"
-/* Prototype of PIOS_Board_Init() function */
-extern void PIOS_Board_Init(void);
-extern void FLASH_Download();
-#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
-
-/* Private typedef -----------------------------------------------------------*/
-typedef void (*pFunction)(void);
-/* Private define ------------------------------------------------------------*/
-/* Private macro -------------------------------------------------------------*/
-/* Private variables ---------------------------------------------------------*/
-pFunction Jump_To_Application;
-uint32_t JumpAddress;
-
-/// LEDs PWM
-uint32_t period1 = 50; // *100 uS -> 5 mS
-uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
-uint32_t period2 = 50; // *100 uS -> 5 mS
-uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
-
-
-////////////////////////////////////////
-uint8_t tempcount = 0;
-
-/// SSP SECTION
-/// SSP TIME SOURCE
-#define SSP_TIMER	TIM7
-uint32_t ssp_time = 0;
-#define MAX_PACKET_DATA_LEN	255
-#define MAX_PACKET_BUF_SIZE	(1+1+MAX_PACKET_DATA_LEN+2)
-#define UART_BUFFER_SIZE 1024
-uint8_t rx_buffer[UART_BUFFER_SIZE] __attribute__ ((aligned(4)));
-// align to 32-bit to try and provide speed improvement;
-// master buffers...
-uint8_t SSP_TxBuf[MAX_PACKET_BUF_SIZE];
-uint8_t SSP_RxBuf[MAX_PACKET_BUF_SIZE];
-void SSP_CallBack(uint8_t *buf, uint16_t len);
-int16_t SSP_SerialRead(void);
-void SSP_SerialWrite( uint8_t);
-uint32_t SSP_GetTime(void);
-PortConfig_t SSP_PortConfig = { .rxBuf = SSP_RxBuf,
-		.rxBufSize = MAX_PACKET_DATA_LEN, .txBuf = SSP_TxBuf,
-		.txBufSize = MAX_PACKET_DATA_LEN, .max_retry = 10, .timeoutLen = 1000,
-		.pfCallBack = SSP_CallBack, .pfSerialRead = SSP_SerialRead,
-		.pfSerialWrite = SSP_SerialWrite, .pfGetTime = SSP_GetTime, };
-Port_t ssp_port;
-t_fifo_buffer ssp_buffer;
-
-/* Extern variables ----------------------------------------------------------*/
-DFUStates DeviceState;
-DFUPort ProgPort;
-int16_t status = 0;
-uint8_t JumpToApp = FALSE;
-uint8_t GO_dfu = FALSE;
-uint8_t USB_connected = FALSE;
-uint8_t User_DFU_request = FALSE;
-static uint8_t mReceive_Buffer[64];
-/* Private function prototypes -----------------------------------------------*/
-uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
-uint8_t processRX();
-void jump_to_app();
-uint32_t sspTimeSource();
-
-#define BLUE LED1
-#define RED	LED2
-#define LED_PWM_TIMER	TIM6
-int main() {
-	/* NOTE: Do NOT modify the following start-up sequence */
-	/* Any new initialization functions should be added in OpenPilotInit() */
-
-	/* Brings up System using CMSIS functions, enables the LEDs. */
-	PIOS_SYS_Init();
-	if (BSL_HOLD_STATE == 0)
-		USB_connected = TRUE;
-
-	PIOS_IAP_Init();
-
-	if (PIOS_IAP_CheckRequest() == TRUE) {
-		PIOS_Board_Init();
-		PIOS_DELAY_WaitmS(1000);
-		User_DFU_request = TRUE;
-		PIOS_IAP_ClearRequest();
-	}
-
-	GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE);
-
-	if (GO_dfu == TRUE) {
-		if (USB_connected)
-			ProgPort = Usb;
-		else
-			ProgPort = Serial;
-		PIOS_Board_Init();
-		if (User_DFU_request == TRUE)
-			DeviceState = DFUidle;
-		else
-			DeviceState = BLidle;
-		STOPWATCH_Init(100, LED_PWM_TIMER);
-		if (ProgPort == Serial) {
-			fifoBuf_init(&ssp_buffer, rx_buffer, UART_BUFFER_SIZE);
-			STOPWATCH_Init(100, SSP_TIMER);//nao devia ser 1000?
-			STOPWATCH_Reset(SSP_TIMER);
-			ssp_Init(&ssp_port, &SSP_PortConfig);
-		}
-		PIOS_OPAHRS_ForceSlaveSelected(true);
-	} else
-		JumpToApp = TRUE;
-
-	STOPWATCH_Reset(LED_PWM_TIMER);
-	while (TRUE) {
-		if (ProgPort == Serial) {
-			ssp_ReceiveProcess(&ssp_port);
-			status = ssp_SendProcess(&ssp_port);
-			while ((status != SSP_TX_IDLE) && (status != SSP_TX_ACKED)) {
-				ssp_ReceiveProcess(&ssp_port);
-				status = ssp_SendProcess(&ssp_port);
-			}
-		}
-		if (JumpToApp == TRUE)
-			jump_to_app();
-		//pwm_period = 50; // *100 uS -> 5 mS
-		//pwm_sweep_steps =100; // * 5 mS -> 500 mS
-
-		switch (DeviceState) {
-		case Last_operation_Success:
-		case uploadingStarting:
-		case DFUidle:
-			period1 = 50;
-			sweep_steps1 = 100;
-			PIOS_LED_Off(RED);
-			period2 = 0;
-			break;
-		case uploading:
-			period1 = 50;
-			sweep_steps1 = 100;
-			period2 = 25;
-			sweep_steps2 = 50;
-			break;
-		case downloading:
-			period1 = 25;
-			sweep_steps1 = 50;
-			PIOS_LED_Off(RED);
-			period2 = 0;
-			break;
-		case BLidle:
-			period1 = 0;
-			PIOS_LED_On(BLUE);
-			period2 = 0;
-			break;
-		default://error
-			period1 = 50;
-			sweep_steps1 = 100;
-			period2 = 50;
-			sweep_steps2 = 100;
-		}
-
-		if (period1 != 0) {
-			if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER)))
-				PIOS_LED_On(BLUE);
-			else
-				PIOS_LED_Off(BLUE);
-		} else
-			PIOS_LED_On(BLUE);
-
-		if (period2 != 0) {
-			if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER)))
-				PIOS_LED_On(RED);
-			else
-				PIOS_LED_Off(RED);
-		} else
-			PIOS_LED_Off(RED);
-
-		if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100)
-			STOPWATCH_Reset(LED_PWM_TIMER);
-		if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState
-				== BLidle))
-			JumpToApp = TRUE;
-
-		processRX();
-		DataDownload(start);
-	}
-}
-
-void jump_to_app() {
-	const struct pios_board_info * bdinfo = &pios_board_info_blob;
-
-	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 {
-		DeviceState = failed_jump;
-		return;
-	}
-}
-uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
-	uint32_t pwm_duty = ((count / pwm_period) % pwm_sweep_steps)
-			/ (pwm_sweep_steps / pwm_period);
-	if ((count % (2 * pwm_period * pwm_sweep_steps)) > pwm_period
-			* pwm_sweep_steps)
-		pwm_duty = pwm_period - pwm_duty; // negative direction each 50*100 ticks
-	return ((count % pwm_period) > pwm_duty) ? 1 : 0;
-}
-
-uint8_t processRX() {
-	if (ProgPort == Usb) {
-		while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
-			if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
-				processComand(mReceive_Buffer);
-			}
-		}
-	} else if (ProgPort == Serial) {
-
-		if (fifoBuf_getUsed(&ssp_buffer) >= 63) {
-			for (int32_t x = 0; x < 63; ++x) {
-				mReceive_Buffer[x] = fifoBuf_getByte(&ssp_buffer);
-			}
-			processComand(mReceive_Buffer);
-		}
-	}
-	return TRUE;
-}
-
-uint32_t sspTimeSource() {
-	if (STOPWATCH_ValueGet(SSP_TIMER) > 5000) {
-		++ssp_time;
-		STOPWATCH_Reset(SSP_TIMER);
-	}
-	return ssp_time;
-}
-void SSP_CallBack(uint8_t *buf, uint16_t len) {
-	fifoBuf_putData(&ssp_buffer, buf, len);
-}
-int16_t SSP_SerialRead(void) {
-	if (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_RF) > 0) {
-		uint8_t byte;
-		if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF, &byte, 1, 0) == 1) {
-			return byte;
-		} else {
-			return -1;
-		}	    
-	} else
-		return -1;
-}
-void SSP_SerialWrite(uint8_t value) {
-	PIOS_COM_SendChar(PIOS_COM_TELEM_RF, value);
-}
-uint32_t SSP_GetTime(void) {
-	return sspTimeSource();
-}

flight/Bootloaders/OpenPilot/pios_board.c

@@ -1,286 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotBL OpenPilot BootLoader
- * @{
- *
- * @file       pios_board.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Defines board specific static initialisers 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.h>
-//#include <openpilot.h>
-//#include <uavobjectsinit.h>
-
-#if defined(PIOS_INCLUDE_SPI)
-
-#include <pios_spi_priv.h>
-
-/* AHRS Interface
- * 
- * NOTE: Leave this declared as const data so that it ends up in the 
- * .rodata section (ie. Flash) rather than in the .bss section (RAM).
- */
-void PIOS_SPI_ahrs_irq_handler(void);
-void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
-void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
-const struct pios_spi_cfg
-		pios_spi_ahrs_cfg = {
-				.regs = SPI2,
-				.init = {
-					.SPI_Mode = SPI_Mode_Master,
-					.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
-					.SPI_DataSize = SPI_DataSize_8b,
-					.SPI_NSS = SPI_NSS_Soft,
-					.SPI_FirstBit = SPI_FirstBit_MSB,
-					.SPI_CRCPolynomial = 7,
-					.SPI_CPOL = SPI_CPOL_High,
-					.SPI_CPHA = SPI_CPHA_2Edge,
-					.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
-				},
-				.use_crc = TRUE,
-				.dma = {
-					.ahb_clk = RCC_AHBPeriph_DMA1,
-
-					.irq = {
-						.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
-						.init = {
-							.NVIC_IRQChannel = DMA1_Channel4_IRQn,
-							.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-							.NVIC_IRQChannelSubPriority = 0,
-							.NVIC_IRQChannelCmd = ENABLE,
-						},
-					},
-
-					.rx = {
-						.channel = DMA1_Channel4,
-						.init = {
-							.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
-							.DMA_DIR = DMA_DIR_PeripheralSRC,
-							.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
-							.DMA_MemoryInc = DMA_MemoryInc_Enable,
-							.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-							.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
-							.DMA_Mode = DMA_Mode_Normal,
-							.DMA_Priority = DMA_Priority_Medium,
-							.DMA_M2M = DMA_M2M_Disable,
-						},
-					},
-					.tx = {
-						.channel = DMA1_Channel5,
-						.init = {
-							.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
-							.DMA_DIR = DMA_DIR_PeripheralDST,
-							.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
-							.DMA_MemoryInc = DMA_MemoryInc_Enable,
-							.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-							.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
-							.DMA_Mode = DMA_Mode_Normal,
-							.DMA_Priority = DMA_Priority_Medium,
-							.DMA_M2M = DMA_M2M_Disable,
-						},
-					},
-				}, .ssel = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_12,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_Out_PP,
-					},
-				}, .sclk = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_13,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_AF_PP,
-					},
-				}, .miso = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_14,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_IN_FLOATING,
-					},
-				}, .mosi = {
-					.gpio = GPIOB,
-					.init = {
-						.GPIO_Pin = GPIO_Pin_15,
-						.GPIO_Speed = GPIO_Speed_50MHz,
-						.GPIO_Mode = GPIO_Mode_AF_PP,
-					},
-				}, };
-
-uint32_t pios_spi_ahrs_id;
-void PIOS_SPI_ahrs_irq_handler(void) {
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_SPI_IRQ_Handler(pios_spi_ahrs_id);
-}
-
-#endif	/* PIOS_INCLUDE_SPI */
-
-#if defined(PIOS_INCLUDE_USART)
-
-#include "pios_usart_priv.h"
-
-/*
- * Telemetry USART
- */
-const struct pios_usart_cfg pios_usart_telem_cfg = {
-	.regs = USART2,
-	.init = {
-#if defined (PIOS_COM_TELEM_BAUDRATE)
-		.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
-#else
-		.USART_BaudRate = 57600,
-#endif
-		.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 = USART2_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-			.NVIC_IRQChannelSubPriority = 0,
-			.NVIC_IRQChannelCmd = ENABLE,
-		},
-	}, .rx = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin = GPIO_Pin_3,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode = GPIO_Mode_IPU,
-		},
-	}, .tx = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin = GPIO_Pin_2,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode = GPIO_Mode_AF_PP,
-		},
-	}, };
-
-#endif	/* PIOS_INCLUDE_USART */
-
-#if defined(PIOS_INCLUDE_COM)
-
-#include "pios_com_priv.h"
-
-#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
-#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
-
-static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN];
-static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN];
-
-#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
-#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
-
-static uint8_t pios_com_telem_rf_rx_buffer[PIOS_COM_TELEM_RF_RX_BUF_LEN];
-static uint8_t pios_com_telem_rf_tx_buffer[PIOS_COM_TELEM_RF_TX_BUF_LEN];
-
-#endif	/* PIOS_INCLUDE_COM */
-
-#if defined(PIOS_INCLUDE_USB_HID)
-#include "pios_usb_hid_priv.h"
-
-static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
-  .irq = {
-    .init    = {
-      .NVIC_IRQChannel                   = USB_LP_CAN1_RX0_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-};
-#endif	/* PIOS_INCLUDE_USB_HID */
-
-extern const struct pios_com_driver pios_usb_com_driver;
-
-uint32_t pios_com_telem_rf_id;
-uint32_t pios_com_telem_usb_id;
-
-#include "pios_opahrs.h"
-
-/**
- * PIOS_Board_Init()
- * initializes all the core subsystems on this specific hardware
- * called from System/openpilot.c
- */
-static bool board_init_complete = false;
-void PIOS_Board_Init(void) {
-	if (board_init_complete) {
-		return;
-	}
-
-	/* Enable Prefetch Buffer */
-	FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
-
-	/* Flash 2 wait state */
-	FLASH_SetLatency(FLASH_Latency_2);
-
-	/* Delay system */
-	PIOS_DELAY_Init();
-
-	/* Initialize the PiOS library */
-#if defined(PIOS_INCLUDE_COM)
-	uint32_t pios_usart_telem_rf_id;
-	if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-	if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver,
-			  pios_usart_telem_rf_id,
-			  pios_com_telem_rf_rx_buffer, sizeof(pios_com_telem_rf_rx_buffer),
-			  pios_com_telem_rf_tx_buffer, sizeof(pios_com_telem_rf_tx_buffer))) {
-		PIOS_DEBUG_Assert(0);
-	}
-#endif	/* PIOS_INCLUDE_COM */
-
-	PIOS_GPIO_Init();
-
-#if defined(PIOS_INCLUDE_USB_HID)
-	uint32_t pios_usb_hid_id;
-	PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
-#if defined(PIOS_INCLUDE_COM)
-	if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
-			  pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer),
-			  pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) {
-		PIOS_Assert(0);
-	}
-#endif	/* PIOS_INCLUDE_COM */
-#endif	/* PIOS_INCLUDE_USB_HID */
-
-	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
-
-	/* Set up the SPI interface to the AHRS */
-	if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-
-	/* Bind the AHRS comms layer to the AHRS SPI link */
-	PIOS_OPAHRS_Attach(pios_spi_ahrs_id);
-
-	board_init_complete = true;
-}
-
-/**
- * @}
- */

flight/Bootloaders/OpenPilot/ssp.c

@@ -1,821 +0,0 @@
-/***********************************************************************************************************
- *
- *  NAME:          ssp.c
- *  DESCRIPTION:   simple serial protocol - packet based serial transport layer.
- *  AUTHOR:        Joe Hlebasko
- *  HISTORY:       Created 1/1/2010
- *
- * Packet Formats
- *   Format:
- *    +------+----+------+---------------------------+--------+
- *    | 225  | L1 | S#   | App Data (0-254 bytes)    | CRC 16 |
- *    +------+----+------+---------------------------+--------+
- *
- * 225 = sync byte, indicates start of a packet
- * L1 = 1 byte for size of data payload. (sequence number is part of data payload.)
- * S# = 1 byte for sequence number.
- * 			Seq of 0 = seq # synchronise request, forces other end to reset receive sequence number to 1.
- * 				sender of synchronise request will reset the tx seq number to 1
- * 			Seq # of 1..127 = normal data packets. Sequence number is incremented by for each transmitted
- * 				packet.  Rolls over from 127 to 1.
- * 			if most sig. bit is set then the packet is an ACK packet of data packet sequence number of the
- * 				lower 7 bits (1..127)
- * App Data may contain 0..254 bytes. The sequence number is consider part of the payload.
- * CRC 16 - 16 bits of CRC values of Sequence # and data bytes.
- *
- * Protocol has two types of packets: data and ack packets. ACK packets have the most sig. bit set in the
- * sequence number, this implies that valid sequence numbers are 1..127
- *
- * This protocol uses the concept of sequences numbers to determine if a given packet has been received. This
- * requires both devices to be able to synchronize sequence numbers. This is accomplished by sending a packet
- * length 1 and sequence number = 0. The receive then resets it's transmit sequence number to 1.
- *
- * ACTIVE_SYNCH is a version that will automatically send a synch request if it receives a synch packet. Only
- * one device in the communication should do otherwise you end up with an endless loops of synchronization.
- * Right now each side needs to manually issues a synch request.
- *
- * This protocol is best used in cases where one device is the master and the other is the slave, or a don't
- * speak unless spoken to type of approach.
- *
- * The following are items are required to initialize a port for communications:
- * 1. The number attempts for each packet
- * 2. time to wait for an ack.
- * 3. pointer to buffer to be used for receiving.
- * 4. pointer to a buffer to be used for transmission
- * 5. length of each buffer (rx and tx)
- * 6. Four functions:
- *  	1. write byte = writes a byte out the serial port (or other comm device)
- * 	2. read byte = retrieves a byte from the serial port. Returns -1 if a byte is not available
- * 	3. callback = function to call when a valid data packet has been received. This function is responsible
- * 		to do what needs to be done with the data when it is received. The primary mission of this function
- * 		should be to copy the data to a private buffer out of the working receive buffer to prevent overrun.
- * 		processing should be kept to a minimum.
- * 	4. get time = function should return the current time. Note that time units are not specified it just
- * 		needs to be some measure of time that increments as time passes by.  The timeout values for a given
- * 		port should the units used/returned by the get time function.
- *
- * All of the state information of a communication port is contained in a Port_t structure. This allows this
- * module to operature on multiple communication ports with a single code base.
- *
- * The ssp_ReceiveProcess and ssp_SendProcess functions need to be called to process data through the
- * respective state machines. Typical implementation would have a serial ISR to pull bytes out of the UART
- * and place into a circular buffer.  The serial read function would then pull bytes out this buffer
- * processing. The TX side has the write function placing bytes into a circular buffer with the TX ISR
- * pulling bytes out of the buffer and putting into the UART.  It is possible to run the receive process from
- * the receive ISR but care must be taken on processing data when it is received to avoid holding up the ISR
- * and sending ACK packets from the receive ISR.
- *
- ***********************************************************************************************************/
-
-/** INCLUDE FILES **/
-
-#include <stdint.h>
-#include <string.h>
-#include <stdio.h>
-#include <pios.h>
-#include "ssp.h"
-/** PRIVATE DEFINITIONS **/
-#define SYNC                        225                     // Sync character used in Serial Protocol
-#define ESC                         224                     // ESC character used in Serial Protocol
-#define ESC_SYNC                    1                       // ESC_SYNC character used in Serial Protocol
-#define ACK_BIT                     0x80                    // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 =
-//    new packet
-// packet location definitions.
-#define LENGTH  0
-#define SEQNUM  1
-#define DATA	2
-
-// Make larger sized integers from smaller sized integers
-#define MAKEWORD16( ub, lb )  ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb) )
-#define MAKEWORD32( uw, lw ) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw)) )
-#define MAKEWORD32B( b3, b2, b1, b0 ) ((uint32_t)((uint32_t)(b3)<< 24) | ((uint32_t)(b2)<<16) | ((uint32_t)(b1)<<8) | ((uint32_t)(b0) )
-
-// Used to extract smaller integers from larger sized intergers
-#define LOWERBYTE( w )        (uint8_t)((w) & 0x00ff )
-#define UPPERBYTE( w )        (uint8_t)(((w) & 0xff00) >> 8 )
-#define UPPERWORD(lw)         (uint16_t)(((lw) & 0xffff0000) >> 16 )
-#define LOWERWORD(lw)         (uint16_t)((lw) & 0x0000ffff)
-
-// Macros to operate on a target and bitmask.
-#define CLEARBIT( a, b )  ((a) = (a) & ~(b))
-#define SETBIT( a, b )    ((a) = (a) | (b) )
-#define	TOGGLEBIT(a,b)	  ((a) = (a) ^ (b) )
-
-// test bit macros operate using a bit mask.
-#define	ISBITSET( a, b )  ( ((a) & (b)) == (b) ? TRUE : FALSE )
-#define	ISBITCLEAR( a, b) (	(~(a) & (b)) == (b) ? TRUE : FALSE )
-
-/** PRIVATE FUNCTIONS **/
-//static void   	sf_SendSynchPacket( Port_t *thisport );
-static uint16_t sf_crc16(uint16_t crc, uint8_t data);
-static void sf_write_byte(Port_t *thisport, uint8_t c);
-static void sf_SetSendTimeout(Port_t *thisport);
-static uint16_t sf_CheckTimeout(Port_t *thisport);
-static int16_t sf_DecodeState(Port_t *thisport, uint8_t c);
-static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c);
-
-static void sf_SendPacket(Port_t *thisport);
-static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber);
-static void sf_MakePacket(uint8_t *buf, const uint8_t * pdata, uint16_t length,
-		uint8_t seqNo);
-static int16_t sf_ReceivePacket(Port_t *thisport);
-
-/* Flag bit masks...*/
-#define SENT_SYNCH		(0x01)
-#define	ACK_RECEIVED	(0x02)
-#define ACK_EXPECTED	(0x04)
-
-#define SSP_AWAITING_ACK	0
-#define SSP_ACKED			1
-#define SSP_IDLE			2
-
-/** PRIVATE DATA **/
-static const uint16_t CRC_TABLE[] = { 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301,
-		0x03C0, 0x0280, 0xC241, 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1,
-		0xC481, 0x0440, 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81,
-		0x0E40, 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
-		0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, 0x1E00,
-		0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, 0x1400, 0xD4C1,
-		0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, 0xD201, 0x12C0, 0x1380,
-		0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, 0xF001, 0x30C0, 0x3180, 0xF141,
-		0x3300, 0xF3C1, 0xF281, 0x3240, 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501,
-		0x35C0, 0x3480, 0xF441, 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0,
-		0x3E80, 0xFE41, 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881,
-		0x3840, 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
-		0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, 0xE401,
-		0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, 0x2200, 0xE2C1,
-		0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, 0xA001, 0x60C0, 0x6180,
-		0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, 0x6600, 0xA6C1, 0xA781, 0x6740,
-		0xA501, 0x65C0, 0x6480, 0xA441, 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01,
-		0x6FC0, 0x6E80, 0xAE41, 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1,
-		0xA881, 0x6840, 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80,
-		0xBA41, 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
-		0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, 0x7200,
-		0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, 0x5000, 0x90C1,
-		0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, 0x9601, 0x56C0, 0x5780,
-		0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, 0x9C01, 0x5CC0, 0x5D80, 0x9D41,
-		0x5F00, 0x9FC1, 0x9E81, 0x5E40, 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901,
-		0x59C0, 0x5880, 0x9841, 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1,
-		0x8A81, 0x4A40, 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80,
-		0x8C41, 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
-		0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 };
-
-/** EXTERNAL DATA **/
-
-/** EXTERNAL FUNCTIONS **/
-
-/** VERIFICATION FUNCTIONS **/
-
-/***********************************************************************************************************/
-
-/*!
- * \brief   Initializes the communication port for use
- * \param   thisport = pointer to port structure to initialize
- * \param	info = config struct with default values.
- * \return  None.
- *
- * \note
- * Must be called before calling the Send or REceive process functions.
- */
-
-void ssp_Init(Port_t *thisport, const PortConfig_t* const info) {
-	thisport->pfCallBack = info->pfCallBack;
-	thisport->pfSerialRead = info->pfSerialRead;
-	thisport->pfSerialWrite = info->pfSerialWrite;
-	thisport->pfGetTime = info->pfGetTime;
-
-	thisport->maxRetryCount = info->max_retry;
-	thisport->timeoutLen = info->timeoutLen;
-	thisport->txBufSize = info->txBufSize;
-	thisport->rxBufSize = info->rxBufSize;
-	thisport->txBuf = info->txBuf;
-	thisport->rxBuf = info->rxBuf;
-	thisport->retryCount = 0;
-	thisport->sendSynch = FALSE; //TRUE;
-	thisport->rxSeqNo = 255;
-	thisport->txSeqNo = 255;
-	thisport->SendState = SSP_IDLE;
-}
-
-/*!
- * \brief   Runs the send process, checks for receipt of ack, timeouts and resends if needed.
- * \param   thisport = which port to use
- * \return  SSP_TX_WAITING - waiting for a valid ACK to arrive
- * \return  SSP_TX_TIMEOUT - failed to receive a valid ACK in the timeout period, after retrying.
- * \return  SSP_TX_IDLE    - not expecting a ACK packet (no current transmissions in progress)
- * \return  SSP_TX_ACKED   - valid ACK received before timeout period.
- *
- * \note
- *
- */
-int16_t ssp_SendProcess(Port_t *thisport) {
-	int16_t value = SSP_TX_WAITING;
-
-	if (thisport->SendState == SSP_AWAITING_ACK) {
-		if (sf_CheckTimeout(thisport) == TRUE) {
-			if (thisport->retryCount < thisport->maxRetryCount) {
-				// Try again
-				sf_SendPacket(thisport);
-				sf_SetSendTimeout(thisport);
-				value = SSP_TX_WAITING;
-			} else {
-				// Give up, # of trys has exceded the limit
-#ifdef DEBUG_SSP
-				char str[63]= {0};
-				sprintf(str,"Send Timeout|");
-				PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
-#endif
-				value = SSP_TX_TIMEOUT;
-				CLEARBIT( thisport->flags, ACK_RECEIVED);
-				thisport->SendState = SSP_IDLE;
-			}
-		} else {
-			value = SSP_TX_WAITING;
-		}
-	} else if (thisport->SendState == SSP_ACKED) {
-		SETBIT( thisport->flags, ACK_RECEIVED);
-		value = SSP_TX_ACKED;
-		thisport->SendState = SSP_IDLE;
-	} else {
-		thisport->SendState = SSP_IDLE;
-		value = SSP_TX_IDLE;
-	}
-	return value;
-}
-
-/*!
- * \brief   Runs the receive process. fetches a byte at a time and runs the byte through the protocol receive state machine.
- * \param   thisport - which port to use.
- * \return  receive status.
- *
- * \note
- *
- */
-int16_t ssp_ReceiveProcess(Port_t *thisport) {
-	int16_t b;
-	int16_t packet_status = SSP_RX_IDLE;
-
-	do {
-		b = thisport->pfSerialRead(); // attempt to read a char from the serial buffer
-		if (b != -1) {
-			packet_status = sf_ReceiveState(thisport, b); // process the newly received byte in the receive state machine
-		}
-		// keep going until either we received a full packet or there are no more bytes to process
-	} while (packet_status != SSP_RX_COMPLETE && b != -1);
-	return packet_status;
-}
-
-/*!
- * \brief   processes a single byte through the receive state machine.
- * \param   thisport = which port to use
- * \return  current receive status
- *
- * \note
- *
- */
-
-int16_t ssp_ReceiveByte(Port_t *thisport) {
-	int16_t b;
-	int16_t packet_status = SSP_RX_IDLE;
-
-	b = thisport->pfSerialRead();
-	if (b != -1) {
-		packet_status = sf_ReceiveState(thisport, b);
-	}
-	return packet_status;
-}
-
-/*!
- * \brief   Sends a data packet and blocks until timeout or ack is received.
- * \param   thisport = which port to use
- * \param	data = pointer to data to send
- * \param	length = number of data bytes to send. Must be less than 254
- * \return  true = ack was received within number of retries
- * \return	false = ack was not received.
- *
- * \note
- *
- */
-uint16_t ssp_SendDataBlock(Port_t *thisport, uint8_t *data, uint16_t length) {
-	int16_t packet_status = SSP_TX_WAITING;
-	uint16_t retval = FALSE;
-
-	packet_status = ssp_SendData(thisport, data, length); // send the data
-	while (packet_status == SSP_TX_WAITING) { // check the status
-		(void) ssp_ReceiveProcess(thisport); // process any bytes received.
-		packet_status = ssp_SendProcess(thisport); // check the send status
-	}
-	if (packet_status == SSP_TX_ACKED) { // figure out what happened to the packet
-		retval = TRUE;
-	} else {
-		retval = FALSE;
-	}
-	return retval;
-}
-
-/*!
- * \brief   sends a chunk of data and does not block
- * \param   thisport = which port to use
- * \param	data = pointer to data to send
- * \param	length = number of bytes to send
- * \return	SSP_TX_BUFOVERRUN = tried to send too much data
- * \return	SSP_TX_WAITING = data sent and waiting for an ack to arrive
- * \return	SSP_TX_BUSY = a packet has already been sent, but not yet acked
- *
- * \note
- *
- */
-int16_t ssp_SendData(Port_t *thisport, const uint8_t *data,
-		const uint16_t length) {
-
-	int16_t value = SSP_TX_WAITING;
-
-	if ((length + 2) > thisport->txBufSize) {
-		// TRYING to send too much data.
-		value = SSP_TX_BUFOVERRUN;
-	} else if (thisport->SendState == SSP_IDLE) {
-#ifdef ACTIVE_SYNCH
-		if( thisport->sendSynch == TRUE ) {
-			sf_SendSynchPacket(thisport);
-		}
-#endif
-
-#ifdef SYNCH_SEND
-		if( length == 0 ) {
-			// TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function.
-			//      could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function
-			//      that must be called before calling this function.
-			// we are attempting to send a synch packet
-			thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us
-			SETBIT(thisport->flags, SENT_SYNCH);
-		} else {
-			// we are sending a data packet
-			CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet
-			thisport->txSeqNo++; // update the sequence number.
-			if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover
-				thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero,
-				// zero is reserviced for synchronization requests
-			}
-		}
-
-#else
-		CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet
-		thisport->txSeqNo++; // update the sequence number.
-		if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover
-			thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero,
-			// zero is reserved for synchronization requests
-		}
-#endif
-		CLEARBIT( thisport->flags, ACK_RECEIVED);
-		thisport->SendState = SSP_AWAITING_ACK;
-		value = SSP_TX_WAITING;
-		thisport->retryCount = 0; // zero out the retry counter for this transmission
-		sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo);
-		sf_SendPacket(thisport); // punch out the packet to the serial port
-		sf_SetSendTimeout(thisport); // do the timeout values
-#ifdef DEBUG_SSP
-		char str[63]= {0};
-		sprintf(str,"Sent DATA PACKET:%d|",thisport->txSeqNo);
-		PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
-#endif
-	} else {
-		// error we are already sending a packet. Need to wait for the current packet to be acked or timeout.
-#ifdef DEBUG_SSP
-		char str[63]= {0};
-		sprintf(str,"Error sending TX was busy|");
-		PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
-#endif
-		value = SSP_TX_BUSY;
-	}
-	return value;
-}
-
-/*!
- * \brief   Attempts to synchronize the sequence numbers with the other end of the connectin.
- * \param   thisport = which port to use
- * \return  true = success
- * \return	false = failed to receive an ACK to our synch request
- *
- * \note
- * A. send a packet with a sequence number equal to zero
- * B. if timed out then:
- * 		send synch packet again
- * 		increment try counter
- * 		if number of tries exceed maximum try limit then exit
- * C. goto A
- */
-uint16_t ssp_Synchronise(Port_t *thisport) {
-	int16_t packet_status;
-	uint16_t retval = FALSE;
-
-#ifndef USE_SENDPACKET_DATA
-	thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us
-	SETBIT(thisport->flags, SENT_SYNCH);
-	// TODO - should this be using ssp_SendPacketData()??
-	sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet
-	sf_SendPacket(thisport);
-	sf_SetSendTimeout(thisport);
-	thisport->SendState = SSP_AWAITING_ACK;
-	packet_status = SSP_TX_WAITING;
-#else
-	packet_status = ssp_SendData( thisport, NULL, 0 );
-#endif
-	while (packet_status == SSP_TX_WAITING) { // we loop until we time out.
-		(void) ssp_ReceiveProcess(thisport); // do the receive process
-		packet_status = ssp_SendProcess(thisport); // do the send process
-	}
-	thisport->sendSynch = FALSE;
-	switch (packet_status) {
-	case SSP_TX_ACKED:
-		retval = TRUE;
-		break;
-	case SSP_TX_BUSY: // intentional fall through.
-	case SSP_TX_TIMEOUT: // intentional fall through.
-	case SSP_TX_BUFOVERRUN:
-		retval = FALSE;
-		break;
-	default:
-		retval = FALSE;
-		break;
-	};
-	return retval;
-}
-
-/*!
- * \brief   sends out a preformatted packet for a give port
- * \param   thisport = which port to use.
- * \return  none.
- *
- * \note
- * Packet should be formed through the use of sf_MakePacket before calling this function.
- */
-static void sf_SendPacket(Port_t *thisport) {
-	// add 3 to packet data length for: 1 length + 2 CRC (packet overhead)
-	uint8_t packetLen = thisport->txBuf[LENGTH] + 3;
-
-	// use the raw serial write function so the SYNC byte does not get 'escaped'
-	thisport->pfSerialWrite(SYNC);
-	for (uint8_t x = 0; x < packetLen; x++) {
-		sf_write_byte(thisport, thisport->txBuf[x]);
-	}
-	thisport->retryCount++;
-}
-
-/*!
- * \brief   converts data to transport layer protocol packet format.
- * \param   txbuf = buffer to use when forming the packet
- * \param	pdata = pointer to data to use
- * \param	length = number of bytes to use
- * \param	seqNo = sequence number of this packet
- * \return  none.
- *
- * \note
- *  1. This function does not try to interpret ACK or SYNCH packets.  This should
- *      be done by the caller of this function.
- *  2. This function will attempt to format all data upto the size of the tx buffer.
- *      Any extra data beyond that will be ignored.
- *  3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size?
- *
- */
-void sf_MakePacket(uint8_t *txBuf, const uint8_t * pdata, uint16_t length,
-		uint8_t seqNo) {
-	uint16_t crc = 0xffff;
-	uint16_t bufPos = 0;
-	uint8_t b;
-
-	// add 1 for the seq. number
-	txBuf[LENGTH] = length + 1;
-	txBuf[SEQNUM] = seqNo;
-	crc = sf_crc16(crc, seqNo);
-
-	length = length + 2; // add two for the length and seqno bytes which are added before the loop.
-	for (bufPos = 2; bufPos < length; bufPos++) {
-		b = *pdata++;
-		txBuf[bufPos] = b;
-		crc = sf_crc16(crc, b); // update CRC value
-	}
-	txBuf[bufPos++] = LOWERBYTE(crc);
-	txBuf[bufPos] = UPPERBYTE(crc);
-
-}
-
-/*!
- * \brief   sends out an ack packet to given sequence number
- * \param   thisport = which port to use
- * \param	seqNumber = sequence number of the packet we would like to ack
- * \return  none.
- *
- * \note
- *
- */
-
-static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber) {
-#ifdef DEBUG_SSP
-	char str[63]= {0};
-	sprintf(str,"Sent ACK PACKET:%d|",seqNumber);
-	PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
-#endif
-	uint8_t AckSeqNumber = SETBIT( seqNumber, ACK_BIT );
-
-	// create the packet, note we pass AckSequenceNumber directly
-	sf_MakePacket(thisport->txBuf, NULL, 0, AckSeqNumber);
-	sf_SendPacket(thisport);
-	// we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol
-}
-
-/*!
- * \brief   writes a byte out the output channel. Adds escape byte where needed
- * \param   thisport = which port to use
- * \param	c = byte to send
- * \return  none.
- *
- * \note
- *
- */
-static void sf_write_byte(Port_t *thisport, uint8_t c) {
-	if (c == SYNC) { // check for SYNC byte
-		thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte
-		thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char
-	} else if (c == ESC) { // Check for ESC character
-		thisport->pfSerialWrite(ESC); // if it is, we need to send it twice
-		thisport->pfSerialWrite(ESC);
-	} else {
-		thisport->pfSerialWrite(c); // otherwise write the byte to serial port
-	}
-}
-
-/************************************************************************************************************
- *
- *  NAME:          uint16_t ssp_crc16( uint16_t crc, uint16_t data )
- *  DESCRIPTION:   Uses crc_table to calculate new crc
- *  ARGUMENTS:
- *        arg1:    crc
- *        arg2:    data - byte to calculate into CRC
- *  RETURN:        New crc
- *  CREATED:    5/8/02
- *
- *************************************************************************************************************/
-/*!
- * \brief   calculates the new CRC value for 'data'
- * \param   crc = current CRC value
- * \param	data = new byte
- * \return  updated CRC value
- *
- * \note
- *
- */
-
-static uint16_t sf_crc16(uint16_t crc, uint8_t data) {
-	return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF];
-}
-
-/*!
- * \brief   sets the timeout for the given packet
- * \param   thisport = which port to use
- * \return  none.
- *
- * \note
- *
- */
-
-static void sf_SetSendTimeout(Port_t *thisport) {
-	uint32_t timeout;
-	timeout = thisport->pfGetTime() + thisport->timeoutLen;
-	thisport->timeout = timeout;
-}
-
-/*!
- * \brief   checks to see if a timeout occured
- * \param   thisport = which port to use
- * \return  true = a timeout has occurred
- * \return	false = has not timed out
- *
- * \note
- *
- */
-static uint16_t sf_CheckTimeout(Port_t *thisport) {
-	uint16_t retval = FALSE;
-	uint32_t current_time;
-
-	current_time = thisport->pfGetTime();
-	if (current_time > thisport->timeout) {
-		retval = TRUE;
-	}
-	return retval;
-}
-
-/****************************************************************************
- *  NAME:   sf_ReceiveState
- *  DESC:   Implements the receive state handling code for escaped and unescaped data
- *  ARGS:	thisport - which port to operate on
- *  		c - incoming byte
- *  RETURN:
- *  CREATED:
- *  NOTES:
- *  1. change from using pointer to functions.
- ****************************************************************************/
-/*!
- * \brief   implements the receive state handling code for escaped and unescaped data
- * \param   thisport = which port to use
- * \param	c = byte to process through the receive state machine
- * \return  receive status
- *
- * \note
- *
- */
-static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c) {
-	int16_t retval = SSP_RX_RECEIVING;
-
-	switch (thisport->InputState) {
-	case state_unescaped_e:
-		if (c == SYNC) {
-			thisport->DecodeState = decode_len1_e;
-		} else if (c == ESC) {
-			thisport->InputState = state_escaped_e;
-		} else {
-			retval = sf_DecodeState(thisport, c);
-		}
-		break; // end of unescaped state.
-	case state_escaped_e:
-		thisport->InputState = state_unescaped_e;
-		if (c == SYNC) {
-			thisport->DecodeState = decode_len1_e;
-		} else if (c == ESC_SYNC) {
-			retval = sf_DecodeState(thisport, SYNC);
-		} else {
-			retval = sf_DecodeState(thisport, c);
-		}
-		break; // end of the escaped state.
-	default:
-		break;
-	}
-	return retval;
-}
-
-/****************************************************************************
- *  NAME:   sf_DecodeState
- *  DESC:   Implements the receive state finite state machine
- *  ARGS:	thisport - which port to operate on
- *  		c - incoming byte
- *  RETURN:
- *  CREATED:
- *  NOTES:
- *  1. change from using pointer to functions.
- ****************************************************************************/
-
-/*!
- * \brief   implements the receiving decoding state machine
- * \param   thisport = which port to use
- * \param	c = byte to process
- * \return  receive status
- *
- * \note
- *
- */
-static int16_t sf_DecodeState(Port_t *thisport, uint8_t c) {
-	int16_t retval;
-	switch (thisport->DecodeState) {
-	case decode_idle_e:
-		// 'c' is ignored in this state as the only way to leave the idle state is
-		// recognition of the SYNC byte in the sf_ReceiveState function.
-		retval = SSP_RX_IDLE;
-		break;
-	case decode_len1_e:
-		thisport->rxBuf[LENGTH] = c;
-		thisport->rxBufLen = c;
-		if (thisport->rxBufLen <= thisport->rxBufSize) {
-			thisport->DecodeState = decode_seqNo_e;
-			retval = SSP_RX_RECEIVING;
-		} else {
-			thisport->DecodeState = decode_idle_e;
-			retval = SSP_RX_IDLE;
-		}
-		break;
-	case decode_seqNo_e:
-		thisport->rxBuf[SEQNUM] = c;
-		thisport->crc = 0xffff;
-		thisport->rxBufLen--; // subtract 1 for the seq. no.
-		thisport->rxBufPos = 2;
-
-		thisport->crc = sf_crc16(thisport->crc, c);
-		if (thisport->rxBufLen > 0) {
-			thisport->DecodeState = decode_data_e;
-		} else {
-			thisport->DecodeState = decode_crc1_e;
-		}
-		retval = SSP_RX_RECEIVING;
-		break;
-	case decode_data_e:
-		thisport->rxBuf[(thisport->rxBufPos)++] = c;
-		thisport->crc = sf_crc16(thisport->crc, c);
-		if (thisport->rxBufPos == (thisport->rxBufLen + 2)) {
-			thisport->DecodeState = decode_crc1_e;
-		}
-		retval = SSP_RX_RECEIVING;
-		break;
-	case decode_crc1_e:
-		thisport->crc = sf_crc16(thisport->crc, c);
-		thisport->DecodeState = decode_crc2_e;
-		retval = SSP_RX_RECEIVING;
-		break;
-	case decode_crc2_e:
-		thisport->DecodeState = decode_idle_e;
-		// verify the CRC value for the packet
-		if (sf_crc16(thisport->crc, c) == 0) {
-			// TODO shouldn't the return value of sf_ReceivePacket() be checked?
-			sf_ReceivePacket(thisport);
-			retval = SSP_RX_COMPLETE;
-		} else {
-			thisport->RxError++;
-			retval = SSP_RX_IDLE;
-		}
-		break;
-	default:
-		thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet.
-		retval = SSP_RX_IDLE;
-		break;
-	}
-	return retval;
-}
-
-/************************************************************************************************************
- *
- *  NAME:             int16_t sf_ReceivePacket( )
- *  DESCRIPTION:      Receive one packet, assumed that data is in rec.buff[]
- *   ARGUMENTS:
- *   RETURN:          0 . no new packet was received, could be ack or same packet
- *                    1 . new packet received
- *                    SSP_PACKET_?
- *                    SSP_PACKET_COMPLETE
- *                    SSP_PACKET_ACK
- *   CREATED:         5/8/02
- *
- *************************************************************************************************************/
-/*!
- * \brief   receive one packet. calls the callback function if needed.
- * \param   thisport = which port to use
- * \return  true = valid data packet received.
- * \return	false = otherwise
- *
- * \note
- *
- *	Created: Oct 7, 2010 12:07:22 AM by joe
- */
-
-static int16_t sf_ReceivePacket(Port_t *thisport) {
-	int16_t value = FALSE;
-
-	if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT )) {
-		//  Received an ACK packet, need to check if it matches the previous sent packet
-		if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) {
-			//  It matches the last packet sent by us
-			SETBIT( thisport->txSeqNo, ACK_BIT );
-			thisport->SendState = SSP_ACKED;
-#ifdef DEBUG_SSP
-			char str[63]= {0};
-			sprintf(str,"Received ACK:%d|",(thisport->txSeqNo & 0x7F));
-			PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
-#endif
-			value = FALSE;
-		}
-		// else ignore the ACK packet
-	} else {
-		//  Received a 'data' packet, figure out what type of packet we received...
-		if (thisport->rxBuf[SEQNUM] == 0) {
-#ifdef DEBUG_SSP
-			PIOS_COM_SendString(PIOS_COM_TELEM_USB,"Received SYNC Request|");
-#endif
-			// Synchronize sequence number with host
-#ifdef ACTIVE_SYNCH
-			thisport->sendSynch = TRUE;
-#endif
-			sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]);
-			thisport->rxSeqNo = 0;
-			value = FALSE;
-		} else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) {
-			// Already seen this packet, just ack it, don't act on the packet.
-			sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]);
-			value = FALSE;
-		} else {
-			//New Packet
-			thisport->rxSeqNo = thisport->rxBuf[SEQNUM];
-			// Let the application do something with the data/packet.
-			if (thisport->pfCallBack != NULL) {
-#ifdef DEBUG_SSP
-				char str[63]= {0};
-				sprintf(str,"Received DATA PACKET:%d [0]=%d %d %d|",thisport->rxSeqNo,(uint8_t)thisport->rxBuf[2],(uint8_t)thisport->rxBuf[3],(uint8_t)thisport->rxBuf[4]);
-				PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
-#endif
-				// skip the first two bytes (length and seq. no.) in the buffer.
-				thisport->pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen);
-			}
-			// after we send the ACK, it is possible for the host to send a new packet.
-			// Thus the application needs to copy the data and reset the receive buffer
-			// inside of thisport->pfCallBack()
-			sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]);
-			value = TRUE;
-		}
-	}
-	return value;
-}
-

flight/Bootloaders/OpenPilot/ssp_timer.c

@@ -1,91 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotBL OpenPilot BootLoader
- * @{
- *
- * @file       ssp_timer.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Timer functions to be used with the SSP.
- * @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 files
-/////////////////////////////////////////////////////////////////////////////
-
-#include "stm32f10x_tim.h"
-
-/////////////////////////////////////////////////////////////////////////////
-// Local definitions
-/////////////////////////////////////////////////////////////////////////////
-
-#define SSP_TIMER_TIMER_BASE                 TIM7
-#define SSP_TIMER_TIMER_RCC   RCC_APB1Periph_TIM7
-
-uint32_t SSP_TIMER_Init(u32 resolution) {
-	// enable timer clock
-	if (SSP_TIMER_TIMER_RCC == RCC_APB2Periph_TIM1 || SSP_TIMER_TIMER_RCC
-			== RCC_APB2Periph_TIM8)
-		RCC_APB2PeriphClockCmd(SSP_TIMER_TIMER_RCC, ENABLE);
-	else
-		RCC_APB1PeriphClockCmd(SSP_TIMER_TIMER_RCC, ENABLE);
-
-	// time base configuration
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-	TIM_TimeBaseStructure.TIM_Period = 0xffff; // max period
-	TIM_TimeBaseStructure.TIM_Prescaler = (72 * resolution) - 1; // <resolution> uS accuracy @ 72 MHz
-	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-	TIM_TimeBaseInit(SSP_TIMER_TIMER_BASE, &TIM_TimeBaseStructure);
-
-	// enable interrupt request
-	TIM_ITConfig(SSP_TIMER_TIMER_BASE, TIM_IT_Update, ENABLE);
-
-	// start counter
-	TIM_Cmd(SSP_TIMER_TIMER_BASE, ENABLE);
-
-	return 0; // no error
-}
-
-/////////////////////////////////////////////////////////////////////////////
-//! Resets the SSP_TIMER
-//! \return < 0 on errors
-/////////////////////////////////////////////////////////////////////////////
-uint32_t SSP_TIMER_Reset(void) {
-	// reset counter
-	SSP_TIMER_TIMER_BASE->CNT = 1; // set to 1 instead of 0 to avoid new IRQ request
-	TIM_ClearITPendingBit(SSP_TIMER_TIMER_BASE, TIM_IT_Update);
-
-	return 0; // no error
-}
-
-/////////////////////////////////////////////////////////////////////////////
-//! Returns current value of SSP_TIMER
-//! \return 1..65535: valid SSP_TIMER value
-//! \return 0xffffffff: counter overrun
-/////////////////////////////////////////////////////////////////////////////
-uint32_t SSP_TIMER_ValueGet(void) {
-	uint32_t value = SSP_TIMER_TIMER_BASE->CNT;
-
-	if (TIM_GetITStatus(SSP_TIMER_TIMER_BASE, TIM_IT_Update) != RESET)
-		SSP_TIMER_Reset();
-
-	return value;
-}
-

flight/Bootloaders/PipXtreme/Makefile

@@ -86,6 +86,7 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS
 RTOSSRCDIR = $(RTOSDIR)/Source
 RTOSINCDIR = $(RTOSSRCDIR)/include
 DOXYGENDIR = ../Doc/Doxygen
+HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
@@ -103,19 +104,22 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
 SRC += $(PIOSSTM32F10X)/pios_irq.c
 SRC += $(PIOSSTM32F10X)/pios_debug.c
 SRC += $(PIOSSTM32F10X)/pios_gpio.c
+SRC += $(PIOSSTM32F10X)/pios_iap.c
+SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
 
 # PIOS USB related files (seperated to make code maintenance more easy)
+SRC += $(PIOSSTM32F10X)/pios_usb.c
+SRC += $(PIOSSTM32F10X)/pios_usbhook.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_desc.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
+SRC += $(OPSYSTEM)/pios_usb_board_data.c
+SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
+SRC += $(PIOSCOMMON)/pios_usb_util.c
 
 ## PIOS Hardware (Common)
 SRC += $(PIOSCOMMON)/pios_board_info.c
-SRC += $(PIOSCOMMON)/pios_com.c
+SRC += $(PIOSCOMMON)/pios_com_msg.c
-SRC += $(PIOSCOMMON)/pios_bl_helper.c
-SRC += $(PIOSCOMMON)/pios_iap.c
 SRC += $(PIOSCOMMON)/printf-stdarg.c
 
 ## Libraries for flight calculations
@@ -196,8 +200,7 @@ EXTRAINCDIRS  += $(MSDDIR)
 EXTRAINCDIRS  += $(RTOSINCDIR)
 EXTRAINCDIRS  += $(APPLIBDIR)
 EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
-
+EXTRAINCDIRS  += $(HWDEFSINC)
-
 
 # List any extra directories to look for library files here.
 # Also add directories where the linker should search for
@@ -285,7 +288,7 @@ CSTANDARD = -std=gnu99
 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
 
 ifeq ($(DEBUG),YES)
-CFLAGS = -DDEBUG
+CFLAGS += -DDEBUG
 endif
 
 CFLAGS += -g$(DEBUGF)
@@ -414,7 +417,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
 $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino
 elf: $(OUTDIR)/$(TARGET).elf

flight/Bootloaders/PipXtreme/inc/pios_config.h

@@ -31,16 +31,16 @@
 #define PIOS_CONFIG_H
 #define PIOS_INCLUDE_BL_HELPER
 #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
-#define USB_HID
 /* Enable/Disable PiOS Modules */
 #define PIOS_INCLUDE_DELAY
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_SYS
+#define PIOS_INCLUDE_USB
 #define PIOS_INCLUDE_USB_HID
-#define PIOS_INCLUDE_COM
+#define PIOS_INCLUDE_COM_MSG
 #define PIOS_INCLUDE_GPIO
-//#define DEBUG_SSP
+#define PIOS_INCLUDE_IAP
 
 /* Defaults for Logging */
 #define LOG_FILENAME 			"PIOS.LOG"

flight/Bootloaders/PipXtreme/inc/pios_usb.h

@@ -1,85 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup   PIOS_USB USB Functions
- * @brief PIOS USB interface code
- * @{
- *
- * @file       pios_usb.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * 	       Parts by Thorsten Klose (tk@midibox.org)
- * @brief      USB functions 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 PIOS_USB_H
-#define PIOS_USB_H
-
-/* Local defines */
-/* Following settings allow to customise the USB device descriptor */
-#ifndef PIOS_USB_VENDOR_ID
-#define PIOS_USB_VENDOR_ID		0x20A0
-#endif
-
-#ifndef PIOS_USB_VENDOR_STR
-#define PIOS_USB_VENDOR_STR		"openpilot.org"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_STR
-#define PIOS_USB_PRODUCT_STR		"PipXtreme"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_ID
-#define PIOS_USB_PRODUCT_ID		0x415C
-#endif
-
-#ifndef PIOS_USB_VERSION_ID
-#define PIOS_USB_VERSION_ID		0x0301	/* PipXtreme (03) Bootloader (01) */
-#endif
-
-/* Internal defines which are used by PIOS USB HID (don't touch) */
-#define PIOS_USB_EP_NUM			2
-
-/* Buffer table base address */
-#define PIOS_USB_BTABLE_ADDRESS		0x000
-
-/* EP0 rx/tx buffer base address */
-#define PIOS_USB_ENDP0_RXADDR		0x040
-#define PIOS_USB_ENDP0_TXADDR		0x080
-
-/* EP1 Rx/Tx buffer base address for HID driver */
-#define PIOS_USB_ENDP1_TXADDR		0x0C0
-#define PIOS_USB_ENDP1_RXADDR		0x100
-
-/* Global Variables */
-extern void (*pEpInt_IN[7])(void);
-extern void (*pEpInt_OUT[7])(void);
-
-/* Public Functions */
-extern int32_t PIOS_USB_Init(uint32_t mode);
-extern int32_t PIOS_USB_IsInitialized(void);
-extern int32_t PIOS_USB_CableConnected(void);
-
-#endif /* PIOS_USB_H */
-
-/**
- * @}
- * @}
- */

flight/Bootloaders/PipXtreme/inc/pios_usb_board_data.h

@@ -0,0 +1,52 @@
+/**
+ ******************************************************************************
+ * @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_PIPXTREME
+#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_PIPXTREME, USB_OP_BOARD_MODE_BL)
+#define PIOS_USB_BOARD_SN_SUFFIX "+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 */

flight/Bootloaders/PipXtreme/main.c

@@ -32,6 +32,7 @@
 #include "usb_lib.h"
 #include "pios_iap.h"
 #include "fifo_buffer.h"
+#include "pios_com_msg.h"
 /* Prototype of PIOS_Board_Init() function */
 extern void PIOS_Board_Init(void);
 extern void FLASH_Download();
@@ -62,27 +63,20 @@ uint8_t JumpToApp = FALSE;
 uint8_t GO_dfu = FALSE;
 uint8_t USB_connected = FALSE;
 uint8_t User_DFU_request = FALSE;
-static uint8_t mReceive_Buffer[64];
+static uint8_t mReceive_Buffer[63];
 /* Private function prototypes -----------------------------------------------*/
 uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
 uint8_t processRX();
 void jump_to_app();
 
-#define BLUE LED1
-#define RED	LED4
 int main() {
-	/* NOTE: Do NOT modify the following start-up sequence */
+	PIOS_SYS_Init();	
-	/* Any new initialization functions should be added in OpenPilotInit() */
+	PIOS_Board_Init();
-
-	/* Brings up System using CMSIS functions, enables the LEDs. */
-	PIOS_SYS_Init();
-	if (BSL_HOLD_STATE == 0)
-		USB_connected = TRUE;
-
 	PIOS_IAP_Init();
+	
+	USB_connected = PIOS_USB_CableConnected(0);
 
 	if (PIOS_IAP_CheckRequest() == TRUE) {
-		PIOS_Board_Init();
 		PIOS_DELAY_WaitmS(1000);
 		User_DFU_request = TRUE;
 		PIOS_IAP_ClearRequest();
@@ -116,7 +110,7 @@ int main() {
 		case DFUidle:
 			period1 = 5000;
 			sweep_steps1 = 100;
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 			period2 = 0;
 			break;
 		case uploading:
@@ -128,12 +122,12 @@ int main() {
 		case downloading:
 			period1 = 2500;
 			sweep_steps1 = 50;
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 			period2 = 0;
 			break;
 		case BLidle:
 			period1 = 0;
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			period2 = 0;
 			break;
 		default://error
@@ -145,19 +139,19 @@ int main() {
 
 		if (period1 != 0) {
 			if (LedPWM(period1, sweep_steps1, stopwatch))
-				PIOS_LED_On(BLUE);
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
 			else
-				PIOS_LED_Off(BLUE);
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 		} else
-			PIOS_LED_On(BLUE);
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
 
 		if (period2 != 0) {
 			if (LedPWM(period2, sweep_steps2, stopwatch))
-				PIOS_LED_On(RED);
+				PIOS_LED_On(PIOS_LED_ALARM);
 			else
-				PIOS_LED_Off(RED);
+				PIOS_LED_Off(PIOS_LED_ALARM);
 		} else
-			PIOS_LED_Off(RED);
+			PIOS_LED_Off(PIOS_LED_ALARM);
 
 		if (stopwatch > 50 * 1000 * 1000)
 			stopwatch = 0;
@@ -181,7 +175,6 @@ void jump_to_app() {
 		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 */
@@ -204,10 +197,8 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
 }
 
 uint8_t processRX() {
-	while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
+	if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
-		if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
+		processComand(mReceive_Buffer);
-			processComand(mReceive_Buffer);
-		}
 	}
 	return TRUE;
 }

flight/Bootloaders/PipXtreme/op_dfu.c

@@ -30,11 +30,8 @@
 #include "pios.h"
 #include "op_dfu.h"
 #include "pios_bl_helper.h"
+#include "pios_com_msg.h"
 #include <pios_board_info.h>
-/* Private typedef -----------------------------------------------------------*/
-/* Private define ------------------------------------------------------------*/
-/* Private macro -------------------------------------------------------------*/
-/* Private variables ---------------------------------------------------------*/
 //programmable devices
 Device devicesTable[10];
 uint8_t numberOfDevices = 0;
@@ -102,33 +99,9 @@ void DataDownload(DownloadAction action) {
 					currentProgrammingDestination)) {
 				DeviceState = Last_operation_failed;
 			}
-			/*
-			 switch (currentProgrammingDestination) {
-			 case Remote_flash_via_spi:
-			 if (downType == Descript) {
-			 SendBuffer[6 + (x * 4)]
-			 = spi_dev_desc[(uint8_t) partoffset];
-			 SendBuffer[7 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset
-			 + 1];
-			 SendBuffer[8 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset
-			 + 2];
-			 SendBuffer[9 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset
-			 + 3];
-			 }
-			 break;
-			 case Self_flash:
-			 SendBuffer[6 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset);
-			 SendBuffer[7 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset + 1);
-			 SendBuffer[8 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset + 2);
-			 SendBuffer[9 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset + 3);
-			 break;
-			 }
-			 */
 		}
-		//PIOS USB_SIL_Write(EP1_IN, (uint8_t*) SendBuffer, 64);
 		downPacketCurrent = downPacketCurrent + 1;
 		if (downPacketCurrent > downPacketTotal - 1) {
-			// STM_EVAL_LEDOn(LED2);
 			DeviceState = Last_operation_Success;
 			Aditionals = (uint32_t) Download;
 		}
@@ -168,7 +141,7 @@ void processComand(uint8_t *xReceive_Buffer) {
 	case EnterDFU:
 		if (((DeviceState == BLidle) && (Data0 < numberOfDevices))
 				|| (DeviceState == DFUidle)) {
-			if (Data0 > 0)//PORQUE???
+			if (Data0 > 0)
 				OPDfuIni(TRUE);
 			DeviceState = DFUidle;
 			currentProgrammingDestination = devicesTable[Data0].programmingType;
@@ -225,7 +198,7 @@ void processComand(uint8_t *xReceive_Buffer) {
 						}
 					}
 					if (result != 1) {
-						DeviceState = Last_operation_failed;//ok
+						DeviceState = Last_operation_failed;
 						Aditionals = (uint32_t) Command;
 					} else {
 
@@ -276,11 +249,9 @@ void processComand(uint8_t *xReceive_Buffer) {
 
 					++Next_Packet;
 				} else {
-
 					DeviceState = wrong_packet_received;
 					Aditionals = Count;
 				}
-				// FLASH_ProgramWord(MemLocations[TransferType]+4,++Next_Packet);//+Count,Data);
 			} else {
 				DeviceState = Last_operation_failed;
 				Aditionals = (uint32_t) Command;
@@ -322,9 +293,12 @@ void processComand(uint8_t *xReceive_Buffer) {
 			Buffer[15] = devicesTable[Data0 - 1].devID;
 		}
 		sendData(Buffer + 1, 63);
-		//PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, Buffer + 1, 63);//FIX+1
 		break;
 	case JumpFW:
+		if (Data == 0x5AFE) {
+			/* Force board into safe mode */
+			PIOS_IAP_WriteBootCount(0xFFFF);
+		}
 		FLASH_Lock();
 		JumpToApp = 1;
 		break;
@@ -351,7 +325,6 @@ void processComand(uint8_t *xReceive_Buffer) {
 				DeviceState = too_few_packets;
 			}
 		}
-
 		break;
 	case Download_Req:
 #ifdef DEBUG_SSP
@@ -474,9 +447,7 @@ uint32_t CalcFirmCRC() {
 
 }
 void sendData(uint8_t * buf, uint16_t size) {
-	PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, buf, size);
+	PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
-	if (DeviceState == downloading)
-		PIOS_DELAY_WaitmS(10);
 }
 
 bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) {

flight/Bootloaders/PipXtreme/pios_board.c

@@ -23,41 +23,9 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+#include "board_hw_defs.c"
 #include <pios.h>
 
-// ***********************************************************************************
-
-#if defined(PIOS_INCLUDE_COM)
-
-#include <pios_com_priv.h>
-
-#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
-#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
-
-static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN];
-static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN];
-
-#endif /* PIOS_INCLUDE_COM */
-
-// ***********************************************************************************
-
-#if defined(PIOS_INCLUDE_USB_HID)
-#include "pios_usb_hid_priv.h"
-
-static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
-  .irq = {
-    .init    = {
-      .NVIC_IRQChannel                   = USB_LP_CAN1_RX0_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-};
-#endif	/* PIOS_INCLUDE_USB_HID */
-
-extern const struct pios_com_driver pios_usb_com_driver;
-
 uint32_t pios_com_telem_usb_id;
 
 /**
@@ -83,17 +51,32 @@ void PIOS_Board_Init(void) {
 	/* Initialize the PiOS library */
 	PIOS_GPIO_Init();
 
-#if defined(PIOS_INCLUDE_USB_HID)
+#if defined(PIOS_INCLUDE_LED)
-	uint32_t pios_usb_hid_id;
+ 	PIOS_LED_Init(&pios_led_cfg);
-	PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
+#endif	/* PIOS_INCLUDE_LED */
-#if defined(PIOS_INCLUDE_COM)
+
-	if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
+#if defined(PIOS_INCLUDE_USB)
-			  pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer),
+	/* Initialize board specific USB data */
-			  pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) {
+	PIOS_USB_BOARD_DATA_Init();
+
+	/* Activate the HID-only USB configuration */
+	PIOS_USB_DESC_HID_ONLY_Init();
+
+	uint32_t pios_usb_id;
+	if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) {
 		PIOS_Assert(0);
 	}
-#endif	/* PIOS_INCLUDE_COM */
+#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
-#endif	/* PIOS_INCLUDE_USB_HID */
+	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 */
+
+#endif	/* PIOS_INCLUDE_USB */
 
 	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
 

flight/Bootloaders/PipXtreme/pios_usb_board_data.c

@@ -0,0 +1,97 @@
+/**
+ ******************************************************************************
+ * @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_* */
+#include "pios_usb_util.h"	 /* PIOS_USB_UTIL_AsciiToUtf8 */
+
+static const uint8_t usb_product_id[20] = {
+	sizeof(usb_product_id),
+	USB_DESC_TYPE_STRING,
+	'P', 0,
+	'i', 0,
+	'p', 0,
+	'X', 0,
+	't', 0,
+	'r', 0,
+	'e', 0,
+	'm', 0,
+	'e', 0,
+};
+
+static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
+	sizeof(usb_serial_number),
+	USB_DESC_TYPE_STRING,
+};
+
+static const struct usb_string_langid usb_lang_id = {
+	.bLength = sizeof(usb_lang_id),
+	.bDescriptorType = USB_DESC_TYPE_STRING,
+	.bLangID = htousbs(USB_LANGID_ENGLISH_US),
+};
+
+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[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
+	PIOS_SYS_SerialNumberGet((char *)sn);
+
+	/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
+	uint8_t * utf8 = &(usb_serial_number[2]);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
+
+	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;
+}

flight/Bootloaders/Revolution/Makefile

@@ -1,8 +1,8 @@
  #####
- # Project: OpenPilot AHRS_BOOTLOADER
+ # Project: OpenPilot INS_BOOTLOADER
  #    
  #  
- # Makefile for OpenPilot AHRS_BOOTLOADER project
+ # Makefile for OpenPilot INS project
  #
  # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
  #    
@@ -38,7 +38,7 @@ OUTDIR := $(TOP)/build/$(TARGET)
 DEBUG ?= NO
 
 # Set to YES when using Code Sourcery toolchain
-CODE_SOURCERY ?= YES
+CODE_SOURCERY ?= NO
 
 ifeq ($(CODE_SOURCERY), YES)
 REMOVE_CMD = cs-rm
@@ -46,77 +46,41 @@ else
 REMOVE_CMD = rm
 endif
 
-FLASH_TOOL = OPENOCD
-
 # Paths
-AHRS_BL = ./
+REVO_BL = $(WHEREAMI)
-AHRS_BLINC = $(AHRS_BL)/inc
+REVO_BLINC = $(REVO_BL)/inc
 PIOS = ../../PiOS
 PIOSINC = $(PIOS)/inc
-FLIGHTLIB = ../Libraries
+FLIGHTLIB = ../../Libraries
-FLIGHTLIBINC = ../Libraries/inc
+FLIGHTLIBINC = ../../Libraries/inc
-PIOSSTM32F10X = $(PIOS)/STM32F10x
+PIOSSTM32F4XX = $(PIOS)/STM32F4xx
 PIOSCOMMON = $(PIOS)/Common
 PIOSBOARDS = $(PIOS)/Boards
-APPLIBDIR = $(PIOSSTM32F10X)/Libraries
+PIOSCOMMONLIB = $(PIOSCOMMON)/Libraries
+APPLIBDIR = $(PIOSSTM32F4XX)/Libraries
 STMLIBDIR = $(APPLIBDIR)
-STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
+STMSPDDIR = $(STMLIBDIR)/STM32F4xx_StdPeriph_Driver
-STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
 STMSPDSRCDIR = $(STMSPDDIR)/src
 STMSPDINCDIR = $(STMSPDDIR)/inc
-CMSISDIR  = $(STMLIBDIR)/CMSIS/Core/CM3
+HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
-
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files
 
 ## BOOTLOADER:
-SRC = main.c
+SRC += main.c
 SRC += pios_board.c
-SRC += bl_fsm.c
+SRC += pios_usb_board_data.c
-#SRC += insgps.c
+SRC += op_dfu.c
-#SRC += $(FLIGHTLIB)/CoordinateConversions.c
 
-## PIOS Hardware (STM32F10x)
+## PIOS Hardware (STM32F4xx)
-SRC += $(PIOSSTM32F10X)/pios_sys.c
+include $(PIOS)/STM32F4xx/library.mk
-SRC += $(PIOSSTM32F10X)/pios_led.c
-SRC += $(PIOSSTM32F10X)/pios_delay.c
-SRC += $(PIOSSTM32F10X)/pios_usart.c
-SRC += $(PIOSSTM32F10X)/pios_irq.c
-#SRC += $(PIOSSTM32F10X)/pios_i2c.c
-SRC += $(PIOSSTM32F10X)/pios_gpio.c
-SRC += $(PIOSSTM32F10X)/pios_spi.c
 
-## PIOS Hardware (Common)
+# PIOS Hardware (Common)
-#SRC += $(PIOSCOMMON)/pios_com.c
-#SRC += $(PIOSCOMMON)/pios_hmc5843.c
 SRC += $(PIOSCOMMON)/pios_board_info.c
-SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
+SRC += $(PIOSCOMMON)/pios_com_msg.c
 SRC += $(PIOSCOMMON)/printf-stdarg.c
-SRC += $(PIOSCOMMON)/pios_bl_helper.c
+SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
-SRC += $(PIOSCOMMON)/pios_iap.c
-
-## CMSIS for STM32
-SRC += $(CMSISDIR)/core_cm3.c
-SRC += $(CMSISDIR)/system_stm32f10x.c
-
-## Used parts of the STM-Library
-SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
-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
@@ -132,29 +96,18 @@ CPPSRC =
 #CPPSRCARM = $(TARGET).cpp
 CPPSRCARM = 
 
-# List Assembler source files here.
-# Make them always end in a capital .S. Files ending in a lowercase .s
-# will not be considered source files but generated files (assembler
-# output from the compiler), and will be deleted upon "make clean"!
-# Even though the DOS/Win* filesystem matches both .s and .S the same,
-# it will preserve the spelling of the filenames, and gcc itself does
-# care about how the name is spelled on its command-line.
-ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
-
-# List Assembler source files here which must be assembled in ARM-Mode..
-ASRCARM = 
-
 # 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  += $(PIOSSTM32F10X)
+EXTRAINCDIRS  += $(PIOSSTM34FXX)
 EXTRAINCDIRS  += $(PIOSCOMMON)
 EXTRAINCDIRS  += $(PIOSBOARDS)
 EXTRAINCDIRS  += $(STMSPDINCDIR)
 EXTRAINCDIRS  += $(CMSISDIR)
-EXTRAINCDIRS  += $(AHRS_BLINC)
+EXTRAINCDIRS  += $(REVO_BLINC)
+EXTRAINCDIRS  += $(HWDEFSINC)
 
 # List any extra directories to look for library files here.
 # Also add directories where the linker should search for
@@ -171,7 +124,7 @@ EXTRA_LIBDIRS =
 EXTRA_LIBS =
 
 # Path to Linker-Scripts
-LINKERSCRIPTPATH = $(PIOSSTM32F10X)
+LINKERSCRIPTPATH = $(PIOSSTM32FXX)
 
 # Optimization level, can be [0, 1, 2, 3, s]. 
 # 0 = turn off optimization. s = optimize for size.
@@ -195,7 +148,9 @@ DEBUGF = dwarf-2
 
 # Place project-specific -D (define) and/or 
 # -U options for C here.
-CDEFS = -DSTM32F10X_$(MODEL)
+CDEFS = -DSTM32F4XX
+CDEFS += -DSYSCLK_FREQ=$(SYSCLK_FREQ)
+CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ)
 CDEFS += -DUSE_STDPERIPH_DRIVER
 CDEFS += -DUSE_$(BOARD)
 
@@ -237,6 +192,11 @@ 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)
@@ -254,7 +214,7 @@ CFLAGS += -fpromote-loop-indices
 endif
 
 CFLAGS += -Wall
-CFLAGS += -Werror
+#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
@@ -287,9 +247,8 @@ LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
 LDFLAGS += $(MATH_LIB)
 LDFLAGS += -lc -lgcc 
 
-# Set linker-script name depending on selected submodel name
+#Linker scripts                                                                                                                                                                                                              
-LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
+LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_BL))
-LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_BL_sections.ld
 
 # Define programs and commands.
 REMOVE  = $(REMOVE_CMD) -f
@@ -353,8 +312,10 @@ $(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)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino
 elf: $(OUTDIR)/$(TARGET).elf

flight/Bootloaders/Revolution/inc/common.h

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @addtogroup CopterControlBL CopterControl BootLoader
- * @brief These files contain the code to the OpenPilot MB Bootloader.
+ * @brief These files contain the code to the CopterControl Bootloader.
  *
  * @{
  * @file       common.c

flight/Bootloaders/Revolution/inc/pios_config.h

@@ -33,8 +33,11 @@
 #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
-#define PIOS_INCLUDE_GPIO
 
 #endif /* PIOS_CONFIG_H */

flight/Bootloaders/Revolution/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_REVOLUTION
+#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, 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 */

flight/Bootloaders/Revolution/main.c

@@ -0,0 +1,208 @@
+/**
+ ******************************************************************************
+ * @addtogroup RevolutionBL Revolution BootLoader
+ * @brief These files contain the code to the Revolution Bootloader.
+ *
+ * @{
+ * @file       main.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This is the file with the main function of the Revolution BootLoader
+ * @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
+ */
+/* Bootloader Includes */
+#include <pios.h>
+#include <pios_board_info.h>
+#include <stdbool.h>
+#include "op_dfu.h"
+#include "pios_iap.h"
+#include "fifo_buffer.h"
+#include "pios_com_msg.h"
+#include "pios_usbhook.h"	/* PIOS_USBHOOK_* */
+/* Prototype of PIOS_Board_Init() function */
+extern void PIOS_Board_Init(void);
+extern void FLASH_Download();
+#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
+
+/* Private typedef -----------------------------------------------------------*/
+typedef void (*pFunction)(void);
+/* Private define ------------------------------------------------------------*/
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+pFunction Jump_To_Application;
+uint32_t JumpAddress;
+
+/// LEDs PWM
+uint32_t period1 = 5000; // 5 mS
+uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
+uint32_t period2 = 5000; // 5 mS
+uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
+
+
+////////////////////////////////////////
+uint8_t tempcount = 0;
+
+/* Extern variables ----------------------------------------------------------*/
+DFUStates DeviceState;
+int16_t status = 0;
+bool JumpToApp = false;
+bool GO_dfu = false;
+bool USB_connected = false;
+bool User_DFU_request = false;
+static uint8_t mReceive_Buffer[63];
+/* Private function prototypes -----------------------------------------------*/
+uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
+uint8_t processRX();
+void jump_to_app();
+
+int main() {
+	PIOS_SYS_Init();	
+	PIOS_Board_Init();
+	PIOS_IAP_Init();
+
+	USB_connected = PIOS_USB_CheckAvailable(0);
+
+	if (PIOS_IAP_CheckRequest() == true) {
+		PIOS_DELAY_WaitmS(1000);
+		User_DFU_request = true;
+		PIOS_IAP_ClearRequest();
+	}
+
+	GO_dfu = (USB_connected == true) || (User_DFU_request == true);
+
+	if (GO_dfu == true) {
+		PIOS_Board_Init();
+		if (User_DFU_request == true)
+			DeviceState = DFUidle;
+		else
+			DeviceState = BLidle;
+	} else
+		JumpToApp = true;
+
+	uint32_t stopwatch = 0;
+	uint32_t prev_ticks = PIOS_DELAY_GetuS();
+	while (true) {
+		/* Update the stopwatch */
+		uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
+		prev_ticks += elapsed_ticks;
+		stopwatch += elapsed_ticks;
+
+		if (JumpToApp == true)
+			jump_to_app();
+
+		switch (DeviceState) {
+		case Last_operation_Success:
+		case uploadingStarting:
+		case DFUidle:
+			period1 = 5000;
+			sweep_steps1 = 100;
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+			period2 = 0;
+			break;
+		case uploading:
+			period1 = 5000;
+			sweep_steps1 = 100;
+			period2 = 2500;
+			sweep_steps2 = 50;
+			break;
+		case downloading:
+			period1 = 2500;
+			sweep_steps1 = 50;
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+			period2 = 0;
+			break;
+		case BLidle:
+			period1 = 0;
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
+			period2 = 0;
+			break;
+		default://error
+			period1 = 5000;
+			sweep_steps1 = 100;
+			period2 = 5000;
+			sweep_steps2 = 100;
+		}
+
+		if (period1 != 0) {
+			if (LedPWM(period1, sweep_steps1, stopwatch))
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
+			else
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+		} else
+			PIOS_LED_On(PIOS_LED_HEARTBEAT);
+
+		if (period2 != 0) {
+			if (LedPWM(period2, sweep_steps2, stopwatch))
+				PIOS_LED_On(PIOS_LED_HEARTBEAT);
+			else
+				PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+		} else
+			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+
+		if (stopwatch > 50 * 1000 * 1000)
+			stopwatch = 0;
+		if ((stopwatch > 6 * 1000 * 1000) && (DeviceState
+				== BLidle))
+			JumpToApp = true;
+
+		processRX();
+		DataDownload(start);
+	}
+}
+
+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);
+
+		PIOS_USBHOOK_Deactivate();
+
+		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 {
+		DeviceState = failed_jump;
+		return;
+	}
+}
+uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
+	uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
+	uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
+
+	uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
+	if (curr_sweep & 1) {
+		pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
+	}
+	return ((count % pwm_period) > pwm_duty) ? 1 : 0;
+}
+
+uint8_t processRX() {
+	if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
+		processComand(mReceive_Buffer);
+	}
+	return true;
+}
+

flight/Bootloaders/Revolution/op_dfu.c

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotBL OpenPilot BootLoader
+ * @addtogroup CopterControlBL CopterControl BootLoader
- * @brief These files contain the code to the OpenPilot MB Bootloader.
+ * @brief These files contain the code to the CopterControl Bootloader.
  *
  * @{
  * @file       op_dfu.c
@@ -28,15 +28,11 @@
 
 /* Includes ------------------------------------------------------------------*/
 #include "pios.h"
+#include <stdbool.h>
 #include "op_dfu.h"
 #include "pios_bl_helper.h"
+#include "pios_com_msg.h"
 #include <pios_board_info.h>
-#include "pios_opahrs.h"
-#include "ssp.h"
-/* Private typedef -----------------------------------------------------------*/
-/* Private define ------------------------------------------------------------*/
-/* Private macro -------------------------------------------------------------*/
-/* Private variables ---------------------------------------------------------*/
 //programmable devices
 Device devicesTable[10];
 uint8_t numberOfDevices = 0;
@@ -75,8 +71,6 @@ DFUTransfer downType = 0;
 /* Extern variables ----------------------------------------------------------*/
 extern DFUStates DeviceState;
 extern uint8_t JumpToApp;
-extern Port_t ssp_port;
-extern DFUPort ProgPort;
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
 void sendData(uint8_t * buf, uint16_t size);
@@ -142,14 +136,14 @@ void processComand(uint8_t *xReceive_Buffer) {
 	Command = Command & 0b00011111;
 
 	if (EchoReqFlag == 1) {
-		memcpy(echoBuffer, Buffer, 64);
+		memcpy(echoBuffer, xReceive_Buffer, 64);
 	}
 	switch (Command) {
 	case EnterDFU:
 		if (((DeviceState == BLidle) && (Data0 < numberOfDevices))
 				|| (DeviceState == DFUidle)) {
 			if (Data0 > 0)
-				OPDfuIni(TRUE);
+				OPDfuIni(true);
 			DeviceState = DFUidle;
 			currentProgrammingDestination = devicesTable[Data0].programmingType;
 			currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01;
@@ -162,7 +156,7 @@ void processComand(uint8_t *xReceive_Buffer) {
 				result = PIOS_BL_HELPER_FLASH_Ini();
 				break;
 			case Remote_flash_via_spi:
-				result = TRUE;
+				result = true;
 				break;
 			default:
 				DeviceState = Last_operation_failed;
@@ -187,42 +181,25 @@ void processComand(uint8_t *xReceive_Buffer) {
 				SizeOfLastPacket = Data1;
 
 				if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1)
-						* 14 * 4 + SizeOfLastPacket * 4) == TRUE) {
+						* 14 * 4 + SizeOfLastPacket * 4) == true) {
 					DeviceState = outsideDevCapabilities;
 					Aditionals = (uint32_t) Command;
 				} else {
 					uint8_t result = 1;
-					struct opahrs_msg_v0 rsp;
 					if (TransferType == FW) {
 						switch (currentProgrammingDestination) {
 						case Self_flash:
 							result = PIOS_BL_HELPER_FLASH_Start();
 							break;
 						case Remote_flash_via_spi:
-							PIOS_OPAHRS_bl_FwupStart(&rsp);
+							result = false;
-							result = FALSE;
-							for (int i = 0; i < 5; ++i) {
-								PIOS_DELAY_WaitmS(1000);
-								PIOS_OPAHRS_bl_resync();
-								if (PIOS_OPAHRS_bl_FwupStatus(&rsp)
-										== OPAHRS_RESULT_OK) {
-									if (rsp.payload.user.v.rsp.fwup_status.status
-											== started) {
-										result = TRUE;
-										break;
-									} else {
-										result = FALSE;
-										break;
-									}
-								}
-							}
 							break;
 						default:
 							break;
 						}
 					}
 					if (result != 1) {
-						DeviceState = Last_operation_failed;//ok
+						DeviceState = Last_operation_failed;
 						Aditionals = (uint32_t) Command;
 					} else {
 
@@ -239,8 +216,6 @@ void processComand(uint8_t *xReceive_Buffer) {
 					{
 						numberOfWords = SizeOfLastPacket;
 					}
-					struct opahrs_msg_v0 rsp;
-					struct opahrs_msg_v0 req;
 					uint8_t result = 0;
 					switch (currentProgrammingDestination) {
 					case Self_flash:
@@ -262,31 +237,7 @@ void processComand(uint8_t *xReceive_Buffer) {
 						}
 						break;
 					case Remote_flash_via_spi:
-						for (uint8_t x = 0; x < numberOfWords; ++x) {
+						result = false; // No support for this for the PipX
-							offset = 4 * x;
-							Data = xReceive_Buffer[DATA + offset] << 24;
-							Data += xReceive_Buffer[DATA + 1 + offset] << 16;
-							Data += xReceive_Buffer[DATA + 2 + offset] << 8;
-							Data += xReceive_Buffer[DATA + 3 + offset];
-							req.payload.user.v.req.fwup_data.data[x] = Data;
-						}
-						aux = (baseOfAdressType(TransferType) + (uint32_t)(
-								Count * 14 * 4));
-						req.payload.user.v.req.fwup_data.adress = aux;
-						req.payload.user.v.req.fwup_data.size = numberOfWords;
-						if (PIOS_OPAHRS_bl_FwupData(&req, &rsp)
-								== OPAHRS_RESULT_OK) {
-							if (rsp.payload.user.v.rsp.fwup_status.status
-									== write_error) {
-								result = FALSE;
-							} else if (rsp.payload.user.v.rsp.fwup_status.status
-									== outside_dev_capabilities) {
-								result = TRUE;
-								DeviceState = outsideDevCapabilities;
-							} else
-								result = TRUE;
-						} else
-							result = FALSE;
 						break;
 					default:
 						result = 0;
@@ -299,7 +250,6 @@ void processComand(uint8_t *xReceive_Buffer) {
 
 					++Next_Packet;
 				} else {
-
 					DeviceState = wrong_packet_received;
 					Aditionals = Count;
 				}
@@ -310,7 +260,7 @@ void processComand(uint8_t *xReceive_Buffer) {
 		}
 		break;
 	case Req_Capabilities:
-		OPDfuIni(TRUE);
+		OPDfuIni(true);
 		Buffer[0] = 0x01;
 		Buffer[1] = Rep_Capabilities;
 		if (Data0 == 0) {
@@ -346,20 +296,12 @@ void processComand(uint8_t *xReceive_Buffer) {
 		sendData(Buffer + 1, 63);
 		break;
 	case JumpFW:
-		if (numberOfDevices > 1) {
+		if (Data == 0x5AFE) {
-			struct opahrs_msg_v0 rsp;
+			/* Force board into safe mode */
-			PIOS_OPAHRS_bl_boot(0);
+			PIOS_IAP_WriteBootCount(0xFFFF);
-			if (PIOS_OPAHRS_bl_FwupStatus(&rsp) == OPAHRS_RESULT_OK) {
-				DeviceState = failed_jump;
-				break;
-			} else {
-				FLASH_Lock();
-				JumpToApp = 1;
-			}
-		} else {
-			FLASH_Lock();
-			JumpToApp = 1;
 		}
+		FLASH_Lock();
+		JumpToApp = 1;
 		break;
 	case Reset:
 		PIOS_SYS_Reset();
@@ -384,7 +326,6 @@ void processComand(uint8_t *xReceive_Buffer) {
 				DeviceState = too_few_packets;
 			}
 		}
-
 		break;
 	case Download_Req:
 #ifdef DEBUG_SSP
@@ -398,8 +339,8 @@ void processComand(uint8_t *xReceive_Buffer) {
 			downType = Data0;
 			downPacketTotal = Count;
 			downSizeOfLastPacket = Data1;
-			if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14
+			if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 * 4
-					+ downSizeOfLastPacket) == 1) {
+					+ downSizeOfLastPacket * 4) == 1) {
 				DeviceState = outsideDevCapabilities;
 				Aditionals = (uint32_t) Command;
 
@@ -442,8 +383,8 @@ void processComand(uint8_t *xReceive_Buffer) {
 
 	}
 	if (EchoReqFlag == 1) {
-		echoBuffer[1] = echoBuffer[1] | EchoAnsFlag;
+		echoBuffer[0] = echoBuffer[0] | (1 << 6);
-		sendData(echoBuffer + 1, 63);
+		sendData(echoBuffer, 63);
 	}
 	return;
 }
@@ -463,41 +404,8 @@ void OPDfuIni(uint8_t discover) {
 	numberOfDevices = 1;
 	devicesTable[0] = dev;
 	if (discover) {
-		uint8_t found_spi_device = FALSE;
+		//TODO check other devices trough spi or whatever
-
-		for (int t = 0; t < 3; ++t) {
-			if (PIOS_OPAHRS_bl_resync() == OPAHRS_RESULT_OK) {
-				found_spi_device = TRUE;
-				dev.FW_Crc = 0;
-				break;
-			}
-			PIOS_DELAY_WaitmS(100);
-		}
-		if (found_spi_device == TRUE) {
-			struct opahrs_msg_v0 rsp;
-			if (PIOS_OPAHRS_bl_GetVersions(&rsp) == OPAHRS_RESULT_OK) {
-				dev.programmingType = Remote_flash_via_spi;
-				dev.BL_Version = rsp.payload.user.v.rsp.versions.bl_version;
-				dev.FW_Crc = rsp.payload.user.v.rsp.versions.fw_crc;
-				dev.devID = rsp.payload.user.v.rsp.versions.hw_version;
-				if (PIOS_OPAHRS_bl_GetMemMap(&rsp) == OPAHRS_RESULT_OK) {
-					dev.readWriteFlags
-							= rsp.payload.user.v.rsp.mem_map.rw_flags;
-					dev.startOfUserCode
-							= rsp.payload.user.v.rsp.mem_map.start_of_user_code;
-					dev.sizeOfCode
-							= rsp.payload.user.v.rsp.mem_map.size_of_code_memory;
-					dev.sizeOfDescription
-							= rsp.payload.user.v.rsp.mem_map.size_of_description;
-					dev.devType = rsp.payload.user.v.rsp.mem_map.density;
-					numberOfDevices = 2;
-					devicesTable[1] = dev;
-				}
-			}
-		} else
-			PIOS_OPAHRS_ForceSlaveSelected(true);
 	}
-	//TODO check other devices trough spi or whatever
 }
 uint32_t baseOfAdressType(DFUTransfer type) {
 	switch (type) {
@@ -508,6 +416,7 @@ uint32_t baseOfAdressType(DFUTransfer type) {
 		return currentDevice.startOfUserCode + currentDevice.sizeOfCode;
 		break;
 	default:
+
 		return 0;
 	}
 }
@@ -520,26 +429,16 @@ uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) {
 		return (size > currentDevice.sizeOfDescription) ? 1 : 0;
 		break;
 	default:
-		return TRUE;
+		return true;
 	}
 }
 
 uint32_t CalcFirmCRC() {
-	struct opahrs_msg_v0 rsp;
 	switch (currentProgrammingDestination) {
 	case Self_flash:
 		return PIOS_BL_HELPER_CRC_Memory_Calc();
 		break;
 	case Remote_flash_via_spi:
-		PIOS_OPAHRS_bl_FwupVerify(&rsp);
-		for (int i = 0; i < 5; ++i) {
-			PIOS_DELAY_WaitmS(1000);
-			PIOS_OPAHRS_bl_resync();
-			if (PIOS_OPAHRS_bl_GetVersions(&rsp) == OPAHRS_RESULT_OK) {
-				return rsp.payload.user.v.rsp.versions.fw_crc;
-			}
-		}
-
 		return 0;
 		break;
 	default:
@@ -549,38 +448,21 @@ uint32_t CalcFirmCRC() {
 
 }
 void sendData(uint8_t * buf, uint16_t size) {
-	if (ProgPort == Usb) {
+	PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
-
-		PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, buf, size);
-		if (DeviceState == downloading)
-			PIOS_DELAY_WaitmS(10);
-
-	} else if (ProgPort == Serial) {
-		ssp_SendData(&ssp_port, buf, size);
-	}
 }
 
 bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) {
-	struct opahrs_msg_v0 rsp;
-	struct opahrs_msg_v0 req;
 	switch (type) {
 	case Remote_flash_via_spi:
-		req.payload.user.v.req.fwdn_data.adress = adr;
+		return false; // We should not get this for the PipX
-		if (PIOS_OPAHRS_bl_FwDlData(&req, &rsp) == OPAHRS_RESULT_OK) {
-			for (uint8_t x = 0; x < 4; ++x) {
-				buffer[x] = rsp.payload.user.v.rsp.fw_dn.data[x];
-			}
-			return TRUE;
-		}
-		return FALSE;
 		break;
 	case Self_flash:
 		for (uint8_t x = 0; x < 4; ++x) {
 			buffer[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x);
 		}
-		return TRUE;
+		return true;
 		break;
 	default:
-		return FALSE;
+		return false;
 	}
 }

flight/Bootloaders/Revolution/pios_board.c

@@ -0,0 +1,78 @@
+/**
+ ******************************************************************************
+ *
+ * @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
+ */
+
+/* Pull in the board-specific static HW definitions.
+ * Including .c files is a bit ugly but this allows all of
+ * the HW definitions to be const and static to limit their
+ * scope.  
+ *
+ * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
+ */
+#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();
+	
+#if defined(PIOS_INCLUDE_LED)
+	PIOS_LED_Init(&pios_led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
+#if defined(PIOS_INCLUDE_USB)
+	/* Initialize board specific USB data */
+	PIOS_USB_BOARD_DATA_Init();
+
+	/* 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;
+}

flight/Bootloaders/Revolution/pios_usb_board_data.c

@@ -0,0 +1,120 @@
+/**
+ ******************************************************************************
+ * @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[22] = {
+	sizeof(usb_product_id),
+	USB_DESC_TYPE_STRING,
+	'R', 0,
+	'e', 0,
+	'v', 0,
+	'o', 0,
+	'l', 0,
+	'u', 0,
+	't', 0,
+	'i', 0,
+	'o', 0,
+	'n', 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;
+}

flight/CopterControl/Makefile

@@ -39,6 +39,7 @@ DEBUG ?= NO
 
 # Include objects that are just nice information to show
 DIAGNOSTICS ?= NO
+DIAG_TASKS ?= NO
 
 # Set to YES to build a FW version that will erase all flash memory
 ERASE_FLASH ?= NO
@@ -48,12 +49,8 @@ ENABLE_DEBUG_PINS ?= NO
 # Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
 ENABLE_AUX_UART ?= NO
 
-USE_GPS ?= YES
-
-USE_I2C ?= NO
-
 # Set to YES when using Code Sourcery toolchain
-CODE_SOURCERY ?= YES
+CODE_SOURCERY ?= NO
 
 # Remove command is different for Code Sourcery on Windows
 ifeq ($(CODE_SOURCERY), YES)
@@ -64,12 +61,41 @@ endif
 
 FLASH_TOOL = OPENOCD
 
-# List of modules to include
+# Optional module and driver defaults
-OPTMODULES = CameraStab
+USE_CAMERASTAB ?= YES
+USE_COMUSBBRIDGE ?= YES
+USE_GPS ?= YES
+USE_TXPID ?= YES
+USE_I2C ?= NO
+USE_ALTITUDE ?= NO
+TEST_FAULTS ?= NO
+
+# List of optional modules to include
+OPTMODULES =
+ifeq ($(USE_CAMERASTAB), YES)
+OPTMODULES += CameraStab
+endif
+ifeq ($(USE_COMUSBBRIDGE), YES)
+OPTMODULES += ComUsbBridge
+endif
 ifeq ($(USE_GPS), YES)
 OPTMODULES += GPS
 endif
+ifeq ($(USE_TXPID), YES)
+OPTMODULES += TxPID
+endif
+ifeq ($(USE_ALTITUDE), YES)
+ifeq ($(USE_I2C), YES)
+OPTMODULES += Altitude
+else
+$(error "Altitude module (USE_ALTITUDE=YES) requires i2c (USE_I2C=YES)")
+endif
+endif
+ifeq ($(TEST_FAULTS), YES)
+OPTMODULES += Fault
+endif
 
+# List of mandatory modules to include
 MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP
 # Telemetry must be last to grab the optional modules (why?)
 MODULES += Telemetry
@@ -117,6 +143,7 @@ PYMITEINC += $(PYMITEPLAT)
 PYMITEINC += $(OUTDIR)
 FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
 FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
+HWDEFSINC = ../board_hw_defs/$(BOARD_NAME)
 
 OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
 
@@ -132,11 +159,9 @@ SRC += ${OPMODULEDIR}/System/systemmod.c
 SRC += $(OPSYSTEM)/coptercontrol.c
 SRC += $(OPSYSTEM)/pios_board.c
 SRC += $(OPSYSTEM)/alarms.c
-SRC += $(OPSYSTEM)/taskmonitor.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
-SRC += $(OPSYSTEM)/pios_usb_hid_desc.c
 else
 ## TESTCODE
 SRC += $(OPTESTS)/test_common.c
@@ -151,6 +176,7 @@ SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
 SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
 SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c
 SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c
+SRC += $(OPUAVSYNTHDIR)/faultsettings.c
 SRC += $(OPUAVSYNTHDIR)/flightstatus.c
 SRC += $(OPUAVSYNTHDIR)/systemstats.c
 SRC += $(OPUAVSYNTHDIR)/systemalarms.c
@@ -160,7 +186,8 @@ SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
 SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
 SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
 SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
-SRC += $(OPUAVSYNTHDIR)/attituderaw.c
+SRC += $(OPUAVSYNTHDIR)/accels.c
+SRC += $(OPUAVSYNTHDIR)/gyros.c
 SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
 SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
 SRC += $(OPUAVSYNTHDIR)/i2cstats.c
@@ -178,6 +205,8 @@ SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
 SRC += $(OPUAVSYNTHDIR)/taskinfo.c
 SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
 SRC += $(OPUAVSYNTHDIR)/ratedesired.c
+SRC += $(OPUAVSYNTHDIR)/baroaltitude.c
+SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
 
 endif
 
@@ -200,30 +229,38 @@ SRC += $(PIOSSTM32F10X)/pios_gpio.c
 SRC += $(PIOSSTM32F10X)/pios_exti.c
 SRC += $(PIOSSTM32F10X)/pios_rtc.c
 SRC += $(PIOSSTM32F10X)/pios_wdg.c
+SRC += $(PIOSSTM32F10X)/pios_iap.c
 SRC += $(PIOSSTM32F10X)/pios_tim.c
+SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
 
-
+# PIOS USB related files (separated to make code maintenance more easy)
-# PIOS USB related files (seperated to make code maintenance more easy)
+SRC += $(PIOSSTM32F10X)/pios_usb.c
+SRC += $(PIOSSTM32F10X)/pios_usbhook.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
+SRC += $(PIOSSTM32F10X)/pios_usb_cdc.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c
 SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
+SRC += $(OPSYSTEM)/pios_usb_board_data.c
+SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c
+SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
+SRC += $(PIOSCOMMON)/pios_usb_util.c
 
 ## PIOS Hardware (Common)
 SRC += $(PIOSCOMMON)/pios_crc.c
 SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c
-SRC += $(PIOSCOMMON)/pios_flash_w25x.c
+SRC += $(PIOSCOMMON)/pios_flash_jedec.c
 SRC += $(PIOSCOMMON)/pios_adxl345.c
+SRC += $(PIOSCOMMON)/pios_mpu6000.c
 SRC += $(PIOSCOMMON)/pios_com.c
-SRC += $(PIOSCOMMON)/pios_i2c_esc.c
+#SRC += $(PIOSCOMMON)/pios_i2c_esc.c
-SRC += $(PIOSCOMMON)/pios_iap.c
+#SRC += $(PIOSCOMMON)/pios_bmp085.c
-SRC += $(PIOSCOMMON)/pios_bl_helper.c
 SRC += $(PIOSCOMMON)/pios_rcvr.c
 SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
 SRC += $(PIOSCOMMON)/printf-stdarg.c
 ## Libraries for flight calculations
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
+SRC += $(FLIGHTLIB)/taskmonitor.c
 
 ## CMSIS for STM32
 SRC += $(CMSISDIR)/core_cm3.c
@@ -337,6 +374,7 @@ EXTRAINCDIRS  += $(APPLIBDIR)
 EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
 EXTRAINCDIRS  += $(AHRSBOOTLOADERINC)
 EXTRAINCDIRS  += $(PYMITEINC)
+EXTRAINCDIRS  += $(HWDEFSINC)
 
 EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES}, ${OPMODULEDIR}/${MOD}/inc} ${OPMODULEDIR}/System/inc
 
@@ -401,6 +439,9 @@ ifeq ($(USE_I2C), YES)
 CDEFS += -DUSE_I2C
 endif
 
+# Declare all non-optional modules as built-in to force inclusion
+CDEFS += ${foreach MOD, ${MODULES}, -DMODULE_$(MOD)_BUILTIN }
+
 # Place project-specific -D and/or -U options for
 # Assembler with preprocessor here.
 #ADEFS = -DUSE_IRQ_ASM_WRAPPER
@@ -427,11 +468,15 @@ CSTANDARD = -std=gnu99
 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
 
 ifeq ($(DEBUG),YES)
-CFLAGS = -DDEBUG
+CFLAGS += -DDEBUG
 endif
 
 ifeq ($(DIAGNOSTICS),YES)
-CFLAGS = -DDIAGNOSTICS
+CFLAGS += -DDIAGNOSTICS
+endif
+
+ifeq ($(DIAG_TASKS),YES)
+CFLAGS += -DDIAG_TASKS
 endif
 
 CFLAGS += -g$(DEBUGF)
@@ -561,7 +606,7 @@ $(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,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino opfw
 elf: $(OUTDIR)/$(TARGET).elf

flight/CopterControl/System/coptercontrol.c

@@ -71,8 +71,10 @@ int main()
 	PIOS_Board_Init();
 
 #ifdef ERASE_FLASH
-	PIOS_Flash_W25X_EraseChip();
+	PIOS_Flash_Jedec_EraseChip();
-	PIOS_LED_Off(LED1);
+#if defined(PIOS_LED_HEARTBEAT)
+	PIOS_LED_Off(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
 	while (1) ;
 #endif
 
@@ -82,19 +84,33 @@ int main()
 	/* swap the stack to use the IRQ stack */
 	Stack_Change();
 
-	/* Start the FreeRTOS scheduler which should never returns.*/
+	/* Start the FreeRTOS scheduler, which should never return.
+	 *
+	 * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls 
+	 * (schedules) function files (modules). These functions never return from their
+	 * while loops, which explains why each module has a while(1){} segment. Thus, 
+	 * the OpenPilot software actually starts at the vTaskStartScheduler() function, 
+	 * even though this is somewhat obscure.
+	 *
+	 * In addition, there are many main() functions in the OpenPilot firmware source tree
+	 * This is because each main() refers to a separate hardware platform. Of course,
+	 * C only allows one main(), so only the relevant main() function is compiled when 
+	 * making a specific firmware.
+	 *
+	 */
 	vTaskStartScheduler();
 
 	/* If all is well we will never reach here as the scheduler will now be running. */
 
 	/* Do some indication to user that something bad just happened */
-	PIOS_LED_Off(LED1);
 	while (1) {
-		PIOS_LED_Toggle(LED1);
+#if defined(PIOS_LED_HEARTBEAT)
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif	/* PIOS_LED_HEARTBEAT */
 		PIOS_DELAY_WaitmS(100);
 	}
 
-    return 0;
+	return 0;
 }
 
 /**

flight/CopterControl/System/inc/FreeRTOSConfig.h

@@ -31,7 +31,7 @@
 #define configTICK_RATE_HZ		( ( portTickType ) 1000 )
 #define configMAX_PRIORITIES		( ( unsigned portBASE_TYPE ) 5 )
 #define configMINIMAL_STACK_SIZE	( ( unsigned short ) 48 )
-#define configTOTAL_HEAP_SIZE		( ( size_t ) ( 54 * 256) )
+#define configTOTAL_HEAP_SIZE		( ( size_t ) ( 53 * 256) )
 #define configMAX_TASK_NAME_LEN		( 16 )
 #define configUSE_TRACE_FACILITY	0
 #define configUSE_16_BIT_TICKS		0
@@ -76,7 +76,7 @@ NVIC value of 255. */
 #endif
 
 /* Enable run time stats collection */
-#if defined(DIAGNOSTICS)
+#if defined(DIAG_TASKS)
 #define configCHECK_FOR_STACK_OVERFLOW	2
 
 #define configGENERATE_RUN_TIME_STATS 1

flight/CopterControl/System/inc/pios_config.h

@@ -36,12 +36,14 @@
 /* Enable/Disable PiOS Modules */
 #define PIOS_INCLUDE_ADC
 #define PIOS_INCLUDE_DELAY
-#if defined(USE_I2C)
+//#if defined(USE_I2C)
-#define PIOS_INCLUDE_I2C
+//#define PIOS_INCLUDE_I2C
-#define PIOS_INCLUDE_I2C_ESC
+//#define PIOS_INCLUDE_I2C_ESC
-#endif
+//#endif
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_LED
+#define PIOS_INCLUDE_IAP
+#define PIOS_INCLUDE_TIM
 
 #define PIOS_INCLUDE_RCVR
 
@@ -61,7 +63,9 @@
 #define PIOS_INCLUDE_SPI
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_USART
+#define PIOS_INCLUDE_USB
 #define PIOS_INCLUDE_USB_HID
+#define PIOS_INCLUDE_USB_CDC
 #define PIOS_INCLUDE_COM
 #define PIOS_INCLUDE_SETTINGS
 #define PIOS_INCLUDE_FREERTOS
@@ -73,14 +77,12 @@
 
 #define PIOS_INCLUDE_ADXL345
 #define PIOS_INCLUDE_FLASH
+#define PIOS_INCLUDE_MPU6000
+#define PIOS_MPU6000_ACCEL
 
 /* A really shitty setting saving implementation */
 #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
 
-/* Defaults for Logging */
-#define LOG_FILENAME 			"PIOS.LOG"
-#define STARTUP_LOG_ENABLED		1
-
 /* Alarm Thresholds */
 #define HEAP_LIMIT_WARNING             220
 #define HEAP_LIMIT_CRITICAL             40

flight/CopterControl/System/inc/pios_usb.h

@@ -1,85 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup   PIOS_USB USB Functions
- * @brief PIOS USB interface code
- * @{
- *
- * @file       pios_usb.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * 	       Parts by Thorsten Klose (tk@midibox.org)
- * @brief      USB functions 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 PIOS_USB_H
-#define PIOS_USB_H
-
-/* Local defines */
-/* Following settings allow to customise the USB device descriptor */
-#ifndef PIOS_USB_VENDOR_ID
-#define PIOS_USB_VENDOR_ID		0x20A0
-#endif
-
-#ifndef PIOS_USB_VENDOR_STR
-#define PIOS_USB_VENDOR_STR		"openpilot.org"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_STR
-#define PIOS_USB_PRODUCT_STR		"CopterControl"
-#endif
-
-#ifndef PIOS_USB_PRODUCT_ID
-#define PIOS_USB_PRODUCT_ID		0x415B
-#endif
-
-#ifndef PIOS_USB_VERSION_ID
-#define PIOS_USB_VERSION_ID		0x0402	/* CopterControl board (04), Running state (02) */
-#endif
-
-/* Internal defines which are used by PIOS USB HID (don't touch) */
-#define PIOS_USB_EP_NUM			2
-
-/* Buffer table base address */
-#define PIOS_USB_BTABLE_ADDRESS		0x000
-
-/* EP0 rx/tx buffer base address */
-#define PIOS_USB_ENDP0_RXADDR		0x040
-#define PIOS_USB_ENDP0_TXADDR		0x080
-
-/* EP1 Rx/Tx buffer base address for HID driver */
-#define PIOS_USB_ENDP1_TXADDR		0x0C0
-#define PIOS_USB_ENDP1_RXADDR		0x100
-
-/* Global Variables */
-extern void (*pEpInt_IN[7]) (void);
-extern void (*pEpInt_OUT[7]) (void);
-
-/* Public Functions */
-extern int32_t PIOS_USB_Init(uint32_t mode);
-extern int32_t PIOS_USB_IsInitialized(void);
-extern int32_t PIOS_USB_CableConnected(void);
-
-#endif /* PIOS_USB_H */
-
-/**
-  * @}
-  * @}
-  */

flight/CopterControl/System/inc/pios_usb_board_data.h

@@ -0,0 +1,46 @@
+/**
+ ******************************************************************************
+ * @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_CDC_DATA_LENGTH 64
+#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
+#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
+
+#define PIOS_USB_BOARD_EP_NUM 4
+
+#include "pios_usb_defs.h" 	/* USB_* macros */
+
+#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL
+#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW)
+#define PIOS_USB_BOARD_SN_SUFFIX "+FW"
+
+#endif	/* PIOS_USB_BOARD_DATA_H */

flight/CopterControl/System/inc/pios_usb_hid_desc.h

@@ -1,56 +0,0 @@
-/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
-* File Name          : usb_desc.h
-* Author             : MCD Application Team
-* Version            : V3.2.1
-* Date               : 07/05/2010
-* Description        : Descriptor Header for Custom HID Demo
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-/* Define to prevent recursive inclusion -------------------------------------*/
-#ifndef __USB_DESC_H
-#define __USB_DESC_H
-
-/* Includes ------------------------------------------------------------------*/
-/* Exported types ------------------------------------------------------------*/
-/* Exported constants --------------------------------------------------------*/
-/* Exported macro ------------------------------------------------------------*/
-/* Exported define -----------------------------------------------------------*/
-#define USB_DEVICE_DESCRIPTOR_TYPE              0x01
-#define USB_CONFIGURATION_DESCRIPTOR_TYPE       0x02
-#define USB_STRING_DESCRIPTOR_TYPE              0x03
-#define USB_INTERFACE_DESCRIPTOR_TYPE           0x04
-#define USB_ENDPOINT_DESCRIPTOR_TYPE            0x05
-
-#define HID_DESCRIPTOR_TYPE                     0x21
-#define PIOS_HID_SIZ_HID_DESC                  0x09
-#define PIOS_HID_OFF_HID_DESC                  0x12
-
-#define PIOS_HID_SIZ_DEVICE_DESC               18
-#define PIOS_HID_SIZ_CONFIG_DESC               41
-#define PIOS_HID_SIZ_REPORT_DESC               36
-#define PIOS_HID_SIZ_STRING_LANGID             4
-#define PIOS_HID_SIZ_STRING_VENDOR             28
-#define PIOS_HID_SIZ_STRING_PRODUCT            28
-#define PIOS_HID_SIZ_STRING_SERIAL             52 /* 96 bits, 12 bytes, 24 characters, 48 in unicode */
-
-#define STANDARD_ENDPOINT_DESC_SIZE             0x09
-
-/* Exported functions ------------------------------------------------------- */
-extern const uint8_t PIOS_HID_DeviceDescriptor[PIOS_HID_SIZ_DEVICE_DESC];
-extern const uint8_t PIOS_HID_ConfigDescriptor[PIOS_HID_SIZ_CONFIG_DESC];
-extern const uint8_t PIOS_HID_ReportDescriptor[PIOS_HID_SIZ_REPORT_DESC];
-extern const uint8_t PIOS_HID_StringLangID[PIOS_HID_SIZ_STRING_LANGID];
-extern const uint8_t PIOS_HID_StringVendor[PIOS_HID_SIZ_STRING_VENDOR];
-extern const uint8_t PIOS_HID_StringProduct[PIOS_HID_SIZ_STRING_PRODUCT];
-extern uint8_t PIOS_HID_StringSerial[PIOS_HID_SIZ_STRING_SERIAL];
-
-#endif /* __USB_DESC_H */
-
-/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/

flight/CopterControl/System/pios_board.c

@@ -27,6 +27,15 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+/* Pull in the board-specific static HW definitions.
+ * Including .c files is a bit ugly but this allows all of
+ * the HW definitions to be const and static to limit their
+ * scope.  
+ *
+ * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
+ */
+#include "board_hw_defs.c"
+
 #include <pios.h>
 #include <openpilot.h>
 #include <uavobjectsinit.h>
@@ -34,1027 +43,6 @@
 #include <manualcontrolsettings.h>
 #include <gcsreceiver.h>
 
-#if defined(PIOS_INCLUDE_SPI)
-
-#include <pios_spi_priv.h>
-
-/* Flash/Accel Interface
- * 
- * NOTE: Leave this declared as const data so that it ends up in the 
- * .rodata section (ie. Flash) rather than in the .bss section (RAM).
- */
-void PIOS_SPI_flash_accel_irq_handler(void);
-void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler")));
-void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler")));
-static const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
-  .regs   = SPI2,
-  .init   = {
-    .SPI_Mode              = SPI_Mode_Master,
-    .SPI_Direction         = SPI_Direction_2Lines_FullDuplex,
-    .SPI_DataSize          = SPI_DataSize_8b,
-    .SPI_NSS               = SPI_NSS_Soft,
-    .SPI_FirstBit          = SPI_FirstBit_MSB,
-    .SPI_CRCPolynomial     = 7,
-    .SPI_CPOL              = SPI_CPOL_High,
-    .SPI_CPHA              = SPI_CPHA_2Edge,
-    .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, 
-  },
-  .use_crc = FALSE,
-  .dma = {
-    .ahb_clk  = RCC_AHBPeriph_DMA1,
-    
-    .irq = {
-      .flags   = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
-      .init    = {
-	.NVIC_IRQChannel                   = DMA1_Channel4_IRQn,
-	.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-	.NVIC_IRQChannelSubPriority        = 0,
-	.NVIC_IRQChannelCmd                = ENABLE,
-      },
-    },
-
-    .rx = {
-      .channel = DMA1_Channel4,
-      .init    = {
-	.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
-	.DMA_DIR                = DMA_DIR_PeripheralSRC,
-	.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-	.DMA_MemoryInc          = DMA_MemoryInc_Enable,
-	.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-	.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-	.DMA_Mode               = DMA_Mode_Normal,
-	.DMA_Priority           = DMA_Priority_High,
-	.DMA_M2M                = DMA_M2M_Disable,
-      },
-    },
-    .tx = {
-      .channel = DMA1_Channel5,
-      .init    = {
-	.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
-	.DMA_DIR                = DMA_DIR_PeripheralDST,
-	.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-	.DMA_MemoryInc          = DMA_MemoryInc_Enable,
-	.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-	.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-	.DMA_Mode               = DMA_Mode_Normal,
-	.DMA_Priority           = DMA_Priority_High,
-	.DMA_M2M                = DMA_M2M_Disable,
-      },
-    },
-  },
-  .ssel = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_12,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_Out_PP,
-    },
-  },
-  .sclk = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_13,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_PP,
-    },
-  },
-  .miso = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_14,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-    },
-  },
-  .mosi = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_15,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_PP,
-    },
-  },
-};
-
-static uint32_t pios_spi_flash_accel_id;
-void PIOS_SPI_flash_accel_irq_handler(void)
-{
-  /* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id);
-}
-
-#endif	/* PIOS_INCLUDE_SPI */
-
-/*
- * ADC system
- */
-#include "pios_adc_priv.h"
-extern void PIOS_ADC_handler(void);
-void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
-// Remap the ADC DMA handler to this one
-static const struct pios_adc_cfg pios_adc_cfg = {
-	.dma = {
-		.ahb_clk  = RCC_AHBPeriph_DMA1,
-		.irq = {
-			.flags   = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
-			.init    = {
-				.NVIC_IRQChannel                   = DMA1_Channel1_IRQn,
-				.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-				.NVIC_IRQChannelSubPriority        = 0,
-				.NVIC_IRQChannelCmd                = ENABLE,
-			},
-		},
-		.rx = {
-			.channel = DMA1_Channel1,
-			.init    = {
-				.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR,
-				.DMA_DIR                = DMA_DIR_PeripheralSRC,
-				.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-				.DMA_MemoryInc          = DMA_MemoryInc_Enable,
-				.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
-				.DMA_MemoryDataSize     = DMA_MemoryDataSize_Word,
-				.DMA_Mode               = DMA_Mode_Circular,
-				.DMA_Priority           = DMA_Priority_High,
-				.DMA_M2M                = DMA_M2M_Disable,
-			},
-		}
-	}, 
-	.half_flag = DMA1_IT_HT1,
-	.full_flag = DMA1_IT_TC1,
-};
-
-struct pios_adc_dev pios_adc_devs[] = {
-	{
-		.cfg = &pios_adc_cfg,
-		.callback_function = NULL,
-	},
-};
-
-uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
-
-void PIOS_ADC_handler() {
-	PIOS_ADC_DMA_Handler();
-}
-
-#include "pios_tim_priv.h"
-
-static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = {
-	.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
-	.TIM_ClockDivision = TIM_CKD_DIV1,
-	.TIM_CounterMode = TIM_CounterMode_Up,
-	.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
-	.TIM_RepetitionCounter = 0x0000,
-};
-
-static const struct pios_tim_clock_cfg tim_1_cfg = {
-	.timer = TIM1,
-	.time_base_init = &tim_1_2_3_4_time_base,
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = TIM1_CC_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-};
-
-static const struct pios_tim_clock_cfg tim_2_cfg = {
-	.timer = TIM2,
-	.time_base_init = &tim_1_2_3_4_time_base,
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = TIM2_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-};
-
-static const struct pios_tim_clock_cfg tim_3_cfg = {
-	.timer = TIM3,
-	.time_base_init = &tim_1_2_3_4_time_base,
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = TIM3_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-};
-
-static const struct pios_tim_clock_cfg tim_4_cfg = {
-	.timer = TIM4,
-	.time_base_init = &tim_1_2_3_4_time_base,
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = TIM4_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-};
-
-static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
-	{
-		.timer = TIM4,
-		.timer_chan = TIM_Channel_1,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_6,
-				.GPIO_Mode  = GPIO_Mode_IPD,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM3,
-		.timer_chan = TIM_Channel_2,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_5,
-				.GPIO_Mode  = GPIO_Mode_IPD,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-		.remap = GPIO_PartialRemap_TIM3,
-	},
-	{
-		.timer = TIM3,
-		.timer_chan = TIM_Channel_3,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_0,
-				.GPIO_Mode  = GPIO_Mode_IPD,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	}, 
-	{
-		.timer = TIM3,
-		.timer_chan = TIM_Channel_4,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_1,
-				.GPIO_Mode  = GPIO_Mode_IPD,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	}, 
-	{ 
-		.timer = TIM2,
-		.timer_chan = TIM_Channel_1,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_0,
-				.GPIO_Mode  = GPIO_Mode_IPD,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},  	
-	{
-		.timer = TIM2,
-		.timer_chan = TIM_Channel_2,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_1,
-				.GPIO_Mode  = GPIO_Mode_IPD,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	}, 		
-};
-
-static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
-	{
-		.timer = TIM4,
-		.timer_chan = TIM_Channel_4,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_9,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM4,
-		.timer_chan = TIM_Channel_3,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_8,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	}, 
-	{
-		.timer = TIM4,
-		.timer_chan = TIM_Channel_2,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_7,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	}, 
-	{
-		.timer = TIM1,
-		.timer_chan = TIM_Channel_1,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_8,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	}, 
-	{
-		.timer = TIM3,
-		.timer_chan = TIM_Channel_1,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_4,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-		.remap = GPIO_PartialRemap_TIM3,
-	},  	
-	{
-		.timer = TIM2,
-		.timer_chan = TIM_Channel_3,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_2,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-};
-
-
-static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = {
-	{
-		.timer = TIM4,
-		.timer_chan = TIM_Channel_4,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_9,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM4,
-		.timer_chan = TIM_Channel_3,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_8,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM4,
-		.timer_chan = TIM_Channel_2,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_7,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM1,
-		.timer_chan = TIM_Channel_1,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_8,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM3,
-		.timer_chan = TIM_Channel_1,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_4,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-		.remap = GPIO_PartialRemap_TIM3,
-	},
-	{
-		.timer = TIM2,
-		.timer_chan = TIM_Channel_3,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_2,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-
-	// Receiver port pins
-	// S3-S6 inputs are used as outputs in this case
-	{
-		.timer = TIM3,
-		.timer_chan = TIM_Channel_3,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_0,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM3,
-		.timer_chan = TIM_Channel_4,
-		.pin = {
-			.gpio = GPIOB,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_1,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM2,
-		.timer_chan = TIM_Channel_1,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_0,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-	{
-		.timer = TIM2,
-		.timer_chan = TIM_Channel_2,
-		.pin = {
-			.gpio = GPIOA,
-			.init = {
-				.GPIO_Pin   = GPIO_Pin_1,
-				.GPIO_Mode  = GPIO_Mode_AF_PP,
-				.GPIO_Speed = GPIO_Speed_2MHz,
-			},
-		},
-	},
-};
-#if defined(PIOS_INCLUDE_USART)
-
-#include "pios_usart_priv.h"
-
-#if defined(PIOS_INCLUDE_TELEMETRY_RF)
-/*
- * Telemetry USART
- */
-static const struct pios_usart_cfg pios_usart_telem_main_cfg = {
-  .regs  = USART1,
-  .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                   = 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_IPU,
-    },
-  },
-  .tx   = {
-    .gpio = GPIOA,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_9,
-      .GPIO_Speed = GPIO_Speed_2MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_PP,
-    },
-  },
-};
-
-static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = {
-  .regs  = USART3,
-  .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                   = USART3_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-  .rx   = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_11,
-      .GPIO_Speed = GPIO_Speed_2MHz,
-      .GPIO_Mode  = GPIO_Mode_IPU,
-    },
-  },
-  .tx   = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_10,
-      .GPIO_Speed = GPIO_Speed_2MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_PP,
-    },
-  },
-};
-#endif /* PIOS_INCLUDE_TELEMETRY_RF */
-
-#if defined(PIOS_INCLUDE_GPS)
-/*
- * GPS USART
- */
-static const struct pios_usart_cfg pios_usart_gps_main_cfg = {
-  .regs = USART1,
-  .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                   = 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_IPU,
-    },
-  },
-  .tx   = {
-    .gpio = GPIOA,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_9,
-      .GPIO_Speed = GPIO_Speed_2MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_PP,
-    },
-  },
-};
-
-static const struct pios_usart_cfg pios_usart_gps_flexi_cfg = {
-  .regs = USART3,
-  .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                   = USART3_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-  .rx   = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_11,
-      .GPIO_Speed = GPIO_Speed_2MHz,
-      .GPIO_Mode  = GPIO_Mode_IPU,
-    },
-  },
-  .tx   = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_10,
-      .GPIO_Speed = GPIO_Speed_2MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_PP,
-    },
-  },
-};
-#endif	/* PIOS_INCLUDE_GPS */
-
-#if defined(PIOS_INCLUDE_DSM)
-/*
- * Spektrum/JR DSM USART
- */
-#include <pios_dsm_priv.h>
-
-static const struct pios_usart_cfg pios_usart_dsm_main_cfg = {
-	.regs = USART1,
-	.init = {
-		.USART_BaudRate            = 115200,
-		.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,
-	},
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = USART1_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		  },
-	},
-	.rx = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_10,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_IPU,
-		},
-	},
-	.tx = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_9,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-		},
-	},
-};
-
-static const struct pios_dsm_cfg pios_dsm_main_cfg = {
-	.bind = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_10,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_Out_PP,
-		},
-	},
-};
-
-static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = {
-	.regs = USART3,
-	.init = {
-		.USART_BaudRate            = 115200,
-		.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,
-	},
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = USART3_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		  },
-	},
-	.rx = {
-		.gpio = GPIOB,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_11,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_IPU,
-		},
-	},
-	.tx = {
-		.gpio = GPIOB,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_10,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-		},
-	},
-};
-
-static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
-	.bind = {
-		.gpio = GPIOB,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_11,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_Out_PP,
-		},
-	},
-};
-
-#endif	/* PIOS_INCLUDE_DSM */
-
-#if defined(PIOS_INCLUDE_SBUS)
-/*
- * S.Bus USART
- */
-#include <pios_sbus_priv.h>
-
-static const struct pios_usart_cfg pios_usart_sbus_main_cfg = {
-	.regs = USART1,
-	.init = {
-		.USART_BaudRate            = 100000,
-		.USART_WordLength          = USART_WordLength_8b,
-		.USART_Parity              = USART_Parity_Even,
-		.USART_StopBits            = USART_StopBits_2,
-		.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
-		.USART_Mode                = USART_Mode_Rx,
-	},
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = USART1_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		  },
-	},
-	.rx = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_10,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_IPU,
-		},
-	},
-	.tx = {
-		.gpio = GPIOA,
-		.init = {
-			.GPIO_Pin   = GPIO_Pin_9,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-		},
-	},
-};
-
-static const struct pios_sbus_cfg pios_sbus_cfg = {
-	/* Inverter configuration */
-	.inv = {
-		.gpio = GPIOB,
-		.init = {
-			.GPIO_Pin = GPIO_Pin_2,
-			.GPIO_Mode = GPIO_Mode_Out_PP,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-		},
-	},
-	.gpio_clk_func = RCC_APB2PeriphClockCmd,
-	.gpio_clk_periph = RCC_APB2Periph_GPIOB,
-	.gpio_inv_enable = Bit_SET,
-};
-
-#endif	/* PIOS_INCLUDE_SBUS */
-
-#endif  /* PIOS_INCLUDE_USART */
-
-#if defined(PIOS_INCLUDE_COM)
-
-#include "pios_com_priv.h"
-
-#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
-#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
-
-#define PIOS_COM_GPS_RX_BUF_LEN 32
-
-#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
-#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
-
-#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_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
-static const struct pios_rtc_cfg pios_rtc_main_cfg = {
-	.clksrc = RCC_RTCCLKSource_HSE_Div128,
-	.prescaler = 100,
-	.irq = {
-		.init = {
-			.NVIC_IRQChannel                   = RTC_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		  },
-	},
-};
-
-void PIOS_RTC_IRQ_Handler (void)
-{
-	PIOS_RTC_irq_handler ();
-}
-
-#endif
-
-/* 
- * Servo outputs 
- */
-#include <pios_servo_priv.h>
-
-const struct pios_servo_cfg pios_servo_cfg = {
-	.tim_oc_init = {
-		.TIM_OCMode = TIM_OCMode_PWM1,
-		.TIM_OutputState = TIM_OutputState_Enable,
-		.TIM_OutputNState = TIM_OutputNState_Disable,
-		.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
-		.TIM_OCPolarity = TIM_OCPolarity_High,
-		.TIM_OCNPolarity = TIM_OCPolarity_High,
-		.TIM_OCIdleState = TIM_OCIdleState_Reset,
-		.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
-	},
-	.channels = pios_tim_servoport_all_pins,
-	.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
-};
-
-const struct pios_servo_cfg pios_servo_rcvr_cfg = {
-	.tim_oc_init = {
-		.TIM_OCMode = TIM_OCMode_PWM1,
-		.TIM_OutputState = TIM_OutputState_Enable,
-		.TIM_OutputNState = TIM_OutputNState_Disable,
-		.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
-		.TIM_OCPolarity = TIM_OCPolarity_High,
-		.TIM_OCNPolarity = TIM_OCPolarity_High,
-		.TIM_OCIdleState = TIM_OCIdleState_Reset,
-		.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
-	},
-	.channels = pios_tim_servoport_rcvrport_pins,
-	.num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins),
-};
-
-
-/*
- * PPM Inputs
- */
-#if defined(PIOS_INCLUDE_PPM)
-#include <pios_ppm_priv.h>
-
-const struct pios_ppm_cfg pios_ppm_cfg = {
-	.tim_ic_init = {
-		.TIM_ICPolarity = TIM_ICPolarity_Rising,
-		.TIM_ICSelection = TIM_ICSelection_DirectTI,
-		.TIM_ICPrescaler = TIM_ICPSC_DIV1,
-		.TIM_ICFilter = 0x0,
-	},
-	/* Use only the first channel for ppm */
-	.channels = &pios_tim_rcvrport_all_channels[0],
-	.num_channels = 1,
-};
-
-#endif	/* PIOS_INCLUDE_PPM */
-
-/* 
- * PWM Inputs 
- */
-#if defined(PIOS_INCLUDE_PWM)
-#include <pios_pwm_priv.h>
-
-const struct pios_pwm_cfg pios_pwm_cfg = {
-	.tim_ic_init = {
-		.TIM_ICPolarity = TIM_ICPolarity_Rising,
-		.TIM_ICSelection = TIM_ICSelection_DirectTI,
-		.TIM_ICPrescaler = TIM_ICPSC_DIV1,
-		.TIM_ICFilter = 0x0,
-	},
-	.channels = pios_tim_rcvrport_all_channels,
-	.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
-};
-#endif
-
-#if defined(PIOS_INCLUDE_I2C)
-
-#include <pios_i2c_priv.h>
-
-/*
- * I2C Adapters
- */
-
-void PIOS_I2C_main_adapter_ev_irq_handler(void);
-void PIOS_I2C_main_adapter_er_irq_handler(void);
-void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
-void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
-
-static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
-  .regs = I2C2,
-  .init = {
-    .I2C_Mode                = I2C_Mode_I2C,
-    .I2C_OwnAddress1         = 0,
-    .I2C_Ack                 = I2C_Ack_Enable,
-    .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-    .I2C_DutyCycle           = I2C_DutyCycle_2,
-    .I2C_ClockSpeed          = 400000,	/* bits/s */
-  },
-  .transfer_timeout_ms = 50,
-  .scl = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_10,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_OD,
-    },
-  },
-  .sda = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_11,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_OD,
-    },
-  },
-  .event = {
-    .flags   = 0,		/* FIXME: check this */
-    .init = {
-      .NVIC_IRQChannel                   = I2C2_EV_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-  .error = {
-    .flags   = 0,		/* FIXME: check this */
-    .init = {
-      .NVIC_IRQChannel                   = I2C2_ER_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-};
-
-uint32_t pios_i2c_main_adapter_id;
-void PIOS_I2C_main_adapter_ev_irq_handler(void)
-{
-  /* Call into the generic code to handle the IRQ for this specific device */
-  PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id);
-}
-
-void PIOS_I2C_main_adapter_er_irq_handler(void)
-{
-  /* Call into the generic code to handle the IRQ for this specific device */
-  PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id);
-}
-
-#endif /* PIOS_INCLUDE_I2C */
-
-#if defined(PIOS_INCLUDE_GCSRCVR)
-#include "pios_gcsrcvr_priv.h"
-#endif	/* PIOS_INCLUDE_GCSRCVR */
-
-#if defined(PIOS_INCLUDE_RCVR)
-#include "pios_rcvr_priv.h"
 
 /* One slot per selectable receiver group.
  *  eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
@@ -1062,63 +50,183 @@ void PIOS_I2C_main_adapter_er_irq_handler(void)
  */
 uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 
-#endif /* PIOS_INCLUDE_RCVR */
+#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32
+#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12
 
-#if defined(PIOS_INCLUDE_USB_HID)
+#define PIOS_COM_GPS_RX_BUF_LEN 32
-#include "pios_usb_hid_priv.h"
 
-static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
+#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65
-  .irq = {
+#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65
-    .init    = {
-      .NVIC_IRQChannel                   = USB_LP_CAN1_RX0_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-};
-#endif	/* PIOS_INCLUDE_USB_HID */
 
-extern const struct pios_com_driver pios_usb_com_driver;
+#define PIOS_COM_BRIDGE_RX_BUF_LEN 65
+#define PIOS_COM_BRIDGE_TX_BUF_LEN 12
 
 uint32_t pios_com_telem_rf_id;
 uint32_t pios_com_telem_usb_id;
+uint32_t pios_com_vcp_id;
 uint32_t pios_com_gps_id;
+uint32_t pios_com_bridge_id;
 
+/**
+ * Configuration for MPU6000 chip
+ */
+#if defined(PIOS_INCLUDE_MPU6000)
+#include "pios_mpu6000.h"
+static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
+	.vector = PIOS_MPU6000_IRQHandler,
+	.line = EXTI_Line3,
+	.pin = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_3,
+			.GPIO_Speed = GPIO_Speed_10MHz,
+			.GPIO_Mode  = GPIO_Mode_IN_FLOATING,
+		},
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = EXTI3_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.exti = {
+		.init = {
+			.EXTI_Line = EXTI_Line3, // matches above GPIO pin
+			.EXTI_Mode = EXTI_Mode_Interrupt,
+			.EXTI_Trigger = EXTI_Trigger_Rising,
+			.EXTI_LineCmd = ENABLE,
+		},
+	},
+};
+
+static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
+	.exti_cfg = &pios_exti_mpu6000_cfg,
+	.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
+	// Clock at 8 khz, downsampled by 8 for 1khz
+	.Smpl_rate_div = 15,
+	.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
+	.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
+	.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN,
+	.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
+	.accel_range = PIOS_MPU6000_ACCEL_8G,
+	.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
+	.filter = PIOS_MPU6000_LOWPASS_256_HZ
+};
+#endif /* PIOS_INCLUDE_MPU6000 */
+
+static const struct flashfs_cfg flashfs_w25x_cfg = {
+	.table_magic = 0x85FB3C35,
+	.obj_magic = 0x3015AE71,
+	.obj_table_start = 0x00000010,
+	.obj_table_end = 0x00001000,
+	.sector_size = 0x00001000,
+	.chip_size = 0x00080000,
+};
+
+static const struct pios_flash_jedec_cfg flash_w25x_cfg = {
+	.sector_erase = 0x20,
+	.chip_erase = 0x60
+};
+
+static const struct flashfs_cfg flashfs_m25p_cfg = {
+	.table_magic = 0x85FB3D35,
+	.obj_magic = 0x3015A371,
+	.obj_table_start = 0x00000010,
+	.obj_table_end = 0x00010000,
+	.sector_size = 0x00010000,
+	.chip_size = 0x00200000,
+};
+
+static const struct pios_flash_jedec_cfg flash_m25p_cfg = {
+	.sector_erase = 0xD8,
+	.chip_erase = 0xC7
+};
+#include <pios_board_info.h>
 /**
  * PIOS_Board_Init()
  * initializes all the core subsystems on this specific hardware
  * called from System/openpilot.c
  */
+int32_t init_test;
 void PIOS_Board_Init(void) {
 
 	/* Delay system */
 	PIOS_DELAY_Init();
 
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	
+#if defined(PIOS_INCLUDE_LED)
+	const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
+	PIOS_Assert(led_cfg);
+	PIOS_LED_Init(led_cfg);
+#endif	/* PIOS_INCLUDE_LED */
+
+#if defined(PIOS_INCLUDE_SPI)
 	/* Set up the SPI interface to the serial flash */
-	if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg)) {
+
-		PIOS_Assert(0);
+	switch(bdinfo->board_rev) {
+		case BOARD_REVISION_CC:
+			if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) {
+				PIOS_Assert(0);
+			}
+			break;
+		case BOARD_REVISION_CC3D:
+			if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) {
+				PIOS_Assert(0);
+			}
+			break;
+		default:
+			PIOS_Assert(0);
 	}
 
-	PIOS_Flash_W25X_Init(pios_spi_flash_accel_id);	
+#endif
-	PIOS_ADXL345_Attach(pios_spi_flash_accel_id);
 
-	PIOS_FLASHFS_Init();
+	switch(bdinfo->board_rev) {
+		case BOARD_REVISION_CC:
+			PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg);	
+			PIOS_FLASHFS_Init(&flashfs_w25x_cfg);
+			break;
+		case BOARD_REVISION_CC3D:
+			PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 0, &flash_m25p_cfg);	
+			PIOS_FLASHFS_Init(&flashfs_m25p_cfg);
+			break;
+		default:
+			PIOS_DEBUG_Assert(0);
+	}
 
 	/* Initialize UAVObject libraries */
 	EventDispatcherInitialize();
 	UAVObjInitialize();
 
-	HwSettingsInitialize();
-	
 #if defined(PIOS_INCLUDE_RTC)
 	/* Initialize the real-time clock and its associated tick */
 	PIOS_RTC_Init(&pios_rtc_main_cfg);
 #endif
 
+	HwSettingsInitialize();
+
+#ifndef ERASE_FLASH
+	/* Initialize watchdog as early as possible to catch faults during init */
+	PIOS_WDG_Init();
+#endif
+
 	/* Initialize the alarms library */
 	AlarmsInitialize();
 
+	/* Check for repeated boot failures */
+	PIOS_IAP_Init();
+	uint16_t boot_count = PIOS_IAP_ReadBootCount();
+	if (boot_count < 3) {
+		PIOS_IAP_WriteBootCount(++boot_count);
+		AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT);
+	} else {
+		/* Too many failed boot attempts, force hwsettings to defaults */
+		HwSettingsSetDefaults(HwSettingsHandle(), 0);
+		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
+	}
+
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
@@ -1128,6 +236,134 @@ void PIOS_Board_Init(void) {
 	PIOS_TIM_InitClock(&tim_3_cfg);
 	PIOS_TIM_InitClock(&tim_4_cfg);
 
+#if defined(PIOS_INCLUDE_USB)
+	/* Initialize board specific USB data */
+	PIOS_USB_BOARD_DATA_Init();
+
+
+	/* Flags to determine if various USB interfaces are advertised */
+	bool usb_hid_present = false;
+	bool usb_cdc_present = false;
+
+#if defined(PIOS_INCLUDE_USB_CDC)
+	if (PIOS_USB_DESC_HID_CDC_Init()) {
+		PIOS_Assert(0);
+	}
+	usb_hid_present = true;
+	usb_cdc_present = true;
+#else
+	if (PIOS_USB_DESC_HID_ONLY_Init()) {
+		PIOS_Assert(0);
+	}
+	usb_hid_present = true;
+#endif
+
+	uint32_t pios_usb_id;
+	
+	switch(bdinfo->board_rev) {
+		case BOARD_REVISION_CC:
+			PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
+			break;
+		case BOARD_REVISION_CC3D:
+			PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
+			break;
+		default:
+			PIOS_Assert(0);
+	}
+
+#if defined(PIOS_INCLUDE_USB_CDC)
+
+	uint8_t hwsettings_usb_vcpport;
+	/* Configure the USB VCP port */
+	HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport);
+
+	if (!usb_cdc_present) {
+		/* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */
+		hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED;
+	}
+
+	switch (hwsettings_usb_vcpport) {
+	case HWSETTINGS_USB_VCPPORT_DISABLED:
+		break;
+	case HWSETTINGS_USB_VCPPORT_USBTELEMETRY:
+#if defined(PIOS_INCLUDE_COM)
+		{
+			uint32_t pios_usb_cdc_id;
+			if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
+				PIOS_Assert(0);
+			}
+			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
+			PIOS_Assert(rx_buffer);
+			PIOS_Assert(tx_buffer);
+			if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
+						rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
+						tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
+				PIOS_Assert(0);
+			}
+		}
+#endif	/* PIOS_INCLUDE_COM */
+		break;
+	case HWSETTINGS_USB_VCPPORT_COMBRIDGE:
+#if defined(PIOS_INCLUDE_COM)
+		{
+			uint32_t pios_usb_cdc_id;
+			if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
+				PIOS_Assert(0);
+			}
+			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
+			PIOS_Assert(rx_buffer);
+			PIOS_Assert(tx_buffer);
+			if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
+						rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
+						tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
+				PIOS_Assert(0);
+			}
+		}
+#endif	/* PIOS_INCLUDE_COM */
+		break;
+	}
+#endif	/* PIOS_INCLUDE_USB_CDC */
+
+#if defined(PIOS_INCLUDE_USB_HID)
+	/* Configure the usb HID port */
+	uint8_t hwsettings_usb_hidport;
+	HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport);
+
+	if (!usb_hid_present) {
+		/* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */
+		hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED;
+	}
+
+	switch (hwsettings_usb_hidport) {
+	case HWSETTINGS_USB_HIDPORT_DISABLED:
+		break;
+	case HWSETTINGS_USB_HIDPORT_USBTELEMETRY:
+#if defined(PIOS_INCLUDE_COM)
+		{
+			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);
+			}
+			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
+			PIOS_Assert(rx_buffer);
+			PIOS_Assert(tx_buffer);
+			if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id,
+						rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
+						tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
+				PIOS_Assert(0);
+			}
+		}
+#endif	/* PIOS_INCLUDE_COM */
+		break;
+	}
+
+#endif	/* PIOS_INCLUDE_USB_HID */
+
+#endif	/* PIOS_INCLUDE_USB */
+
 	/* Configure the main IO port */
 	uint8_t hwsettings_DSMxBind;
 	HwSettingsDSMxBindGet(&hwsettings_DSMxBind);
@@ -1140,8 +376,8 @@ void PIOS_Board_Init(void) {
 	case HWSETTINGS_CC_MAINPORT_TELEMETRY:
 #if defined(PIOS_INCLUDE_TELEMETRY_RF)
 		{
-			uint32_t pios_usart_telem_rf_id;
+			uint32_t pios_usart_generic_id;
-			if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) {
+			if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) {
 				PIOS_Assert(0);
 			}
 
@@ -1149,7 +385,7 @@ void PIOS_Board_Init(void) {
 			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
 			PIOS_Assert(rx_buffer);
 			PIOS_Assert(tx_buffer);
-			if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
+			if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id,
 					  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
 					  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
 				PIOS_Assert(0);
@@ -1182,14 +418,14 @@ void PIOS_Board_Init(void) {
 	case HWSETTINGS_CC_MAINPORT_GPS:
 #if defined(PIOS_INCLUDE_GPS)
 		{
-			uint32_t pios_usart_gps_id;
+			uint32_t pios_usart_generic_id;
-			if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_main_cfg)) {
+			if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) {
 				PIOS_Assert(0);
 			}
 
 			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
 			PIOS_Assert(rx_buffer);
-			if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
+			if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id,
 					  rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
 					  NULL, 0)) {
 				PIOS_Assert(0);
@@ -1242,6 +478,24 @@ void PIOS_Board_Init(void) {
 		break;
 	case HWSETTINGS_CC_MAINPORT_COMAUX:
 		break;
+	case HWSETTINGS_CC_MAINPORT_COMBRIDGE:
+		{
+			uint32_t pios_usart_generic_id;
+			if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) {
+				PIOS_Assert(0);
+			}
+
+			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
+			PIOS_Assert(rx_buffer);
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
+			PIOS_Assert(tx_buffer);
+			if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id,
+						rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
+						tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
+				PIOS_Assert(0);
+			}
+		}
+		break;
 	}
 
 	/* Configure the flexi port */
@@ -1254,15 +508,15 @@ void PIOS_Board_Init(void) {
 	case HWSETTINGS_CC_FLEXIPORT_TELEMETRY:
 #if defined(PIOS_INCLUDE_TELEMETRY_RF)
 		{
-			uint32_t pios_usart_telem_rf_id;
+			uint32_t pios_usart_generic_id;
-			if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_flexi_cfg)) {
+			if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) {
 				PIOS_Assert(0);
 			}
 			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
 			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
 			PIOS_Assert(rx_buffer);
 			PIOS_Assert(tx_buffer);
-			if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
+			if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id,
   					  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
 					  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
 				PIOS_Assert(0);
@@ -1270,16 +524,34 @@ void PIOS_Board_Init(void) {
 		}
 #endif /* PIOS_INCLUDE_TELEMETRY_RF */
 		break;
+	case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE:
+		{
+			uint32_t pios_usart_generic_id;
+			if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) {
+				PIOS_Assert(0);
+			}
+
+			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
+			PIOS_Assert(rx_buffer);
+			PIOS_Assert(tx_buffer);
+			if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id,
+						rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
+						tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
+				PIOS_Assert(0);
+			}
+		}
+		break;
 	case HWSETTINGS_CC_FLEXIPORT_GPS:
 #if defined(PIOS_INCLUDE_GPS)
 		{
-			uint32_t pios_usart_gps_id;
+			uint32_t pios_usart_generic_id;
-			if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_flexi_cfg)) {
+			if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) {
 				PIOS_Assert(0);
 			}
 			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
 			PIOS_Assert(rx_buffer);
-			if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
+			if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id,
 					  rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
 					  NULL, 0)) {
 				PIOS_Assert(0);
@@ -1335,7 +607,7 @@ void PIOS_Board_Init(void) {
 	case HWSETTINGS_CC_FLEXIPORT_I2C:
 #if defined(PIOS_INCLUDE_I2C)
 		{
-			if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
+			if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) {
 				PIOS_Assert(0);
 			}
 		}
@@ -1383,15 +655,16 @@ void PIOS_Board_Init(void) {
 
 #if defined(PIOS_INCLUDE_GCSRCVR)
 	GCSReceiverInitialize();
-	PIOS_GCSRCVR_Init();
+	uint32_t pios_gcsrcvr_id;
+	PIOS_GCSRCVR_Init(&pios_gcsrcvr_id);
 	uint32_t pios_gcsrcvr_rcvr_id;
-	if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) {
+	if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) {
 		PIOS_Assert(0);
 	}
 	pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;
 #endif	/* PIOS_INCLUDE_GCSRCVR */
 
-	/* Remap AFIO pin */
+	/* Remap AFIO pin for PB4 (Servo 5 Out)*/
 	GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
 
 #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
@@ -1409,30 +682,39 @@ void PIOS_Board_Init(void) {
 #else
 	PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
 #endif	/* PIOS_DEBUG_ENABLE_DEBUG_PINS */
-	
+
-	PIOS_ADC_Init();
+	switch(bdinfo->board_rev) {
+		case BOARD_REVISION_CC:
+			// Revision 1 with invensense gyros, start the ADC
+#if defined(PIOS_INCLUDE_ADC)
+			PIOS_ADC_Init(&pios_adc_cfg);
+#endif
+#if defined(PIOS_INCLUDE_ADXL345)
+			PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0);
+#endif
+			break;
+		case BOARD_REVISION_CC3D:
+			// Revision 2 with L3GD20 gyros, start a SPI interface and connect to it
+			GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
+
+#if defined(PIOS_INCLUDE_MPU6000)
+			// Set up the SPI interface to the serial flash 
+			if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
+				PIOS_Assert(0);
+			}
+			PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg);
+			init_test = PIOS_MPU6000_Test();
+#endif /* PIOS_INCLUDE_MPU6000 */
+
+			break;
+		default:
+			PIOS_Assert(0);
+	}
+
 	PIOS_GPIO_Init();
 
-#if defined(PIOS_INCLUDE_USB_HID)
+	/* Make sure we have at least one telemetry link configured or else fail initialization */
-	uint32_t pios_usb_hid_id;
+	PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
-	PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
-#if defined(PIOS_INCLUDE_COM)
-	uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
-	uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
-	PIOS_Assert(rx_buffer);
-	PIOS_Assert(tx_buffer);
-	if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
-			  rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
-			  tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
-		PIOS_Assert(0);
-	}
-#endif	/* PIOS_INCLUDE_COM */
-#endif	/* PIOS_INCLUDE_USB_HID */
-
-	PIOS_IAP_Init();
-#ifndef ERASE_FLASH
-	PIOS_WDG_Init();
-#endif
 }
 
 /**

flight/CopterControl/System/pios_usb_board_data.c

@@ -0,0 +1,101 @@
+/**
+ ******************************************************************************
+ * @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_* */
+#include "pios_usb_util.h"	 /* PIOS_USB_UTIL_AsciiToUtf8 */
+
+static const uint8_t usb_product_id[28] = {
+	sizeof(usb_product_id),
+	USB_DESC_TYPE_STRING,
+	'C', 0,
+	'o', 0,
+	'p', 0,
+	't', 0,
+	'e', 0,
+	'r', 0,
+	'C', 0,
+	'o', 0,
+	'n', 0,
+	't', 0,
+	'r', 0,
+	'o', 0,
+	'l', 0,
+};
+
+static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = {
+	sizeof(usb_serial_number),
+	USB_DESC_TYPE_STRING,
+};
+
+static const struct usb_string_langid usb_lang_id = {
+	.bLength = sizeof(usb_lang_id),
+	.bDescriptorType = USB_DESC_TYPE_STRING,
+	.bLangID = htousbs(USB_LANGID_ENGLISH_US),
+};
+
+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[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
+	PIOS_SYS_SerialNumberGet((char *)sn);
+
+	/* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
+	uint8_t * utf8 = &(usb_serial_number[2]);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
+	utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1);
+
+	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;
+}

flight/CopterControl/System/pios_usb_hid_desc.c

@@ -1,224 +0,0 @@
-/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
-* File Name          : usb_desc.c
-* Author             : MCD Application Team
-* Version            : V3.2.1
-* Date               : 07/05/2010
-* Description        : Descriptors for Custom HID Demo
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#include "usb_lib.h"
-#include "pios_usb.h"
-#include "pios_usb_hid.h"
-#include "pios_usb_hid_desc.h"
-
-// *************************************************
-// USB Standard Device Descriptor
-
-const uint8_t PIOS_HID_DeviceDescriptor[PIOS_HID_SIZ_DEVICE_DESC] =
-  {
-    0x12,                       // bLength
-    USB_DEVICE_DESCRIPTOR_TYPE, // bDescriptorType
-    0x00,                       // bcdUSB
-    0x02,
-    0x00,                       // bDeviceClass
-    0x00,                       // bDeviceSubClass
-    0x00,                       // bDeviceProtocol
-    0x40,                       // bMaxPacketSize40
-    (uint8_t)((PIOS_USB_VENDOR_ID) & 0xff),   // idVendor
-    (uint8_t)((PIOS_USB_VENDOR_ID) >> 8),
-    (uint8_t)((PIOS_USB_PRODUCT_ID) & 0xff),  // idProduct
-    (uint8_t)((PIOS_USB_PRODUCT_ID) >> 8),
-    (uint8_t)((PIOS_USB_VERSION_ID) & 0xff),  // bcdDevice
-    (uint8_t)((PIOS_USB_VERSION_ID) >> 8),
-    0x01,                        // Index of string descriptor describing manufacturer
-    0x02,                        // Index of string descriptor describing product
-    0x03,                        // Index of string descriptor describing the device serial number
-    0x01                         // bNumConfigurations
-  };
-
-// *************************************************
-// USB Configuration Descriptor
-//   All Descriptors (Configuration, Interface, Endpoint, Class, Vendor
-
-const uint8_t PIOS_HID_ConfigDescriptor[PIOS_HID_SIZ_CONFIG_DESC] =
-  {
-    0x09,         // bLength: Configuation Descriptor size
-    USB_CONFIGURATION_DESCRIPTOR_TYPE, // bDescriptorType: Configuration
-    PIOS_HID_SIZ_CONFIG_DESC,
-    // wTotalLength: Bytes returned
-    0x00,
-    0x01,         // bNumInterfaces: 1 interface
-    0x01,         // bConfigurationValue: Configuration value
-    0x00,         // iConfiguration: Index of string descriptor describing the configuration
-    0xC0,         // bmAttributes: Bus powered
-    0x7D,         // MaxPower 250 mA - needed to power the RF transmitter
-
-    // ************** Descriptor of Custom HID interface ****************
-    // 09
-    0x09,         // bLength: Interface Descriptor size
-    USB_INTERFACE_DESCRIPTOR_TYPE, // bDescriptorType: Interface descriptor type
-    0x00,         // bInterfaceNumber: Number of Interface
-    0x00,         // bAlternateSetting: Alternate setting
-    0x02,         // bNumEndpoints
-    0x03,         // bInterfaceClass: HID
-    0x00,         // bInterfaceSubClass : 1=BOOT, 0=no boot
-    0x00,         // nInterfaceProtocol : 0=none, 1=keyboard, 2=mouse
-    0,            // iInterface: Index of string descriptor
-
-    // ******************** Descriptor of Custom HID HID ********************
-    // 18
-    0x09,         // bLength: HID Descriptor size
-    HID_DESCRIPTOR_TYPE, // bDescriptorType: HID
-    0x10,         // bcdHID: HID Class Spec release number
-    0x01,
-    0x00,         // bCountryCode: Hardware target country
-    0x01,         // bNumDescriptors: Number of HID class descriptors to follow
-    0x22,         // bDescriptorType
-    PIOS_HID_SIZ_REPORT_DESC, // wItemLength: Total length of Report descriptor
-    0x00,
-
-    // ******************** Descriptor of Custom HID endpoints ******************
-    // 27
-    0x07,          // bLength: Endpoint Descriptor size
-    USB_ENDPOINT_DESCRIPTOR_TYPE, // bDescriptorType:
-
-    0x81,          // bEndpointAddress: Endpoint Address (IN)
-    0x03,          // bmAttributes: Interrupt endpoint
-    0x40,          // wMaxPacketSize: 2 Bytes max
-    0x00,
-    0x04,          // bInterval: Polling Interval in ms
-    // 34
-    	
-    0x07,	// bLength: Endpoint Descriptor size
-    USB_ENDPOINT_DESCRIPTOR_TYPE,	// bDescriptorType:
-			// Endpoint descriptor type
-    0x01,	// bEndpointAddress:
-			//	Endpoint Address (OUT)
-    0x03,	// bmAttributes: Interrupt endpoint
-    0x40,	// wMaxPacketSize: 2 Bytes max
-    0x00,
-    0x04,	// bInterval: Polling Interval in ms
-    // 41
-  };
-
-// *************************************************
-
- const uint8_t PIOS_HID_ReportDescriptor[PIOS_HID_SIZ_REPORT_DESC] =
-  {                    
-    0x06, 0x9c, 0xff,      // USAGE_PAGE (Vendor Page: 0xFF00)
-    0x09, 0x01,            // USAGE (Demo Kit)
-    0xa1, 0x01,            // COLLECTION (Application)
-    // 6
-    
-    // Data 1
-    0x85, 0x01,            //     REPORT_ID (1)
-    0x09, 0x02,            //     USAGE (LED 1)
-    0x15, 0x00,            //     LOGICAL_MINIMUM (0)
-    0x25, 0xff,            //     LOGICAL_MAXIMUM (255)
-    0x75, 0x08,            //     REPORT_SIZE (8)
-    0x95, PIOS_USB_HID_DATA_LENGTH+1,            //     REPORT_COUNT (1)
-    0x81, 0x83,            //     INPUT (Const,Var,Array)
-    // 20
-	  
-    // Data 1
-    0x85, 0x02,            //     REPORT_ID (2)
-    0x09, 0x03,            //     USAGE (LED 1)
-    0x15, 0x00,            //     LOGICAL_MINIMUM (0)
-    0x25, 0xff,            //     LOGICAL_MAXIMUM (255)
-    0x75, 0x08,            //     REPORT_SIZE (8)
-    0x95, PIOS_USB_HID_DATA_LENGTH+1,            //     REPORT_COUNT (1)
-    0x91, 0x82,            //     OUTPUT (Data,Var,Abs,Vol)
-    // 34
-	  
-    0xc0 	          //     END_COLLECTION
-  };
-
-// *************************************************
-// USB String Descriptors (optional)
-
-const uint8_t PIOS_HID_StringLangID[PIOS_HID_SIZ_STRING_LANGID] =
-  {
-    PIOS_HID_SIZ_STRING_LANGID,
-    USB_STRING_DESCRIPTOR_TYPE,
-    0x09, 0x08			// LangID = 0x0809: UK. English
-//    0x09, 0x04			// LangID = 0x0409: U.S. English
-  };
-
-const uint8_t PIOS_HID_StringVendor[PIOS_HID_SIZ_STRING_VENDOR] =
-  {
-    PIOS_HID_SIZ_STRING_VENDOR, // Size of Vendor string
-    USB_STRING_DESCRIPTOR_TYPE,  // bDescriptorType
-    // Manufacturer: "STMicroelectronics"
-    '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
-  };
-
-const uint8_t PIOS_HID_StringProduct[PIOS_HID_SIZ_STRING_PRODUCT] =
-  {
-    PIOS_HID_SIZ_STRING_PRODUCT,       // bLength
-    USB_STRING_DESCRIPTOR_TYPE,        // bDescriptorType
-    'C', 0,
-    'o', 0,
-    'p', 0,
-    't', 0,
-    'e', 0,
-    'r', 0,
-    'C', 0,
-    'o', 0,
-    'n', 0,
-    't', 0,
-	'r', 0,
-	'o', 0,
-	'l', 0
-  };
-
-uint8_t PIOS_HID_StringSerial[PIOS_HID_SIZ_STRING_SERIAL] =
-  {
-    PIOS_HID_SIZ_STRING_SERIAL,        // bLength
-    USB_STRING_DESCRIPTOR_TYPE,        // bDescriptorType
-    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
-  };
-
-// *************************************************

flight/CopterControl/System/taskmonitor.c

@@ -1,131 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @{
- * @addtogroup OpenPilotLibraries OpenPilot System Libraries
- * @{
- * @file       taskmonitor.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Task monitoring library
- * @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 "taskmonitor.h"
-
-// Private constants
-
-// Private types
-
-// Private variables
-static xSemaphoreHandle lock;
-static xTaskHandle handles[TASKINFO_RUNNING_NUMELEM];
-static uint32_t lastMonitorTime;
-
-// Private functions
-
-/**
- * Initialize library
- */
-int32_t TaskMonitorInitialize(void)
-{
-	lock = xSemaphoreCreateRecursiveMutex();
-	memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
-	lastMonitorTime = 0;
-#if defined(DIAGNOSTICS)
-	lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
-#endif
-	return 0;
-}
-
-/**
- * Register a task handle with the library
- */
-int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
-{
-	if (task < TASKINFO_RUNNING_NUMELEM)
-	{
-	    xSemaphoreTakeRecursive(lock, portMAX_DELAY);
-		handles[task] = handle;
-		xSemaphoreGiveRecursive(lock);
-		return 0;
-	}
-	else
-	{
-		return -1;
-	}
-}
-
-/**
- * Update the status of all tasks
- */
-void TaskMonitorUpdateAll(void)
-{
-#if defined(DIAGNOSTICS)
-	TaskInfoData data;
-	int n;
-	
-	// Lock
-	xSemaphoreTakeRecursive(lock, portMAX_DELAY);
-	
-#if ( configGENERATE_RUN_TIME_STATS == 1 )
-	uint32_t currentTime;
-	uint32_t deltaTime;
-	
-	/*
-	 * Calculate the amount of elapsed run time between the last time we
-	 * measured and now. Scale so that we can convert task run times
-	 * directly to percentages.
-	 */
-	currentTime = portGET_RUN_TIME_COUNTER_VALUE();
-	deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */
-	lastMonitorTime = currentTime;			
-#endif
-	
-	// Update all task information
-	for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)
-	{
-		if (handles[n] != 0)
-		{
-			data.Running[n] = TASKINFO_RUNNING_TRUE;
-#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
-			data.StackRemaining[n] = 10000;
-#else
-			data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;
-#if ( configGENERATE_RUN_TIME_STATS == 1 )
-			/* Generate run time stats */
-			data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;
-#endif
-#endif
-			
-		}
-		else
-		{
-			data.Running[n] = TASKINFO_RUNNING_FALSE;
-			data.StackRemaining[n] = 0;
-			data.RunningTime[n] = 0;
-		}
-	}
-	
-	// Update object
-	TaskInfoSet(&data);
-	
-	// Done
-	xSemaphoreGiveRecursive(lock);
-#endif
-}

flight/Doc/Doxygen/doxygen.cfg

@@ -574,7 +574,7 @@ WARN_LOGFILE           =
 # directories like "/usr/src/myproject". Separate the files or directories
 # with spaces.
 
-INPUT                  =  OpenPilot PiOS PiOS/STM32F10x
+INPUT                  =  OpenPilot PiOS PiOS/STM32F10x PiOS/STM32F2xx
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is

flight/EntireFlash/Makefile

@@ -0,0 +1,54 @@
+#####
+# Makefile for Entire Flash (EF) images
+#
+# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2012.
+#
+#
+# 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 := ef_$(BOARD_NAME)
+
+# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
+OUTDIR := $(TOP)/build/$(TARGET)
+
+.PHONY: bin
+bin: $(OUTDIR)/$(TARGET).bin
+
+FW_PAD = $(shell echo $$[$(FW_BANK_BASE)-$(BL_BANK_BASE)-$(BL_BANK_SIZE)])
+$(OUTDIR)/$(TARGET).pad:
+	$(V0) @echo $(MSG_PADDING) $@
+	$(V1) dd status=noxfer if=/dev/zero count=$(FW_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@
+
+BL_BIN = $(TOP)/build/bl_$(BOARD_NAME)/bl_$(BOARD_NAME).bin
+FW_BIN = $(TOP)/build/fw_$(BOARD_NAME)/fw_$(BOARD_NAME).bin
+$(OUTDIR)/$(TARGET).bin: $(BL_BIN) $(FW_BIN) $(OUTDIR)/$(TARGET).pad
+	$(V0) @echo $(MSG_FLASH_IMG) $@
+	$(V1) cat $(BL_BIN) $(OUTDIR)/$(TARGET).pad $(FW_BIN) > $@
+
+.PHONY: dfu
+dfu: $(OUTDIR)/$(TARGET).bin
+	$(V0) @echo " DFU RESCUE $<"
+	$(V1) ( \
+		sudo $(DFU_CMD) -l && \
+		sudo $(DFU_CMD) -d 0483:df11 -c 1 -i 0 -a 0 -D $< -s $(BL_BANK_BASE) ; \
+	      )
+

flight/INS/Makefile

@@ -1,429 +0,0 @@
- #####
- # Project: OpenPilot INS
- #
- #
- # Makefile for OpenPilot INS project
- #
- # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
- #
- #
- # 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 := fw_$(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 ?= YES
-
-# Set to YES when using Code Sourcery toolchain
-CODE_SOURCERY ?= YES
-
-ifeq ($(CODE_SOURCERY), YES)
-REMOVE_CMD = cs-rm
-else
-REMOVE_CMD = rm
-endif
-
-FLASH_TOOL = OPENOCD
-
-# Paths
-INS = ./
-INSINC = $(INS)/inc
-PIOS = ../PiOS
-PIOSINC = $(PIOS)/inc
-FLIGHTLIB = ../Libraries
-FLIGHTLIBINC = ../Libraries/inc
-PIOSSTM32F10X = $(PIOS)/STM32F10x
-PIOSCOMMON = $(PIOS)/Common
-PIOSBOARDS = $(PIOS)/Boards
-APPLIBDIR = $(PIOSSTM32F10X)/Libraries
-STMLIBDIR = $(APPLIBDIR)
-STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
-STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
-STMSPDSRCDIR = $(STMSPDDIR)/src
-STMSPDINCDIR = $(STMSPDDIR)/inc
-CMSISDIR  = $(STMLIBDIR)/CMSIS/Core/CM3
-OPDIR = ../OpenPilot
-OPUAVOBJ = ../UAVObjects
-OPUAVOBJINC = $(OPUAVOBJ)/inc
-OPSYSINC = $(OPDIR)/System/inc
-BOOT = ../Bootloaders/INS
-BOOTINC = $(BOOT)/inc
-
-OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
-
-# List C source files here. (C dependencies are automatically generated.)
-# use file-extension c for "c-only"-files
-
-## INS:
-SRC = ins.c
-SRC += pios_board.c
-#SRC += ins_timer.c
-#SRC += insgps13state.c
-SRC += $(FLIGHTLIB)/fifo_buffer.c
-#SRC += $(FLIGHTLIB)/ins_spi_comm.c
-#SRC += $(FLIGHTLIB)/ins_comm_objects.c
-#SRC += $(FLIGHTLIB)/CoordinateConversions.c
-#SRC += $(BOOT)/ins_spi_program_slave.c
-#SRC += $(BOOT)/ins_slave_test.c
-#SRC += $(BOOT)/ins_spi_program.c
-
-## PIOS Hardware (STM32F10x)
-SRC += $(PIOSSTM32F10X)/pios_sys.c
-SRC += $(PIOSSTM32F10X)/pios_led.c
-SRC += $(PIOSSTM32F10X)/pios_delay.c
-SRC += $(PIOSSTM32F10X)/pios_usart.c
-SRC += $(PIOSSTM32F10X)/pios_irq.c
-SRC += $(PIOSSTM32F10X)/pios_i2c.c
-SRC += $(PIOSSTM32F10X)/pios_debug.c
-SRC += $(PIOSSTM32F10X)/pios_gpio.c
-SRC += $(PIOSSTM32F10X)/pios_spi.c
-SRC += $(PIOSSTM32F10X)/pios_exti.c
-
-## PIOS Hardware (Common)
-SRC += $(PIOSCOMMON)/pios_com.c
-SRC += $(PIOSCOMMON)/printf-stdarg.c
-SRC += $(PIOSCOMMON)/pios_iap.c
-SRC += $(PIOSCOMMON)/pios_bma180.c
-SRC += $(PIOSCOMMON)/pios_hmc5883.c
-SRC += $(PIOSCOMMON)/pios_bmp085.c
-SRC += $(PIOSCOMMON)/pios_imu3000.c
-
-## CMSIS for STM32
-SRC += $(CMSISDIR)/core_cm3.c
-SRC += $(CMSISDIR)/system_stm32f10x.c
-
-## Used parts of the STM-Library
-SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
-SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
-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 =
-
-# 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 Assembler source files here.
-# Make them always end in a capital .S. Files ending in a lowercase .s
-# will not be considered source files but generated files (assembler
-# output from the compiler), and will be deleted upon "make clean"!
-# Even though the DOS/Win* filesystem matches both .s and .S the same,
-# it will preserve the spelling of the filenames, and gcc itself does
-# care about how the name is spelled on its command-line.
-ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
-
-# List Assembler source files here which must be assembled in ARM-Mode..
-ASRCARM =
-
-# 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  += $(PIOSSTM32F10X)
-EXTRAINCDIRS  += $(PIOSCOMMON)
-EXTRAINCDIRS  += $(PIOSBOARDS)
-EXTRAINCDIRS  += $(STMSPDINCDIR)
-EXTRAINCDIRS  += $(CMSISDIR)
-EXTRAINCDIRS  += $(INSINC)
-EXTRAINCDIRS  += $(OPUAVSYNTHDIR)
-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 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 = $(PIOSSTM32F10X)
-
-# 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)
-CFLAGS += -Os
-CFLAGS += -DGENERAL_COV
-else
-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,
-#  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)
-CDEFS += -DIN_INS
-
-# 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 += -g$(DEBUGF) #-DDEBUG
-else
-CFLAGS += -g$(DEBUGF)
-endif
-
-
-CFLAGS += -ffast-math
-
-CFLAGS += -mcpu=$(MCU)
-CFLAGS += $(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
-LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
-LDFLAGS += -lc
-LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
-LDFLAGS += $(MATH_LIB)
-LDFLAGS += -lc -lgcc
-
-# Set linker-script name depending on selected submodel name
-LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
-LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
-
-# 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: gccversion build
-
-ifeq ($(LOADFORMAT),ihex)
-build: elf hex lss sym
-else
-ifeq ($(LOADFORMAT),binary)
-build: elf bin lss sym
-else
-ifeq ($(LOADFORMAT),both)
-build: elf hex bin lss 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,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
-
-.PHONY: elf lss sym hex bin bino opfw
-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
-opfw: $(OUTDIR)/$(TARGET).opfw
-
-# 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).opfw
-ifneq ($(INSTALL_DIR),)
-	@echo $(MSG_INSTALLING) $(call toprel, $<)
-	$(V1) mkdir -p $(INSTALL_DIR)
-	$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
-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) $(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 $(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

flight/INS/inc/ins.h

@@ -1,129 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup INS INS
-
- * @brief The main INS headers
- *
- * @{
- * @addtogroup INS_Main
- * @brief INS headers
- * @{
- *
- * @file       ins.h
- * @author     David "Buzz" Carlson (buzz@chebuzz.com)
- * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
- * @brief      INS Headers
- * @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_H
-#define INS_H
-
-/* PIOS Includes */
-#include <pios.h>
-
-struct mag_sensor {
-	uint8_t id[4];
-	uint8_t updated;
-	struct {
-		int16_t axis[3];
-	} raw;
-	struct {
-		float axis[3];
-	} scaled;
-	struct {
-		float bias[3];
-		float scale[3];
-		float variance[3];
-	} calibration;
-};
-
-//! Contains the data from the accelerometer
-struct accel_sensor {
-	struct {
-		uint16_t x;
-		uint16_t y;
-		uint16_t z;
-	} raw;
-	struct {
-		float x;
-		float y;
-		float z;
-	} filtered;
-	struct {
-		float scale[3][4];
-		float variance[3];
-	} calibration;
-};
-
-//! Contains the data from the gyro
-struct gyro_sensor {
-	struct {
-		uint16_t x;
-		uint16_t y;
-		uint16_t z;
-	} raw;
-	struct {
-		float x;
-		float y;
-		float z;
-	} filtered;
-	struct {
-		float bias[3];
-		float scale[3];
-		float variance[3];
-		float tempcompfactor[3];
-	} calibration;
-	struct {
-		uint16_t xy;
-		uint16_t z;
-	} temp;
-};
-
-//! Conains the current estimate of the attitude
-struct attitude_solution {
-	struct {
-		float q1;
-		float q2;
-		float q3;
-		float q4;
-	} quaternion;
-};
-
-//! Contains data from the altitude sensor
-struct altitude_sensor {
-	float altitude;
-	bool updated;
-};
-
-//! Contains data from the GPS (via the SPI link)
-struct gps_sensor {
-	float NED[3];
-	float heading;
-	float groundspeed;
-	float quality;
-	bool updated;
-};
-
-#endif /* INS_H */
-
-/**
- * @}
- * @}
- */

flight/INS/inc/pios_config.h

@@ -1,61 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup INS INS
-
- * @brief The INS configuration
- *
- * @{
- * @addtogroup INS_Main
- * @brief INS configuration
- * @{
- *
- * @file       pios_config.h  
- * @author     David "Buzz" Carlson (buzz@chebuzz.com)
- * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
- * @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_I2C
-#define PIOS_INCLUDE_IRQ
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_SPI
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_USART
-#define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_GPS
-#define PIOS_INCLUDE_HMC5883
-#define PIOS_INCLUDE_BMP085
-#define PIOS_INCLUDE_IMU3000
-#define PIOS_INCLUDE_GPIO
-#define PIOS_INCLUDE_EXTI
-
-#define PIOS_INCLUDE_BMA180
-
-#endif /* PIOS_CONFIG_H */
-/**
- * @}
- * @}
- */

flight/INS/ins.c

@@ -1,218 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup INS INS
-
- * @brief The INS Modules perform
- *
- * @{
- * @addtogroup INS_Main
- * @brief Main function which does the hardware dependent stuff
- * @{
- *
- *
- * @file       ins.c
- * @author     David "Buzz" Carlson (buzz@chebuzz.com)
- * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
- * @brief      INSGPS Test Program
- * @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 "ins.h"
-#include "pios.h"
-#include <stdbool.h>
-#include "fifo_buffer.h"
-
-void reset_values();
-
-/**
- * @addtogroup INS_Global_Data INS Global Data
- * @{
- * Public data.  Used by both EKF and the sender
- */
-
-//! Contains the data from the mag sensor chip
-struct mag_sensor mag_data;
-
-//! Contains the data from the accelerometer
-struct accel_sensor  accel_data;
-
-//! Contains the data from the gyro
-struct gyro_sensor gyro_data;
-
-//! Conains the current estimate of the attitude
-struct attitude_solution attitude_data;
-
-//! Contains data from the altitude sensor
-struct altitude_sensor altitude_data;
-
-//! Contains data from the GPS (via the SPI link)
-struct gps_sensor gps_data;
-
-//! Offset correction of barometric alt, to match gps data
-//static float baro_offset = 0;
-
-//static float mag_len = 0;
-
-typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states;
-volatile int32_t ekf_too_slow;
-volatile int32_t total_conversion_blocks;
-
-/**
- * @}
- */
-
-/* INS functions */
-void blink(int led, int times)
-{
-	for(int i=0; i<times; i++)
-	{
-		PIOS_LED_Toggle(led);
-		PIOS_DELAY_WaitmS(250);
-		PIOS_LED_Toggle(led);
-		PIOS_DELAY_WaitmS(250);
-	}
-}
-
-
-void test_accel()
-{
-	if(PIOS_BMA180_Test())
-		blink(LED1, 1);
-	else
-		blink(LED2, 1);
-}
-
-#if defined (PIOS_INCLUDE_HMC5883)
-void test_mag()
-{
-	if(PIOS_HMC5883_Test())
-		blink(LED1, 2);
-	else
-		blink(LED2, 2);
-}
-#endif
-
-#if defined (PIOS_INCLUDE_BMP085)
-void test_pressure()
-{
-	if(PIOS_BMP085_Test())
-		blink(LED1, 3);
-	else
-		blink(LED2, 3);
-}
-#endif
-
-#if defined (PIOS_INCLUDE_IMU3000)
-void test_imu()
-{
-	if(PIOS_IMU3000_Test())
-		blink(LED1, 4);
-	else
-		blink(LED2, 4);
-}
-#endif
-
-
-extern void PIOS_Board_Init(void);
-struct pios_bma180_data bma180_data;
-
-/**
- * @brief INS Main function
- */
-int main()
-{
-	reset_values();
-
-	PIOS_Board_Init();
-
-	while (1)
-	{
-		test_accel();
-		PIOS_DELAY_WaitmS(1000);
-
-#if defined (PIOS_INCLUDE_HMC5883)
-		test_mag();
-		PIOS_DELAY_WaitmS(1000);
-#endif
-
-#if defined (PIOS_INCLUDE_BMP085)
-		test_pressure();
-		PIOS_DELAY_WaitmS(1000);
-#endif
-
-#if defined (PIOS_INCLUDE_IMU3000)
-		test_imu();
-		PIOS_DELAY_WaitmS(1000);
-#endif
-		PIOS_DELAY_WaitmS(3000);
-	}
-
-}
-
-
-
-/**
- * @brief Populate fields with initial values
- */
-void reset_values()
-{
-	accel_data.calibration.scale[0][1] = 0;
-	accel_data.calibration.scale[1][0] = 0;
-	accel_data.calibration.scale[0][2] = 0;
-	accel_data.calibration.scale[2][0] = 0;
-	accel_data.calibration.scale[1][2] = 0;
-	accel_data.calibration.scale[2][1] = 0;
-
-	accel_data.calibration.scale[0][0] = 0.0359;
-	accel_data.calibration.scale[1][1] = 0.0359;
-	accel_data.calibration.scale[2][2] = 0.0359;
-	accel_data.calibration.scale[0][3] = -73.5;
-	accel_data.calibration.scale[1][3] = -73.5;
-	accel_data.calibration.scale[2][3] = -73.5;
-
-	accel_data.calibration.variance[0] = 1e-4;
-	accel_data.calibration.variance[1] = 1e-4;
-	accel_data.calibration.variance[2] = 1e-4;
-
-	gyro_data.calibration.scale[0] = -0.014;
-	gyro_data.calibration.scale[1] = 0.014;
-	gyro_data.calibration.scale[2] = -0.014;
-	gyro_data.calibration.bias[0] = -24;
-	gyro_data.calibration.bias[1] = -24;
-	gyro_data.calibration.bias[2] = -24;
-	gyro_data.calibration.variance[0] = 1;
-	gyro_data.calibration.variance[1] = 1;
-	gyro_data.calibration.variance[2] = 1;
-	mag_data.calibration.scale[0] = 1;
-	mag_data.calibration.scale[1] = 1;
-	mag_data.calibration.scale[2] = 1;
-	mag_data.calibration.bias[0] = 0;
-	mag_data.calibration.bias[1] = 0;
-	mag_data.calibration.bias[2] = 0;
-	mag_data.calibration.variance[0] = 50;
-	mag_data.calibration.variance[1] = 50;
-	mag_data.calibration.variance[2] = 50;
-}
-
-
-/**
- * @}
- */
-

flight/INS/pios_board.c

@@ -1,586 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup PIOS_HMC5883 HMC5883 Functions
- * @brief Deals with the hardware interface to the magnetometers
- * @{
- *
- * @file       pios_board.c
- * @author     David "Buzz" Carlson (buzz@chebuzz.com)
- * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
- * @brief      Defines board specific static initializers for hardware for the INS 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.h>
-
-#if defined(PIOS_INCLUDE_SPI)
-
-#include <pios_spi_priv.h>
-
-/* SPI2 Interface
- *      - Used for mainboard communications and magnetometer
- *
- * NOTE: Leave this declared as const data so that it ends up in the 
- * .rodata section (ie. Flash) rather than in the .bss section (RAM).
- */
-void PIOS_SPI_op_mag_irq_handler(void);
-void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler")));
-void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler")));
-static const struct pios_spi_cfg pios_spi_op_mag_cfg = {
-	.regs = SPI2,
-	.init = {
-		 .SPI_Mode = SPI_Mode_Slave,
-		 .SPI_Direction = SPI_Direction_2Lines_FullDuplex,
-		 .SPI_DataSize = SPI_DataSize_8b,
-		 .SPI_NSS = SPI_NSS_Hard,
-		 .SPI_FirstBit = SPI_FirstBit_MSB,
-		 .SPI_CRCPolynomial = 7,
-		 .SPI_CPOL = SPI_CPOL_High,
-		 .SPI_CPHA = SPI_CPHA_2Edge,
-		 },
-	.use_crc = TRUE,
-	.dma = {
-		.ahb_clk = RCC_AHBPeriph_DMA1,
-
-		.irq = {
-			.flags =
-			(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
-			 DMA1_FLAG_GL4),
-			.init = {
-				 .NVIC_IRQChannel = DMA1_Channel4_IRQn,
-				 .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-				 .NVIC_IRQChannelSubPriority = 0,
-				 .NVIC_IRQChannelCmd = ENABLE,
-				 },
-			},
-
-		.rx = {
-		       .channel = DMA1_Channel4,
-		       .init = {
-				.DMA_PeripheralBaseAddr =
-				(uint32_t) & (SPI2->DR),
-				.DMA_DIR = DMA_DIR_PeripheralSRC,
-				.DMA_PeripheralInc =
-				DMA_PeripheralInc_Disable,
-				.DMA_MemoryInc = DMA_MemoryInc_Enable,
-				.DMA_PeripheralDataSize =
-				DMA_PeripheralDataSize_Byte,
-				.DMA_MemoryDataSize =
-				DMA_MemoryDataSize_Byte,
-				.DMA_Mode = DMA_Mode_Normal,
-				.DMA_Priority = DMA_Priority_Medium,
-				.DMA_M2M = DMA_M2M_Disable,
-				},
-		       },
-		.tx = {
-		       .channel = DMA1_Channel5,
-		       .init = {
-				.DMA_PeripheralBaseAddr =
-				(uint32_t) & (SPI2->DR),
-				.DMA_DIR = DMA_DIR_PeripheralDST,
-				.DMA_PeripheralInc =
-				DMA_PeripheralInc_Disable,
-				.DMA_MemoryInc = DMA_MemoryInc_Enable,
-				.DMA_PeripheralDataSize =
-				DMA_PeripheralDataSize_Byte,
-				.DMA_MemoryDataSize =
-				DMA_MemoryDataSize_Byte,
-				.DMA_Mode = DMA_Mode_Normal,
-				.DMA_Priority = DMA_Priority_Medium,
-				.DMA_M2M = DMA_M2M_Disable,
-				},
-		       },
-		},
-	.ssel = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_12,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_IN_FLOATING,
-			  },
-		 },
-	.sclk = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_13,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_IN_FLOATING,
-			  },
-		 },
-	.miso = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_14,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_AF_PP,
-			  },
-		 },
-	.mosi = {
-		 .gpio = GPIOB,
-		 .init = {
-			  .GPIO_Pin = GPIO_Pin_15,
-			  .GPIO_Speed = GPIO_Speed_10MHz,
-			  .GPIO_Mode = GPIO_Mode_IN_FLOATING,
-			  },
-		 },
-};
-
-uint32_t pios_spi_op_mag_id;
-void PIOS_SPI_op_mag_irq_handler(void)
-{
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_SPI_IRQ_Handler(pios_spi_op_mag_id);
-}
-
-/* SPI1 Interface
- *    - Used for BMA180 accelerometer
- */
-void PIOS_SPI_accel_irq_handler(void);
-void DMA1_Channel2_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler")));
-void DMA1_Channel3_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler")));
-static const struct pios_spi_cfg pios_spi_accel_cfg = {
-	.regs = SPI1,
-	.init   = {
-			.SPI_Mode              = SPI_Mode_Master,
-			.SPI_Direction         = SPI_Direction_2Lines_FullDuplex,
-			.SPI_DataSize          = SPI_DataSize_8b,
-			.SPI_NSS               = SPI_NSS_Soft,
-			.SPI_FirstBit          = SPI_FirstBit_MSB,
-			.SPI_CRCPolynomial     = 7,
-			.SPI_CPOL              = SPI_CPOL_High,
-			.SPI_CPHA              = SPI_CPHA_1Edge,
-			.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
-	},
-	.use_crc = FALSE,
-	.dma = {
-			.ahb_clk  = RCC_AHBPeriph_DMA1,
-
-			.irq = {
-					.flags   = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
-					.init    = {
-							.NVIC_IRQChannel                   = DMA1_Channel2_IRQn,
-							.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-							.NVIC_IRQChannelSubPriority        = 0,
-							.NVIC_IRQChannelCmd                = ENABLE,
-					},
-			},
-
-			.rx = {
-					.channel = DMA1_Channel2,
-					.init    = {
-							.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
-							.DMA_DIR                = DMA_DIR_PeripheralSRC,
-							.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-							.DMA_MemoryInc          = DMA_MemoryInc_Enable,
-							.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-							.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-							.DMA_Mode               = DMA_Mode_Normal,
-							.DMA_Priority           = DMA_Priority_Medium,
-							.DMA_M2M                = DMA_M2M_Disable,
-					},
-			},
-			.tx = {
-					.channel = DMA1_Channel3,
-					.init    = {
-							.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
-							.DMA_DIR                = DMA_DIR_PeripheralDST,
-							.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-							.DMA_MemoryInc          = DMA_MemoryInc_Enable,
-							.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-							.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-							.DMA_Mode               = DMA_Mode_Normal,
-							.DMA_Priority           = DMA_Priority_High,
-							.DMA_M2M                = DMA_M2M_Disable,
-					},
-			},
-	},
-	.ssel = {
-			.gpio = GPIOA,
-			.init = {
-					.GPIO_Pin   = GPIO_Pin_4,
-					.GPIO_Speed = GPIO_Speed_10MHz,
-					.GPIO_Mode  = GPIO_Mode_Out_PP,
-			},
-	},
-	.sclk = {
-			.gpio = GPIOA,
-			.init = {
-					.GPIO_Pin   = GPIO_Pin_5,
-					.GPIO_Speed = GPIO_Speed_10MHz,
-					.GPIO_Mode  = GPIO_Mode_AF_PP,
-			},
-	},
-	.miso = {
-			.gpio = GPIOA,
-			.init = {
-					.GPIO_Pin   = GPIO_Pin_6,
-					.GPIO_Speed = GPIO_Speed_10MHz,
-					.GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-			},
-	},
-	.mosi = {
-			.gpio = GPIOA,
-			.init = {
-					.GPIO_Pin   = GPIO_Pin_7,
-					.GPIO_Speed = GPIO_Speed_10MHz,
-					.GPIO_Mode  = GPIO_Mode_AF_PP,
-			},
-	},
-};
-
-static uint32_t pios_spi_accel_id;
-void PIOS_SPI_accel_irq_handler(void)
-{
-  /* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_SPI_IRQ_Handler(pios_spi_accel_id);
-}
-
-#endif /* PIOS_INCLUDE_SPI */
-
-
-
-#if defined(PIOS_INCLUDE_GPS)
-#include <pios_usart_priv.h>
-
-/*
- * GPS USART
- */
-static const struct pios_usart_cfg pios_usart_gps_cfg = {
-	.regs = USART1,
-	.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 = 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_IPU,
-			},
-	       },
-	.tx = {
-	       .gpio = GPIOA,
-	       .init = {
-			.GPIO_Pin = GPIO_Pin_9,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode = GPIO_Mode_AF_PP,
-			},
-	       },
-};
-
-#endif /* PIOS_INCLUDE_GPS */
-
-#ifdef PIOS_COM_AUX
-/*
- * AUX USART
- */
-static const struct pios_usart_cfg pios_usart_aux_cfg = {
-	.regs = USART4,
-	.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 = USART4_IRQn,
-			 .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-			 .NVIC_IRQChannelSubPriority = 0,
-			 .NVIC_IRQChannelCmd = ENABLE,
-			 },
-		},
-	.rx = {
-	       .gpio = GPIOB,
-	       .init = {
-			.GPIO_Pin = GPIO_Pin_11,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode = GPIO_Mode_IPU,
-			},
-	       },
-	.tx = {
-	       .gpio = GPIOB,
-	       .init = {
-			.GPIO_Pin = GPIO_Pin_10,
-			.GPIO_Speed = GPIO_Speed_2MHz,
-			.GPIO_Mode = GPIO_Mode_AF_PP,
-			},
-	       },
-};
-
-#endif /* PIOS_COM_AUX */
-
-
-#if defined(PIOS_INCLUDE_COM)
-
-#include <pios_com_priv.h>
-
-#if 0
-#define PIOS_COM_AUX_TX_BUF_LEN 192
-static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
-#endif
-
-#define PIOS_COM_GPS_RX_BUF_LEN 96
-static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN];
-
-
-#endif /* PIOS_INCLUDE_COM */
-
-#if defined(PIOS_INCLUDE_I2C)
-
-#include <pios_i2c_priv.h>
-
-/*
- * I2C Adapters
- */
-
-void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void);
-void PIOS_I2C_pres_mag_adapter_er_irq_handler(void);
-void I2C1_EV_IRQHandler()
-    __attribute__ ((alias("PIOS_I2C_pres_mag_adapter_ev_irq_handler")));
-void I2C1_ER_IRQHandler()
-    __attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler")));
-
-static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
-	.regs = I2C1,
-	.init = {
-		 .I2C_Mode = I2C_Mode_I2C,
-		 .I2C_OwnAddress1 = 0,
-		 .I2C_Ack = I2C_Ack_Enable,
-		 .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-		 .I2C_DutyCycle = I2C_DutyCycle_2,
-		 .I2C_ClockSpeed = 200000,	/* bits/s */
-		 },
-	.transfer_timeout_ms = 50,
-	.scl = {
-		.gpio = GPIOB,
-		.init = {
-			 .GPIO_Pin = GPIO_Pin_6,
-			 .GPIO_Speed = GPIO_Speed_10MHz,
-			 .GPIO_Mode = GPIO_Mode_AF_OD,
-			 },
-		},
-	.sda = {
-		.gpio = GPIOB,
-		.init = {
-			 .GPIO_Pin = GPIO_Pin_7,
-			 .GPIO_Speed = GPIO_Speed_10MHz,
-			 .GPIO_Mode = GPIO_Mode_AF_OD,
-			 },
-		},
-	.event = {
-		  .flags = 0,	/* FIXME: check this */
-		  .init = {
-			   .NVIC_IRQChannel = I2C1_EV_IRQn,
-			   .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-			   .NVIC_IRQChannelSubPriority = 0,
-			   .NVIC_IRQChannelCmd = ENABLE,
-			   },
-		  },
-	.error = {
-		  .flags = 0,	/* FIXME: check this */
-		  .init = {
-			   .NVIC_IRQChannel = I2C1_ER_IRQn,
-			   .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-			   .NVIC_IRQChannelSubPriority = 0,
-			   .NVIC_IRQChannelCmd = ENABLE,
-			   },
-		  },
-};
-
-uint32_t pios_i2c_pres_mag_adapter_id;
-void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void)
-{
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_I2C_EV_IRQ_Handler(pios_i2c_pres_mag_adapter_id);
-}
-
-void PIOS_I2C_pres_mag_adapter_er_irq_handler(void)
-{
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_I2C_ER_IRQ_Handler(pios_i2c_pres_mag_adapter_id);
-}
-
-
-void PIOS_I2C_gyro_adapter_ev_irq_handler(void);
-void PIOS_I2C_gyro_adapter_er_irq_handler(void);
-void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler")));
-void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler")));
-
-static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
-  .regs = I2C2,
-  .init = {
-    .I2C_Mode                = I2C_Mode_I2C,
-    .I2C_OwnAddress1         = 0,
-    .I2C_Ack                 = I2C_Ack_Enable,
-    .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-    .I2C_DutyCycle           = I2C_DutyCycle_2,
-    .I2C_ClockSpeed          = 400000,	/* bits/s */
-  },
-  .transfer_timeout_ms = 50,
-  .scl = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_10,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_OD,
-    },
-  },
-  .sda = {
-    .gpio = GPIOB,
-    .init = {
-      .GPIO_Pin   = GPIO_Pin_11,
-      .GPIO_Speed = GPIO_Speed_10MHz,
-      .GPIO_Mode  = GPIO_Mode_AF_OD,
-    },
-  },
-  .event = {
-    .flags   = 0,		/* FIXME: check this */
-    .init = {
-      .NVIC_IRQChannel                   = I2C2_EV_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-  .error = {
-    .flags   = 0,		/* FIXME: check this */
-    .init = {
-      .NVIC_IRQChannel                   = I2C2_ER_IRQn,
-      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-      .NVIC_IRQChannelSubPriority        = 0,
-      .NVIC_IRQChannelCmd                = ENABLE,
-    },
-  },
-};
-
-uint32_t pios_i2c_gyro_adapter_id;
-void PIOS_I2C_gyro_adapter_ev_irq_handler(void)
-{
-	/* Call into the generic code to handle the IRQ for this specific device */
-	PIOS_I2C_EV_IRQ_Handler(pios_i2c_gyro_adapter_id);
-}
-
-void PIOS_I2C_gyro_adapter_er_irq_handler(void)
-{
-  /* Call into the generic code to handle the IRQ for this specific device */
-  PIOS_I2C_ER_IRQ_Handler(pios_i2c_gyro_adapter_id);
-}
-
-#endif /* PIOS_INCLUDE_I2C */
-
-
-
-extern const struct pios_com_driver pios_usart_com_driver;
-
-uint32_t pios_com_aux_id;
-uint32_t pios_com_gps_id;
-
-/**
- * PIOS_Board_Init()
- * initializes all the core subsystems on this specific hardware
- * called from System/openpilot.c
- */
-void PIOS_Board_Init(void) {
-	/* Brings up System using CMSIS functions, enables the LEDs. */
-	PIOS_SYS_Init();
-
-	/* Delay system */
-	PIOS_DELAY_Init();
-
-	/* IAP System Setup */
-	PIOS_IAP_Init();
-
-#if defined(PIOS_INCLUDE_COM)
-#if defined(PIOS_INCLUDE_GPS)
-	uint32_t pios_usart_gps_id;
-	if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-	if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
-			  pios_com_gps_rx_buffer, sizeof(pios_com_gps_rx_buffer),
-			  NULL, 0)) {
-		PIOS_DEBUG_Assert(0);
-	}
-#endif	/* PIOS_INCLUDE_GPS */
-#endif	/* PIOS_INCLUDE_COM */
-
-#if defined (PIOS_INCLUDE_I2C)
-	if (PIOS_I2C_Init(&pios_i2c_pres_mag_adapter_id, &pios_i2c_pres_mag_adapter_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-#if defined (PIOS_INCLUDE_BMP085)
-	PIOS_BMP085_Init();
-#endif /* PIOS_INCLUDE_BMP085 */
-#if defined (PIOS_INCLUDE_HMC5883)
-	PIOS_HMC5883_Init();
-#endif /* PIOS_INCLUDE_HMC5883 */
-
-#if defined(PIOS_INCLUDE_IMU3000)
-	if (PIOS_I2C_Init(&pios_i2c_gyro_adapter_id, &pios_i2c_gyro_adapter_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-	PIOS_IMU3000_Init();
-#endif /* PIOS_INCLUDE_IMU3000 */
-#endif /* PIOS_INCLUDE_I2C */
-
-#if defined(PIOS_INCLUDE_SPI)
-	/* Set up the SPI interface to the accelerometer*/
-	if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) {
-		PIOS_DEBUG_Assert(0);
-	}
-
-	PIOS_BMA180_Attach(pios_spi_accel_id);
-
-// #include "ahrs_spi_comm.h"
-//	InsInitComms();
-//
-//	/* Set up the SPI interface to the OP board */
-//	if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
-//		PIOS_DEBUG_Assert(0);
-//	}
-//
-//	InsConnect(pios_spi_op_id);
-#endif /* PIOS_INCLUDE_SPI */
-}
-
-/**
- * @}
- * @}
- */
-

flight/Libraries/CoordinateConversions.c

@@ -31,16 +31,17 @@
 #include <stdint.h>
 #include "CoordinateConversions.h"
 
-#define RAD2DEG (180.0/M_PI)
+#define F_PI 3.14159265358979323846f
-#define DEG2RAD (M_PI/180.0)
+#define RAD2DEG (180.0f/ F_PI)
+#define DEG2RAD (F_PI /180.0f)
 
 // ****** convert Lat,Lon,Alt to ECEF  ************
-void LLA2ECEF(double LLA[3], double ECEF[3])
+void LLA2ECEF(float LLA[3], float ECEF[3])
 {
-	const double a = 6378137.0;	// Equatorial Radius
+	const float a = 6378137.0;	// Equatorial Radius
-	const double e = 8.1819190842622e-2;	// Eccentricity
+	const float e = 8.1819190842622e-2;	// Eccentricity
-	double sinLat, sinLon, cosLat, cosLon;
+	float sinLat, sinLon, cosLat, cosLon;
-	double N;
+	float N;
 
 	sinLat = sin(DEG2RAD * LLA[0]);
 	sinLon = sin(DEG2RAD * LLA[1]);
@@ -55,7 +56,7 @@ void LLA2ECEF(double LLA[3], double ECEF[3])
 }
 
 // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
-uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
+uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
 {
 	/**
 	 * LLA parameter is used to prime the iteration.
@@ -66,10 +67,10 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
 	 * Suggestion: [0,0,0]
 	 **/
 
-	const double a = 6378137.0;	// Equatorial Radius
+	const float a = 6378137.0;	// Equatorial Radius
-	const double e = 8.1819190842622e-2;	// Eccentricity
+	const float e = 8.1819190842622e-2;	// Eccentricity
-	double x = ECEF[0], y = ECEF[1], z = ECEF[2];
+	float x = ECEF[0], y = ECEF[1], z = ECEF[2];
-	double Lat, N, NplusH, delta, esLat;
+	float Lat, N, NplusH, delta, esLat;
 	uint16_t iter;
 #define MAX_ITER 10		// should not take more than 5 for valid coordinates
 #define ACCURACY 1.0e-11	// used to be e-14, but we don't need sub micrometer exact calculations
@@ -99,7 +100,7 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
 }
 
 // ****** find ECEF to NED rotation matrix ********
-void RneFromLLA(double LLA[3], float Rne[3][3])
+void RneFromLLA(float LLA[3], float Rne[3][3])
 {
 	float sinLat, sinLon, cosLat, cosLon;
 
@@ -128,10 +129,10 @@ void Quaternion2RPY(const float q[4], float rpy[3])
 	float q2s = q[2] * q[2];
 	float q3s = q[3] * q[3];
 
-	R13 = 2 * (q[1] * q[3] - q[0] * q[2]);
+	R13 = 2.0f * (q[1] * q[3] - q[0] * q[2]);
 	R11 = q0s + q1s - q2s - q3s;
-	R12 = 2 * (q[1] * q[2] + q[0] * q[3]);
+	R12 = 2.0f * (q[1] * q[2] + q[0] * q[3]);
-	R23 = 2 * (q[2] * q[3] + q[0] * q[1]);
+	R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]);
 	R33 = q0s - q1s - q2s + q3s;
 
 	rpy[1] = RAD2DEG * asinf(-R13);	// pitch always between -pi/2 to pi/2
@@ -188,9 +189,9 @@ void Quaternion2R(float q[4], float Rbe[3][3])
 }
 
 // ****** Express LLA in a local NED Base Frame ********
-void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
+void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3])
 {
-	double ECEF[3];
+	float ECEF[3];
 	float diff[3];
 
 	LLA2ECEF(LLA, ECEF);
@@ -205,7 +206,7 @@ void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
 }
 
 // ****** Express ECEF in a local NED Base Frame ********
-void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3])
+void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3])
 {
 	float diff[3];
 
@@ -239,7 +240,7 @@ void R2Quaternion(float R[3][3], float q[4])
 			index = i;
 		}
 	}
-	mag = 2*sqrt(mag);
+	mag = 2*sqrtf(mag);
 
 	if (index == 0) {
 		q[0] = mag/4;
@@ -373,7 +374,7 @@ void CrossProduct(const float v1[3], const float v2[3], float result[3])
 // ****** Vector Magnitude ********
 float VectorMagnitude(const float v[3])
 {
-	return(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
+	return(sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
 }
 
 /**

flight/Libraries/ahrs_comm_objects.c

@@ -30,15 +30,16 @@
 
 static AttitudeRawData AttitudeRaw;
 static AttitudeActualData AttitudeActual;
-static AHRSCalibrationData AHRSCalibration;
-static AhrsStatusData AhrsStatus;
 static BaroAltitudeData BaroAltitude;
 static GPSPositionData GPSPosition;
+static HomeLocationData HomeLocation;
+static InsStatusData InsStatus;
+static InsSettingsData InsSettings;
+static FirmwareIAPObjData FirmwareIAPObj;
 static PositionActualData PositionActual;
 static VelocityActualData VelocityActual;
-static HomeLocationData HomeLocation;
+static GPSSatellitesData GPSSatellites;
-static AHRSSettingsData AHRSSettings;
+static GPSTimeData GPSTime;
-static FirmwareIAPObjData FirmwareIAPObj;
 
 AhrsSharedObject objectHandles[MAX_AHRS_OBJECTS];
 
@@ -54,17 +55,18 @@ AhrsSharedObject objectHandles[MAX_AHRS_OBJECTS];
 
 CREATEHANDLE(0, AttitudeRaw);
 CREATEHANDLE(1, AttitudeActual);
-CREATEHANDLE(2, AHRSCalibration);
+CREATEHANDLE(2, InsSettings);
-CREATEHANDLE(3, AhrsStatus);
+CREATEHANDLE(3, InsStatus);
 CREATEHANDLE(4, BaroAltitude);
 CREATEHANDLE(5, GPSPosition);
 CREATEHANDLE(6, PositionActual);
 CREATEHANDLE(7, VelocityActual);
 CREATEHANDLE(8, HomeLocation);
-CREATEHANDLE(9, AHRSSettings);
+CREATEHANDLE(9, FirmwareIAPObj);
-CREATEHANDLE(10, FirmwareIAPObj);
+CREATEHANDLE(10, GPSSatellites);
+CREATEHANDLE(11, GPSTime);
 
-#if 11 != MAX_AHRS_OBJECTS	//sanity check
+#if 12 != MAX_AHRS_OBJECTS	//sanity check
 #error We did not create the correct number of xxxHandle() functions
 #endif
 
@@ -97,15 +99,17 @@ void AhrsInitHandles(void)
 //the last has the lowest priority
 	ADDHANDLE(idx++, AttitudeRaw);
 	ADDHANDLE(idx++, AttitudeActual);
-	ADDHANDLE(idx++, AHRSCalibration);
+	ADDHANDLE(idx++, InsSettings);
-	ADDHANDLE(idx++, AhrsStatus);
+	ADDHANDLE(idx++, InsStatus);
 	ADDHANDLE(idx++, BaroAltitude);
 	ADDHANDLE(idx++, GPSPosition);
 	ADDHANDLE(idx++, PositionActual);
 	ADDHANDLE(idx++, VelocityActual);
 	ADDHANDLE(idx++, HomeLocation);
-	ADDHANDLE(idx++, AHRSSettings);
 	ADDHANDLE(idx++, FirmwareIAPObj);
+	ADDHANDLE(idx++, GPSSatellites);
+	ADDHANDLE(idx++, GPSTime);
+
 	if (idx != MAX_AHRS_OBJECTS) {
 		PIOS_DEBUG_Assert(0);
 	}
@@ -113,11 +117,8 @@ void AhrsInitHandles(void)
 //When the AHRS writes to these the data does a round trip
 //AHRS->OP->AHRS due to these events
 #ifndef IN_AHRS
-	AHRSSettingsConnectCallback(ObjectUpdatedCb);
+	InsSettingsConnectCallback(ObjectUpdatedCb);
-	BaroAltitudeConnectCallback(ObjectUpdatedCb);
-	GPSPositionConnectCallback(ObjectUpdatedCb);
 	HomeLocationConnectCallback(ObjectUpdatedCb);
-	AHRSCalibrationConnectCallback(ObjectUpdatedCb);
 	FirmwareIAPObjConnectCallback(ObjectUpdatedCb);
 #endif
 }

flight/Libraries/ahrs_spi_comm.c

@@ -33,7 +33,7 @@
 #include "pios_spi.h"
 #include "pios_irq.h"
 #include "ahrs_spi_program_slave.h"
-#include "STM32103CB_AHRS.h"
+//#include "STM32103CB_AHRS.h"
 #endif
 
 /*transmit and receive packet magic numbers.
@@ -53,7 +53,7 @@ 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
-#define MAX_UPDATE_OBJECTS 1
+#define MAX_UPDATE_OBJECTS 2
 
 //Number of transmissions + 1 before we expect to see the data acknowledge
 //This is controlled by the SPI hardware.
@@ -298,13 +298,11 @@ int32_t AhrsConnectCallBack(AhrsObjHandle obj, AhrsEventCallback cb)
 	return (0);
 }
 
-AhrsCommStatus AhrsGetStatus()
+void AhrsGetStatus(AhrsCommStatus * status)
 {
-	AhrsCommStatus status;
+	status->remote = rxPacket.status;
-	status.remote = rxPacket.status;
+	status->local = txPacket.status;
-	status.local = txPacket.status;
+	status->linkOk = linkOk;
-	status.linkOk = linkOk;
-	return (status);
 }
 
 
@@ -439,7 +437,9 @@ void AhrsSendObjects(void)
 
 void SendPacket(void)
 {
+#ifndef IN_AHRS
 	PIOS_SPI_RC_PinSet(opahrs_spi_id, 0);
+#endif
 	//no point checking if this failed. There isn't much we could do about it if it did fail
 	PIOS_SPI_TransferBlock(opahrs_spi_id, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
 }

flight/Libraries/fifo_buffer.c

@@ -1,266 +1,266 @@
-/**
+/**
- ******************************************************************************
+ ******************************************************************************
- *
+ *
- * @file       fifo_buffer.c
+ * @file       fifo_buffer.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      GPIO input functions
+ * @brief      GPIO input functions
- * @see        The GNU Public License (GPL) Version 3
+ * @see        The GNU Public License (GPL) Version 3
- *
+ *
- *****************************************************************************/
+ *****************************************************************************/
-/*
+/*
- * This program is free software; you can redistribute it and/or modify
+ * 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
+ * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
+ * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
+ * (at your option) any later version.
- *
+ *
- * This program is distributed in the hope that it will be useful, but
+ * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
+ * for more details.
- *
+ *
- * You should have received a copy of the GNU General Public License along
+ * 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.,
+ * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
+ */
-
+
-#include <string.h>
+#include <string.h>
-
+
-#include "fifo_buffer.h"
+#include "fifo_buffer.h"
-
+
-// *****************************************************************************
+// *****************************************************************************
-// circular buffer functions
+// circular buffer functions
-
+
-uint16_t fifoBuf_getSize(t_fifo_buffer *buf)
+uint16_t fifoBuf_getSize(t_fifo_buffer *buf)
-{       // return the usable size of the buffer
+{       // return the usable size of the buffer
-
+
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-
+
-    if (buf_size > 0)
+    if (buf_size > 0)
-      return (buf_size - 1);
+      return (buf_size - 1);
-    else
+    else
-      return 0;
+      return 0;
-}
+}
-
+
-uint16_t fifoBuf_getUsed(t_fifo_buffer *buf)
+uint16_t fifoBuf_getUsed(t_fifo_buffer *buf)
-{       // return the number of bytes available in the rx buffer
+{       // return the number of bytes available in the rx buffer
-
+
-    uint16_t rd = buf->rd;
+    uint16_t rd = buf->rd;
-    uint16_t wr = buf->wr;
+    uint16_t wr = buf->wr;
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-
+
-    uint16_t num_bytes = wr - rd;
+    uint16_t num_bytes = wr - rd;
-    if (wr < rd)
+    if (wr < rd)
-        num_bytes = (buf_size - rd) + wr;
+        num_bytes = (buf_size - rd) + wr;
-
+
-    return num_bytes;
+    return num_bytes;
-}
+}
-
+
-uint16_t fifoBuf_getFree(t_fifo_buffer *buf)
+uint16_t fifoBuf_getFree(t_fifo_buffer *buf)
-{       // return the free space size in the buffer
+{       // return the free space size in the buffer
-
+
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-
+
-    uint16_t num_bytes = fifoBuf_getUsed(buf);
+    uint16_t num_bytes = fifoBuf_getUsed(buf);
-
+
-    return ((buf_size - num_bytes) - 1);
+    return ((buf_size - num_bytes) - 1);
-}
+}
-
+
-void fifoBuf_clearData(t_fifo_buffer *buf)
+void fifoBuf_clearData(t_fifo_buffer *buf)
-{       // remove all data from the buffer
+{       // remove all data from the buffer
-	buf->rd = buf->wr;
+	buf->rd = buf->wr;
-}
+}
-
+
-void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len)
+void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len)
-{       // remove a number of bytes from the buffer
+{       // remove a number of bytes from the buffer
-
+
-    uint16_t rd = buf->rd;
+    uint16_t rd = buf->rd;
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-
+
-    // get number of bytes available
+    // get number of bytes available
-    uint16_t num_bytes = fifoBuf_getUsed(buf);
+    uint16_t num_bytes = fifoBuf_getUsed(buf);
-
+
-    if (num_bytes > len)
+    if (num_bytes > len)
-        num_bytes = len;
+        num_bytes = len;
-
+
-    if (num_bytes < 1)
+    if (num_bytes < 1)
-        return;                         // nothing to remove
+        return;                         // nothing to remove
-
+
-    rd += num_bytes;
+    rd += num_bytes;
-    if (rd >= buf_size)
+    if (rd >= buf_size)
-        rd -= buf_size;
+        rd -= buf_size;
-
+
-    buf->rd = rd;
+    buf->rd = rd;
-}
+}
-
+
-int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf)
+int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf)
-{	// get a data byte from the buffer without removing it
+{	// get a data byte from the buffer without removing it
-
+
-    uint16_t rd = buf->rd;
+    uint16_t rd = buf->rd;
-
+
-    // get number of bytes available
+    // get number of bytes available
-    uint16_t num_bytes = fifoBuf_getUsed(buf);
+    uint16_t num_bytes = fifoBuf_getUsed(buf);
-
+
-    if (num_bytes < 1)
+    if (num_bytes < 1)
-        return -1;                      // no byte retuened
+        return -1;                      // no byte retuened
-
+
-    return buf->buf_ptr[rd];            // return the byte
+    return buf->buf_ptr[rd];            // return the byte
-}
+}
-
+
-int16_t fifoBuf_getByte(t_fifo_buffer *buf)
+int16_t fifoBuf_getByte(t_fifo_buffer *buf)
-{       // get a data byte from the buffer
+{       // get a data byte from the buffer
-
+
-    uint16_t rd = buf->rd;
+    uint16_t rd = buf->rd;
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-    uint8_t *buff = buf->buf_ptr;
+    uint8_t *buff = buf->buf_ptr;
-
+
-    // get number of bytes available
+    // get number of bytes available
-    uint16_t num_bytes = fifoBuf_getUsed(buf);
+    uint16_t num_bytes = fifoBuf_getUsed(buf);
-
+
-    if (num_bytes < 1)
+    if (num_bytes < 1)
-        return -1;                      // no byte returned
+        return -1;                      // no byte returned
-
+
-    uint8_t b = buff[rd];
+    uint8_t b = buff[rd];
-    if (++rd >= buf_size)
+    if (++rd >= buf_size)
-        rd = 0;
+        rd = 0;
-
+
-    buf->rd = rd;
+    buf->rd = rd;
-
+
-    return b;                           // return the byte
+    return b;                           // return the byte
-}
+}
-
+
-uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
+uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
-{       // get data from the buffer without removing it
+{       // get data from the buffer without removing it
-
+
-    uint16_t rd = buf->rd;
+    uint16_t rd = buf->rd;
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-    uint8_t *buff = buf->buf_ptr;
+    uint8_t *buff = buf->buf_ptr;
-
+
-    // get number of bytes available
+    // get number of bytes available
-    uint16_t num_bytes = fifoBuf_getUsed(buf);
+    uint16_t num_bytes = fifoBuf_getUsed(buf);
-
+
-    if (num_bytes > len)
+    if (num_bytes > len)
-        num_bytes = len;
+        num_bytes = len;
-
+
-    if (num_bytes < 1)
+    if (num_bytes < 1)
-        return 0;		// return number of bytes copied
+        return 0;		// return number of bytes copied
-
+
-    uint8_t *p = (uint8_t *)data;
+    uint8_t *p = (uint8_t *)data;
-    uint16_t i = 0;
+    uint16_t i = 0;
-
+
-    while (num_bytes > 0)
+    while (num_bytes > 0)
-    {
+    {
-        uint16_t j = buf_size - rd;
+        uint16_t j = buf_size - rd;
-        if (j > num_bytes)
+        if (j > num_bytes)
-            j = num_bytes;
+            j = num_bytes;
-        memcpy(p + i, buff + rd, j);
+        memcpy(p + i, buff + rd, j);
-        i += j;
+        i += j;
-        num_bytes -= j;
+        num_bytes -= j;
-        rd += j;
+        rd += j;
-        if (rd >= buf_size)
+        if (rd >= buf_size)
-            rd = 0;
+            rd = 0;
-    }
+    }
-
+
-    return i;                   // return number of bytes copied
+    return i;                   // return number of bytes copied
-}
+}
-
+
-uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
+uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
-{       // get data from our rx buffer
+{       // get data from our rx buffer
-
+
-    uint16_t rd = buf->rd;
+    uint16_t rd = buf->rd;
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-    uint8_t *buff = buf->buf_ptr;
+    uint8_t *buff = buf->buf_ptr;
-
+
-    // get number of bytes available
+    // get number of bytes available
-    uint16_t num_bytes = fifoBuf_getUsed(buf);
+    uint16_t num_bytes = fifoBuf_getUsed(buf);
-
+
-    if (num_bytes > len)
+    if (num_bytes > len)
-        num_bytes = len;
+        num_bytes = len;
-
+
-    if (num_bytes < 1)
+    if (num_bytes < 1)
-        return 0;               // return number of bytes copied
+        return 0;               // return number of bytes copied
-
+
-    uint8_t *p = (uint8_t *)data;
+    uint8_t *p = (uint8_t *)data;
-    uint16_t i = 0;
+    uint16_t i = 0;
-
+
-    while (num_bytes > 0)
+    while (num_bytes > 0)
-    {
+    {
-        uint16_t j = buf_size - rd;
+        uint16_t j = buf_size - rd;
-        if (j > num_bytes)
+        if (j > num_bytes)
-            j = num_bytes;
+            j = num_bytes;
-        memcpy(p + i, buff + rd, j);
+        memcpy(p + i, buff + rd, j);
-        i += j;
+        i += j;
-        num_bytes -= j;
+        num_bytes -= j;
-        rd += j;
+        rd += j;
-        if (rd >= buf_size)
+        if (rd >= buf_size)
-            rd = 0;
+            rd = 0;
-    }
+    }
-
+
-    buf->rd = rd;
+    buf->rd = rd;
-
+
-    return i;                   // return number of bytes copied
+    return i;                   // return number of bytes copied
-}
+}
-
+
-uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b)
+uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b)
-{       // add a data byte to the buffer
+{       // add a data byte to the buffer
-
+
-    uint16_t wr = buf->wr;
+    uint16_t wr = buf->wr;
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-    uint8_t *buff = buf->buf_ptr;
+    uint8_t *buff = buf->buf_ptr;
-
+
-    uint16_t num_bytes = fifoBuf_getFree(buf);
+    uint16_t num_bytes = fifoBuf_getFree(buf);
-    if (num_bytes < 1)
+    if (num_bytes < 1)
-        return 0;
+        return 0;
-
+
-    buff[wr] = b;
+    buff[wr] = b;
-    if (++wr >= buf_size)
+    if (++wr >= buf_size)
-        wr = 0;
+        wr = 0;
-
+
-    buf->wr = wr;
+    buf->wr = wr;
-
+
-    return 1;                   // return number of bytes copied
+    return 1;                   // return number of bytes copied
-}
+}
-
+
-uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
+uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
-{       // add data to the buffer
+{       // add data to the buffer
-
+
-    uint16_t wr = buf->wr;
+    uint16_t wr = buf->wr;
-    uint16_t buf_size = buf->buf_size;
+    uint16_t buf_size = buf->buf_size;
-    uint8_t *buff = buf->buf_ptr;
+    uint8_t *buff = buf->buf_ptr;
-
+
-    uint16_t num_bytes = fifoBuf_getFree(buf);
+    uint16_t num_bytes = fifoBuf_getFree(buf);
-    if (num_bytes > len)
+    if (num_bytes > len)
-        num_bytes = len;
+        num_bytes = len;
-
+
-    if (num_bytes < 1)
+    if (num_bytes < 1)
-        return 0;               // return number of bytes copied
+        return 0;               // return number of bytes copied
-
+
-    uint8_t *p = (uint8_t *)data;
+    uint8_t *p = (uint8_t *)data;
-    uint16_t i = 0;
+    uint16_t i = 0;
-
+
-    while (num_bytes > 0)
+    while (num_bytes > 0)
-    {
+    {
-        uint16_t j = buf_size - wr;
+        uint16_t j = buf_size - wr;
-        if (j > num_bytes)
+        if (j > num_bytes)
-            j = num_bytes;
+            j = num_bytes;
-        memcpy(buff + wr, p + i, j);
+        memcpy(buff + wr, p + i, j);
-        i += j;
+        i += j;
-        num_bytes -= j;
+        num_bytes -= j;
-        wr += j;
+        wr += j;
-        if (wr >= buf_size)
+        if (wr >= buf_size)
-            wr = 0;
+            wr = 0;
-    }
+    }
-
+
-    buf->wr = wr;
+    buf->wr = wr;
-
+
-    return i;                   // return number of bytes copied
+    return i;                   // return number of bytes copied
-}
+}
-
+
-void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size)
+void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size)
-{
+{
-    buf->buf_ptr = (uint8_t *)buffer;
+    buf->buf_ptr = (uint8_t *)buffer;
-    buf->rd = 0;
+    buf->rd = 0;
-    buf->wr = 0;
+    buf->wr = 0;
-    buf->buf_size = buffer_size;
+    buf->buf_size = buffer_size;
-}
+}
-
+
-// *****************************************************************************
+// *****************************************************************************

flight/Libraries/inc/CoordinateConversions.h

@@ -31,12 +31,12 @@
 #define COORDINATECONVERSIONS_H_
 
     // ****** convert Lat,Lon,Alt to ECEF  ************
-void LLA2ECEF(double LLA[3], double ECEF[3]);
+void LLA2ECEF(float LLA[3], float ECEF[3]);
 
     // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
-uint16_t ECEF2LLA(double ECEF[3], double LLA[3]);
+uint16_t ECEF2LLA(float ECEF[3], float LLA[3]);
 
-void RneFromLLA(double LLA[3], float Rne[3][3]);
+void RneFromLLA(float LLA[3], float Rne[3][3]);
 
     // ****** find rotation matrix from rotation vector
 void Rv2Rot(float Rv[3], float R[3][3]);
@@ -51,10 +51,10 @@ void RPY2Quaternion(const float rpy[3], float q[4]);
 void Quaternion2R(float q[4], float Rbe[3][3]);
 
 	// ****** Express LLA in a local NED Base Frame ********
-void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
+void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]);
 
 	// ****** Express ECEF in a local NED Base Frame ********
-void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
+void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]);
 
 	// ****** convert Rotation Matrix to Quaternion ********
 	// ****** if R converts from e to b, q is rotation from e to b ****

flight/Libraries/inc/NMEA.h

@@ -1,40 +1,42 @@
-/**
+/**
- ******************************************************************************
+ ******************************************************************************
- *
+ * @addtogroup OpenPilotModules OpenPilot Modules
- * @file       api_comms.h
+ * @{
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GSPModule GPS Module
- * @brief      RF Module hardware layer
+ * @brief Process GPS information
- * @see        The GNU Public License (GPL) Version 3
+ * @{
- *
+ *
- *****************************************************************************/
+ * @file       NMEA.h
-/*
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * This program is free software; you can redistribute it and/or modify
+ * @brief      GPS module, handles GPS and NMEA stream
- * it under the terms of the GNU General Public License as published by
+ * @see        The GNU Public License (GPL) Version 3
- * 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
+ * This program is free software; you can redistribute it and/or modify
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * it under the terms of the GNU General Public License as published by
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * the Free Software Foundation; either version 3 of the License, or
- * for more details.
+ * (at your option) any later version.
- *
+ *
- * You should have received a copy of the GNU General Public License along
+ * This program is distributed in the hope that it will be useful, but
- * with this program; if not, write to the Free Software Foundation, Inc.,
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- */
+ * for more details.
-
+ *
-#ifndef __API_COMMS_H__
+ * You should have received a copy of the GNU General Public License along
-#define __API_COMMS_H__
+ * 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"
+ */
-
+
-// *****************************************************************************
+#ifndef NMEA_H
-
+#define NMEA_H
-void api_1ms_tick(void);
+
-void api_process(void);
+#include <stdbool.h>
-
+#include <stdint.h>
-void api_init(void);
+
-
+#define NMEA_MAX_PACKET_LENGTH          96
-// *****************************************************************************
+
-
+extern bool NMEA_update_position(char *nmea_sentence);
-#endif
+extern bool NMEA_checksum(char *nmea_sentence);
+
+#endif /* NMEA_H */

flight/Libraries/inc/ahrs_comm_objects.h

@@ -29,36 +29,38 @@
 
 #include "attitudeactual.h"
 #include "attituderaw.h"
-#include "ahrsstatus.h"
 #include "baroaltitude.h"
 #include "gpsposition.h"
+#include "homelocation.h"
+#include "insstatus.h"
+#include "inssettings.h"
 #include "positionactual.h"
 #include "velocityactual.h"
-#include "homelocation.h"
-#include "ahrscalibration.h"
-#include "ahrssettings.h"
 #include "firmwareiapobj.h"
-
+#include "gpsposition.h"
+#include "gpssatellites.h"
+#include "gpstime.h"
 /** union that will fit any UAVObject.
 */
 
 typedef union {
 	AttitudeRawData AttitudeRaw;
 	AttitudeActualData AttitudeActual;
-	AHRSCalibrationData AHRSCalibration;
+	InsStatusData AhrsStatus;
-	AhrsStatusData AhrsStatus;
 	BaroAltitudeData BaroAltitude;
 	GPSPositionData GPSPosition;
 	PositionActualData PositionActual;
 	VelocityActualData VelocityActual;
 	HomeLocationData HomeLocation;
-	AHRSSettingsData AHRSSettings;
+	InsSettingsData InsSettings;
 	FirmwareIAPObjData FirmwareIAPObj;
+	GPSSatellitesData GPSSatellites;
+	GPSTimeData GPSTime;
 } __attribute__ ((packed)) AhrsSharedData;
 
 /** The number of UAVObjects we will be dealing with.
 */
-#define MAX_AHRS_OBJECTS 11
+#define MAX_AHRS_OBJECTS 12
 
 /** Our own version of a UAVObject.
 */

flight/Libraries/inc/ahrs_spi_comm.h

@@ -110,7 +110,7 @@ int32_t AhrsConnectCallBack(AhrsObjHandle obj, AhrsEventCallback cb);
 Returns: the status.
 Note: the remote status will only be valid if the link is up and running
  */
-AhrsCommStatus AhrsGetStatus();
+void AhrsGetStatus(AhrsCommStatus * status);
 
 #ifdef IN_AHRS			//slave only
 /** Send the latest objects to the OP

flight/Libraries/inc/fifo_buffer.h

@@ -1,65 +1,65 @@
-/**
+/**
- ******************************************************************************
+ ******************************************************************************
- *
+ *
- * @file       fifo_buffer.h
+ * @file       fifo_buffer.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      GPIO functions header.
+ * @brief      GPIO functions header.
- * @see        The GNU Public License (GPL) Version 3
+ * @see        The GNU Public License (GPL) Version 3
- *
+ *
- *****************************************************************************/
+ *****************************************************************************/
-/*
+/*
- * This program is free software; you can redistribute it and/or modify
+ * 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
+ * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
+ * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
+ * (at your option) any later version.
- *
+ *
- * This program is distributed in the hope that it will be useful, but
+ * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
+ * for more details.
- *
+ *
- * You should have received a copy of the GNU General Public License along
+ * 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.,
+ * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
+ */
-
+
-#ifndef _FIFO_BUFFER_H_
+#ifndef _FIFO_BUFFER_H_
-#define _FIFO_BUFFER_H_
+#define _FIFO_BUFFER_H_
-
+
-#include <stdint.h>
+#include "stdint.h"
-
+
-// *********************
+// *********************
-
+
-typedef struct
+typedef struct
-{
+{
-    uint8_t *buf_ptr;
+    uint8_t *buf_ptr;
-    volatile uint16_t rd;
+    volatile uint16_t rd;
-    volatile uint16_t wr;
+    volatile uint16_t wr;
-    uint16_t buf_size;
+    uint16_t buf_size;
-} t_fifo_buffer;
+} t_fifo_buffer;
-
+
-// *********************
+// *********************
-
+
-uint16_t fifoBuf_getSize(t_fifo_buffer *buf);
+uint16_t fifoBuf_getSize(t_fifo_buffer *buf);
-
+
-uint16_t fifoBuf_getUsed(t_fifo_buffer *buf);
+uint16_t fifoBuf_getUsed(t_fifo_buffer *buf);
-uint16_t fifoBuf_getFree(t_fifo_buffer *buf);
+uint16_t fifoBuf_getFree(t_fifo_buffer *buf);
-
+
-void fifoBuf_clearData(t_fifo_buffer *buf);
+void fifoBuf_clearData(t_fifo_buffer *buf);
-void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len);
+void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len);
-
+
-int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf);
+int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf);
-int16_t fifoBuf_getByte(t_fifo_buffer *buf);
+int16_t fifoBuf_getByte(t_fifo_buffer *buf);
-
+
-uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len);
+uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len);
-uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len);
+uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len);
-
+
-uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b);
+uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b);
-
+
-uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len);
+uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len);
-
+
-void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size);
+void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size);
-
+
-// *********************
+// *********************
-
+
-#endif
+#endif

flight/Libraries/inc/packet_handler.h

@@ -0,0 +1,121 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotSystem OpenPilot System
+ * @{
+ * @addtogroup OpenPilotLibraries OpenPilot System Libraries
+ * @{
+ *
+ * @file       packet_handler.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      A packet handler for handeling radio packet transmission.
+ * @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 __PACKET_HANDLER_H__
+#define __PACKET_HANDLER_H__
+
+// Public defines / macros
+#define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p)
+#define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size  + RS_ECC_NPARITY - (uint8_t*)p)
+
+// Public types
+typedef enum {
+	PACKET_TYPE_NONE = 0,
+	PACKET_TYPE_CONNECT,        // for requesting a connection
+	PACKET_TYPE_DISCONNECT,     // to tell the other modem they cannot connect to us
+	PACKET_TYPE_READY,          // tells the other modem we are ready to accept more data
+	PACKET_TYPE_NOTREADY,       // tells the other modem we're not ready to accept more data - we can also send user data in this packet type
+	PACKET_TYPE_STATUS,         // broadcasts status of this modem
+	PACKET_TYPE_DATARATE,       // for changing the RF data rate
+	PACKET_TYPE_PING,           // used to check link is still up
+	PACKET_TYPE_ADJUST_TX_PWR,  // used to ask the other modem to adjust it's tx power
+	PACKET_TYPE_DATA,           // data packet (packet contains user data)
+	PACKET_TYPE_ACKED_DATA,     // data packet that requies an ACK
+	PACKET_TYPE_PPM,            // PPM relay values
+	PACKET_TYPE_ACK,
+	PACKET_TYPE_NACK
+} PHPacketType;
+
+typedef struct {
+	uint32_t destination_id;
+	uint32_t source_id;
+	uint8_t type;
+	uint8_t data_size;
+	uint8_t tx_seq;
+	uint8_t rx_seq;
+} PHPacketHeader;
+
+#define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY)
+#define PH_PACKET_SIZE(p) (p->data + p->header.data_size - (uint8_t*)p + RS_ECC_NPARITY)
+typedef struct {
+	PHPacketHeader header;
+	uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
+} PHPacket, *PHPacketHandle;
+
+#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
+typedef struct {
+	PHPacketHeader header;
+	uint16_t channels[PIOS_RCVR_MAX_CHANNELS];
+	uint8_t ecc[RS_ECC_NPARITY];
+} PHPpmPacket, *PHPpmPacketHandle;
+
+#define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
+typedef struct {
+	PHPacketHeader header;
+	uint16_t retries;
+	uint16_t errors;
+	uint16_t uavtalk_errors;
+	uint16_t dropped;
+	uint16_t resets;
+	uint8_t ecc[RS_ECC_NPARITY];
+} PHStatusPacket, *PHStatusPacketHandle;
+
+typedef struct {
+	uint8_t winSize;
+	uint16_t maxConnections;
+} PacketHandlerConfig;
+
+typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
+typedef void (*PHDataHandler)(uint8_t *data, uint8_t len, int8_t rssi, int8_t afc);
+typedef void (*PHStatusHandler)(PHStatusPacketHandle s, int8_t rssi, int8_t afc);
+typedef void (*PHPPMHandler)(uint16_t *channels);
+
+typedef uint32_t PHInstHandle;
+
+// Public functions
+PHInstHandle PHInitialize(PacketHandlerConfig *cfg);
+void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f);
+void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f);
+void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f);
+void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
+uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
+PHPacketHandle PHGetRXPacket(PHInstHandle h);
+void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
+PHPacketHandle PHGetTXPacket(PHInstHandle h);
+void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
+uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
+int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len);
+uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error);
+
+#endif // __PACKET_HANDLER_H__
+
+/**
+ * @}
+ * @}
+ */

flight/Libraries/insgps13state.c

@@ -76,36 +76,52 @@ uint16_t ins_get_num_states()
 
 void INSGPSInit()		//pretty much just a place holder for now
 {
-	Be[0] = 1;
+	Be[0] = 1.0f;
-	Be[1] = 0;
+	Be[1] = 0.0f;
-	Be[2] = 0;		// local magnetic unit vector
+	Be[2] = 0.0f;		// local magnetic unit vector
 
 	for (int i = 0; i < NUMX; i++) {
 		for (int j = 0; j < NUMX; j++) {
-			P[i][j] = 0; // zero all terms
+			P[i][j] = 0.0f; // zero all terms
+			F[i][j] = 0.0f;
 		}
+		
+		for (int j = 0; j < NUMW; j++)
+			G[i][j] = 0.0f;
+			
+		for (int j = 0; j < NUMV; j++) {
+			H[j][i] = 0.0f;
+			K[i][j] = 0.0f;
+		}
+			
+		X[i] = 0.0f;
 	}
+	for (int i = 0; i < NUMW; i++)
+		Q[i] = 0.0f;
+	for (int i = 0; i < NUMV; i++) 
+		R[i] = 0.0f;
+
 	
-	P[0][0] = P[1][1] = P[2][2] = 25;	// initial position variance (m^2)
+	P[0][0] = P[1][1] = P[2][2] = 25.0f;            // initial position variance (m^2)
-	P[3][3] = P[4][4] = P[5][5] = 5;	// initial velocity variance (m/s)^2
+	P[3][3] = P[4][4] = P[5][5] = 5.0f;             // initial velocity variance (m/s)^2
-	P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5;	// initial quaternion variance
+	P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f;  // initial quaternion variance
-	P[10][10] = P[11][11] = P[12][12] = 1e-5;	// initial gyro bias variance (rad/s)^2
+	P[10][10] = P[11][11] = P[12][12] = 1e-9f;      // initial gyro bias variance (rad/s)^2
 
-	X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0;	// initial pos and vel (m)
+	X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f;	// initial pos and vel (m)
-	X[6] = 1;
+	X[6] = 1.0f;
-	X[7] = X[8] = X[9] = 0;	// initial quaternion (level and North) (m/s)
+	X[7] = X[8] = X[9] = 0.0f;	    // initial quaternion (level and North) (m/s)
-	X[10] = X[11] = X[12] = 0;	// initial gyro bias (rad/s)
+	X[10] = X[11] = X[12] = 0.0f;	// initial gyro bias (rad/s)
 
-	Q[0] = Q[1] = Q[2] = 50e-8;	// gyro noise variance (rad/s)^2
+	Q[0] = Q[1] = Q[2] = 50e-4f;	// gyro noise variance (rad/s)^2
-	Q[3] = Q[4] = Q[5] = 0.01;	// accelerometer noise variance (m/s^2)^2
+	Q[3] = Q[4] = Q[5] = 0.00001f;	// accelerometer noise variance (m/s^2)^2
-	Q[6] = Q[7] = Q[8] = 2e-9;	// gyro bias random walk variance (rad/s^2)^2
+	Q[6] = Q[7] = Q[8] = 2e-8f;	    // gyro bias random walk variance (rad/s^2)^2
 
-	R[0] = R[1] = 0.004;	// High freq GPS horizontal position noise variance (m^2)
+	R[0] = R[1] = 0.004f;	// High freq GPS horizontal position noise variance (m^2)
-	R[2] = 0.036;		// High freq GPS vertical position noise variance (m^2)
+	R[2] = 0.036f;          // High freq GPS vertical position noise variance (m^2)
-	R[3] = R[4] = 0.004;	// High freq GPS horizontal velocity noise variance (m/s)^2
+	R[3] = R[4] = 0.004f;   // High freq GPS horizontal velocity noise variance (m/s)^2
-	R[5] = 100;		// High freq GPS vertical velocity noise variance (m/s)^2
+	R[5] = 100.0f;          // High freq GPS vertical velocity noise variance (m/s)^2
-	R[6] = R[7] = R[8] = 0.005;	// magnetometer unit vector noise variance
+	R[6] = R[7] = R[8] = 0.005f;    // magnetometer unit vector noise variance
-	R[9] = .05;		// High freq altimeter noise variance (m^2)
+	R[9] = .05f;                    // High freq altimeter noise variance (m^2)
 }
 
 void INSResetP(float PDiag[NUMX])
@@ -116,7 +132,7 @@ void INSResetP(float PDiag[NUMX])
 	for (i=0;i<NUMX;i++){
 		if (PDiag != 0){
 			for (j=0;j<NUMX;j++)
-				P[i][j]=P[j][i]=0;
+				P[i][j]=P[j][i]=0.0f;
 			P[i][i]=PDiag[i];
 		}
 	}
@@ -223,7 +239,7 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
 	// EKF prediction step
 	LinearizeFG(X, U, F, G);
 	RungeKutta(X, U, dT);
-	qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
+	qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
 	X[6] /= qmag;
 	X[7] /= qmag;
 	X[8] /= qmag;
@@ -307,7 +323,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
 
 	// magnetometer data in any units (use unit vector) and in body frame
 	Bmag =
-	    sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
+	    sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
 		 mag_data[2] * mag_data[2]);
 	Z[6] = mag_data[0] / Bmag;
 	Z[7] = mag_data[1] / Bmag;
@@ -320,7 +336,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
 	LinearizeH(X, Be, H);
 	MeasurementEq(X, Be, Y);
 	SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
-	qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
+	qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
 	X[6] /= qmag;
 	X[7] /= qmag;
 	X[8] /= qmag;

flight/Libraries/insgps_helper.c

@@ -0,0 +1,328 @@
+
+#include "ins.h"
+#include "pios.h"
+#include "ahrs_spi_comm.h"
+#include "insgps.h"
+#include "CoordinateConversions.h"
+
+#define DEG_TO_RAD         (M_PI / 180.0)
+#define RAD_TO_DEG         (180.0 / M_PI)
+
+#define INSGPS_GPS_TIMEOUT 2   /* 2 seconds triggers reinit of position */
+#define INSGPS_GPS_MINSAT  6   /* 2 seconds triggers reinit of position */
+#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates    */
+ /* If GPS is more than this distance on any dimension then wait a few updates */
+ /* and reinitialize there */
+#define INSGPS_GPS_FAR     10 
+
+//! Contains the data from the mag sensor chip
+extern struct mag_sensor mag_data;
+
+//! Contains the data from the accelerometer
+extern struct accel_sensor  accel_data;
+
+//! Contains the data from the gyro
+extern struct gyro_sensor gyro_data;
+
+//! Conains the current estimate of the attitude
+extern struct attitude_solution attitude_data;
+
+//! Contains data from the altitude sensor
+extern struct altitude_sensor altitude_data;
+
+//! Contains data from the GPS (via the SPI link)
+extern struct gps_sensor gps_data;
+
+//! Offset correction of barometric alt, to match gps data
+static float baro_offset = 0;
+
+extern void send_calibration(void);
+extern void send_attitude(void);
+extern void send_velocity(void);
+extern void send_position(void);
+extern volatile int8_t ahrs_algorithm;
+extern void get_accel_gyro_data();
+extern void get_mag_data();
+
+/* INS functions */
+/**
+ * @brief Update the EKF when in outdoor mode.  The primary difference is using the GPS values.
+ */
+uint32_t total_far_count = 0;
+uint32_t relocated = 0;
+void ins_outdoor_update()
+{
+	static uint32_t ins_last_time;
+	float gyro[3], accel[3], vel[3];
+	float dT;
+	uint16_t sensors;
+	static uint32_t gps_far_count = 0;
+	
+	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
+	ins_last_time = PIOS_DELAY_GetRaw();
+	
+	// This should only happen at start up or at mode switches
+	if(dT > 0.01)
+		dT = 0.01;
+		
+	// format data for INS algo
+	gyro[0] = gyro_data.filtered.x;
+	gyro[1] = gyro_data.filtered.y;
+	gyro[2] = gyro_data.filtered.z;
+	accel[0] = accel_data.filtered.x,
+	accel[1] = accel_data.filtered.y,
+	accel[2] = accel_data.filtered.z,
+	
+	INSStatePrediction(gyro, accel, dT);
+	attitude_data.quaternion.q1 = Nav.q[0];
+	attitude_data.quaternion.q2 = Nav.q[1];
+	attitude_data.quaternion.q3 = Nav.q[2];
+	attitude_data.quaternion.q4 = Nav.q[3];
+	send_attitude();  // get message out quickly
+	INSCovariancePrediction(dT);
+
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	positionActual.North = Nav.Pos[0];
+	positionActual.East = Nav.Pos[1];
+	positionActual.Down = Nav.Pos[2];
+	PositionActualSet(&positionActual);
+	
+	VelocityActualData velocityActual;
+	VelocityActualGet(&velocityActual);
+	velocityActual.North = Nav.Vel[0];
+	velocityActual.East = Nav.Vel[1];
+	velocityActual.Down = Nav.Vel[2];
+	VelocityActualSet(&velocityActual);
+
+	sensors = 0;
+	
+	if (gps_data.updated)
+	{
+		vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
+		vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
+		vel[2] = 0;
+		
+		if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR ||
+		    abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR ||
+		    abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR ||
+		    abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR ||
+		    abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) {
+			gps_far_count++;
+			total_far_count++;
+			gps_data.updated = false;
+			
+			if(gps_far_count > 30) {
+				INSPosVelReset(gps_data.NED,vel);
+				relocated++;
+			}
+		}
+		else {			
+			sensors |= HORIZ_SENSORS | POS_SENSORS;
+			
+			/*
+			 * When using gps need to make sure that barometer is brought into NED frame
+			 * we should try and see if the altitude from the home location is good enough
+			 * to use for the offset but for now starting with this conservative filter
+			 */
+			if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
+				baro_offset = gps_data.NED[2] + altitude_data.altitude;
+			} else {
+				/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
+				baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
+			}
+			gps_data.updated = false;
+		}
+	}
+	
+	if(mag_data.updated) {
+		sensors |= MAG_SENSORS;
+		mag_data.updated = false;
+	}
+	
+	if(altitude_data.updated) {
+		sensors |= BARO_SENSOR;
+		altitude_data.updated = false;
+	}
+	
+	/*
+	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
+	 * although probably should occur within INS itself
+	 */
+	INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);
+	
+	if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
+		float zeros[3] = {0,0,0};
+		INSSetGyroBias(zeros);
+	}
+}
+
+/**
+ * @brief Update the EKF when in indoor mode
+ */
+void ins_indoor_update()
+{
+	static uint32_t updated_without_gps = 0;
+	
+	float gyro[3], accel[3];
+	float zeros[3] = {0, 0, 0};
+	static uint32_t ins_last_time = 0;
+	uint16_t sensors = 0;
+	float dT;
+
+	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
+	ins_last_time = PIOS_DELAY_GetRaw();
+	
+	// This should only happen at start up or at mode switches
+	if(dT > 0.01)
+		dT = 0.01;
+
+	// format data for INS algo
+	gyro[0] = gyro_data.filtered.x;
+	gyro[1] = gyro_data.filtered.y;
+	gyro[2] = gyro_data.filtered.z;
+	accel[0] = accel_data.filtered.x,
+	accel[1] = accel_data.filtered.y,
+	accel[2] = accel_data.filtered.z,
+	
+	INSStatePrediction(gyro, accel, dT);
+	attitude_data.quaternion.q1 = Nav.q[0];
+	attitude_data.quaternion.q2 = Nav.q[1];
+	attitude_data.quaternion.q3 = Nav.q[2];
+	attitude_data.quaternion.q4 = Nav.q[3];
+	send_attitude();  // get message out quickly
+	INSCovariancePrediction(dT);
+	
+	/* Indoors, update with zero position and velocity and high covariance */
+	sensors = HORIZ_SENSORS | VERT_SENSORS;
+	
+	if(mag_data.updated && (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
+		sensors |= MAG_SENSORS;
+		mag_data.updated = false;
+	}
+	
+	if(altitude_data.updated) {
+		sensors |= BARO_SENSOR;
+		altitude_data.updated = false;
+	}
+	
+	if(gps_data.updated) {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+		positionActual.North = gps_data.NED[0];
+		positionActual.East = gps_data.NED[1];
+		positionActual.Down = Nav.Pos[2];
+		PositionActualSet(&positionActual);
+		
+		VelocityActualData velocityActual;
+		VelocityActualGet(&velocityActual);
+		velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
+		velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
+		velocityActual.Down = Nav.Vel[2];
+		VelocityActualSet(&velocityActual);
+		
+		updated_without_gps = 0;
+		gps_data.updated = false;
+	} else {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+
+		VelocityActualData velocityActual;
+		VelocityActualGet(&velocityActual);
+
+		positionActual.Down = Nav.Pos[2];
+		velocityActual.Down = Nav.Vel[2];
+
+		if(updated_without_gps > 500) {
+			// After 2-3 seconds without a GPS update set velocity estimate to NAN
+			positionActual.North = NAN;
+			positionActual.East = NAN;
+			velocityActual.North = NAN;
+			velocityActual.East = NAN;
+		} else
+			updated_without_gps++;
+
+		PositionActualSet(&positionActual);
+		VelocityActualSet(&velocityActual);
+	}
+	
+	/*
+	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
+	 * although probably should occur within INS itself
+	 */
+	INSCorrection(mag_data.scaled.axis, zeros, zeros, altitude_data.altitude, sensors);
+	
+	if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
+		float zeros[3] = {0,0,0};
+		INSSetGyroBias(zeros);
+	}
+
+}
+
+/**
+ * @brief Initialize the EKF assuming stationary
+ */
+bool inited = false;
+float init_q[4];
+void ins_init_algorithm()
+{
+	inited = true;
+	float Rbe[3][3], q[4], accels[3], rpy[3], mag;
+	float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
+	bool using_mags, using_gps;
+	
+	INSGPSInit();
+	
+	HomeLocationData home;
+	HomeLocationGet(&home);
+	
+	accels[0]=accel_data.filtered.x;
+	accels[1]=accel_data.filtered.y;
+	accels[2]=accel_data.filtered.z;
+	
+	using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR);
+	using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0);  /* only use mags when valid home location */
+	
+	using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT);
+	
+	/* Block till a data update */
+	get_accel_gyro_data();
+	
+	/* Ensure we get mag data in a timely manner */
+	uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec
+	while(using_mags && !mag_data.updated && fail_count--) {
+		get_mag_data();
+		get_accel_gyro_data();
+		AhrsPoll();
+		PIOS_DELAY_WaituS(2000);
+	}
+	using_mags &= mag_data.updated;
+	
+	if (using_mags) {		
+		RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
+		R2Quaternion(Rbe,q);
+		if (using_gps)
+			INSSetState(gps_data.NED, zeros, q, zeros, zeros);
+		else
+			INSSetState(zeros, zeros, q, zeros, zeros);
+	} else {
+		// assume yaw = 0
+		mag = VectorMagnitude(accels);
+		rpy[1] = asinf(-accels[0]/mag);
+		rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
+		rpy[2] = 0;
+		RPY2Quaternion(rpy,init_q);
+		if (using_gps)
+			INSSetState(gps_data.NED, zeros, init_q, zeros, zeros);
+		else {
+			for (uint32_t i = 0; i < 5; i++) {
+				INSSetState(zeros, zeros, init_q, zeros, zeros);
+				ins_indoor_update();
+			}
+		}
+	}
+	
+	INSResetP(Pdiag);
+	
+	// TODO: include initial estimate of gyro bias?
+}

flight/Libraries/packet_handler.c

@@ -0,0 +1,507 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotSystem OpenPilot System
+ * @{
+ * @addtogroup OpenPilotLibraries OpenPilot System Libraries
+ * @{
+ *
+ * @file       packet_handler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      A packet handler for handeling radio packet transmission.
+ * @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 "packet_handler.h"
+#include "aes.h"
+#include "ecc.h"
+
+extern char *debug_msg;
+
+// Private types and constants
+typedef struct {
+	PacketHandlerConfig cfg;
+	PHPacket *tx_packets;
+	uint8_t tx_win_start;
+	uint8_t tx_win_end;
+	PHPacket *rx_packets;
+	uint8_t rx_win_start;
+	uint8_t rx_win_end;
+	uint16_t tx_seq_id;
+	PHOutputStream stream;
+	xSemaphoreHandle lock;
+	PHOutputStream output_stream;
+	PHDataHandler data_handler;
+	PHStatusHandler status_handler;
+	PHPPMHandler ppm_handler;
+} PHPacketData, *PHPacketDataHandle;
+
+// Private functions
+static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p);
+static uint8_t PHLSendNAck(PHPacketDataHandle data, PHPacketHandle p);
+static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p);
+
+/**
+ * Initialize the Packet Handler library
+ * \param[in] txWinSize The transmission window size (number of tx packet buffers).
+ * \param[in] streme A callback function for transmitting the packet.
+ * \param[in] id The source ID of transmitter.
+ * \return PHInstHandle The Pachet Handler instance data.
+ * \return 0 Failure
+ */
+PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
+{
+	// Allocate the primary structure
+	PHPacketDataHandle data = pvPortMalloc(sizeof(PHPacketData));
+	if (!data)
+		return 0;
+	data->cfg = *cfg;
+	data->tx_seq_id = 0;
+
+	// Allocate the packet windows
+	data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
+	data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
+
+	// Initialize the windows
+	data->tx_win_start = data->tx_win_end = 0;
+	data->rx_win_start = data->rx_win_end = 0;
+	for (uint8_t i = 0; i < data->cfg.winSize; ++i)
+	{
+		data->tx_packets[i].header.type = PACKET_TYPE_NONE;
+		data->rx_packets[i].header.type = PACKET_TYPE_NONE;
+	}
+
+	// Create the lock
+	data->lock = xSemaphoreCreateRecursiveMutex();
+
+	// Initialize the ECC library.
+	initialize_ecc();
+
+	// Return the structure.
+	return (PHInstHandle)data;
+}
+
+/**
+ * Register an output stream handler
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] f The output stream handler function
+ */
+void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	data->output_stream = f;
+}
+
+/**
+ * Register a data handler
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] f The data handler function
+ */
+void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	data->data_handler = f;
+}
+
+/**
+ * Register a PPM packet handler
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] f The PPM handler function
+ */
+void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	data->status_handler = f;
+}
+
+/**
+ * Register a PPM packet handler
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] f The PPM handler function
+ */
+void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	data->ppm_handler = f;
+}
+
+/**
+ * Get a packet out of the transmit buffer.
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] dest_id The destination ID of this connection
+ * \return PHPacketHandle A pointer to the packet buffer.
+ * \return 0 No packets buffers avaiable in the transmit window.
+ */
+uint32_t PHConnect(PHInstHandle h, uint32_t dest_id)
+{
+	return 1;
+}
+
+/**
+ * Get a packet out of the transmit buffer.
+ * \param[in] h The packet handler instance data pointer.
+ * \return PHPacketHandle A pointer to the packet buffer.
+ * \return 0 No packets buffers avaiable in the transmit window.
+ */
+PHPacketHandle PHGetTXPacket(PHInstHandle h)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	// Lock
+	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
+	PHPacketHandle p = data->tx_packets + data->tx_win_end;
+
+	// Is the window full?
+	uint8_t next_end = (data->tx_win_end + 1) % data->cfg.winSize;
+	if(next_end == data->tx_win_start)
+	{
+		xSemaphoreGiveRecursive(data->lock);
+		return NULL;
+	}
+	data->tx_win_end = next_end;
+
+	// Release lock
+	xSemaphoreGiveRecursive(data->lock);
+
+	// Return a pointer to the packet at the end of the TX window.
+	return p;
+}
+
+/**
+ * Release a packet from the transmit packet buffer window.
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer.
+ * \return Nothing
+ */
+void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	// Lock
+	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
+
+	// Change the packet type so we know this packet is unused.
+	p->header.type = PACKET_TYPE_NONE;
+
+	// If this packet is at the start of the window, increment the start index.
+	while ((data->tx_win_start != data->tx_win_end) &&
+	       (data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE))
+		data->tx_win_start = (data->tx_win_start + 1) % data->cfg.winSize;
+
+	// Release lock
+	xSemaphoreGiveRecursive(data->lock);
+}
+
+/**
+ * Get a packet out of the receive buffer.
+ * \param[in] h The packet handler instance data pointer.
+ * \return PHPacketHandle A pointer to the packet buffer.
+ * \return 0 No packets buffers avaiable in the transmit window.
+ */
+PHPacketHandle PHGetRXPacket(PHInstHandle h)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	// Lock
+	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
+	PHPacketHandle p = data->rx_packets + data->rx_win_end;
+
+	// Is the window full?
+	uint8_t next_end = (data->rx_win_end + 1) % data->cfg.winSize;
+	if(next_end == data->rx_win_start)
+	{
+		// Release lock
+		xSemaphoreGiveRecursive(data->lock);
+		return NULL;
+	}
+	data->rx_win_end = next_end;
+
+	// Release lock
+	xSemaphoreGiveRecursive(data->lock);
+
+	// Return a pointer to the packet at the end of the TX window.
+	return p;
+}
+
+/**
+ * Release a packet from the receive packet buffer window.
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer.
+ * \return Nothing
+ */
+void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	// Lock
+	xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
+
+	// Change the packet type so we know this packet is unused.
+	p->header.type = PACKET_TYPE_NONE;
+
+	// If this packet is at the start of the window, increment the start index.
+	while ((data->rx_win_start != data->rx_win_end) &&
+				 (data->rx_packets[data->rx_win_start].header.type == PACKET_TYPE_NONE))
+		data->rx_win_start = (data->rx_win_start + 1) % data->cfg.winSize;
+
+	// Release lock
+	xSemaphoreGiveRecursive(data->lock);
+}
+
+/**
+ * Transmit a packet from the transmit packet buffer window.
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer.
+ * \return 1 Success
+ * \return 0 Failure
+ */
+uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+
+	// Try to transmit the packet.
+	if (!PHLTransmitPacket(data, p))
+		return 0;
+
+	// If this packet doesn't require an ACK, remove it from the TX window.
+	switch (p->header.type) {
+	case PACKET_TYPE_READY:
+	case PACKET_TYPE_NOTREADY:
+	case PACKET_TYPE_DATA:
+	case PACKET_TYPE_PPM:
+		PHReleaseTXPacket(h, p);
+		break;
+	}
+
+	return 1;
+}
+
+/**
+ * Verify that a buffer contains a valid packet.
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer.
+ * \param[in] received_len The length of data received.
+ * \return < 0 Failure
+ * \return > 0 Number of bytes consumed.
+ */
+int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
+{
+
+	// Verify the packet length.
+	// Note: The last two bytes should be the RSSI and AFC.
+	uint16_t len = PHPacketSizeECC(p);
+	if (received_len < (len + 2))
+	{
+		DEBUG_PRINTF(1, "Packet length error %d %d\n\r", received_len, len + 2);
+		return -1;
+	}
+
+	// Attempt to correct any errors in the packet.
+	decode_data((unsigned char*)p, len);
+
+	// Check that there were no unfixed errors.
+	bool rx_error = check_syndrome() != 0;
+	if(rx_error)
+	{
+		DEBUG_PRINTF(1, "Error in packet\n\r");
+		return -2;
+	}
+
+	return len + 2;
+}
+
+/**
+ * Process a packet that has been received.
+ * \param[in] h The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer.
+ * \param[in] received_len The length of data received.
+ * \return 0 Failure
+ * \return 1 Success
+ */
+uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
+{
+	PHPacketDataHandle data = (PHPacketDataHandle)h;
+	uint16_t len = PHPacketSizeECC(p);
+
+	// Extract the RSSI and AFC.
+	int8_t rssi = *(((int8_t*)p) + len);
+	int8_t afc = *(((int8_t*)p) + len + 1);
+
+	switch (p->header.type) {
+
+	case PACKET_TYPE_STATUS:
+
+		if (!rx_error)
+
+			// Pass on the channels to the status handler.
+			if(data->status_handler)
+				data->status_handler((PHStatusPacketHandle)p, rssi, afc);
+
+		break;
+
+	case PACKET_TYPE_ACKED_DATA:
+
+		// Send the ACK / NACK
+		if (rx_error)
+		{
+			DEBUG_PRINTF(1, "Sending NACK\n\r");
+			PHLSendNAck(data, p);
+		}
+		else
+		{
+
+			PHLSendAck(data, p);
+
+			// Pass on the data.
+			if(data->data_handler)
+				data->data_handler(p->data, p->header.data_size, rssi, afc);
+		}
+
+		break;
+
+	case PACKET_TYPE_ACK:
+	{
+		// Find the packet ID in the TX buffer, and free it.
+		unsigned int i = 0;
+		for (unsigned int i = 0; i < data->cfg.winSize; ++i)
+			if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
+				PHReleaseTXPacket(h, data->tx_packets + i);
+#ifdef DEBUG_LEVEL
+		if (i == data->cfg.winSize)
+			DEBUG_PRINTF(1, "Error finding acked packet to release\n\r");
+#endif
+	}
+	break;
+
+	case PACKET_TYPE_NACK:
+	{
+		// Resend the packet.
+		unsigned int i = 0;
+		for ( ; i < data->cfg.winSize; ++i)
+			if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
+				PHLTransmitPacket(data, data->tx_packets + i);
+#ifdef DEBUG_LEVEL
+		if (i == data->cfg.winSize)
+			DEBUG_PRINTF(1, "Error finding acked packet to NACK\n\r");
+		DEBUG_PRINTF(1, "Resending after NACK\n\r");
+#endif
+	}
+	break;
+
+	case PACKET_TYPE_PPM:
+
+		if (!rx_error)
+
+			// Pass on the channels to the PPM handler.
+			if(data->ppm_handler)
+				data->ppm_handler(((PHPpmPacketHandle)p)->channels);
+
+		break;
+
+	case PACKET_TYPE_DATA:
+
+		if (!rx_error)
+
+			// Pass on the data to the data handler.
+			if(data->data_handler)
+				data->data_handler(p->data, p->header.data_size, rssi, afc);
+
+		break;
+
+	default:
+		break;
+	}
+
+	// Release the packet.
+	PHReleaseRXPacket(h, p);
+	
+	return 1;
+}
+
+/**
+ * Transmit a packet from the transmit packet buffer window.
+ * \param[in] data The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer.
+ * \return 1 Success
+ * \return 0 Failure
+ */
+static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p)
+{
+
+	if(!data->output_stream)
+		return 0;
+
+	// Set the sequence ID to the current ID.
+	p->header.tx_seq = data->tx_seq_id++;
+
+	// Add the error correcting code.
+	encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
+
+	// Transmit the packet using the output stream.
+	if(data->output_stream(p) == -1)
+		return 0;
+
+	return 1;
+}
+
+/**
+ * Send an ACK packet.
+ * \param[in] data The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer of the packet to be ACKed.
+ * \return 1 Success
+ * \return 0 Failure
+ */
+static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p)
+{
+
+	// Create the ACK message
+	PHPacketHeader ack;
+	ack.destination_id = p->header.source_id;
+	ack.type = PACKET_TYPE_ACK;
+	ack.rx_seq = p->header.tx_seq;
+	ack.data_size = 0;
+
+	// Send the packet.
+	return PHLTransmitPacket(data, (PHPacketHandle)&ack);
+}
+
+/**
+ * Send an NAck packet.
+ * \param[in] data The packet handler instance data pointer.
+ * \param[in] p A pointer to the packet buffer of the packet to be ACKed.
+ * \return 1 Success
+ * \return 0 Failure
+ */
+static uint8_t PHLSendNAck(PHPacketDataHandle data, PHPacketHandle p)
+{
+
+	// Create the NAck message
+	PHPacketHeader ack;
+	ack.destination_id = p->header.source_id;
+	ack.type = PACKET_TYPE_NACK;
+	ack.rx_seq = p->header.tx_seq;
+	ack.data_size = 0;
+
+	// Set the packet.
+	return PHLTransmitPacket(data, (PHPacketHandle)&ack);
+}

flight/Libraries/rscode/LICENSE

@@ -0,0 +1,10 @@
+ * (C) Henry Minsky (hqm@alum.mit.edu) 1991-2009
+ *
+ * This software library is licensed under terms of the GNU GENERAL
+ * PUBLIC LICENSE. [See file gpl.txt]
+ * 
+ * Commercial licensing is available under a separate license, please
+ * contact author for details.
+ *
+ * Latest source code and other info at http://rscode.sourceforge.net
+

flight/Libraries/rscode/Makefile

@@ -0,0 +1,50 @@
+# Makefile for Cross Interleaved Reed Solomon encoder/decoder
+#
+# (c) Henry Minsky, Universal Access 1991-1996
+#
+
+RANLIB = ranlib
+AR = ar
+
+
+VERSION = 1.0
+DIRNAME= rscode-$(VERSION)
+
+
+CC = gcc
+# OPTIMIZE_FLAGS = -O69
+DEBUG_FLAGS = -g
+CFLAGS = -Wall -Wstrict-prototypes  $(OPTIMIZE_FLAGS) $(DEBUG_FLAGS) -I..
+LDFLAGS = $(OPTIMIZE_FLAGS) $(DEBUG_FLAGS)
+
+LIB_CSRC = rs.c galois.c berlekamp.c crcgen.c 
+LIB_HSRC = ecc.h
+LIB_OBJS = rs.o galois.o berlekamp.o crcgen.o 
+
+TARGET_LIB = libecc.a
+TEST_PROGS = example
+
+TARGETS = $(TARGET_LIB) $(TEST_PROGS)
+
+all: $(TARGETS)
+
+$(TARGET_LIB): $(LIB_OBJS)
+	$(RM) $@
+	$(AR) cq $@ $(LIB_OBJS)
+	if [ "$(RANLIB)" ]; then $(RANLIB) $@; fi
+
+example: example.o galois.o berlekamp.o crcgen.o rs.o
+	gcc -o example example.o -L. -lecc
+
+clean:
+	rm -f *.o example libecc.a
+	rm -f *~
+
+dist:
+	(cd ..; tar -cvf rscode-$(VERSION).tar $(DIRNAME))
+
+depend:
+	makedepend $(SRCS)
+
+# DO NOT DELETE THIS LINE -- make depend depends on it.
+

flight/Libraries/rscode/README

@@ -0,0 +1,27 @@
+RSCODE version 1.3
+
+See the files
+
+config.doc 	documentation of some compile time parameters
+rs.doc 		overview of the Reed-Solomon coding program
+rs.man		a man page, slightly outdated at this point
+example.c	a simple example of encoding,decoding, and error correction
+
+Makefile	should work on a Sun system, may require GNU make.
+
+
+Henry Minsky
+hqm@alum.mit.edu
+
+
+ * (c) Henry Minsky (hqm@alum.mit.edu) 1991-2009
+ *
+ * This software library is licensed under terms of the GNU GENERAL
+ * PUBLIC LICENSE. (See gpl.txt)
+ * 
+ * Commercial licensing is available under a separate license, please
+ * contact author for details.
+ *
+ * Source code is available at http://rscode.sourceforge.net
+
+

flight/Libraries/rscode/berlekamp.c

@@ -0,0 +1,324 @@
+/***********************************************************************
+ * Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
+ *
+ * This software library is licensed under terms of the GNU GENERAL
+ * PUBLIC LICENSE
+ * 
+ *
+ * RSCODE 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.
+ *
+ * RSCODE 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 Rscode.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Commercial licensing is available under a separate license, please
+ * contact author for details.
+ *
+ * Source code is available at http://rscode.sourceforge.net
+ * Berlekamp-Peterson and Berlekamp-Massey Algorithms for error-location
+ *
+ * From Cain, Clark, "Error-Correction Coding For Digital Communications", pp. 205.
+ *
+ * This finds the coefficients of the error locator polynomial.
+ *
+ * The roots are then found by looking for the values of a^n
+ * where evaluating the polynomial yields zero.
+ *
+ * Error correction is done using the error-evaluator equation  on pp 207.
+ *
+ */
+
+#include <stdio.h>
+#include "ecc.h"
+
+/* The Error Locator Polynomial, also known as Lambda or Sigma. Lambda[0] == 1 */
+static int Lambda[MAXDEG];
+
+/* The Error Evaluator Polynomial */
+static int Omega[MAXDEG];
+
+/* local ANSI declarations */
+static int compute_discrepancy(int lambda[], int S[], int L, int n);
+static void init_gamma(int gamma[]);
+static void compute_modified_omega (void);
+static void mul_z_poly (int src[]);
+
+/* error locations found using Chien's search*/
+static int ErrorLocs[256];
+static int NErrors;
+
+/* erasure flags */
+static int ErasureLocs[256];
+static int NErasures;
+
+/* From  Cain, Clark, "Error-Correction Coding For Digital Communications", pp. 216. */
+void
+Modified_Berlekamp_Massey (void)
+{	
+  int n, L, L2, k, d, i;
+  int psi[MAXDEG], psi2[MAXDEG], D[MAXDEG];
+  int gamma[MAXDEG];
+	
+  /* initialize Gamma, the erasure locator polynomial */
+  init_gamma(gamma);
+
+  /* initialize to z */
+  copy_poly(D, gamma);
+  mul_z_poly(D);
+	
+  copy_poly(psi, gamma);	
+  k = -1; L = NErasures;
+	
+  for (n = NErasures; n < RS_ECC_NPARITY; n++) {
+	
+    d = compute_discrepancy(psi, synBytes, L, n);
+		
+    if (d != 0) {
+		
+      /* psi2 = psi - d*D */
+      for (i = 0; i < MAXDEG; i++) psi2[i] = psi[i] ^ gmult(d, D[i]);
+		
+		
+      if (L < (n-k)) {
+	L2 = n-k;
+	k = n-L;
+	/* D = scale_poly(ginv(d), psi); */
+	for (i = 0; i < MAXDEG; i++) D[i] = gmult(psi[i], ginv(d));
+	L = L2;
+      }
+			
+      /* psi = psi2 */
+      for (i = 0; i < MAXDEG; i++) psi[i] = psi2[i];
+    }
+		
+    mul_z_poly(D);
+  }
+	
+  for(i = 0; i < MAXDEG; i++) Lambda[i] = psi[i];
+  compute_modified_omega();
+
+	
+}
+
+/* given Psi (called Lambda in Modified_Berlekamp_Massey) and synBytes,
+   compute the combined erasure/error evaluator polynomial as 
+   Psi*S mod z^4
+  */
+void
+compute_modified_omega ()
+{
+  int i;
+  int product[MAXDEG*2];
+	
+  mult_polys(product, Lambda, synBytes);	
+  zero_poly(Omega);
+  for(i = 0; i < RS_ECC_NPARITY; i++) Omega[i] = product[i];
+
+}
+
+/* polynomial multiplication */
+void
+mult_polys (int dst[], int p1[], int p2[])
+{
+  int i, j;
+  int tmp1[MAXDEG*2];
+	
+  for (i=0; i < (MAXDEG*2); i++) dst[i] = 0;
+	
+  for (i = 0; i < MAXDEG; i++) {
+    for(j=MAXDEG; j<(MAXDEG*2); j++) tmp1[j]=0;
+		
+    /* scale tmp1 by p1[i] */
+    for(j=0; j<MAXDEG; j++) tmp1[j]=gmult(p2[j], p1[i]);
+    /* and mult (shift) tmp1 right by i */
+    for (j = (MAXDEG*2)-1; j >= i; j--) tmp1[j] = tmp1[j-i];
+    for (j = 0; j < i; j++) tmp1[j] = 0;
+		
+    /* add into partial product */
+    for(j=0; j < (MAXDEG*2); j++) dst[j] ^= tmp1[j];
+  }
+}
+
+
+	
+/* gamma = product (1-z*a^Ij) for erasure locs Ij */
+void
+init_gamma (int gamma[])
+{
+  int e, tmp[MAXDEG];
+	
+  zero_poly(gamma);
+  zero_poly(tmp);
+  gamma[0] = 1;
+	
+  for (e = 0; e < NErasures; e++) {
+    copy_poly(tmp, gamma);
+    scale_poly(gexp[ErasureLocs[e]], tmp);
+    mul_z_poly(tmp);
+    add_polys(gamma, tmp);
+  }
+}
+	
+	
+	
+void 
+compute_next_omega (int d, int A[], int dst[], int src[])
+{
+  int i;
+  for ( i = 0; i < MAXDEG;  i++) {
+    dst[i] = src[i] ^ gmult(d, A[i]);
+  }
+}
+	
+
+
+int
+compute_discrepancy (int lambda[], int S[], int L, int n)
+{
+  int i, sum=0;
+	
+  for (i = 0; i <= L; i++) 
+    sum ^= gmult(lambda[i], S[n-i]);
+  return (sum);
+}
+
+/********** polynomial arithmetic *******************/
+
+void add_polys (int dst[], int src[]) 
+{
+  int i;
+  for (i = 0; i < MAXDEG; i++) dst[i] ^= src[i];
+}
+
+void copy_poly (int dst[], int src[]) 
+{
+  int i;
+  for (i = 0; i < MAXDEG; i++) dst[i] = src[i];
+}
+
+void scale_poly (int k, int poly[]) 
+{	
+  int i;
+  for (i = 0; i < MAXDEG; i++) poly[i] = gmult(k, poly[i]);
+}
+
+
+void zero_poly (int poly[]) 
+{
+  int i;
+  for (i = 0; i < MAXDEG; i++) poly[i] = 0;
+}
+
+
+/* multiply by z, i.e., shift right by 1 */
+static void mul_z_poly (int src[])
+{
+  int i;
+  for (i = MAXDEG-1; i > 0; i--) src[i] = src[i-1];
+  src[0] = 0;
+}
+
+
+/* Finds all the roots of an error-locator polynomial with coefficients
+ * Lambda[j] by evaluating Lambda at successive values of alpha. 
+ * 
+ * This can be tested with the decoder's equations case.
+ */
+
+
+void 
+Find_Roots (void)
+{
+  int sum, r, k;	
+  NErrors = 0;
+  
+  for (r = 1; r < 256; r++) {
+    sum = 0;
+    /* evaluate lambda at r */
+    for (k = 0; k < RS_ECC_NPARITY+1; k++) {
+      sum ^= gmult(gexp[(k*r)%255], Lambda[k]);
+    }
+    if (sum == 0) 
+      { 
+	ErrorLocs[NErrors] = (255-r); NErrors++; 
+	//if (DEBUG) fprintf(stderr, "Root found at r = %d, (255-r) = %d\n", r, (255-r));
+      }
+  }
+}
+
+/* Combined Erasure And Error Magnitude Computation 
+ * 
+ * Pass in the codeword, its size in bytes, as well as
+ * an array of any known erasure locations, along the number
+ * of these erasures.
+ * 
+ * Evaluate Omega(actually Psi)/Lambda' at the roots
+ * alpha^(-i) for error locs i. 
+ *
+ * Returns 1 if everything ok, or 0 if an out-of-bounds error is found
+ *
+ */
+
+int
+correct_errors_erasures (unsigned char codeword[], 
+			 int csize,
+			 int nerasures,
+			 int erasures[])
+{
+  int r, i, j, err;
+
+  /* If you want to take advantage of erasure correction, be sure to
+     set NErasures and ErasureLocs[] with the locations of erasures. 
+     */
+  NErasures = nerasures;
+  for (i = 0; i < NErasures; i++) ErasureLocs[i] = erasures[i];
+
+  Modified_Berlekamp_Massey();
+  Find_Roots();
+  
+
+  if ((NErrors <= RS_ECC_NPARITY) && NErrors > 0) { 
+
+    /* first check for illegal error locs */
+    for (r = 0; r < NErrors; r++) {
+      if (ErrorLocs[r] >= csize) {
+				//if (DEBUG) fprintf(stderr, "Error loc i=%d outside of codeword length %d\n", i, csize);
+	return(0);
+      }
+    }
+
+    for (r = 0; r < NErrors; r++) {
+      int num, denom;
+      i = ErrorLocs[r];
+      /* evaluate Omega at alpha^(-i) */
+
+      num = 0;
+      for (j = 0; j < MAXDEG; j++) 
+	num ^= gmult(Omega[j], gexp[((255-i)*j)%255]);
+      
+      /* evaluate Lambda' (derivative) at alpha^(-i) ; all odd powers disappear */
+      denom = 0;
+      for (j = 1; j < MAXDEG; j += 2) {
+	denom ^= gmult(Lambda[j], gexp[((255-i)*(j-1)) % 255]);
+      }
+      
+      err = gmult(num, ginv(denom));
+      //if (DEBUG) fprintf(stderr, "Error magnitude %#x at loc %d\n", err, csize-i);
+      
+      codeword[csize-i-1] ^= err;
+    }
+    return(1);
+  }
+  else {
+    //if (DEBUG && NErrors) fprintf(stderr, "Uncorrectable codeword\n");
+    return(0);
+  }
+}
+

flight/Libraries/rscode/config.doc

@@ -0,0 +1,18 @@
+The basic coding parameters are defined using
+macros, and an executable can be made by compiling using macro
+definitions defining the values of the following names in the file
+"ecc.h":
+
+The important compile time parameter is the number of parity bytes,
+specified by the #define NPAR.
+
+The library is shipped with 
+
+#define NPAR 4
+
+The error-correction routines are polynomial in the number of
+parity bytes, so try to keep NPAR small for high performance.
+
+Remember, the sum of the message length (in bytes) plus parity bytes
+must be less than or equal to 255. 
+

flight/Libraries/rscode/crcgen.c

@@ -0,0 +1,66 @@
+/*****************************
+ * Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
+ *
+ * This software library is licensed under terms of the GNU GENERAL
+ * PUBLIC LICENSE
+ *
+ * RSCODE 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.
+ *
+ * RSCODE 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 Rscode.  If not, see <http://www.gnu.org/licenses/>.
+
+ * Commercial licensing is available under a separate license, please
+ * contact author for details.
+ *
+ * Source code is available at http://rscode.sourceforge.net
+ *
+ * CRC-CCITT generator simulator for byte wide data.
+ *
+ *
+ * CRC-CCITT = x^16 + x^12 + x^5 + 1
+ *
+ *
+ ******************************/
+ 
+ 
+#include "ecc.h"
+
+BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum);
+
+/* Computes the CRC-CCITT checksum on array of byte data, length len
+*/
+BIT16 crc_ccitt(unsigned char *msg, int len)
+{
+	int i;
+	BIT16 acc = 0;
+
+	for (i = 0; i < len; i++) {
+		acc = crchware((BIT16) msg[i], (BIT16) 0x1021, acc);
+	}
+	
+	return(acc);
+}
+	
+/* models crc hardware (minor variation on polynomial division algorithm) */
+BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum)
+{
+	static BIT16 i;
+	data <<= 8;
+	for (i = 8; i > 0; i--) {
+		if ((data ^ accum) & 0x8000)
+			accum = ((accum << 1) ^ genpoly) & 0xFFFF;
+		else
+			accum = (accum<<1) & 0xFFFF;
+		data = (data<<1) & 0xFFFF;
+	}
+	return (accum);
+}
+		

flight/Libraries/rscode/ecc.h

@@ -0,0 +1,98 @@
+/* Reed Solomon Coding for glyphs
+ * Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
+ *
+ * This software library is licensed under terms of the GNU GENERAL
+ * PUBLIC LICENSE
+ *
+ * RSCODE 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.
+ *
+ * RSCODE 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 Rscode.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Source code is available at http://rscode.sourceforge.net
+ *
+ * Commercial licensing is available under a separate license, please
+ * contact author for details.
+ *
+ */
+
+/****************************************************************
+  
+  Below is NPAR, the only compile-time parameter you should have to
+  modify.
+  
+  It is the number of parity bytes which will be appended to
+  your data to create a codeword.
+
+  Note that the maximum codeword size is 255, so the
+  sum of your message length plus parity should be less than
+  or equal to this maximum limit.
+
+  In practice, you will get slooow error correction and decoding
+  if you use more than a reasonably small number of parity bytes.
+  (say, 10 or 20)
+
+  ****************************************************************/
+
+/****************************************************************/
+
+
+#include <openpilot.h>
+
+#define TRUE 1
+#define FALSE 0
+
+typedef unsigned long BIT32;
+typedef unsigned short BIT16;
+
+/* **************************************************************** */
+
+/* Maximum degree of various polynomials. */
+#define MAXDEG (RS_ECC_NPARITY*2)
+
+/*************************************/
+/* Encoder parity bytes */
+extern int pBytes[MAXDEG];
+
+/* Decoder syndrome bytes */
+extern int synBytes[MAXDEG];
+
+/* print debugging info */
+extern int DEBUG;
+
+/* Reed Solomon encode/decode routines */
+void initialize_ecc (void);
+int check_syndrome (void);
+void decode_data (unsigned char data[], int nbytes);
+void encode_data (unsigned char msg[], int nbytes, unsigned char dst[]);
+
+/* CRC-CCITT checksum generator */
+BIT16 crc_ccitt(unsigned char *msg, int len);
+
+/* galois arithmetic tables */
+extern const int gexp[];
+extern const int glog[];
+
+void init_galois_tables (void);
+int ginv(int elt); 
+int gmult(int a, int b);
+
+
+/* Error location routines */
+int correct_errors_erasures (unsigned char codeword[], int csize,int nerasures, int erasures[]);
+
+/* polynomial arithmetic */
+void add_polys(int dst[], int src[]) ;
+void scale_poly(int k, int poly[]);
+void mult_polys(int dst[], int p1[], int p2[]);
+
+void copy_poly(int dst[], int src[]);
+void zero_poly(int poly[]);

flight/Libraries/rscode/example.c

@@ -0,0 +1,128 @@
+/* Example use of Reed-Solomon library 
+ *
+ * Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
+ *
+ * This software library is licensed under terms of the GNU GENERAL
+ * PUBLIC LICENSE
+ *
+ * RSCODE 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.
+ *
+ * RSCODE 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 Rscode.  If not, see <http://www.gnu.org/licenses/>.
+
+ * Commercial licensing is available under a separate license, please
+ * contact author for details.
+ *
+ * This same code demonstrates the use of the encodier and 
+ * decoder/error-correction routines. 
+ *
+ * We are assuming we have at least four bytes of parity (NPAR >= 4).
+ * 
+ * This gives us the ability to correct up to two errors, or 
+ * four erasures. 
+ *
+ * In general, with E errors, and K erasures, you will need
+ * 2E + K bytes of parity to be able to correct the codeword
+ * back to recover the original message data.
+ *
+ * You could say that each error 'consumes' two bytes of the parity,
+ * whereas each erasure 'consumes' one byte.
+ *
+ * Thus, as demonstrated below, we can inject one error (location unknown)
+ * and two erasures (with their locations specified) and the 
+ * error-correction routine will be able to correct the codeword
+ * back to the original message.
+ * */
+ 
+#include <stdio.h>
+#include <stdlib.h>
+#include "ecc.h"
+ 
+unsigned char msg[] = "Nervously I loaded the twin ducks aboard the revolving pl\
+atform.";
+unsigned char codeword[256];
+ 
+/* Some debugging routines to introduce errors or erasures
+   into a codeword. 
+   */
+
+/* Introduce a byte error at LOC */
+void
+byte_err (int err, int loc, unsigned char *dst)
+{
+  printf("Adding Error at loc %d, data %#x\n", loc, dst[loc-1]);
+  dst[loc-1] ^= err;
+}
+
+/* Pass in location of error (first byte position is
+   labeled starting at 1, not 0), and the codeword.
+*/
+void
+byte_erasure (int loc, unsigned char dst[], int cwsize, int erasures[]) 
+{
+  printf("Erasure at loc %d, data %#x\n", loc, dst[loc-1]);
+  dst[loc-1] = 0;
+}
+
+
+int
+main (int argc, char *argv[])
+{
+ 
+  int erasures[16];
+  int nerasures = 0;
+
+  /* Initialization the ECC library */
+ 
+  initialize_ecc ();
+ 
+  /* ************** */
+ 
+  /* Encode data into codeword, adding NPAR parity bytes */
+  encode_data(msg, sizeof(msg), codeword);
+ 
+  printf("Encoded data is: \"%s\"\n", codeword);
+ 
+#define ML (sizeof (msg) + NPAR)
+
+
+  /* Add one error and two erasures */
+  byte_err(0x35, 3, codeword);
+
+  byte_err(0x23, 17, codeword);
+  byte_err(0x34, 19, codeword);
+
+
+  printf("with some errors: \"%s\"\n", codeword);
+
+  /* We need to indicate the position of the erasures.  Eraseure
+     positions are indexed (1 based) from the end of the message... */
+
+  erasures[nerasures++] = ML-17;
+  erasures[nerasures++] = ML-19;
+
+ 
+  /* Now decode -- encoded codeword size must be passed */
+  decode_data(codeword, ML);
+
+  /* check if syndrome is all zeros */
+  if (check_syndrome () != 0) {
+    correct_errors_erasures (codeword, 
+			     ML,
+			     nerasures, 
+			     erasures);
+ 
+    printf("Corrected codeword: \"%s\"\n", codeword);
+  }
+ 
+  exit(0);
+}
+