Merge branch 'next' into MikeL

Conflicts:
	flight/Bootloaders/Revolution/inc/pios_config.h
	flight/Modules/OveroSync/inc/overosync.h
	flight/Modules/Sensors/inc/sensors.h
	flight/PiOS/Boards/STM32F4xx_Revolution.h
	flight/PiOS/STM32F4xx/pios_iap.c
	flight/Revolution/System/inc/pios_config.h
	ground/openpilotgcs/src/plugins/config/config.pro
	ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
	ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp

.gitmodules

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

HISTORY.txt

@@ -1,5 +1,22 @@
 Short summary of changes.  For a complete list see the git log.
 
+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.

Makefile

@@ -72,6 +72,7 @@ help:
 	@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"
@@ -172,7 +173,6 @@ 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 --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 "*"
@@ -209,36 +209,173 @@ 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; \
+	  $(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
@@ -260,6 +397,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
@@ -429,50 +598,64 @@ 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)_bin
+	$(V1) mkdir -p $(BUILD_DIR)/ef_$(1)/dep
+	$(V1) cd $(ROOT_DIR)/flight/EntireFlash && \
+		$$(MAKE) -r --no-print-directory \
+		BOARD_NAME=$(1) \
+		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
+
 # $(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 revolution
 
 # 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
 
 # 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, $(BL_BOARDS))
 BU_BOARDS  := $(filter-out ins, $(BU_BOARDS))
 
-# FIXME: The CC bootloader updaters don't work anymore due to
-#        differences between CC and CC3D
-BU_BOARDS  := $(filter-out coptercontrol, $(BU_BOARDS))
-
-# FIXME: PipXtreme bootloader updater doesn't work due to missing
-#        definitions for LEDs
-BU_BOARDS  := $(filter-out pipxtreme, $(BU_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))
@@ -486,9 +669,13 @@ 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))))
@@ -502,6 +689,9 @@ $(foreach board, $(ALL_BOARDS), $(eval $(call FW_TEMPLATE,$(board),$($(board)_fr
 # Expand the bootloader rules
 $(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_friendly))))
 
+# Expand the entire-flash rules
+$(foreach board, $(ALL_BOARDS), $(eval $(call EF_TEMPLATE,$(board),$($(board)_friendly))))
+
 .PHONY: sim_posix
 sim_posix: sim_posix_elf
 

flight/AHRS/Makefile

@@ -1,424 +0,0 @@
- #####
- # Project: OpenPilot AHRS
- #
- #
- # Makefile for OpenPilot AHRS project
- #
- # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
- #
- #
- # This program is free software; you can redistribute it and/or modify
- # it under the terms of the GNU General Public License as published by
- # the Free Software Foundation; either version 3 of the License, or
- # (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 ?= NO
-
-# 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
-AHRS = ./
-AHRSINC = $(AHRS)/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/AHRS
-BOOTINC = $(BOOT)/inc
-HWDEFSINC = ../board_hw_defs/$(BOARD_NAME)
-
-OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
-
-# List C source files here. (C dependencies are automatically generated.)
-# use file-extension c for "c-only"-files
-
-## AHRS:
-SRC = ahrs.c
-SRC += pios_board.c
-SRC += ahrs_timer.c
-SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
-SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
-SRC += $(BOOT)/ahrs_spi_program_slave.c
-SRC += $(BOOT)/ahrs_slave_test.c
-SRC += $(BOOT)/ahrs_spi_program.c
-SRC += insgps13state.c
-SRC += $(FLIGHTLIB)/CoordinateConversions.c
-SRC += $(FLIGHTLIB)/fifo_buffer.c
-
-## PIOS Hardware (STM32F10x)
-SRC += $(PIOSSTM32F10X)/pios_adc.c
-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_gpio.c
-SRC += $(PIOSSTM32F10X)/pios_spi.c
-SRC += $(PIOSSTM32F10X)/pios_exti.c
-
-## PIOS Hardware (Common)
-SRC += $(PIOSCOMMON)/pios_com.c
-SRC += $(PIOSCOMMON)/pios_hmc5843.c
-SRC += $(PIOSCOMMON)/printf-stdarg.c
-SRC += $(PIOSCOMMON)/pios_iap.c
-SRC += $(PIOSCOMMON)/pios_bl_helper.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  += $(AHRSINC)
-EXTRAINCDIRS  += $(OPUAVSYNTHDIR)
-EXTRAINCDIRS  += $(BOOTINC)
-EXTRAINCDIRS  += $(HWDEFSINC)
-
-# List any extra directories to look for library files here.
-# Also add directories where the linker should search for
-# includes from linker-script to the list
-#     Each directory must be seperated by a space.
-EXTRA_LIBDIRS =
-
-# Extra Libraries
-#    Each library-name must be seperated by a space.
-#    i.e. to link with libxyz.a, libabc.a and libefsl.a:
-#    EXTRA_LIBS = xyz abc efsl
-# for newlib-lpc (file: libnewlibc-lpc.a):
-#    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
-
-# Path to Linker-Scripts
-LINKERSCRIPTPATH = $(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 += -O0
-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_AHRS
-
-# 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++)
-
-CFLAGS += -g$(DEBUGF)
-
-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) $(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 $(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/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(PIOS_LED_HEARTBEAT);	// not keeping up
-	else
-		PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-	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(PIOS_LED_HEARTBEAT);
-	else {
-		PIOS_LED_On(PIOS_LED_HEARTBEAT);
-	}	
-}
-#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(PIOS_LED_HEARTBEAT);                              // 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(PIOS_LED_HEARTBEAT);          // all data not sent
-		else
-			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-	}
-#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(PIOS_LED_HEARTBEAT);
-
-		// 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/inc/pios_config.h

@@ -1,46 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pios_config.h  
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      PiOS configuration header. 
- *                 - Central compile time config for the project.
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/* 
- * This program is free software; you can redistribute it and/or modify 
- * it under the terms of the GNU General Public License as published by 
- * the Free Software Foundation; either version 3 of the License, or 
- * (at your option) any later version.
- * 
- * This program is distributed in the hope that it will be useful, but 
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
- * for more details.
- * 
- * You should have received a copy of the GNU General Public License along 
- * with this program; if not, write to the Free Software Foundation, Inc., 
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#ifndef PIOS_CONFIG_H
-#define PIOS_CONFIG_H
-
-/* Enable/Disable PiOS Modules */
-#define PIOS_INCLUDE_ADC
-#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_HMC5843
-#define PIOS_INCLUDE_GPIO
-#define PIOS_INCLUDE_EXTI
-#define PIOS_INCLUDE_BL_HELPER
-#define PIOS_INCLUDE_IAP
-
-#endif /* PIOS_CONFIG_H */

flight/AHRS/pios_board.c

@@ -1,118 +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
- */
-
-/* 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>
-
-#define PIOS_COM_AUX_TX_BUF_LEN 192
-static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
-
-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();
-
-#if defined(PIOS_INCLUDE_LED)
-	PIOS_LED_Init(&pios_led_cfg);
-#endif	/* PIOS_INCLUDE_LED */
-
-#if defined(PIOS_LED_HEARTBEAT)
-	PIOS_LED_On(PIOS_LED_HEARTBEAT);
-#endif	/* PIOS_LED_HEARTBEAT */
-
-	/* Delay system */
-	PIOS_DELAY_Init();
-
-	/* 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(PIOS_LED_HEARTBEAT);
-	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(PIOS_LED_HEARTBEAT);
-	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,268 +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;
-		}
-	}
-
-	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(PIOS_LED_HEARTBEAT);
-	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(PIOS_LED_HEARTBEAT);
-		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:
-		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(PIOS_LED_HEARTBEAT);
-		opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
-		if (!(user_rx_v0.payload.user.v.req.fwup_data.adress
-				< bdinfo->fw_base)) {
-			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(PIOS_LED_HEARTBEAT);
-		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(PIOS_LED_HEARTBEAT);
-		if (PIOS_BL_HELPER_FLASH_Start() == TRUE) {
-			boot_status = started;
-			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-		} 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(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);
-
-		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/BootloaderUpdater/main.c

@@ -33,7 +33,7 @@
 /* Prototype of PIOS_Board_Init() function */
 extern void PIOS_Board_Init(void);
 extern void FLASH_Download();
-void error(int);
+void error(void);
 
 /* The ADDRESSES of the _binary_* symbols are the important
  * data.  This is non-intuitive for _binary_size where you
@@ -50,14 +50,11 @@ int main() {
 
 	PIOS_SYS_Init();
 	PIOS_Board_Init();
-	PIOS_LED_On(PIOS_LED_HEARTBEAT);
-	PIOS_DELAY_WaitmS(3000);
-	PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 
 	/// Self overwrite check
 	uint32_t base_address = SCB->VTOR;
 	if ((0x08000000 + embedded_image_size) > base_address)
-		error(PIOS_LED_HEARTBEAT);
+		error();
 	///
 	FLASH_Unlock();
 
@@ -82,7 +79,7 @@ int main() {
 	}
 
 	if (fail == true)
-		error(PIOS_LED_HEARTBEAT);
+		error();
 
 
 	///
@@ -90,7 +87,6 @@ int main() {
 	/// Bootloader programing
 	for (uint32_t offset = 0; offset < embedded_image_size/sizeof(uint32_t); ++offset) {
 		bool result = false;
-		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,15 +94,9 @@ int main() {
 			}
 		}
 		if (result == false)
-			error(PIOS_LED_HEARTBEAT);
+			error();
 	}
 	///
-	for (uint8_t x = 0; x < 3; ++x) {
-			PIOS_LED_On(PIOS_LED_HEARTBEAT);
-			PIOS_DELAY_WaitmS(1000);
-			PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-			PIOS_DELAY_WaitmS(1000);
-	}
 
 	/// Invalidate the bootloader updater so we won't run
 	/// the update again on the next power cycle.
@@ -119,11 +109,7 @@ int main() {
 
 }
 
-void error(int led) {
+void error(void) {
 	for (;;) {
-		PIOS_LED_On(led);
-		PIOS_DELAY_WaitmS(500);
-		PIOS_LED_Off(led);
-		PIOS_DELAY_WaitmS(500);
 	}
 }

flight/Bootloaders/BootloaderUpdater/pios_board.c

@@ -46,9 +46,4 @@ void PIOS_Board_Init(void) {
 	
 	/* Initialize the PiOS library */
 	PIOS_GPIO_Init();
-
-#if defined(PIOS_INCLUDE_LED)
-	PIOS_LED_Init(&pios_led_cfg);
-#endif	/* PIOS_INCLUDE_LED */
-
 }

flight/Bootloaders/CopterControl/Makefile

@@ -104,6 +104,8 @@ 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
@@ -117,8 +119,6 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
 ## PIOS Hardware (Common)
 SRC += $(PIOSCOMMON)/pios_board_info.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
@@ -414,7 +414,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/main.c

@@ -70,14 +70,13 @@ uint8_t processRX();
 void jump_to_app();
 
 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_CheckAvailable(0);
 
 	if (PIOS_IAP_CheckRequest() == TRUE) {
-		PIOS_Board_Init();
 		PIOS_DELAY_WaitmS(1000);
 		User_DFU_request = TRUE;
 		PIOS_IAP_ClearRequest();

flight/Bootloaders/CopterControl/pios_board.c

@@ -31,7 +31,7 @@
  * 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;
@@ -59,8 +59,20 @@ void PIOS_Board_Init(void) {
 	/* Initialize the PiOS library */
 	PIOS_GPIO_Init();
 
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	
 #if defined(PIOS_INCLUDE_LED)
-	PIOS_LED_Init(&pios_led_cfg);
+	switch(bdinfo->board_rev) {
+		case 0x01: // Revision 1
+			PIOS_LED_Init(&pios_led_cfg_cc);
+			break;
+		case 0x02: // Revision 2
+			GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
+			PIOS_LED_Init(&pios_led_cfg_cc3d);
+			break;
+		default:
+			PIOS_Assert(0);
+	}
 #endif	/* PIOS_INCLUDE_LED */
 
 #if defined(PIOS_INCLUDE_USB)
@@ -71,8 +83,15 @@ void PIOS_Board_Init(void) {
 	PIOS_USB_DESC_HID_ONLY_Init();
 
 	uint32_t pios_usb_id;
-	if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) {
+	switch(bdinfo->board_rev) {
-		PIOS_Assert(0);
+		case 0x01: // Revision 1
+			PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
+			break;
+		case 0x02: // Revision 2
+			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;
@@ -90,3 +109,7 @@ void PIOS_Board_Init(void) {
 
 	board_init_complete = true;
 }
+
+void PIOS_ADC_DMA_Handler()
+{
+}

flight/Bootloaders/OpenPilot/Makefile

@@ -1,491 +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
-HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
-
-# 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.c
-SRC += $(PIOSSTM32F10X)/pios_usbhook.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
-SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
-SRC += $(OPSYSTEM)/pios_usb_board_data.c
-SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
-
-## PIOS Hardware (Common)
-SRC += $(PIOSCOMMON)/pios_board_info.c
-SRC += $(PIOSCOMMON)/pios_com_msg.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
-EXTRAINCDIRS  += $(HWDEFSINC)
-
-
-
-# List any extra directories to look for library files here.
-# Also add directories where the linker should search for
-# includes from linker-script to the list
-#     Each directory must be seperated by a space.
-EXTRA_LIBDIRS = 
-
-# Extra Libraries
-#    Each library-name must be seperated by a space.
-#    i.e. to link with libxyz.a, libabc.a and libefsl.a: 
-#    EXTRA_LIBS = xyz abc efsl
-# for newlib-lpc (file: libnewlibc-lpc.a):
-#    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
-
-# Path to Linker-Scripts
-LINKERSCRIPTPATH = $(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_config.h

@@ -1,58 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotBL OpenPilot BootLoader
- * @{
- * @file       pios_config.h  
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      PiOS configuration header. 
- *             Central compile time config for the project.
- *             In particular, pios_config.h is where you define which PiOS libraries
- *             and features are included in the firmware.
- * @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
-#define PIOS_INCLUDE_BL_HELPER
-#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
-/* Enable/Disable PiOS Modules */
-#define PIOS_INCLUDE_DELAY
-#define PIOS_INCLUDE_IRQ
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_SPI
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_USART
-#define PIOS_INCLUDE_USB
-#define PIOS_INCLUDE_USB_HID
-#define PIOS_INCLUDE_OPAHRS
-#define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_COM_MSG
-#define PIOS_INCLUDE_GPIO
-#define PIOS_INCLUDE_IAP
-//#define DEBUG_SSP
-
-/* Defaults for Logging */
-#define LOG_FILENAME 			"PIOS.LOG"
-#define STARTUP_LOG_ENABLED		1
-
-#endif /* PIOS_CONFIG_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,284 +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"
-#include "pios_com_msg.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[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();
-uint32_t sspTimeSource();
-
-#define LED_PWM_TIMER	TIM6
-int main() {
-	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(PIOS_LED_ALARM);
-			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(PIOS_LED_ALARM);
-			period2 = 0;
-			break;
-		case BLidle:
-			period1 = 0;
-			PIOS_LED_On(PIOS_LED_HEARTBEAT);
-			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(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_ValueGet(LED_PWM_TIMER)))
-				PIOS_LED_On(PIOS_LED_ALARM);
-			else
-				PIOS_LED_Off(PIOS_LED_ALARM);
-		} else
-			PIOS_LED_Off(PIOS_LED_ALARM);
-
-		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) {
-		if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
-			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) {
-	uint8_t byte;
-	if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF, &byte, 1, 0) == 1) {
-		return byte;
-	} 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,128 +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
- */
-
-/* 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>
-
-#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];
-
-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_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;
-	if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) {
-		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_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
-
-	/* 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

@@ -103,6 +103,8 @@ 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
@@ -416,7 +418,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/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,17 @@ 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.
@@ -172,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.
@@ -196,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)
 
@@ -238,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)
@@ -255,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
@@ -288,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
@@ -354,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,9 +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
-#define PIOS_INCLUDE_IAP
 
 #endif /* PIOS_CONFIG_H */

flight/Bootloaders/Revolution/inc/pios_usb_board_data.h

@@ -37,8 +37,8 @@
 
 #include "pios_usb_defs.h" 	/* struct usb_* */
 
-#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPENPILOT_MAIN
+#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION
-#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPENPILOT_MAIN, USB_OP_BOARD_MODE_BL)
+#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

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,12 +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"
 //programmable devices
 Device devicesTable[10];
 uint8_t numberOfDevices = 0;
@@ -72,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);
@@ -139,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;
@@ -159,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;
@@ -184,35 +181,18 @@ 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;
@@ -236,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:
@@ -259,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;
@@ -306,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) {
@@ -342,28 +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 {
-				if (Data == 0x5AFE) {
-					/* Force board into safe mode */
-					PIOS_IAP_WriteBootCount(0xFFFF);
-				}
-				FLASH_Lock();
-				JumpToApp = 1;
-			}
-		} else {
-			if (Data == 0x5AFE) {
-				/* Force board into safe mode */
-				PIOS_IAP_WriteBootCount(0xFFFF);
-			}
-			FLASH_Lock();
-			JumpToApp = 1;
 		}
+		FLASH_Lock();
+		JumpToApp = 1;
 		break;
 	case Reset:
 		PIOS_SYS_Reset();
@@ -401,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;
 
@@ -445,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;
 }
@@ -466,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) {
@@ -524,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:
@@ -553,34 +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_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
-	} 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

@@ -32,9 +32,10 @@
  */
 #include "board_hw_defs.c"
 
+#include <pios_board_info.h>
 #include <pios.h>
 
-#include "bl_fsm.h"		/* lfsm_* */
+uint32_t pios_com_telem_usb_id;
 
 static bool board_init_complete = false;
 void PIOS_Board_Init() {
@@ -42,16 +43,36 @@ void PIOS_Board_Init() {
 		return;
 	}
 
+	/* Delay system */
+	PIOS_DELAY_Init();
+	
 #if defined(PIOS_INCLUDE_LED)
 	PIOS_LED_Init(&pios_led_cfg);
 #endif	/* PIOS_INCLUDE_LED */
 
-	/* Set up the SPI interface to the OP board */
+#if defined(PIOS_INCLUDE_USB)
-	if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
+	/* Initialize board specific USB data */
-		PIOS_DEBUG_Assert(0);
+	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);
 	}
-	lfsm_attach(pios_spi_op_id);
+	if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
-	lfsm_init();
+		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

@@ -32,18 +32,19 @@
 #include "pios_sys.h"		 /* PIOS_SYS_SerialNumberGet */
 #include "pios_usbhook.h"	 /* PIOS_USBHOOK_* */
 
-static const uint8_t usb_product_id[20] = {
+static const uint8_t usb_product_id[22] = {
 	sizeof(usb_product_id),
 	USB_DESC_TYPE_STRING,
-	'O', 0,
+	'R', 0,
-	'p', 0,
 	'e', 0,
-	'n', 0,
+	'v', 0,
-	'P', 0,
-	'i', 0,
-	'l', 0,
 	'o', 0,
+	'l', 0,
+	'u', 0,
 	't', 0,
+	'i', 0,
+	'o', 0,
+	'n', 0,
 };
 
 static uint8_t usb_serial_number[52] = {

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
@@ -49,7 +50,7 @@ ENABLE_DEBUG_PINS ?= NO
 ENABLE_AUX_UART ?= 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)
@@ -185,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
@@ -227,8 +229,9 @@ 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)
 SRC += $(PIOSSTM32F10X)/pios_usb.c
@@ -244,13 +247,12 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.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_bmp085.c
+#SRC += $(PIOSCOMMON)/pios_bmp085.c
-SRC += $(PIOSCOMMON)/pios_iap.c
-SRC += $(PIOSCOMMON)/pios_bl_helper.c
 SRC += $(PIOSCOMMON)/pios_rcvr.c
 SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
 SRC += $(PIOSCOMMON)/printf-stdarg.c
@@ -603,7 +605,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,7 +71,7 @@ int main()
 	PIOS_Board_Init();
 
 #ifdef ERASE_FLASH
-	PIOS_Flash_W25X_EraseChip();
+	PIOS_Flash_Jedec_EraseChip();
 #if defined(PIOS_LED_HEARTBEAT)
 	PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 #endif	/* PIOS_LED_HEARTBEAT */

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

flight/CopterControl/System/inc/pios_config.h

@@ -36,10 +36,10 @@
 /* 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
@@ -77,8 +77,8 @@
 
 #define PIOS_INCLUDE_ADXL345
 #define PIOS_INCLUDE_FLASH
-
+#define PIOS_INCLUDE_MPU6000
-#define PIOS_INCLUDE_BMP085
+#define PIOS_MPU6000_ACCEL
 
 /* A really shitty setting saving implementation */
 #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS

flight/CopterControl/System/pios_board.c

@@ -43,6 +43,7 @@
 #include <manualcontrolsettings.h>
 #include <gcsreceiver.h>
 
+
 /* One slot per selectable receiver group.
  *  eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
  * NOTE: No slot in this map for NONE.
@@ -66,25 +67,140 @@ 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,
+};
+
+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,
+};
+
+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)
+	switch(bdinfo->board_rev) {
+		case 0x01: // Revision 1
+			PIOS_LED_Init(&pios_led_cfg_cc);
+			break;
+		case 0x02: // Revision 2
+			GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
+			PIOS_LED_Init(&pios_led_cfg_cc3d);
+			break;
+		default:
+			PIOS_Assert(0);
+	}
+#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 0x01: // Revision 1
+			if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) {
+				PIOS_Assert(0);
+			}
+			break;
+		case 0x02: // Revision 2
+			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 0x01: // Revision 1
+			PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg);	
+			PIOS_FLASHFS_Init(&flashfs_w25x_cfg);
+			break;
+		case 0x02: // Revision 2
+			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();
@@ -95,10 +211,6 @@ void PIOS_Board_Init(void) {
 	PIOS_RTC_Init(&pios_rtc_main_cfg);
 #endif
 
-#if defined(PIOS_INCLUDE_LED)
-	PIOS_LED_Init(&pios_led_cfg);
-#endif	/* PIOS_INCLUDE_LED */
-
 	HwSettingsInitialize();
 
 #ifndef ERASE_FLASH
@@ -162,7 +274,17 @@ void PIOS_Board_Init(void) {
 	}
 
 	uint32_t pios_usb_id;
-	PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
+	
+	switch(bdinfo->board_rev) {
+		case 0x01: // Revision 1
+			PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
+			break;
+		case 0x02: // Revision 2
+			PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
+			break;
+		default:
+			PIOS_Assert(0);
+	}
 
 #if defined(PIOS_INCLUDE_USB_CDC)
 
@@ -575,8 +697,35 @@ 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 0x01:
+			// 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 0x02:
+			// 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();
 
 	/* Make sure we have at least one telemetry link configured or else fail initialization */

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 " 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 " 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,431 +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
-HWDEFSINC = ../board_hw_defs/$(BOARD_NAME)
-
-OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
-
-# List C source files here. (C dependencies are automatically generated.)
-# use file-extension c for "c-only"-files
-
-## 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)
-EXTRAINCDIRS  += $(HWDEFSINC)
-
-# List any extra directories to look for library files here.
-# Also add directories where the linker should search for
-# includes from linker-script to the list
-#     Each directory must be seperated by a space.
-EXTRA_LIBDIRS =
-
-# Extra Libraries
-#    Each library-name must be seperated by a space.
-#    i.e. to link with libxyz.a, libabc.a and libefsl.a:
-#    EXTRA_LIBS = xyz abc efsl
-# for newlib-lpc (file: libnewlibc-lpc.a):
-#    EXTRA_LIBS = newlib-lpc
-EXTRA_LIBS =
-
-# Path to Linker-Scripts
-LINKERSCRIPTPATH = $(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,62 +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_IAP
-
-#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(PIOS_LED_HEARTBEAT, 1);
-	else
-		blink(PIOS_LED_ALARM, 1);
-}
-
-#if defined (PIOS_INCLUDE_HMC5883)
-void test_mag()
-{
-	if(PIOS_HMC5883_Test())
-		blink(PIOS_LED_HEARTBEAT, 2);
-	else
-		blink(PIOS_LED_ALARM, 2);
-}
-#endif
-
-#if defined (PIOS_INCLUDE_BMP085)
-void test_pressure()
-{
-	if(PIOS_BMP085_Test())
-		blink(PIOS_LED_HEARTBEAT, 3);
-	else
-		blink(PIOS_LED_ALARM, 3);
-}
-#endif
-
-#if defined (PIOS_INCLUDE_IMU3000)
-void test_imu()
-{
-	if(PIOS_IMU3000_Test())
-		blink(PIOS_LED_HEARTBEAT, 4);
-	else
-		blink(PIOS_LED_ALARM, 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,130 +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
- */
-
-/* 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>
-
-#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];
-
-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();
-
-#if defined(PIOS_INCLUDE_LED)
-	PIOS_LED_Init(&pios_led_cfg);
-#endif	/* PIOS_INCLUDE_LED */
-
-	/* 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,52 +1,42 @@
-/**
+/**
- ******************************************************************************
+ ******************************************************************************
- *
+ * @addtogroup OpenPilotModules OpenPilot Modules
- * @file       ahrs_bl.h
+ * @{
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GSPModule GPS Module
- * @brief      Main AHRS_BL header.
+ * @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 AHRS_BL_H
+ * You should have received a copy of the GNU General Public License along
-#define AHRS_BL_H
+ * with this program; if not, write to the Free Software Foundation, Inc.,
-
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
-/* PIOS Includes */
+ */
-#include <pios.h>
+
-
+#ifndef NMEA_H
-/** Start programming
+#define NMEA_H
- returns: true if FLASH erased and ready to program
+
- */
+#include <stdbool.h>
-bool
+#include <stdint.h>
-StartProgramming(void);
+
-
+#define NMEA_MAX_PACKET_LENGTH          96
-/** Write a block to FLASH
+
- buffer contains the data to be written
+extern bool NMEA_update_position(char *nmea_sentence);
- returns: true if FLASH programmed correctly
+extern bool NMEA_checksum(char *nmea_sentence);
- */
+
-bool
+#endif /* NMEA_H */
-WriteData(uint32_t offset, uint8_t *buffer, uint32_t size);
-
-/** Read a block from FLASH
- returns: true if FLASH read correctly.
- Buffer is set to the read data
- */
-bool
-ReadData(uint32_t offset, uint8_t *buffer, uint32_t size);
-
-#endif /* AHRS_BL_H */

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/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/Modules/AHRSComms/ahrs_comms.c

@@ -52,11 +52,10 @@
 
 #include "ahrs_comms.h"
 #include "ahrs_spi_comm.h"
-#include "ahrsstatus.h"
+#include "insstatus.h"
-#include "ahrscalibration.h"
 
 // Private constants
-#define STACK_SIZE configMINIMAL_STACK_SIZE-128
+#define STACK_SIZE configMINIMAL_STACK_SIZE
 #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
 
 // Private types
@@ -87,9 +86,10 @@ int32_t AHRSCommsStart(void)
  */
 int32_t AHRSCommsInitialize(void)
 {
-	AhrsStatusInitialize();
+	InsStatusInitialize();
-	AHRSCalibrationInitialize();
+	InsSettingsInitialize();
 	AttitudeRawInitialize();
+	AttitudeActualInitialize();
 	VelocityActualInitialize();
 	PositionActualInitialize();
 
@@ -109,19 +109,17 @@ static void ahrscommsTask(void *parameters)
 	// Main task loop
 	while (1) {
 		PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS);
+		AhrsCommStatus stat;
 		
-		AHRSSettingsData settings;
-		AHRSSettingsGet(&settings);
-
 		AhrsSendObjects();
-		AhrsCommStatus stat = AhrsGetStatus();
+		AhrsGetStatus(&stat);
 		if (stat.linkOk) {
 			AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
 		} else {
 			AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
 		}
-		AhrsStatusData sData;
+		InsStatusData sData;
-		AhrsStatusGet(&sData);
+		InsStatusGet(&sData);
 
 		sData.LinkRunning = stat.linkOk;
 		sData.AhrsKickstarts = stat.remote.kickStarts;
@@ -132,9 +130,9 @@ static void ahrscommsTask(void *parameters)
 		sData.OpRetries = stat.local.retries;
 		sData.OpInvalidPackets = stat.local.invalidPacket;
 
-		AhrsStatusSet(&sData);
+		InsStatusSet(&sData);
 		/* Wait for the next update interval */
-		vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS);
+		vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS);
 
 	}
 }

flight/Modules/Actuator/actuator.c

@@ -216,7 +216,7 @@ static void actuatorTask(void* parameters)
 		AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
 
 		bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
-		bool positiveThrottle = desired.Throttle >= 0.00;
+		bool positiveThrottle = desired.Throttle >= 0.00f;
 		bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
 
 		float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
@@ -325,26 +325,29 @@ static void actuatorTask(void* parameters)
 				else
 					status[ct] = -1;
 			}
-
-			command.Channel[ct] = scaleChannel(status[ct],
-							   ChannelMax[ct],
-							   ChannelMin[ct],
-							   ChannelNeutral[ct]);
 		}
-#if defined(DIAGNOSTICS)
+		
-		MixerStatusSet(&mixerStatus);
+		for(int i = 0; i < MAX_MIX_ACTUATORS; i++) 
-#endif
+			command.Channel[i] = scaleChannel(status[i],
-
+							   ChannelMax[i],
+							   ChannelMin[i],
+							   ChannelNeutral[i]);
+			
 		// Store update time
-		command.UpdateTime = 1000*dT;
+		command.UpdateTime = 1000.0f*dT;
-		if(1000*dT > command.MaxUpdateTime)
+		if(1000.0f*dT > command.MaxUpdateTime)
-			command.MaxUpdateTime = 1000*dT;
+			command.MaxUpdateTime = 1000.0f*dT;
-
+		
 		// Update output object
 		ActuatorCommandSet(&command);
 		// Update in case read only (eg. during servo configuration)
 		ActuatorCommandGet(&command);
 
+#if defined(DIAGNOSTICS)
+		MixerStatusSet(&mixerStatus);
+#endif
+		
+
 		// Update servo outputs
 		bool success = true;
 
@@ -367,22 +370,21 @@ static void actuatorTask(void* parameters)
 /**
  *Process mixing for one actuator
  */
-
 float ProcessMixer(const int index, const float curve1, const float curve2,
 		   MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period)
 {
 	Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects
 	Mixer_t * mixer = &mixers[index];
-	float result = ((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
+	float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
-	((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
+	(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
-	((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
+	(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
-	((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
+	(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
-	((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
+	(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
 	if(mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR)
 	{
-		if(result < 0) //idle throttle
+		if(result < 0.0f) //idle throttle
 		{
-			result = 0;
+			result = 0.0f;
 		}
 
 		//feed forward
@@ -392,7 +394,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
 		result += accumulator;
 		if(period !=0)
 		{
-			if(accumulator > 0)
+			if(accumulator > 0.0f)
 			{
 				float filter = mixerSettings->AccelTime / period;
 				if(filter <1)
@@ -422,6 +424,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
 		}
 		lastFilteredResult[index] = result;
 	}
+
 	return(result);
 }
 
@@ -432,7 +435,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
  */
 static float MixerCurve(const float throttle, const float* curve, uint8_t elements)
 {
-	float scale = throttle * (elements - 1);
+	float scale = throttle * (float) (elements - 1);
 	int idx1 = scale;
 	scale -= (float)idx1; //remainder
 	if(curve[0] < -1)
@@ -453,7 +456,7 @@ static float MixerCurve(const float throttle, const float* curve, uint8_t elemen
 			idx1 = elements -1;
 		}
 	}
-	return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale));
+	return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
 }
 
 
@@ -464,7 +467,7 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
 {
 	int16_t valueScaled;
 	// Scale
-	if ( value >= 0.0)
+	if ( value >= 0.0f)
 	{
 		valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral;
 	}
@@ -520,6 +523,8 @@ static void setFailsafe()
 		{
 			Channel[n] = 0;
 		}
+		
+		
 	}
 
 	// Set alarm
@@ -532,7 +537,7 @@ static void setFailsafe()
 	}
 
 	// Update output object's parts that we changed
-	ActuatorCommandChannelGet(Channel);
+	ActuatorCommandChannelSet(Channel);
 }
 
 

flight/Modules/Altitude/altitude.c

@@ -120,7 +120,7 @@ static void altitudeTask(void *parameters)
 {
 	BaroAltitudeData data;
 	portTickType lastSysTime;
-
+	
 #if defined(PIOS_INCLUDE_HCSR04)
 	SonarAltitudeData sonardata;
 	int32_t value=0,timeout=5;
@@ -129,7 +129,7 @@ static void altitudeTask(void *parameters)
 	PIOS_HCSR04_Trigger();
 #endif
 	PIOS_BMP085_Init();
-
+	
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	while (1)
@@ -145,7 +145,7 @@ static void altitudeTask(void *parameters)
 				height_out = (height_out * (1 - coeff)) + (height_in * coeff);
 				sonardata.Altitude = height_out; // m/us
 			}
-
+			
 			// Update the AltitudeActual UAVObject
 			SonarAltitudeSet(&sonardata);
 			timeout=5;
@@ -167,7 +167,7 @@ static void altitudeTask(void *parameters)
 #endif
 		PIOS_BMP085_ReadADC();
 		alt_ds_temp += PIOS_BMP085_GetTemperature();
-
+		
 		// Update the pressure data
 		PIOS_BMP085_StartADC(PressureConv);
 #ifdef PIOS_BMP085_HAS_GPIOS
@@ -177,7 +177,7 @@ static void altitudeTask(void *parameters)
 #endif
 		PIOS_BMP085_ReadADC();
 		alt_ds_pres += PIOS_BMP085_GetPressure();
-
+		
 		if (++alt_ds_count >= alt_ds_size)
 		{
 			alt_ds_count = 0;
@@ -203,6 +203,6 @@ static void altitudeTask(void *parameters)
 }
 
 /**
-  * @}
+ * @}
  * @}
  */

flight/Modules/Altitude/revolution/altitude.c

@@ -0,0 +1,158 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup AltitudeModule Altitude Module
+ * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object"
+ * @{ 
+ *
+ * @file       altitude.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Altitude module, handles temperature and pressure readings from BMP085
+ *
+ * @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
+ */
+
+/**
+ * Output object: BaroAltitude
+ *
+ * This module will periodically update the value of the BaroAltitude object.
+ *
+ */
+
+#include "openpilot.h"
+#include "altitude.h"
+#include "baroaltitude.h"	// object that will be updated by the module
+#if defined(PIOS_INCLUDE_HCSR04)
+#include "sonaraltitude.h"	// object that will be updated by the module
+#endif
+
+// Private constants
+#define STACK_SIZE_BYTES 500
+#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
+
+// Private types
+
+// Private variables
+static xTaskHandle taskHandle;
+
+// Private functions
+static void altitudeTask(void *parameters);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AltitudeStart()
+{	
+#if defined(PIOS_INCLUDE_HCSR04)
+	SonarAltitudeInitialze();
+#endif
+	
+	// Start main task
+	xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
+	TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);
+
+	return 0;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AltitudeInitialize()
+{
+	BaroAltitudeInitialize();
+
+	return 0;
+}
+MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
+/**
+ * Module thread, should not return.
+ */
+static void altitudeTask(void *parameters)
+{
+	BaroAltitudeData data;
+	
+#if defined(PIOS_INCLUDE_HCSR04)
+	SonarAltitudeData sonardata;
+	int32_t value=0,timeout=5;
+	float coeff=0.25,height_out=0,height_in=0;
+	PIOS_HCSR04_Init();
+	PIOS_HCSR04_Trigger();
+#endif
+
+	// TODO: Check the pressure sensor and set a warning if it fails test
+	
+	// Main task loop
+	while (1)
+	{
+#if defined(PIOS_INCLUDE_HCSR04)
+		// Compute the current altitude (all pressures in kPa)
+		if(PIOS_HCSR04_Completed())
+		{
+			value = PIOS_HCSR04_Get();
+			if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
+			{
+				height_in = value*0.00034;
+				height_out = (height_out * (1 - coeff)) + (height_in * coeff);
+				sonardata.Altitude = height_out; // m/us
+			}
+			
+			// Update the AltitudeActual UAVObject
+			SonarAltitudeSet(&sonardata);
+			timeout=5;
+			PIOS_HCSR04_Trigger();
+		}
+		if(timeout--)
+		{
+			//retrigger
+			timeout=5;
+			PIOS_HCSR04_Trigger();
+		}
+#endif
+		float temp, press;
+		
+		// Update the temperature data
+		PIOS_MS5611_StartADC(TemperatureConv);
+		vTaskDelay(PIOS_MS5611_GetDelay());
+		PIOS_MS5611_ReadADC();
+		
+		// Update the pressure data
+		PIOS_MS5611_StartADC(PressureConv);
+		vTaskDelay(PIOS_MS5611_GetDelay());
+		PIOS_MS5611_ReadADC();
+
+		
+		temp = PIOS_MS5611_GetTemperature();
+		press = PIOS_MS5611_GetPressure();
+		
+		data.Temperature = temp;
+		data.Pressure = press;
+		data.Altitude = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f)));
+	
+		// Update the AltitudeActual UAVObject
+		BaroAltitudeSet(&data);
+	}
+}
+
+/**
+ * @}
+ * @}
+ */

flight/Modules/Altitude/revolution/inc/altitude.h

@@ -1,45 +1,41 @@
 /**
  ******************************************************************************
- * @addtogroup AHRS AHRS Control
+ * @addtogroup OpenPilotModules OpenPilot Modules
- * @brief The AHRS Modules perform
- *
  * @{ 
- * @addtogroup AHRS_TIMER
+ * @addtogroup AltitudeModule Altitude Module
- * @brief Sets up a simple timer that can be polled to estimate idle time
+ * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
  * @{ 
  *
- *
+ * @file       altitude.h
- * @file       ahrs.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      INSGPS Test Program
+ * @brief      Altitude module, reads temperature and pressure from BMP085
+ *
  * @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.
- * 
+ *
- * 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.
- * 
+ *
- * 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
  */
+#ifndef ALTITUDE_H
+#define ALTITUDE_H
 
-#ifndef AHRS_TIMER
+int32_t AltitudeInitialize();
-#define AHRS_TIMER
 
-#include <pios.h>
+#endif // ALTITUDE_H
 
-#define TIMER_RATE (8e6 / 128)
+/**
-
+ * @}
-void timer_start();
+ * @}
-uint32_t timer_count();
+ */
-uint32_t timer_rate();
-
-#endif

flight/Modules/AltitudeHold/altitudehold.c

@@ -0,0 +1,372 @@
+/**
+ ******************************************************************************
+ *
+ * @file       guidance.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint 
+ * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
+ * of @ref ManualControlCommand is Auto.
+ *
+ * @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
+ */
+
+/**
+ * Input object: ActiveWaypoint
+ * Input object: PositionActual
+ * Input object: ManualControlCommand
+ * Output object: AttitudeDesired
+ *
+ * This module will periodically update the value of the AttitudeDesired object.
+ *
+ * The module executes in its own thread in this example.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+#include <math.h>
+#include "CoordinateConversions.h"
+#include "altholdsmoothed.h"
+#include "attitudeactual.h"
+#include "altitudeholdsettings.h"
+#include "altitudeholddesired.h"	// object that will be updated by the module
+#include "baroaltitude.h"
+#include "positionactual.h"
+#include "flightstatus.h"
+#include "stabilizationdesired.h"
+#include "accels.h"
+
+// Private constants
+#define MAX_QUEUE_SIZE 2
+#define STACK_SIZE_BYTES 1024
+#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
+// Private types
+
+// Private variables
+static xTaskHandle altitudeHoldTaskHandle;
+static xQueueHandle queue;
+static AltitudeHoldSettingsData altitudeHoldSettings;
+
+// Private functions
+static void altitudeHoldTask(void *parameters);
+static void SettingsUpdatedCb(UAVObjEvent * ev);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AltitudeHoldStart()
+{
+	// Start main task
+	xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
+	TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
+
+	return 0;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AltitudeHoldInitialize()
+{
+	AltitudeHoldSettingsInitialize();
+	AltitudeHoldDesiredInitialize();
+	AltHoldSmoothedInitialize();
+
+	// Create object queue
+	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
+
+	AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
+
+	return 0;
+}
+MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart)
+
+float tau;
+float throttleIntegral;
+float velocity;
+float decay;
+float velocity_decay;
+bool running = false;
+float error;
+float switchThrottle;
+float smoothed_altitude;
+float starting_altitude;
+
+/**
+ * Module thread, should not return.
+ */
+static void altitudeHoldTask(void *parameters)
+{
+	AltitudeHoldDesiredData altitudeHoldDesired;
+	StabilizationDesiredData stabilizationDesired;
+
+	portTickType thisTime, lastUpdateTime;
+	UAVObjEvent ev;
+	
+	// Force update of the settings
+	SettingsUpdatedCb(&ev);
+
+	// Listen for updates.
+	AltitudeHoldDesiredConnectQueue(queue);
+	BaroAltitudeConnectQueue(queue);
+	FlightStatusConnectQueue(queue);
+	AccelsConnectQueue(queue);
+	
+	BaroAltitudeAltitudeGet(&smoothed_altitude);
+	running = false;
+	enum init_state {WAITING_BARO, WAITIING_INIT, INITED} init = WAITING_BARO;
+
+	// Main task loop
+	bool baro_updated = false;
+	while (1) {		
+		// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
+		if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE )
+		{
+			if(!running)
+				throttleIntegral = 0;
+
+			// Todo: Add alarm if it should be running
+			continue;
+		} else if (ev.obj == BaroAltitudeHandle()) {
+			baro_updated = true;
+			
+			init = (init == WAITING_BARO) ? WAITIING_INIT : init;
+		} else if (ev.obj == FlightStatusHandle()) {
+			FlightStatusData flightStatus;
+			FlightStatusGet(&flightStatus);
+
+			if(flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) {
+				// Copy the current throttle as a starting point for integral
+				StabilizationDesiredThrottleGet(&throttleIntegral);
+				switchThrottle = throttleIntegral;
+				error = 0;
+				velocity = 0;
+				running = true;
+
+				AltHoldSmoothedData altHold;
+				AltHoldSmoothedGet(&altHold);
+				starting_altitude = altHold.Altitude;
+			} else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
+				running = false;
+		} else if (ev.obj == AccelsHandle()) {
+			static uint32_t timeval;
+			
+			static float z[4] = {0, 0, 0, 0};
+			float z_new[4];
+			float P[4][4], K[4][2], x[2];
+			float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7};
+			static float V[4][4] = {{10.0f, 0, 0, 0}, {0, 100.0f, 0, 0}, {0, 0, 100.0f, 0}, {0, 0, 0, 1000.0f}};
+
+			float dT;
+			static float S[2] = {1.0f,10.0f};
+
+			thisTime = xTaskGetTickCount();
+
+			/* Somehow this always assigns to zero.  Compiler bug? Race condition? */
+			S[0] = altitudeHoldSettings.PressureNoise;
+			S[1] = altitudeHoldSettings.AccelNoise;
+			G[2] = altitudeHoldSettings.AccelDrift; 
+
+			AccelsData accels;
+			AccelsGet(&accels);
+			AttitudeActualData attitudeActual;
+			AttitudeActualGet(&attitudeActual);
+			BaroAltitudeData baro;
+			BaroAltitudeGet(&baro);
+
+			if (init == WAITIING_INIT) {
+				z[0] = baro.Altitude;
+				z[1] = 0;
+				z[2] = accels.z;
+				z[3] = 0;
+				init = INITED;
+			} else if (init == WAITING_BARO)
+				continue;
+
+			x[0] = baro.Altitude;
+			//rotate avg accels into earth frame and store it
+			if(1) {
+				float q[4], Rbe[3][3];
+				q[0]=attitudeActual.q1;
+				q[1]=attitudeActual.q2;
+				q[2]=attitudeActual.q3;
+				q[3]=attitudeActual.q4;
+				Quaternion2R(q, Rbe);
+				x[1] = -(Rbe[0][2]*accels.x+ Rbe[1][2]*accels.y + Rbe[2][2]*accels.z + 9.81f);
+			} else {
+				x[1] = -accels.z + 9.81f;
+			}
+
+			dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
+			timeval = PIOS_DELAY_GetRaw();
+
+			P[0][0] = dT*(V[0][1]+dT*V[1][1])+V[0][0]+G[0]+dT*V[1][0];
+			P[0][1] = dT*(V[0][2]+dT*V[1][2])+V[0][1]+dT*V[1][1];
+			P[0][2] = V[0][2]+dT*V[1][2];
+			P[0][3] = V[0][3]+dT*V[1][3];
+			P[1][0] = dT*(V[1][1]+dT*V[2][1])+V[1][0]+dT*V[2][0];
+			P[1][1] = dT*(V[1][2]+dT*V[2][2])+V[1][1]+G[1]+dT*V[2][1];
+			P[1][2] = V[1][2]+dT*V[2][2];
+			P[1][3] = V[1][3]+dT*V[2][3];
+			P[2][0] = V[2][0]+dT*V[2][1];
+			P[2][1] = V[2][1]+dT*V[2][2];
+			P[2][2] = V[2][2]+G[2];
+			P[2][3] = V[2][3];
+			P[3][0] = V[3][0]+dT*V[3][1];
+			P[3][1] = V[3][1]+dT*V[3][2];
+			P[3][2] = V[3][2];
+			P[3][3] = V[3][3]+G[3];
+
+			if (baro_updated) {
+				K[0][0] = -(V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])+1.0f;
+				K[0][1] = ((V[0][2]+V[0][3])*S[0]+dT*(V[1][2]+V[1][3])*S[0])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
+				K[1][0] = (V[1][0]*G[2]+V[1][0]*G[3]+V[1][0]*S[1]+V[1][0]*V[2][2]-V[2][0]*V[1][2]+V[1][0]*V[2][3]+V[1][0]*V[3][2]-V[2][0]*V[1][3]-V[1][2]*V[3][0]+V[1][0]*V[3][3]-V[3][0]*V[1][3]+(dT*dT)*V[2][1]*V[3][2]-(dT*dT)*V[2][2]*V[3][1]+(dT*dT)*V[2][1]*V[3][3]-(dT*dT)*V[3][1]*V[2][3]+dT*V[1][1]*G[2]+dT*V[2][0]*G[2]+dT*V[1][1]*G[3]+dT*V[2][0]*G[3]+dT*V[1][1]*S[1]+dT*V[2][0]*S[1]+(dT*dT)*V[2][1]*G[2]+(dT*dT)*V[2][1]*G[3]+(dT*dT)*V[2][1]*S[1]+dT*V[1][1]*V[2][2]-dT*V[1][2]*V[2][1]+dT*V[1][1]*V[2][3]+dT*V[1][1]*V[3][2]+dT*V[2][0]*V[3][2]-dT*V[1][2]*V[3][1]-dT*V[2][1]*V[1][3]-dT*V[3][0]*V[2][2]+dT*V[1][1]*V[3][3]+dT*V[2][0]*V[3][3]-dT*V[3][0]*V[2][3]-dT*V[1][3]*V[3][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
+				K[1][1] = (V[1][2]*G[0]+V[1][3]*G[0]+V[1][2]*S[0]+V[1][3]*S[0]+V[0][0]*V[1][2]-V[1][0]*V[0][2]+V[0][0]*V[1][3]-V[1][0]*V[0][3]+(dT*dT)*V[0][1]*V[2][2]+(dT*dT)*V[1][0]*V[2][2]-(dT*dT)*V[0][2]*V[2][1]-(dT*dT)*V[2][0]*V[1][2]+(dT*dT)*V[0][1]*V[2][3]+(dT*dT)*V[1][0]*V[2][3]-(dT*dT)*V[2][0]*V[1][3]-(dT*dT)*V[0][3]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][2]-(dT*dT*dT)*V[1][2]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][3]-(dT*dT*dT)*V[2][1]*V[1][3]+dT*V[2][2]*G[0]+dT*V[2][3]*G[0]+dT*V[2][2]*S[0]+dT*V[2][3]*S[0]+dT*V[0][0]*V[2][2]+dT*V[0][1]*V[1][2]-dT*V[0][2]*V[1][1]-dT*V[0][2]*V[2][0]+dT*V[0][0]*V[2][3]+dT*V[0][1]*V[1][3]-dT*V[1][1]*V[0][3]-dT*V[2][0]*V[0][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
+				K[2][0] = (V[2][0]*G[3]-V[3][0]*G[2]+V[2][0]*S[1]+V[2][0]*V[3][2]-V[3][0]*V[2][2]+V[2][0]*V[3][3]-V[3][0]*V[2][3]+dT*V[2][1]*G[3]-dT*V[3][1]*G[2]+dT*V[2][1]*S[1]+dT*V[2][1]*V[3][2]-dT*V[2][2]*V[3][1]+dT*V[2][1]*V[3][3]-dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
+				K[2][1] = (V[0][0]*G[2]+V[2][2]*G[0]+V[2][3]*G[0]+V[2][2]*S[0]+V[2][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]-V[2][0]*V[0][3]+G[0]*G[2]+G[2]*S[0]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]-(dT*dT)*V[2][1]*V[1][3]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+(dT*dT)*V[1][1]*G[2]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[1][0]*V[2][3]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
+				K[3][0] = (-V[2][0]*G[3]+V[3][0]*G[2]+V[3][0]*S[1]-V[2][0]*V[3][2]+V[3][0]*V[2][2]-V[2][0]*V[3][3]+V[3][0]*V[2][3]-dT*V[2][1]*G[3]+dT*V[3][1]*G[2]+dT*V[3][1]*S[1]-dT*V[2][1]*V[3][2]+dT*V[2][2]*V[3][1]-dT*V[2][1]*V[3][3]+dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
+				K[3][1] = (V[0][0]*G[3]+V[3][2]*G[0]+V[3][3]*G[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[3][2]-V[0][2]*V[3][0]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[3]+G[3]*S[0]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+(dT*dT)*V[1][1]*G[3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
+				
+				z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]-K[0][1]*(-x[1]+z[2]+z[3])+z[0];
+				z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]-K[1][1]*(-x[1]+z[2]+z[3])+z[1];
+				z_new[2] = -K[2][0]*(dT*z[1]-x[0]+z[0])-K[2][1]*(-x[1]+z[2]+z[3])+z[2];
+				z_new[3] = -K[3][0]*(dT*z[1]-x[0]+z[0])-K[3][1]*(-x[1]+z[2]+z[3])+z[3];
+
+				memcpy(z, z_new, sizeof(z_new));
+				
+				V[0][0] = -K[0][1]*P[2][0]-K[0][1]*P[3][0]-P[0][0]*(K[0][0]-1.0f);
+				V[0][1] = -K[0][1]*P[2][1]-K[0][1]*P[3][2]-P[0][1]*(K[0][0]-1.0f);
+				V[0][2] = -K[0][1]*P[2][2]-K[0][1]*P[3][2]-P[0][2]*(K[0][0]-1.0f);
+				V[0][3] = -K[0][1]*P[2][3]-K[0][1]*P[3][3]-P[0][3]*(K[0][0]-1.0f);
+				V[1][0] = P[1][0]-K[1][0]*P[0][0]-K[1][1]*P[2][0]-K[1][1]*P[3][0];
+				V[1][1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1]-K[1][1]*P[3][2];
+				V[1][2] = P[1][2]-K[1][0]*P[0][2]-K[1][1]*P[2][2]-K[1][1]*P[3][2];
+				V[1][3] = P[1][3]-K[1][0]*P[0][3]-K[1][1]*P[2][3]-K[1][1]*P[3][3];
+				V[2][0] = -K[2][0]*P[0][0]-K[2][1]*P[3][0]-P[2][0]*(K[2][1]-1.0f);
+				V[2][1] = -K[2][0]*P[0][1]-K[2][1]*P[3][2]-P[2][1]*(K[2][1]-1.0f);
+				V[2][2] = -K[2][0]*P[0][2]-K[2][1]*P[3][2]-P[2][2]*(K[2][1]-1.0f);
+				V[2][3] = -K[2][0]*P[0][3]-K[2][1]*P[3][3]-P[2][3]*(K[2][1]-1.0f);
+				V[3][0] = -K[3][0]*P[0][0]-K[3][1]*P[2][0]-P[3][0]*(K[3][1]-1.0f);
+				V[3][1] = -K[3][0]*P[0][1]-K[3][1]*P[2][1]-P[3][2]*(K[3][1]-1.0f);
+				V[3][2] = -K[3][0]*P[0][2]-K[3][1]*P[2][2]-P[3][2]*(K[3][1]-1.0f);
+				V[3][3] = -K[3][0]*P[0][3]-K[3][1]*P[2][3]-P[3][3]*(K[3][1]-1.0f);
+				
+
+				baro_updated = false;
+			} else {
+				K[0][0] = (V[0][2]+V[0][3]+dT*V[1][2]+dT*V[1][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
+				K[1][0] = (V[1][2]+V[1][3]+dT*V[2][2]+dT*V[2][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
+				K[2][0] = (V[2][2]+V[2][3]+G[2])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
+				K[3][0] = (V[3][2]+V[3][3]+G[3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
+				
+
+				z_new[0] = dT*z[1]-K[0][0]*(-x[1]+z[2]+z[3])+z[0];
+				z_new[1] = dT*z[2]-K[1][0]*(-x[1]+z[2]+z[3])+z[1];
+				z_new[2] = -K[2][0]*(-x[1]+z[2]+z[3])+z[2];
+				z_new[3] = -K[3][0]*(-x[1]+z[2]+z[3])+z[3];
+
+				memcpy(z, z_new, sizeof(z_new));
+
+				V[0][0] = P[0][0]-K[0][0]*P[2][0]-K[0][0]*P[3][0];
+				V[0][1] = P[0][1]-K[0][0]*P[2][1]-K[0][0]*P[3][2];
+				V[0][2] = P[0][2]-K[0][0]*P[2][2]-K[0][0]*P[3][2];
+				V[0][3] = P[0][3]-K[0][0]*P[2][3]-K[0][0]*P[3][3];
+				V[1][0] = P[1][0]-K[1][0]*P[2][0]-K[1][0]*P[3][0];
+				V[1][1] = P[1][1]-K[1][0]*P[2][1]-K[1][0]*P[3][2];
+				V[1][2] = P[1][2]-K[1][0]*P[2][2]-K[1][0]*P[3][2];
+				V[1][3] = P[1][3]-K[1][0]*P[2][3]-K[1][0]*P[3][3];
+				V[2][0] = -K[2][0]*P[3][0]-P[2][0]*(K[2][0]-1.0f);
+				V[2][1] = -K[2][0]*P[3][2]-P[2][1]*(K[2][0]-1.0f);
+				V[2][2] = -K[2][0]*P[3][2]-P[2][2]*(K[2][0]-1.0f);
+				V[2][3] = -K[2][0]*P[3][3]-P[2][3]*(K[2][0]-1.0f);
+				V[3][0] = -K[3][0]*P[2][0]-P[3][0]*(K[3][0]-1.0f);
+				V[3][1] = -K[3][0]*P[2][1]-P[3][2]*(K[3][0]-1.0f);
+				V[3][2] = -K[3][0]*P[2][2]-P[3][2]*(K[3][0]-1.0f);
+				V[3][3] = -K[3][0]*P[2][3]-P[3][3]*(K[3][0]-1.0f);
+			}
+
+			AltHoldSmoothedData altHold;
+			AltHoldSmoothedGet(&altHold);
+			altHold.Altitude = z[0];
+			altHold.Velocity = z[1];
+			altHold.Accel = z[2];
+			AltHoldSmoothedSet(&altHold);
+
+			AltHoldSmoothedGet(&altHold);
+
+			// Verify that we are  in altitude hold mode
+			FlightStatusData flightStatus;
+			FlightStatusGet(&flightStatus);
+			if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
+				running = false;
+			}
+
+			if (!running)
+				continue;
+
+			// Compute the altitude error
+			error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
+
+			// Compute integral off altitude error
+			throttleIntegral += error * altitudeHoldSettings.Ki * dT;
+
+			// Only update stabilizationDesired less frequently
+			if((thisTime - lastUpdateTime) < 20)
+				continue;
+
+			lastUpdateTime = thisTime;
+
+			// Instead of explicit limit on integral you output limit feedback
+			StabilizationDesiredGet(&stabilizationDesired);
+			stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
+			altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
+			if(stabilizationDesired.Throttle > 1) {
+				throttleIntegral -= (stabilizationDesired.Throttle - 1);
+				stabilizationDesired.Throttle = 1;
+			}
+			else if (stabilizationDesired.Throttle < 0) {
+				throttleIntegral -= stabilizationDesired.Throttle;
+				stabilizationDesired.Throttle = 0;
+			}
+
+			stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+			stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+			stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;		
+			stabilizationDesired.Roll = altitudeHoldDesired.Roll;
+			stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
+			stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
+			StabilizationDesiredSet(&stabilizationDesired);
+
+		} else if (ev.obj == AltitudeHoldDesiredHandle()) {
+			AltitudeHoldDesiredGet(&altitudeHoldDesired);
+		}
+
+	}
+}
+
+static void SettingsUpdatedCb(UAVObjEvent * ev)
+{
+	AltitudeHoldSettingsGet(&altitudeHoldSettings);
+}

flight/Modules/AltitudeHold/inc/altitudehold.h

@@ -1,9 +1,9 @@
 /**
  ******************************************************************************
  *
- * @file       ahrs_spi_program_slave.h
+ * @file       examplemodperiodic.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      AHRS programming over SPI link - slave(AHRS) end.
+ * @brief      Example module to be used as a template for actual modules.
  *
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -23,13 +23,9 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#ifndef EXAMPLEMODPERIODIC_H
+#define EXAMPLEMODPERIODIC_H
 
-#ifndef AHRS_SPI_PROGRAM_SLAVE_H
+int32_t ExampleModPeriodicInitialize();
-#define AHRS_SPI_PROGRAM_SLAVE_H
+int32_t GuidanceInitialize(void);
-
+#endif // EXAMPLEMODPERIODIC_H
-/** Check if OpenPilot is trying to program AHRS
- * If so, it will program the FLASH then return
- * If not it just returns.
- */
-void AhrsProgramReceive(uint32_t spi_id);
-#endif //AHRS_PROGRAM_SLAVE_H

flight/Modules/Attitude/attitude.c

@@ -50,19 +50,21 @@
 
 #include "pios.h"
 #include "attitude.h"
-#include "attituderaw.h"
+#include "gyros.h"
+#include "accels.h"
 #include "attitudeactual.h"
 #include "attitudesettings.h"
 #include "flightstatus.h"
 #include "manualcontrolcommand.h"
 #include "CoordinateConversions.h"
-#include "pios_flash_w25x.h"
+#include <pios_board_info.h>
-
+ 
 // Private constants
 #define STACK_SIZE_BYTES 540
 #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
 
-#define UPDATE_RATE  2.0f
+#define SENSOR_PERIOD 4
+#define UPDATE_RATE  25.0f
 #define GYRO_NEUTRAL 1665
 
 #define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
@@ -77,8 +79,9 @@ static void AttitudeTask(void *parameters);
 static float gyro_correct_int[3] = {0,0,0};
 static xQueueHandle gyro_queue;
 
-static int8_t updateSensors(AttitudeRawData *);
+static int32_t updateSensors(AccelsData *, GyrosData *);
-static void updateAttitude(AttitudeRawData *);
+static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData);
+static void updateAttitude(AccelsData *, GyrosData *);
 static void settingsUpdatedCb(UAVObjEvent * objEv);
 
 static float accelKi = 0;
@@ -124,8 +127,9 @@ int32_t AttitudeStart(void)
 int32_t AttitudeInitialize(void)
 {
 	AttitudeActualInitialize();
-	AttitudeRawInitialize();
 	AttitudeSettingsInitialize();
+	AccelsInitialize();
+	GyrosInitialize();
 	
 	// Initialize quaternion
 	AttitudeActualData attitude;
@@ -151,14 +155,6 @@ int32_t AttitudeInitialize(void)
 	
 	trim_requested = false;
 	
-	// Create queue for passing gyro data, allow 2 back samples in case
-	gyro_queue = xQueueCreate(1, sizeof(float) * 4);
-	if(gyro_queue == NULL)
-		return -1;
-	
-	
-	PIOS_ADC_SetQueue(gyro_queue);
-	
 	AttitudeSettingsConnectCallback(&settingsUpdatedCb);
 	
 	return 0;
@@ -169,23 +165,42 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
 /**
  * Module thread, should not return.
  */
+ 
+int32_t accel_test;
+int32_t gyro_test;
 static void AttitudeTask(void *parameters)
 {
 	uint8_t init = 0;
 	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 	
-	PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
-	
-	// Keep flash CS pin high while talking accel
-	PIOS_FLASH_DISABLE;
-	PIOS_ADXL345_Init();
-	
 	// Set critical error and wait until the accel is producing data
 	while(PIOS_ADXL345_FifoElements() == 0) {
 		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
 		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
 	}
 	
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	
+	bool cc3d = bdinfo->board_rev == 0x02;
+
+	if(cc3d) {
+#if defined(PIOS_INCLUDE_MPU6000)
+		gyro_test = PIOS_MPU6000_Test();
+#endif
+	} else {
+#if defined(PIOS_INCLUDE_ADXL345)
+		accel_test = PIOS_ADXL345_Test();
+#endif
+
+#if defined(PIOS_INCLUDE_ADC)
+		// Create queue for passing gyro data, allow 2 back samples in case
+		gyro_queue = xQueueCreate(1, sizeof(float) * 4);
+		PIOS_Assert(gyro_queue != NULL);
+		PIOS_ADC_SetQueue(gyro_queue);
+		PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
+#endif
+
+	}
 	// Force settings update to make sure rotation loaded
 	settingsUpdatedCb(AttitudeSettingsHandle());
 	
@@ -216,27 +231,33 @@ static void AttitudeTask(void *parameters)
 		}
 		
 		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
-		
+
-		AttitudeRawData attitudeRaw;
+		AccelsData accels;
-		AttitudeRawGet(&attitudeRaw);
+		GyrosData gyros;
-		if(updateSensors(&attitudeRaw) != 0)
+		int32_t retval;
+		if(cc3d) 
+			retval = updateSensorsCC3D(&accels, &gyros);
+		else
+			retval = updateSensors(&accels, &gyros);
+
+		if(retval != 0)
 			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
 		else {
 			// Only update attitude when sensor data is good
-			updateAttitude(&attitudeRaw);
+			updateAttitude(&accels, &gyros);
-			AttitudeRawSet(&attitudeRaw);
 			AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 		}
-		
 	}
 }
 
+float gyros_passed[3];
+
 /**
  * Get an update from the sensors
  * @param[in] attitudeRaw Populate the UAVO instead of saving right here
  * @return 0 if successfull, -1 if not
  */
-static int8_t updateSensors(AttitudeRawData * attitudeRaw)
+static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
 {
 	struct pios_adxl345_data accel_data;
 	float gyro[4];
@@ -252,9 +273,9 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
 		return -1;
 	
 	// First sample is temperature
-	attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
+	gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
-	attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
+	gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
-	attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
+	gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
 	
 	int32_t x = 0;
 	int32_t y = 0;
@@ -268,26 +289,25 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
 		y += -accel_data.y;
 		z += -accel_data.z;
 	} while ( (i < 32) && (samples_remaining > 0) );
-	attitudeRaw->gyrotemp[0] = samples_remaining;
+	gyros->temperature = samples_remaining;
-	attitudeRaw->gyrotemp[1] = i;
+
-	
 	float accel[3] = {(float) x / i, (float) y / i, (float) z / i};
 	
 	if(rotate) {
 		// TODO: rotate sensors too so stabilization is well behaved
 		float vec_out[3];
 		rot_mult(R, accel, vec_out);
-		attitudeRaw->accels[0] = vec_out[0];
+		accels->x = vec_out[0];
-		attitudeRaw->accels[1] = vec_out[1];
+		accels->y = vec_out[1];
-		attitudeRaw->accels[2] = vec_out[2];
+		accels->z = vec_out[2];
-		rot_mult(R, attitudeRaw->gyros, vec_out);
+		rot_mult(R, &gyros->x, vec_out);
-		attitudeRaw->gyros[0] = vec_out[0];
+		gyros->x = vec_out[0];
-		attitudeRaw->gyros[1] = vec_out[1];
+		gyros->y = vec_out[1];
-		attitudeRaw->gyros[2] = vec_out[2];
+		gyros->z = vec_out[2];
 	} else {
-		attitudeRaw->accels[0] = accel[0];
+		accels->x = accel[0];
-		attitudeRaw->accels[1] = accel[1];
+		accels->y = accel[1];
-		attitudeRaw->accels[2] = accel[2];
+		accels->z = accel[2];
 	}
 	
 	if (trim_requested) {
@@ -301,33 +321,100 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
 			if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
 				trim_samples++;
 				// Store the digitally scaled version since that is what we use for bias
-				trim_accels[0] += attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
+				trim_accels[0] += accels->x;
-				trim_accels[1] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Y];
+				trim_accels[1] += accels->y;
-				trim_accels[2] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Z];	
+				trim_accels[2] += accels->z;
 			}
 		}
 	}
 	
 	// Scale accels and correct bias
-	attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * ACCEL_SCALE;
+	accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE;
-	attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * ACCEL_SCALE;
+	accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE;
-	attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * ACCEL_SCALE;
+	accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE;
 	
 	if(bias_correct_gyro) {
 		// Applying integral component here so it can be seen on the gyros and correct bias
-		attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
+		gyros->x += gyro_correct_int[0];
-		attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
+		gyros->y += gyro_correct_int[1];
-		attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
+		gyros->z += gyro_correct_int[2];
 	}
 	
 	// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
 	// and make it average zero (weakly)
-	gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
+	gyro_correct_int[2] += - gyros->z * yawBiasRate;
-	
+
+	GyrosSet(gyros);
+	AccelsSet(accels);
+
 	return 0;
 }
 
-static void updateAttitude(AttitudeRawData * attitudeRaw)
+/**
+ * Get an update from the sensors
+ * @param[in] attitudeRaw Populate the UAVO instead of saving right here
+ * @return 0 if successfull, -1 if not
+ */
+struct pios_mpu6000_data mpu6000_data;
+static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
+{
+	float accels[3], gyros[3];
+	
+#if defined(PIOS_INCLUDE_MPU6000)
+	
+	xQueueHandle queue = PIOS_MPU6000_GetQueue();
+	
+	if(xQueueReceive(queue, (void *) &mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY)
+		return -1;	// Error, no data
+
+	gyros[0] = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
+	gyros[1] = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
+	gyros[2] = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
+	
+	accels[0] = -mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
+	accels[1] = -mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
+	accels[2] = -mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
+
+	gyrosData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
+	accelsData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
+#endif
+
+	if(rotate) {
+		// TODO: rotate sensors too so stabilization is well behaved
+		float vec_out[3];
+		rot_mult(R, accels, vec_out);
+		accels[0] = vec_out[0];
+		accels[1] = vec_out[1];
+		accels[2] = vec_out[2];
+		rot_mult(R, gyros, vec_out);
+		gyros[0] = vec_out[0];
+		gyros[1] = vec_out[1];
+		gyros[2] = vec_out[2];
+	}
+
+	accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
+	accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
+	accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
+	AccelsSet(&accelsData);
+
+	gyrosData->x = gyros[0];
+	gyrosData->y = gyros[1];
+	gyrosData->z = gyros[2];
+
+	if(bias_correct_gyro) {
+		// Applying integral component here so it can be seen on the gyros and correct bias
+		gyrosData->x += gyro_correct_int[0];
+		gyrosData->y += gyro_correct_int[1];
+		gyrosData->z += gyro_correct_int[2];
+	}
+
+	GyrosSet(gyrosData);
+	AccelsSet(accelsData);
+
+	return 0;
+}
+
+static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
 {
 	float dT;
 	portTickType thisSysTime = xTaskGetTickCount();
@@ -337,48 +424,46 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
 	lastSysTime = thisSysTime;
 	
 	// Bad practice to assume structure order, but saves memory
-	float gyro[3];
+	float * gyros = &gyrosData->x;
-	gyro[0] = attitudeRaw->gyros[0];
+	float * accels = &accelsData->x;
-	gyro[1] = attitudeRaw->gyros[1];
-	gyro[2] = attitudeRaw->gyros[2];
 	
-	{
+	float grot[3];
-		float * accels = attitudeRaw->accels;
+	float accel_err[3];
-		float grot[3];
+	
-		float accel_err[3];
+	// Rotate gravity to body frame and cross with accels
-		
+	grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
-		// Rotate gravity to body frame and cross with accels
+	grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
-		grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
+	grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
-		grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
+	CrossProduct((const float *) accels, (const float *) grot, accel_err);
-		grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
+	
-		CrossProduct((const float *) accels, (const float *) grot, accel_err);
+	// Account for accel magnitude
-		
+	float accel_mag = sqrtf(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
-		// Account for accel magnitude
+	if(accel_mag < 1.0e-3f)
-		float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
+		return;
-		accel_err[0] /= accel_mag;
+
-		accel_err[1] /= accel_mag;
+	accel_err[0] /= accel_mag;
-		accel_err[2] /= accel_mag;
+	accel_err[1] /= accel_mag;
-		
+	accel_err[2] /= accel_mag;
-		// Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
+	
-		gyro_correct_int[0] += accel_err[0] * accelKi;
+	// Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
-		gyro_correct_int[1] += accel_err[1] * accelKi;
+	gyro_correct_int[0] += accel_err[0] * accelKi;
-		
+	gyro_correct_int[1] += accel_err[1] * accelKi;
-		//gyro_correct_int[2] += accel_err[2] * accelKi;
+	
-		
+	//gyro_correct_int[2] += accel_err[2] * accelKi;
-		// Correct rates based on error, integral component dealt with in updateSensors
+	
-		gyro[0] += accel_err[0] * accelKp / dT;
+	// Correct rates based on error, integral component dealt with in updateSensors
-		gyro[1] += accel_err[1] * accelKp / dT;
+	gyros[0] += accel_err[0] * accelKp / dT;
-		gyro[2] += accel_err[2] * accelKp / dT;
+	gyros[1] += accel_err[1] * accelKp / dT;
-	}
+	gyros[2] += accel_err[2] * accelKp / dT;
 	
 	{ // scoping variables to save memory
 		// Work out time derivative from INSAlgo writeup
 		// Also accounts for the fact that gyros are in deg/s
 		float qdot[4];
-		qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2;
+		qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * M_PI / 180 / 2;
-		qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
+		qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * M_PI / 180 / 2;
-		qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
+		qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * M_PI / 180 / 2;
-		qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
+		qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * M_PI / 180 / 2;
 		
 		// Take a time step
 		q[0] = q[0] + qdot[0];
@@ -395,7 +480,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
 	}
 	
 	// Renomalize
-	float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
+	float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
 	q[0] = q[0] / qmag;
 	q[1] = q[1] / qmag;
 	q[2] = q[2] / qmag;

flight/Modules/Attitude/revolution/attitude.c

@@ -0,0 +1,629 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup Attitude Copter Control Attitude Estimation
+ * @brief Acquires sensor data and computes attitude estimate
+ * Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
+ * @{
+ *
+ * @file       attitude.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Module to handle all comms to the AHRS on a periodic basis.
+ *
+ * @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
+ */
+
+/**
+ * Input objects: None, takes sensor data via pios
+ * Output objects: @ref AttitudeRaw @ref AttitudeActual
+ *
+ * This module computes an attitude estimate from the sensor data
+ *
+ * The module executes in its own thread.
+ *
+ * UAVObjects are automatically generated by the UAVObjectGenerator from
+ * the object definition XML file.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "pios.h"
+#include "attitude.h"
+#include "magnetometer.h"
+#include "accels.h"
+#include "gyros.h"
+#include "gyrosbias.h"
+#include "attitudeactual.h"
+#include "attitudesettings.h"
+#include "positionactual.h"
+#include "velocityactual.h"
+#include "gpsposition.h"
+#include "baroaltitude.h"
+#include "flightstatus.h"
+#include "homelocation.h"
+#include "CoordinateConversions.h"
+
+// Private constants
+#define STACK_SIZE_BYTES 5540
+#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
+#define FAILSAFE_TIMEOUT_MS 10
+
+#define F_PI 3.14159265358979323846f
+#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
+// Private types
+
+// Private variables
+static xTaskHandle attitudeTaskHandle;
+
+static xQueueHandle gyroQueue;
+static xQueueHandle accelQueue;
+static xQueueHandle magQueue;
+static xQueueHandle baroQueue;
+static xQueueHandle gpsQueue;
+const uint32_t SENSOR_QUEUE_SIZE = 10;
+
+// Private functions
+static void AttitudeTask(void *parameters);
+
+static int32_t updateAttitudeComplimentary(bool first_run);
+static int32_t updateAttitudeINSGPS(bool first_run);
+static void settingsUpdatedCb(UAVObjEvent * objEv);
+
+static float accelKi = 0;
+static float accelKp = 0;
+static float yawBiasRate = 0;
+static float gyroGain = 0.42;
+static int16_t accelbias[3];
+static float R[3][3];
+static int8_t rotate = 0;
+static bool zero_during_arming = false;
+
+
+/**
+ * API for sensor fusion algorithms:
+ * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
+ *   Stores all the queues the algorithm will pull data from
+ * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
+ * Update() -- queries queues and updates the attitude estiamte
+ */
+
+
+/**
+ * Initialise the module.  Called before the start function
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AttitudeInitialize(void)
+{
+	AttitudeActualInitialize();
+	AttitudeSettingsInitialize();
+	PositionActualInitialize();
+	VelocityActualInitialize();
+	
+	// Initialize this here while we aren't setting the homelocation in GPS
+	HomeLocationInitialize();
+
+	// Initialize quaternion
+	AttitudeActualData attitude;
+	AttitudeActualGet(&attitude);
+	attitude.q1 = 1;
+	attitude.q2 = 0;
+	attitude.q3 = 0;
+	attitude.q4 = 0;
+	AttitudeActualSet(&attitude);
+	
+	// Cannot trust the values to init right above if BL runs
+	GyrosBiasData gyrosBias;
+	GyrosBiasGet(&gyrosBias);
+	gyrosBias.x = 0;
+	gyrosBias.y = 0;
+	gyrosBias.z = 0;
+	GyrosBiasSet(&gyrosBias);
+	
+	for(uint8_t i = 0; i < 3; i++)
+		for(uint8_t j = 0; j < 3; j++)
+			R[i][j] = 0;
+	
+	AttitudeSettingsConnectCallback(&settingsUpdatedCb);
+	
+	return 0;
+}
+
+/**
+ * Start the task.  Expects all objects to be initialized by this point.
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AttitudeStart(void)
+{
+	// Create the queues for the sensors
+	gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+	accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+	magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+	baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+	gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+			
+	// Start main task
+	xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
+	TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
+	PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
+	
+	GyrosConnectQueue(gyroQueue);
+	AccelsConnectQueue(accelQueue);
+	MagnetometerConnectQueue(magQueue);
+	BaroAltitudeConnectQueue(baroQueue);
+	GPSPositionConnectQueue(gpsQueue);
+	
+	return 0;
+}
+
+MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
+
+/**
+ * Module thread, should not return.
+ */
+static void AttitudeTask(void *parameters)
+{
+	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+
+	// Force settings update to make sure rotation loaded
+	settingsUpdatedCb(AttitudeSettingsHandle());
+	
+	bool first_run = true;
+	
+	// Wait for all the sensors be to read
+	vTaskDelay(100);
+	
+	// Main task loop
+	while (1) {
+	
+		// This  function blocks on data queue
+		if(1) 
+			updateAttitudeComplimentary(first_run);
+		else
+			updateAttitudeINSGPS(first_run);
+			
+		if (first_run)
+			first_run = false;
+		
+		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
+	}
+}
+
+float accel_mag;
+float qmag;
+float attitudeDt;
+float mag_err[3];
+float magKi = 0.000001f;
+float magKp = 0.0001f;
+
+static int32_t updateAttitudeComplimentary(bool first_run)
+{
+	UAVObjEvent ev;
+	GyrosData gyrosData;
+	AccelsData accelsData;
+	static int32_t timeval;
+	float dT;
+	static uint8_t init = 0;
+
+	// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
+	if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
+	{
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
+		return -1;
+	}
+	if ( xQueueReceive(accelQueue, &ev, 0) != pdTRUE )
+	{
+		// When one of these is updated so should the other
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
+		return -1;
+	}
+	
+	// During initialization and 
+	FlightStatusData flightStatus;
+	FlightStatusGet(&flightStatus);
+	if(first_run)
+		init = 0;
+
+	if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
+		// For first 7 seconds use accels to get gyro bias
+		accelKp = 1;
+		accelKi = 0.9;
+		yawBiasRate = 0.23;
+	} else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
+		accelKp = 1;
+		accelKi = 0.9;
+		yawBiasRate = 0.23;
+		init = 0;
+	} else if (init == 0) {
+		// Reload settings (all the rates)
+		AttitudeSettingsAccelKiGet(&accelKi);
+		AttitudeSettingsAccelKpGet(&accelKp);
+		AttitudeSettingsYawBiasRateGet(&yawBiasRate);
+		init = 1;
+	}	
+	
+	GyrosGet(&gyrosData);
+	AccelsGet(&accelsData);
+
+	// Compute the dT using the cpu clock
+	dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
+	timeval = PIOS_DELAY_GetRaw();
+	
+	float q[4];
+	
+	AttitudeActualData attitudeActual;
+	AttitudeActualGet(&attitudeActual);
+
+	float grot[3];
+	float accel_err[3];
+	
+	// Get the current attitude estimate
+	quat_copy(&attitudeActual.q1, q);
+	
+	// Rotate gravity to body frame and cross with accels
+	grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
+	grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
+	grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
+	CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
+
+	// Account for accel magnitude
+	accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
+	accel_mag = sqrtf(accel_mag);
+	accel_err[0] /= accel_mag;
+	accel_err[1] /= accel_mag;
+	accel_err[2] /= accel_mag;	
+	
+	if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
+	{
+		// Rotate gravity to body frame and cross with accels
+		float brot[3];
+		float Rbe[3][3];
+		MagnetometerData mag;
+		HomeLocationData home;
+
+		Quaternion2R(q, Rbe);
+		MagnetometerGet(&mag);
+		HomeLocationGet(&home);
+		rot_mult(Rbe, home.Be, brot);
+		
+		float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
+		mag.x /= mag_len;
+		mag.y /= mag_len;
+		mag.z /= mag_len;
+		
+		float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
+		brot[0] /= bmag;
+		brot[1] /= bmag;
+		brot[2] /= bmag;
+		
+		// Only compute if neither vector is null
+		if (bmag < 1 || mag_len < 1)
+			mag_err[0] = mag_err[1] = mag_err[2] = 0;
+		else
+			CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
+	} else {
+		mag_err[0] = mag_err[1] = mag_err[2] = 0;
+	}
+	
+	// Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
+	GyrosBiasData gyrosBias;
+	GyrosBiasGet(&gyrosBias);
+	gyrosBias.x += accel_err[0] * accelKi;
+	gyrosBias.y += accel_err[1] * accelKi;
+	gyrosBias.z += mag_err[2] * magKi;
+	GyrosBiasSet(&gyrosBias);
+
+	// Correct rates based on error, integral component dealt with in updateSensors
+	gyrosData.x += accel_err[0] * accelKp / dT;
+	gyrosData.y += accel_err[1] * accelKp / dT;
+	gyrosData.z += accel_err[2] * accelKp / dT + mag_err[2] * magKp / dT;
+
+	// Work out time derivative from INSAlgo writeup
+	// Also accounts for the fact that gyros are in deg/s
+	float qdot[4];
+	qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
+	qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
+	qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
+	qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
+	
+	// Take a time step
+	q[0] = q[0] + qdot[0];
+	q[1] = q[1] + qdot[1];
+	q[2] = q[2] + qdot[2];
+	q[3] = q[3] + qdot[3];
+	
+	if(q[0] < 0) {
+		q[0] = -q[0];
+		q[1] = -q[1];
+		q[2] = -q[2];
+		q[3] = -q[3];
+	}
+	
+	// Renomalize
+	qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
+	q[0] = q[0] / qmag;
+	q[1] = q[1] / qmag;
+	q[2] = q[2] / qmag;
+	q[3] = q[3] / qmag;
+
+	// If quaternion has become inappropriately short or is nan reinit.
+	// THIS SHOULD NEVER ACTUALLY HAPPEN
+	if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
+		q[0] = 1;
+		q[1] = 0;
+		q[2] = 0;
+		q[3] = 0;
+	}
+
+	quat_copy(q, &attitudeActual.q1);
+
+	// Convert into eueler degrees (makes assumptions about RPY order)
+	Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
+
+	AttitudeActualSet(&attitudeActual);
+	
+	// Flush these queues for avoid errors
+	if ( xQueueReceive(baroQueue, &ev, 0) != pdTRUE )
+	{
+	}
+	if ( xQueueReceive(gpsQueue, &ev, 0) != pdTRUE )
+	{
+	}
+
+	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+
+	return 0;
+}
+
+#include "insgps.h"
+int32_t ins_failed = 0;
+extern struct NavStruct Nav;
+static int32_t updateAttitudeINSGPS(bool first_run)
+{
+	UAVObjEvent ev;
+	GyrosData gyrosData;
+	AccelsData accelsData;
+	MagnetometerData magData;
+	BaroAltitudeData baroData;
+	
+	static uint32_t ins_last_time = 0;
+
+	static bool inited;
+	if (first_run)
+		inited = false;
+	
+	// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
+	if ( (xQueueReceive(gyroQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) ||
+		(xQueueReceive(accelQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) )
+	{
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
+		return -1;
+	}
+	
+	// Get most recent data
+	// TODO: Acquire all data in a queue
+	GyrosGet(&gyrosData);
+	AccelsGet(&accelsData);
+	MagnetometerGet(&magData);
+	BaroAltitudeGet(&baroData);
+	
+	bool mag_updated;
+	bool baro_updated;
+	bool gps_updated;
+	
+	if (inited) {
+		mag_updated = 0;
+		baro_updated = 0;
+	}
+	
+	mag_updated |= xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
+	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
+	gps_updated |= xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
+	
+	if (!inited && (!mag_updated || !baro_updated || !gps_updated)) {
+		// Don't initialize until all sensors are read
+		return -1;
+	} else if (!inited ) {
+		inited = true;
+
+		float Rbe[3][3], q[4];
+		float ge[3]={0.0f,0.0f,-9.81f};
+		float zeros[3]={0.0f,0.0f,0.0f};
+		float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
+		float vel[3], NED[3];
+		
+		INSGPSInit();
+		
+		HomeLocationData home;
+		HomeLocationGet(&home);
+		
+		GPSPositionData gpsPosition;
+		GPSPositionGet(&gpsPosition);
+		
+		vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
+		vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
+		vel[2] = 0;
+		
+		// convert from cm back to meters
+		float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
+		// put in local NED frame
+		float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
+		LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
+
+		RotFrom2Vectors(&accelsData.x, ge, &magData.x, home.Be, Rbe);
+		R2Quaternion(Rbe,q);
+		INSSetState(NED, vel, q, &gyrosData.x, zeros);
+		INSSetGyroBias(&gyrosData.x);
+		INSResetP(Pdiag);
+		
+		ins_last_time = PIOS_DELAY_GetRaw();	
+		return 0;
+	}
+	
+	// Perform the update	
+	uint16_t sensors = 0;
+	float dT;
+	
+	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
+	ins_last_time = PIOS_DELAY_GetRaw();
+	
+	// This should only happen at start up or at mode switches
+	if(dT > 0.01f)
+		dT = 0.01f;
+	else if(dT <= 0.001f)
+		dT = 0.001f;
+	
+	
+	GyrosBiasData gyrosBias;
+	GyrosBiasGet(&gyrosBias);
+
+	float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, 
+		(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, 
+		(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
+	
+	// Advance the state estimate
+	INSStatePrediction(gyros, &accelsData.x, dT);
+	
+	// Copy the attitude into the UAVO
+	AttitudeActualData attitude;
+	AttitudeActualGet(&attitude);
+	attitude.q1 = Nav.q[0];
+	attitude.q2 = Nav.q[1];
+	attitude.q3 = Nav.q[2];
+	attitude.q4 = Nav.q[3];
+	Quaternion2RPY(&attitude.q1,&attitude.Roll);
+	AttitudeActualSet(&attitude);
+	
+	// Copy the gyro bias into the UAVO
+	gyrosBias.x = Nav.gyro_bias[0];
+	gyrosBias.y = Nav.gyro_bias[1];
+	gyrosBias.z = Nav.gyro_bias[2];
+	GyrosBiasSet(&gyrosBias);
+	
+	// Advance the covariance estimate
+	INSCovariancePrediction(dT);
+	
+	if(mag_updated)
+		sensors |= MAG_SENSORS;
+	if(baro_updated)
+		sensors |= BARO_SENSOR;
+
+	float NED[3] = {0,0,0};
+	float vel[3] = {0,0,0};
+	
+	if(gps_updated)
+	{
+		sensors = HORIZ_SENSORS | VERT_SENSORS;
+		GPSPositionData gpsPosition;
+		GPSPositionGet(&gpsPosition);
+		
+		vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
+		vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
+		vel[2] = 0;
+
+		HomeLocationData home;
+		HomeLocationGet(&home);
+
+		// convert from cm back to meters
+		float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
+		// put in local NED frame
+		float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
+		LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
+	}
+	
+	/*
+	 * 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(&magData.x, NED, vel, baroData.Altitude, sensors);
+	
+	// Copy the position and velocity into the UAVO
+	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);
+	
+
+	if(fabs(Nav.gyro_bias[0]) > 0.1f || fabs(Nav.gyro_bias[1]) > 0.1f || fabs(Nav.gyro_bias[2]) > 0.1f) {
+		float zeros[3] = {0.0f,0.0f,0.0f};
+		INSSetGyroBias(zeros);
+	}
+
+	return 0;
+}
+
+static void settingsUpdatedCb(UAVObjEvent * objEv) 
+{
+	AttitudeSettingsData attitudeSettings;
+	AttitudeSettingsGet(&attitudeSettings);
+
+
+	accelKp = attitudeSettings.AccelKp;
+	accelKi = attitudeSettings.AccelKi;
+	yawBiasRate = attitudeSettings.YawBiasRate;
+	gyroGain = attitudeSettings.GyroGain;
+
+	zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
+
+	accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
+	accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
+	accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
+
+	GyrosBiasData gyrosBias;
+	GyrosBiasGet(&gyrosBias);
+	gyrosBias.x = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
+	gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
+	gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
+	GyrosBiasSet(&gyrosBias);
+	
+	// Indicates not to expend cycles on rotation
+	if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
+	   attitudeSettings.BoardRotation[2] == 0) {
+		rotate = 0;
+
+		// Shouldn't be used but to be safe
+		float rotationQuat[4] = {1,0,0,0};
+		Quaternion2R(rotationQuat, R);
+	} else {
+		float rotationQuat[4];
+		const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
+			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
+			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
+		RPY2Quaternion(rpy, rotationQuat);
+		Quaternion2R(rotationQuat, R);
+		rotate = 1;
+	}
+}
+/**
+  * @}
+  * @}
+  */

flight/Modules/Attitude/revolution/inc/attitude.h

@@ -1,37 +1,37 @@
-/**
+/**
- ******************************************************************************
+ ******************************************************************************
- *
+ * @addtogroup OpenPilotModules OpenPilot Modules
- * @file       test_common.c
+ * @{ 
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup Attitude Attitude Module
- * @brief      Sets up ans runs main OpenPilot tasks.
+ * @{ 
- * @see        The GNU Public License (GPL) Version 3
+ *
- *
+ * @file       attitude.h
- *****************************************************************************/
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
-/*
+ * @brief      Acquires sensor data and fuses it into attitude estimate for CC
- * 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
+ * @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.
-
+ *
-/* OpenPilot Includes */
+ * You should have received a copy of the GNU General Public License along
-#include "openpilot.h"
+ * with this program; if not, write to the Free Software Foundation, Inc.,
-
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
-/**
+ */
- * Called by the RTOS when a stack overflow is detected.
+#ifndef ATTITUDE_H
- */
+#define ATTITUDE_H
-void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed portCHAR *pcTaskName )
+
-{
+#include "openpilot.h"
-	PIOS_DEBUG_Panic("STACK OVERFLOW");
+
-}
+int32_t AttitudeInitialize(void);
-
+
-
+#endif // ATTITUDE_H

flight/Modules/FirmwareIAP/firmwareiap.c

@@ -66,8 +66,6 @@ static portTickType lastResetSysTime;
 // Private functions
 static void FirmwareIAPCallback(UAVObjEvent* ev);
 
-FirmwareIAPObjData 	data;
-
 static uint32_t	get_time(void);
 
 // Private types
@@ -96,6 +94,9 @@ int32_t FirmwareIAPInitialize()
 	
 	const struct pios_board_info * bdinfo = &pios_board_info_blob;
 
+	FirmwareIAPObjData data;
+	FirmwareIAPObjGet(&data);
+
 	data.BoardType= bdinfo->board_type;
 	PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
 	PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
@@ -125,6 +126,9 @@ static void FirmwareIAPCallback(UAVObjEvent* ev)
 	
 	if(iap_state == IAP_STATE_RESETTING)
 		return;
+
+	FirmwareIAPObjData data;
+	FirmwareIAPObjGet(&data);
 	
 	if ( ev->obj == FirmwareIAPObjHandle() )	{
 		// Get the input object data
@@ -238,7 +242,10 @@ static void resetTask(UAVObjEvent * ev)
 #if defined (PIOS_LED_ALARM)
 	PIOS_LED_Toggle(PIOS_LED_ALARM);
 #endif	/* PIOS_LED_ALARM */
-	
+
+	FirmwareIAPObjData data;
+	FirmwareIAPObjGet(&data);
+
 	if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) {
 		lastResetSysTime = xTaskGetTickCount();
 		data.BoardType=0xFF;

flight/Modules/GPS/GPS.c

@@ -144,7 +144,6 @@ int32_t GPSInitialize(void)
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 		HomeLocationInitialize();
 #endif
-		HwSettingsInitialize();
 		updateSettings();
 
 		gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);

flight/Modules/GPS/NMEA.c

@@ -216,7 +216,7 @@ static float NMEA_real_to_float(char *nmea_real)
 	}
 
 	/* Convert to float */
-	return (((float)whole) + fract * pow(10, -fract_units));
+	return (((float)whole) + fract * powf(10.0f, -fract_units));
 }
 
 /*
@@ -239,37 +239,38 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool
 	}
 
 	/* scale up the mmmm[mm] field apropriately depending on # of digits */
+	/* not using 1eN notation because that forces fixed point and lost precision */
 	switch (units) {
 	case 0:
 		/* no digits, value is zero so no scaling */
 		break;
 	case 1:		/* m       */
-		num_m *= 1e6;	/* m000000 */
+		num_m *= 1000000;	/* m000000 */
 		break;
 	case 2:		/* mm      */
-		num_m *= 1e5;	/* mm00000 */
+		num_m *= 100000;	/* mm00000 */
 		break;
 	case 3:		/* mmm     */
-		num_m *= 1e4;	/* mmm0000 */
+		num_m *= 10000;	/* mmm0000 */
 		break;
 	case 4:		/* mmmm    */
-		num_m *= 1e3;	/* mmmm000 */
+		num_m *= 1000;	/* mmmm000 */
 		break;
 	case 5:		/* mmmmm   */
-		num_m *= 1e2;	/* mmmmm00 */
+		num_m *= 100;	/* mmmmm00 */
 		break;
 	case 6:		/* mmmmmm  */
-		num_m *= 1e1;	/* mmmmmm0 */
+		num_m *= 10;	/* mmmmmm0 */
 		break;
 	default:
 		/* unhandled format */
-		num_m = 0;
+		num_m = 0.0f;
 		break;
 	}
 
-	*latlon = (num_DDDMM / 100) * 1e7;	/* scale the whole degrees */
+	*latlon = (num_DDDMM / 100) * 10000000;        /* scale the whole degrees */
-	*latlon += (num_DDDMM % 100) * 1e7 / 60;	/* add in the scaled decimal whole minutes */
+	*latlon += (num_DDDMM % 100) * 10000000 / 60;  /* add in the scaled decimal whole minutes */
-	*latlon += num_m / 60;	/* add in the scaled decimal fractional minutes */
+	*latlon += num_m / 60;	                  /* add in the scaled decimal fractional minutes */
 
 	if (negative)
 		*latlon *= -1;

flight/Modules/Guidance/guidance.c

@@ -192,9 +192,9 @@ static void guidanceTask(void *parameters)
 		NedAccelData accelData;
 		NedAccelGet(&accelData);
 		// Convert from m/s to cm/s
-		accelData.North = accel_ned[0] * 100;
+		accelData.North = accel_ned[0];
-		accelData.East = accel_ned[1] * 100;
+		accelData.East = accel_ned[1];
-		accelData.Down = accel_ned[2] * 100;
+		accelData.Down = accel_ned[2];
 		NedAccelSet(&accelData);
 		
 		

flight/Modules/ManualControl/inc/manualcontrol.h

@@ -39,6 +39,7 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
 	(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
+	(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
 	)

flight/Modules/ManualControl/manualcontrol.c

@@ -44,20 +44,23 @@
 #include "flightstatus.h"
 #include "accessorydesired.h"
 #include "receiveractivity.h"
+#include "altitudeholddesired.h"
+#include "positionactual.h"
+#include "baroaltitude.h"
 
 // Private constants
 #if defined(PIOS_MANUAL_STACK_SIZE)
 #define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
 #else
-#define STACK_SIZE_BYTES 824
+#define STACK_SIZE_BYTES 1024
 #endif
 
 #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
 #define UPDATE_PERIOD_MS 20
-#define THROTTLE_FAILSAFE -0.1
+#define THROTTLE_FAILSAFE -0.1f
-#define FLIGHT_MODE_LIMIT 1.0/3.0
+#define FLIGHT_MODE_LIMIT 1.0f/3.0f
 #define ARMED_TIME_MS      1000
-#define ARMED_THRESHOLD    0.50
+#define ARMED_THRESHOLD    0.50f
 //safe band to allow a bit of calibration error or trim offset (in microseconds)
 #define CONNECTION_OFFSET 150
 
@@ -79,6 +82,7 @@ static portTickType lastSysTime;
 // Private functions
 static void updateActuatorDesired(ManualControlCommandData * cmd);
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
+static void altitudeHoldDesired(ManualControlCommandData * cmd);
 static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
 static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
 static void setArmedIfChanged(uint8_t val);
@@ -197,7 +201,7 @@ static void manualControlTask(void *parameters)
 				/* trying to fly via GCS and lost connection.  fall back to transmitter */
 				UAVObjMetadata metadata;
 				ManualControlCommandGetMetadata(&metadata);
-				metadata.access = ACCESS_READWRITE;
+				UAVObjSetAccess(&metadata, ACCESS_READWRITE);
 				ManualControlCommandSetMetadata(&metadata);
 			}
 		}
@@ -383,7 +387,13 @@ static void manualControlTask(void *parameters)
 				updateStabilizationDesired(&cmd, &settings);
 				break;
 			case FLIGHTMODE_GUIDANCE:
-				// TODO: Implement
+				switch(flightStatus.FlightMode) {
+					case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
+						altitudeHoldDesired(&cmd);
+						break;
+					default:
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+				}
 				break;
 		}
 	}
@@ -596,6 +606,52 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
 	StabilizationDesiredSet(&stabilization);
 }
 
+#if defined(REVOLUTION)
+// TODO: Need compile flag to exclude this from copter control
+static void altitudeHoldDesired(ManualControlCommandData * cmd)
+{
+	const float DEADBAND_HIGH = 0.55;
+	const float DEADBAND_LOW = 0.45;
+	
+	static portTickType lastSysTime;
+	static bool zeroed = false;
+	portTickType thisSysTime;
+	float dT;
+	AltitudeHoldDesiredData altitudeHoldDesired;
+	AltitudeHoldDesiredGet(&altitudeHoldDesired);
+
+	StabilizationSettingsData stabSettings;
+	StabilizationSettingsGet(&stabSettings);
+
+	thisSysTime = xTaskGetTickCount();
+	dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
+	lastSysTime = thisSysTime;
+
+	altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
+	altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
+	altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
+	
+	float currentDown;
+	PositionActualDownGet(&currentDown);
+	if(dT > 1) {
+		// After not being in this mode for a while init at current height
+		altitudeHoldDesired.Altitude = 0;
+		zeroed = false;
+	} else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
+		altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
+	else if (cmd->Throttle < DEADBAND_LOW && zeroed)
+		altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
+	else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH)  // Require the stick to enter the dead band before they can move height
+		zeroed = true;
+	
+	AltitudeHoldDesiredSet(&altitudeHoldDesired);
+}
+#else
+static void altitudeHoldDesired(ManualControlCommandData * cmd)
+{
+	AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
+}
+#endif
 /**
  * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
  */

flight/Modules/Osd/OsdEtStd/OsdEtStd.c

@@ -248,7 +248,7 @@ static bool Read(uint32_t start, uint8_t length, uint8_t * buffer)
 	cmd[3] = (uint8_t) (start >> 8);
 	cmd[4] = length;
 
-	return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
+	return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0;
 }
 
 static bool Write(uint32_t start, uint8_t length, const uint8_t * buffer)

flight/Modules/OveroSync/inc/overosync.h

@@ -0,0 +1,46 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup TelemetryModule Telemetry Module
+ * @brief Main telemetry module
+ * Starts three tasks (RX, TX, and priority TX) that watch event queues
+ * and handle all the telemetry of the UAVobjects
+ * @{ 
+ *
+ * @file       telemetry.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Include file of the telemetry module.
+ * 	       As with all modules only the initialize function is exposed all other
+ * 	       interactions with the module take place through the event queue and
+ *             objects.
+ * @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 TELEMETRY_H
+#define TELEMETRY_H
+
+int32_t TelemetryInitialize(void);
+
+#endif // TELEMETRY_H
+
+/**
+  * @}
+  * @}
+  */

flight/Modules/OveroSync/overosync.c

@@ -0,0 +1,371 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup TelemetryModule Telemetry Module
+ * @brief Main telemetry module
+ * Starts three tasks (RX, TX, and priority TX) that watch event queues
+ * and handle all the telemetry of the UAVobjects
+ * @{ 
+ *
+ * @file       telemetry.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Telemetry module, handles telemetry and UAVObject updates
+ * @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 "overosync.h"
+#include "overosyncstats.h"
+#include "systemstats.h"
+
+// Private constants
+#define OVEROSYNC_PACKET_SIZE 1024
+#define MAX_QUEUE_SIZE   10
+#define STACK_SIZE_BYTES 4096
+#define TASK_PRIORITY (tskIDLE_PRIORITY + 0)
+
+// Private types
+
+// Private variables
+static xQueueHandle queue;
+static UAVTalkConnection uavTalkCon;
+static xTaskHandle overoSyncTaskHandle;
+volatile bool buffer_swap_failed;
+volatile uint32_t buffer_swap_timeval;
+
+// Private functions
+static void overoSyncTask(void *parameters);
+static int32_t packData(uint8_t * data, int32_t length);
+static int32_t transmitData();
+static void transmitDataDone(bool crc_ok, uint8_t crc_val);
+static void registerObject(UAVObjHandle obj);
+
+// External variables
+extern int32_t pios_spi_overo_id;
+
+struct dma_transaction {
+	uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
+	uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
+};
+
+struct overosync {
+	struct dma_transaction transactions[2];
+	uint32_t active_transaction_id;
+	uint32_t loading_transaction_id;
+	xSemaphoreHandle transaction_lock;
+	xSemaphoreHandle buffer_lock;
+	volatile bool transaction_done;
+	uint32_t sent_bytes;
+	uint32_t write_pointer;
+	uint32_t sent_objects;
+	uint32_t failed_objects;
+	uint32_t received_objects;
+	uint32_t framesync_error;
+};
+
+struct overosync *overosync;
+
+static void PIOS_OVERO_IRQHandler();
+
+static const struct pios_exti_cfg pios_exti_overo_cfg __exti_config = {
+	.vector = PIOS_OVERO_IRQHandler,
+	.line = EXTI_Line15,
+	.pin = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_15,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode = GPIO_Mode_IN,
+			.GPIO_OType = GPIO_OType_OD,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL,
+		},
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = EXTI15_10_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.exti = {
+		.init = {
+			.EXTI_Line = EXTI_Line15, // matches above GPIO pin
+			.EXTI_Mode = EXTI_Mode_Interrupt,
+			.EXTI_Trigger = EXTI_Trigger_Rising,
+			.EXTI_LineCmd = ENABLE,
+		},
+	},
+};
+
+/**
+ * On the rising edge of NSS schedule a new transaction.  This cannot be
+ * done by the DMA complete because there is 150 us between that and the
+ * Overo deasserting the CS line.  We don't want to spin that long in an
+ * isr
+ */
+void PIOS_OVERO_IRQHandler()
+{
+	// transmitData must not block to get semaphore for when we get out of
+	// frame and transaction is still running here.  -1 indicates the transaction
+	// semaphore is blocked and we are still in a transaction, thus a framesync
+	// error occurred.  This shouldn't happen.  Race condition?
+	if(transmitData() == -1)
+		overosync->framesync_error++;
+}
+
+/**
+ * Initialise the telemetry module
+ * \return -1 if initialisation failed
+ * \return 0 on success
+ */
+int32_t OveroSyncInitialize(void)
+{
+	if(pios_spi_overo_id == 0)
+		return -1;
+	
+	OveroSyncStatsInitialize();
+
+	PIOS_EXTI_Init(&pios_exti_overo_cfg);
+	// Create object queues
+	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
+
+	// Initialise UAVTalk
+	uavTalkCon = UAVTalkInitialize(&packData);
+
+	return 0;
+}
+
+/**
+ * Initialise the telemetry module
+ * \return -1 if initialisation failed
+ * \return 0 on success
+ */
+int32_t OveroSyncStart(void)
+{
+	if(pios_spi_overo_id == 0)
+		return -1;
+
+	overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync));
+	if(overosync == NULL)
+		return -1;
+
+	overosync->transaction_lock = xSemaphoreCreateMutex();
+	if(overosync->transaction_lock == NULL)
+		return -1;
+
+	overosync->buffer_lock = xSemaphoreCreateMutex();
+	if(overosync->buffer_lock == NULL)
+		return -1;
+
+	overosync->active_transaction_id = 0;
+	overosync->loading_transaction_id = 0;
+	overosync->write_pointer = 0;
+	overosync->sent_bytes = 0;
+	overosync->framesync_error = 0;
+
+	// Process all registered objects and connect queue for updates
+	UAVObjIterate(&registerObject);
+	
+	// Start telemetry tasks
+	xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle);
+	
+	TaskMonitorAdd(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle);
+	
+	return 0;
+}
+
+MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart)
+
+/**
+ * Register a new object, adds object to local list and connects the queue depending on the object's
+ * telemetry settings.
+ * \param[in] obj Object to connect
+ */
+static void registerObject(UAVObjHandle obj)
+{
+	int32_t eventMask;
+	eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+	if (UAVObjIsMetaobject(obj)) {
+		eventMask |= EV_UNPACKED;	// we also need to act on remote updates (unpack events)
+	}
+	UAVObjConnectQueue(obj, queue, eventMask);
+}
+
+/**
+ * Telemetry transmit task, regular priority
+ *
+ * Logic: We need to double buffer the DMA transfers.  Pack the buffer until either
+ * 1) it is full (and then we should record the number of missed events then)
+ * 2) the current transaction is done (we should immediately schedule since we are slave)
+ * when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
+ * and then take the semaphrore
+ */
+static void overoSyncTask(void *parameters)
+{
+	UAVObjEvent ev;
+
+	// Kick off SPI transfers (once one is completed another will automatically transmit)
+	overosync->transaction_done = true;
+	overosync->sent_objects = 0;
+	overosync->failed_objects = 0;
+	overosync->received_objects = 0;
+	
+	portTickType lastUpdateTime = xTaskGetTickCount();
+	portTickType updateTime;
+	
+	// Loop forever
+	while (1) {
+		// Wait for queue message
+		if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
+			// Process event.  This calls transmitData
+			UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
+
+			updateTime = xTaskGetTickCount();
+			if(((portTickType) (updateTime - lastUpdateTime)) > 1000) {
+				// Update stats.  This will trigger a local send event too
+				OveroSyncStatsData syncStats;
+				syncStats.Send = overosync->sent_bytes;
+				syncStats.Received = 0;
+				syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE;
+				syncStats.DroppedUpdates = overosync->failed_objects;
+				OveroSyncStatsSet(&syncStats);
+				overosync->failed_objects = 0;
+				overosync->sent_bytes = 0;
+				lastUpdateTime = updateTime;
+			}
+		}
+	}
+}
+
+static void transmitDataDone(bool crc_ok, uint8_t crc_val)
+{
+	uint8_t *rx_buffer;
+	static signed portBASE_TYPE xHigherPriorityTaskWoken;
+
+	rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
+
+	// Release the semaphore and start another transaction (which grabs semaphore again but then
+	// returns instantly).  Because this is called by the DMA ISR we need to be aware of context
+	// switches.
+	xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
+	portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
+
+	overosync->transaction_done = true;
+	
+	// Parse the data from overo
+	for (uint32_t i = 0; rx_buffer[0] != 0 && i < sizeof(rx_buffer) ; i++)
+		UAVTalkProcessInputStream(uavTalkCon, rx_buffer[i]);
+}
+
+/**
+ * Transmit data buffer to the modem or USB port.
+ * \param[in] data Data buffer to send
+ * \param[in] length Length of buffer
+ * \return -1 on failure
+ * \return number of bytes transmitted on success
+ */
+uint32_t too_long = 0;
+static int32_t packData(uint8_t * data, int32_t length)
+{
+	uint8_t *tx_buffer;
+	
+	portTickType tickTime = xTaskGetTickCount();
+
+	// Get the lock for manipulating the buffer
+	xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY);
+
+	// Check this packet will fit
+	if ((overosync->write_pointer + length + sizeof(tickTime)) >
+		sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
+		overosync->failed_objects ++;
+		xSemaphoreGive(overosync->buffer_lock);
+		return -1;
+	}
+
+	// Get offset into buffer and copy contents
+	tx_buffer = overosync->transactions[overosync->loading_transaction_id].tx_buffer +
+		overosync->write_pointer;
+	memcpy(tx_buffer, &tickTime, sizeof(tickTime));
+	memcpy(tx_buffer + sizeof(tickTime),data,length);
+	overosync->write_pointer += length + sizeof(tickTime);
+	overosync->sent_bytes += length;
+	overosync->sent_objects++;
+
+	xSemaphoreGive(overosync->buffer_lock);
+	
+	// When the NSS line rises while we are packing data then a transaction doesn't start
+	// because that means we will be here very shortly afterwards (priority of task making that
+	// not always perfectly true) schedule the transaction here.
+	if (buffer_swap_failed && (PIOS_DELAY_DiffuS(buffer_swap_timeval) < 50)) {
+		buffer_swap_failed = false;
+		transmitData();
+	} else if (buffer_swap_failed) {
+		buffer_swap_failed = false;
+		too_long++;
+	}
+
+	return length;
+}
+
+static int32_t transmitData()
+{
+	uint8_t *tx_buffer, *rx_buffer;
+	static signed portBASE_TYPE xHigherPriorityTaskWoken;
+
+	// Get this lock first so we don't swap buffers and then fail
+	// to start
+	if (xSemaphoreTake(overosync->transaction_lock, 0) == pdFALSE)
+		return -1;
+
+	// Get lock to manipulate buffers
+	if(xSemaphoreTake(overosync->buffer_lock, 0) == pdFALSE) {
+		xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
+		portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
+		buffer_swap_failed = true;
+		buffer_swap_timeval = PIOS_DELAY_GetRaw();
+		return -2;
+	}
+
+	overosync->transaction_done = false;
+
+	// Swap buffers
+	overosync->active_transaction_id = overosync->loading_transaction_id;
+	overosync->loading_transaction_id = (overosync->loading_transaction_id + 1) % 
+		NELEMENTS(overosync->transactions);
+		
+	tx_buffer = overosync->transactions[overosync->active_transaction_id].tx_buffer;
+	rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
+	
+	// Prepare the new loading buffer
+	memset(overosync->transactions[overosync->loading_transaction_id].tx_buffer, 0xff, 
+		   sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer));
+	overosync->write_pointer = 0;
+
+	xSemaphoreGiveFromISR(overosync->buffer_lock, &xHigherPriorityTaskWoken);
+	portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
+
+	return PIOS_SPI_TransferBlock(pios_spi_overo_id, (uint8_t *) tx_buffer, (uint8_t *) rx_buffer, sizeof(overosync->transactions[overosync->active_transaction_id].tx_buffer), &transmitDataDone) == 0 ? 0 : -3;
+}
+
+/**
+  * @}
+  * @}
+  */

flight/Modules/Sensors/inc/sensors.h

@@ -0,0 +1,37 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup Sensors Sensors Module
+ * @{ 
+ *
+ * @file       attitude.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      Acquires sensor data and fuses it into attitude estimate for CC
+ *
+ * @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 SENSORS_H
+#define SENSORS_H
+
+#include "openpilot.h"
+
+int32_t SensorsInitialize(void);
+
+#endif // SENSORS_H

flight/Modules/Sensors/sensors.c

@@ -0,0 +1,480 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup Sensors
+ * @brief Acquires sensor data 
+ * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects
+ * @{
+ *
+ * @file       sensors.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Module to handle all comms to the AHRS on a periodic basis.
+ *
+ * @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
+ */
+
+/**
+ * Input objects: None, takes sensor data via pios
+ * Output objects: @ref Gyros @ref Accels @ref Magnetometer
+ *
+ * The module executes in its own thread.
+ *
+ * UAVObjects are automatically generated by the UAVObjectGenerator from
+ * the object definition XML file.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "pios.h"
+#include "attitude.h"
+#include "magnetometer.h"
+#include "accels.h"
+#include "gyros.h"
+#include "gyrosbias.h"
+#include "attitudeactual.h"
+#include "attitudesettings.h"
+#include "revocalibration.h"
+#include "flightstatus.h"
+#include "gpsposition.h"
+#include "baroaltitude.h"
+#include "CoordinateConversions.h"
+
+#include <pios_board_info.h>
+
+// Private constants
+#define STACK_SIZE_BYTES 1540
+#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
+#define SENSOR_PERIOD 2
+
+#define F_PI 3.14159265358979323846f
+#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
+// Private types
+
+// Private variables
+static xTaskHandle sensorsTaskHandle;
+static bool gps_updated = false;
+static bool baro_updated = false;
+
+// Private functions
+static void SensorsTask(void *parameters);
+static void settingsUpdatedCb(UAVObjEvent * objEv);
+static void sensorsUpdatedCb(UAVObjEvent * objEv);
+
+// These values are initialized by settings but can be updated by the attitude algorithm
+static bool bias_correct_gyro = true;
+
+static float mag_bias[3] = {0,0,0};
+static float mag_scale[3] = {0,0,0};
+static float accel_bias[3] = {0,0,0};
+static float accel_scale[3] = {0,0,0};
+
+static float R[3][3] = {{0}};
+static int8_t rotate = 0;
+
+/**
+ * API for sensor fusion algorithms:
+ * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
+ *   Stores all the queues the algorithm will pull data from
+ * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
+ * Update() -- queries queues and updates the attitude estiamte
+ */
+
+
+/**
+ * Initialise the module.  Called before the start function
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t SensorsInitialize(void)
+{
+	GyrosInitialize();
+	GyrosBiasInitialize();
+	AccelsInitialize();
+	MagnetometerInitialize();
+	RevoCalibrationInitialize();
+	AttitudeSettingsInitialize();
+
+	rotate = 0;
+
+	RevoCalibrationConnectCallback(&settingsUpdatedCb);
+	AttitudeSettingsConnectCallback(&settingsUpdatedCb);
+
+	return 0;
+}
+
+/**
+ * Start the task.  Expects all objects to be initialized by this point.
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t SensorsStart(void)
+{
+	// Start main task
+	xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
+	TaskMonitorAdd(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
+	PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
+
+	return 0;
+}
+
+MODULE_INITCALL(SensorsInitialize, SensorsStart)
+
+int32_t accel_test;
+int32_t gyro_test;
+int32_t mag_test;
+//int32_t pressure_test;
+
+
+/**
+ * The sensor task.  This polls the gyros at 500 Hz and pumps that data to
+ * stabilization and to the attitude loop
+ * 
+ * This function has a lot of if/defs right now to allow these configurations:
+ * 1. BMA180 accel and MPU6000 gyro
+ * 2. MPU6000 gyro and accel
+ * 3. BMA180 accel and L3GD20 gyro
+ */
+
+uint32_t sensor_dt_us;
+static void SensorsTask(void *parameters)
+{
+	portTickType lastSysTime;
+	uint32_t accel_samples = 0;
+	uint32_t gyro_samples = 0;
+	int32_t accel_accum[3] = {0, 0, 0};
+	int32_t gyro_accum[3] = {0,0,0};
+	float gyro_scaling = 0;
+	float accel_scaling = 0;
+	static int32_t timeval;
+
+	AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
+
+	UAVObjEvent ev;
+	settingsUpdatedCb(&ev);
+
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;	
+
+	switch(bdinfo->board_rev) {
+		case 0x01:
+#if defined(PIOS_INCLUDE_L3GD20)
+			gyro_test = PIOS_L3GD20_Test();
+#endif
+#if defined(PIOS_INCLUDE_BMA180)
+			accel_test = PIOS_BMA180_Test();
+#endif
+			break;
+		case 0x02:
+#if defined(PIOS_INCLUDE_MPU6000)
+			gyro_test = PIOS_MPU6000_Test();
+			accel_test = gyro_test;
+#endif
+			break;
+		default:
+			PIOS_DEBUG_Assert(0);
+	}
+
+#if defined(PIOS_INCLUDE_HMC5883)
+	mag_test = PIOS_HMC5883_Test();
+#endif
+
+	if(accel_test < 0 || gyro_test < 0 || mag_test < 0) {
+		AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
+		while(1) {
+			PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
+			vTaskDelay(10);
+		}
+	}
+	
+	// If debugging connect callback
+	if(pios_com_aux_id != 0) {
+		BaroAltitudeConnectCallback(&sensorsUpdatedCb);
+		GPSPositionConnectCallback(&sensorsUpdatedCb);
+	}
+	
+	// Main task loop
+	lastSysTime = xTaskGetTickCount();
+	bool error = false;
+	uint32_t mag_update_time = PIOS_DELAY_GetRaw();
+	while (1) {
+		// TODO: add timeouts to the sensor reads and set an error if the fail
+		sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
+		timeval = PIOS_DELAY_GetRaw();
+
+		if (error) {
+			PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
+			lastSysTime = xTaskGetTickCount();
+			vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
+			AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
+			error = false;
+		} else {
+			AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
+		}
+
+		int32_t read_good;
+		int32_t count;
+
+		for (int i = 0; i < 3; i++) {
+			accel_accum[i] = 0;
+			gyro_accum[i] = 0;
+		}
+		accel_samples = 0;
+		gyro_samples = 0;
+
+		AccelsData accelsData;
+		GyrosData gyrosData;
+
+		switch(bdinfo->board_rev) {
+			case 0x01:  // L3GD20 + BMA180 board
+#if defined(PIOS_INCLUDE_BMA180)
+			{
+				struct pios_bma180_data accel;
+				
+				count = 0;
+				while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
+					error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
+				if (error) {
+					// Unfortunately if the BMA180 ever misses getting read, then it will not
+					// trigger more interrupts.  In this case we must force a read to kickstarts
+					// it.
+					struct pios_bma180_data data;
+					PIOS_BMA180_ReadAccels(&data);
+					continue;
+				}
+				while(read_good == 0) {	
+					count++;
+					
+					accel_accum[0] += accel.x;
+					accel_accum[1] += accel.y;
+					accel_accum[2] += accel.z;
+					
+					read_good = PIOS_BMA180_ReadFifo(&accel);
+				}
+				accel_samples = count;
+				accel_scaling = PIOS_BMA180_GetScale();
+				
+				// Get temp from last reading
+				accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
+			}
+#endif
+#if defined(PIOS_INCLUDE_L3GD20)
+			{
+				struct pios_l3gd20_data gyro;
+				gyro_samples = 0;
+				xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
+				
+				if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) {
+					error = true;
+					continue;
+				}
+				
+				gyro_samples = 1;
+				gyro_accum[0] += gyro.gyro_x;
+				gyro_accum[1] += gyro.gyro_y;
+				gyro_accum[2] += gyro.gyro_z;
+				
+				gyro_scaling = PIOS_L3GD20_GetScale();
+
+				// Get temp from last reading
+				gyrosData.temperature = gyro.temperature;
+			}
+#endif
+				break;
+			case 0x02:  // MPU6000 board
+#if defined(PIOS_INCLUDE_MPU6000)
+			{
+				struct pios_mpu6000_data mpu6000_data;
+				xQueueHandle queue = PIOS_MPU6000_GetQueue();
+				
+				while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY)
+				{
+					gyro_accum[0] += mpu6000_data.gyro_x;
+					gyro_accum[1] += mpu6000_data.gyro_y;
+					gyro_accum[2] += mpu6000_data.gyro_z;
+
+					accel_accum[0] += mpu6000_data.accel_x;
+					accel_accum[1] += mpu6000_data.accel_y;
+					accel_accum[2] += mpu6000_data.accel_z;
+
+					gyro_samples ++;
+					accel_samples ++;
+				}
+				
+				if (gyro_samples == 0) {
+					PIOS_MPU6000_ReadGyros(&mpu6000_data);
+					error = true;
+					continue;
+				}
+
+				gyro_scaling = PIOS_MPU6000_GetScale();
+				accel_scaling = PIOS_MPU6000_GetAccelScale();
+
+				gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
+				accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
+			}
+#endif /* PIOS_INCLUDE_MPU6000 */
+				break;
+			default:
+				PIOS_DEBUG_Assert(0);
+		}
+
+		// Scale the accels
+		float accels[3] = {(float) accel_accum[1] / accel_samples, 
+		                   (float) accel_accum[0] / accel_samples,
+		                  -(float) accel_accum[2] / accel_samples};
+		float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
+		                       accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
+		                       accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]};
+		if (rotate) {
+			rot_mult(R, accels_out, accels);
+			accelsData.x = accels[0];
+			accelsData.y = accels[1];
+			accelsData.z = accels[2];
+		} else {
+			accelsData.x = accels_out[0];
+			accelsData.y = accels_out[1];
+			accelsData.z = accels_out[2];
+		}
+		AccelsSet(&accelsData);
+
+		// Scale the gyros
+		float gyros[3] = {(float) gyro_accum[1] / gyro_samples,
+		                  (float) gyro_accum[0] / gyro_samples,
+		                 -(float) gyro_accum[2] / gyro_samples};
+		float gyros_out[3] = {gyros[0] * gyro_scaling,
+		                      gyros[1] * gyro_scaling,
+		                      gyros[2] * gyro_scaling};
+		if (rotate) {
+			rot_mult(R, gyros_out, gyros);
+			gyrosData.x = gyros[0];
+			gyrosData.y = gyros[1];
+			gyrosData.z = gyros[2];
+		} else {
+			gyrosData.x = gyros_out[0];
+			gyrosData.y = gyros_out[1];
+			gyrosData.z = gyros_out[2];
+		}
+		
+		if (bias_correct_gyro) {
+			// Apply bias correction to the gyros
+			GyrosBiasData gyrosBias;
+			GyrosBiasGet(&gyrosBias);
+			gyrosData.x += gyrosBias.x;
+			gyrosData.y += gyrosBias.y;
+			gyrosData.z += gyrosBias.z;
+		}
+		GyrosSet(&gyrosData);
+		
+		// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
+		// and make it average zero (weakly)
+
+#if defined(PIOS_INCLUDE_HMC5883)
+		MagnetometerData mag;
+		if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
+			int16_t values[3];
+			PIOS_HMC5883_ReadMag(values);
+			float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0],
+			                (float) values[0] * mag_scale[1] - mag_bias[1],
+			                -(float) values[2] * mag_scale[2] - mag_bias[2]};
+			if (rotate) {
+				float mag_out[3];
+				rot_mult(R, mags, mag_out);
+				mag.x = mag_out[0];
+				mag.y = mag_out[1];
+				mag.z = mag_out[2];
+			} else {
+				mag.x = mags[0];
+				mag.y = mags[1];
+				mag.z = mags[2];
+			}
+			MagnetometerSet(&mag);
+			mag_update_time = PIOS_DELAY_GetRaw();
+		}
+#endif
+
+		PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
+
+		switch(bdinfo->board_rev) {
+			case 0x01:  // L3GD20 + BMA180 board
+				lastSysTime = xTaskGetTickCount();
+				break;
+			case 0x02:
+				vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
+				break;
+			default:
+				PIOS_DEBUG_Assert(0);
+		}
+	}
+}
+
+/**
+ * Indicate that these sensors have been updated
+ */
+static void sensorsUpdatedCb(UAVObjEvent * objEv)
+{
+	if(objEv->obj == GPSPositionHandle())
+		gps_updated = true;
+	if(objEv->obj == BaroAltitudeHandle())
+		baro_updated = true;
+}
+
+/**
+ * Locally cache some variables from the AtttitudeSettings object
+ */
+static void settingsUpdatedCb(UAVObjEvent * objEv) {
+	RevoCalibrationData cal;
+	RevoCalibrationGet(&cal);
+	
+	mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
+	mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y];
+	mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z];
+	mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X];
+	mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y];
+	mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z];
+	accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X];
+	accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y];
+	accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z];
+	accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
+	accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
+	accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
+
+	AttitudeSettingsData attitudeSettings;
+	AttitudeSettingsGet(&attitudeSettings);
+
+	// Indicates not to expend cycles on rotation
+	if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
+	   attitudeSettings.BoardRotation[2] == 0) {
+		rotate = 0;
+	} else {
+		float rotationQuat[4];
+		const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
+			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
+			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
+		RPY2Quaternion(rpy, rotationQuat);
+		Quaternion2R(rotationQuat, R);
+		rotate = 1;
+	}
+
+}
+/**
+  * @}
+  * @}
+  */

flight/Modules/Stabilization/stabilization.c

@@ -38,7 +38,7 @@
 #include "ratedesired.h"
 #include "stabilizationdesired.h"
 #include "attitudeactual.h"
-#include "attituderaw.h"
+#include "gyros.h"
 #include "flightstatus.h"
 #include "manualcontrol.h" // Just to get a macro
 #include "CoordinateConversions.h"
@@ -124,7 +124,7 @@ int32_t StabilizationInitialize()
 
 	// Listen for updates.
 	//	AttitudeActualConnectQueue(queue);
-	AttitudeRawConnectQueue(queue);
+	GyrosConnectQueue(queue);
 
 	StabilizationSettingsConnectCallback(SettingsUpdatedCb);
 	SettingsUpdatedCb(StabilizationSettingsHandle());
@@ -140,22 +140,20 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
  */
 static void stabilizationTask(void* parameters)
 {
-	portTickType lastSysTime;
-	portTickType thisSysTime;
 	UAVObjEvent ev;
 
+	uint32_t timeval = PIOS_DELAY_GetRaw();
 
 	ActuatorDesiredData actuatorDesired;
 	StabilizationDesiredData stabDesired;
 	RateDesiredData rateDesired;
 	AttitudeActualData attitudeActual;
-	AttitudeRawData attitudeRaw;
+	GyrosData gyrosData;
 	FlightStatusData flightStatus;
 
 	SettingsUpdatedCb((UAVObjEvent *) NULL);
 
 	// Main task loop
-	lastSysTime = xTaskGetTickCount();
 	ZeroPids();
 	while(1) {
 		PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
@@ -167,16 +165,13 @@ static void stabilizationTask(void* parameters)
 			continue;
 		}
 
-		// Check how long since last update
+		dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
-		thisSysTime = xTaskGetTickCount();
+		timeval = PIOS_DELAY_GetRaw();
-		if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
-			dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
-		lastSysTime = thisSysTime;
 
 		FlightStatusGet(&flightStatus);
 		StabilizationDesiredGet(&stabDesired);
 		AttitudeActualGet(&attitudeActual);
-		AttitudeRawGet(&attitudeRaw);
+		GyrosGet(&gyrosData);
 
 #if defined(DIAGNOSTICS)
 		RateDesiredGet(&rateDesired);
@@ -216,13 +211,13 @@ static void stabilizationTask(void* parameters)
 		float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
 			stabDesired.Pitch - attitudeActual.Pitch,
 			stabDesired.Yaw - attitudeActual.Yaw};
-		local_error[2] = fmod(local_error[2] + 180, 360) - 180;
+		local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
 #endif
 
 
-		for(uint8_t i = 0; i < MAX_AXES; i++) {
+		gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
-			gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha);
+		gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
-		}
+		gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
 
 		float *attitudeDesiredAxis = &stabDesired.Roll;
 		float *actuatorDesiredAxis = &actuatorDesired.Roll;
@@ -373,24 +368,24 @@ float ApplyPid(pid_type * pid, const float err)
 	pid->lastErr = err;
 
 	// Scale up accumulator by 1000 while computing to avoid losing precision
-	pid->iAccumulator += err * (pid->i * dT * 1000);
+	pid->iAccumulator += err * (pid->i * dT * 1000.0f);
-	if(pid->iAccumulator > (pid->iLim * 1000)) {
+	if(pid->iAccumulator > (pid->iLim * 1000.0f)) {
-		pid->iAccumulator = pid->iLim * 1000;
+		pid->iAccumulator = pid->iLim * 1000.0f;
-	} else if (pid->iAccumulator < -(pid->iLim * 1000)) {
+	} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
-		pid->iAccumulator = -pid->iLim * 1000;
+		pid->iAccumulator = -pid->iLim * 1000.0f;
 	}
-	return ((err * pid->p) + pid->iAccumulator / 1000 + (diff * pid->d / dT));
+	return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
 }
 
 
 static void ZeroPids(void)
 {
 	for(int8_t ct = 0; ct < PID_MAX; ct++) {
-		pids[ct].iAccumulator = 0;
+		pids[ct].iAccumulator = 0.0f;
-		pids[ct].lastErr = 0;
+		pids[ct].lastErr = 0.0f;
 	}
 	for(uint8_t i = 0; i < 3; i++)
-		axis_lock_accum[i] = 0;
+		axis_lock_accum[i] = 0.0f;
 }
 
 
@@ -399,10 +394,10 @@ static void ZeroPids(void)
  */
 static float bound(float val)
 {
-	if(val < -1) {
+	if(val < -1.0f) {
-		val = -1;
+		val = -1.0f;
-	} else if(val > 1) {
+	} else if(val > 1.0f) {
-		val = 1;
+		val = 1.0f;
 	}
 	return val;
 }
@@ -466,7 +461,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
 	if(settings.GyroTau < 0.0001)
 		gyro_alpha = 0;   // not trusting this to resolve to 0
 	else
-		gyro_alpha = exp(-fakeDt  / settings.GyroTau);
+		gyro_alpha = expf(-fakeDt  / settings.GyroTau);
 }
 
 

flight/Modules/System/systemmod.c

@@ -384,7 +384,7 @@ static void updateStats()
 	if (now > lastTickCount) {
 		uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS;	// in ms
 		stats.CPULoad =
-			100 - (uint8_t) round(100.0 * ((float)idleCounter / ((float)dT / 1000.0)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
+			100 - (uint8_t) roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
 	} //else: TickCount has wrapped, do not calc now
 	lastTickCount = now;
 	idleCounterClear = 1;
@@ -457,11 +457,21 @@ static void updateSystemAlarms()
 	EventGetStats(&evStats);
 	UAVObjClearStats();
 	EventClearStats();
-	if (objStats.eventErrors > 0 || evStats.eventErrors > 0) {
+	if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0  || evStats.eventErrors > 0) {
 		AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
 	} else {
 		AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
 	}
+	
+	if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
+		SystemStatsData sysStats;
+		SystemStatsGet(&sysStats);
+		sysStats.EventSystemWarningID = evStats.lastErrorID;
+		sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
+		sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;
+		SystemStatsSet(&sysStats);
+	}
+		
 }
 
 /**

flight/Modules/Telemetry/telemetry.c

@@ -73,7 +73,7 @@ static void telemetryTxTask(void *parameters);
 static void telemetryRxTask(void *parameters);
 static int32_t transmitData(uint8_t * data, int32_t length);
 static void registerObject(UAVObjHandle obj);
-static void updateObject(UAVObjHandle obj);
+static void updateObject(UAVObjHandle obj, int32_t eventType);
 static int32_t addObject(UAVObjHandle obj);
 static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
 static void processObjEvent(UAVObjEvent * ev);
@@ -158,32 +158,34 @@ static void registerObject(UAVObjHandle obj)
 	addObject(obj);
 
 	// Setup object for telemetry updates
-	updateObject(obj);
+	updateObject(obj, EV_NONE);
 }
 
 /**
  * Update object's queue connections and timer, depending on object's settings
  * \param[in] obj Object to updates
  */
-static void updateObject(UAVObjHandle obj)
+static void updateObject(UAVObjHandle obj, int32_t eventType)
 {
 	UAVObjMetadata metadata;
+	UAVObjUpdateMode updateMode;
 	int32_t eventMask;
 
 	// Get metadata
 	UAVObjGetMetadata(obj, &metadata);
+	updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
 
 	// Setup object depending on update mode
-	if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC) {
+	if (updateMode == UPDATEMODE_PERIODIC) {
 		// Set update period
 		setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
 		// Connect queue
-		eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+		eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
 		if (UAVObjIsMetaobject(obj)) {
 			eventMask |= EV_UNPACKED;	// we also need to act on remote updates (unpack events)
 		}
 		UAVObjConnectQueue(obj, priorityQueue, eventMask);
-	} else if (metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE) {
+	} else if (updateMode == UPDATEMODE_ONCHANGE) {
 		// Set update period
 		setUpdatePeriod(obj, 0);
 		// Connect queue
@@ -192,7 +194,22 @@ static void updateObject(UAVObjHandle obj)
 			eventMask |= EV_UNPACKED;	// we also need to act on remote updates (unpack events)
 		}
 		UAVObjConnectQueue(obj, priorityQueue, eventMask);
-	} else if (metadata.telemetryUpdateMode == UPDATEMODE_MANUAL) {
+	} else if (updateMode == UPDATEMODE_THROTTLED) {
+		if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) {
+			// If we received a periodic update, we can change back to update on change
+			eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+			// Set update period on initialization and metadata change
+			if (eventType == EV_NONE)
+				setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
+		} else {
+			// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
+			eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+		}
+		if (UAVObjIsMetaobject(obj)) {
+			eventMask |= EV_UNPACKED;	// we also need to act on remote updates (unpack events)
+		}
+		UAVObjConnectQueue(obj, priorityQueue, eventMask);
+	} else if (updateMode == UPDATEMODE_MANUAL) {
 		// Set update period
 		setUpdatePeriod(obj, 0);
 		// Connect queue
@@ -201,11 +218,6 @@ static void updateObject(UAVObjHandle obj)
 			eventMask |= EV_UNPACKED;	// we also need to act on remote updates (unpack events)
 		}
 		UAVObjConnectQueue(obj, priorityQueue, eventMask);
-	} else if (metadata.telemetryUpdateMode == UPDATEMODE_NEVER) {
-		// Set update period
-		setUpdatePeriod(obj, 0);
-		// Disconnect queue
-		UAVObjDisconnectQueue(obj, priorityQueue);
 	}
 }
 
@@ -215,6 +227,7 @@ static void updateObject(UAVObjHandle obj)
 static void processObjEvent(UAVObjEvent * ev)
 {
 	UAVObjMetadata metadata;
+	UAVObjUpdateMode updateMode;
 	FlightTelemetryStatsData flightStats;
 	int32_t retries;
 	int32_t success;
@@ -226,16 +239,17 @@ static void processObjEvent(UAVObjEvent * ev)
 	} else {
 		// Only process event if connected to GCS or if object FlightTelemetryStats is updated
 		FlightTelemetryStatsGet(&flightStats);
+		// Get object metadata
+		UAVObjGetMetadata(ev->obj, &metadata);
+		updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
 		if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle()) {
-			// Get object metadata
-			UAVObjGetMetadata(ev->obj, &metadata);
 			// Act on event
 			retries = 0;
 			success = -1;
-			if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
+			if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
 				// Send update to GCS (with retries)
 				while (retries < MAX_RETRIES && success == -1) {
-					success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS);	// call blocks until ack is received or timeout
+					success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);	// call blocks until ack is received or timeout
 					++retries;
 				}
 				// Update stats
@@ -257,9 +271,13 @@ static void processObjEvent(UAVObjEvent * ev)
 			}
 			// If this is a metaobject then make necessary telemetry updates
 			if (UAVObjIsMetaobject(ev->obj)) {
-				updateObject(UAVObjGetLinkedObj(ev->obj));	// linked object will be the actual object the metadata are for
+				updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);	// linked object will be the actual object the metadata are for
 			}
 		}
+		if((updateMode == UPDATEMODE_THROTTLED) && !UAVObjIsMetaobject(ev->obj)) {
+			// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
+			updateObject(ev->obj, ev->event);
+		}
 	}
 }
 
@@ -376,7 +394,7 @@ static int32_t addObject(UAVObjHandle obj)
 	// Add object for periodic updates
 	ev.obj = obj;
 	ev.instId = UAVOBJ_ALL_INSTANCES;
-	ev.event = EV_UPDATED_MANUAL;
+	ev.event = EV_UPDATED_PERIODIC;
 	return EventPeriodicQueueCreate(&ev, queue, 0);
 }
 
@@ -394,7 +412,7 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 	// Add object for periodic updates
 	ev.obj = obj;
 	ev.instId = UAVOBJ_ALL_INSTANCES;
-	ev.event = EV_UPDATED_MANUAL;
+	ev.event = EV_UPDATED_PERIODIC;
 	return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
 }
 
@@ -513,8 +531,8 @@ static void updateTelemetryStats()
  */
 static void updateSettings()
 {
+	
 	if (telemetryPort) {
-
 		// Retrieve settings
 		uint8_t speed;
 		HwSettingsTelemetrySpeedGet(&speed);

flight/OpenPilot/Makefile.posix

@@ -1,665 +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
- #####
-
-
-# Set developer code and compile options
-# Set to YES to compile for debugging
-DEBUG ?= YES
-
-# Include objects that are just nice information to show
-DIAGNOSTICS ?= YES
-
-# 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
-
-#
-USE_BOOTLOADER ?= NO
-
-
-# Set to YES when using Code Sourcery toolchain
-CODE_SOURCERY ?= NO
-
-# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
-TCHAIN_PREFIX ?= ""
-
-# Remove command is different for Code Sourcery on Windows
-REMOVE_CMD ?= rm
-
-FLASH_TOOL = OPENOCD
-
-# YES enables -mthumb option to flags for source-files listed
-# in SRC and CPPSRC
-USE_THUMB_MODE = YES
-
-# List of modules to include
-OPTMODULES = CameraStab GPS
-MODULES =  Telemetry Actuator Stabilization Guidance ManualControl 
-#MODULES =  Telemetry ManualControl Actuator Attitude Stabilization
-#MODULES = Telemetry Example
-#MODULES = Telemetry MK/MKSerial
-PYMODULES = FlightPlan
-
-#MODULES += Osd/OsdEtStd
-
-
-# MCU name, submodel and board
-# - MCU used for compiler-option (-mtune)
-# - MODEL used for linker-script name (-T) and passed as define
-# - BOARD just passed as define (optional)
-MCU      = i686
-#CHIP     = STM32F103RET
-#BOARD    = STM3210E_OP
-MODEL	 = HD
-ifeq ($(USE_BOOTLOADER), YES)
-BOOT_MODEL    = $(MODEL)_BL
-
-else
-BOOT_MODEL    = $(MODEL)_NB
-endif
-
-# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
-OUTDIR = ../../build/sitl_posix
-
-# Target file name (without extension).
-TARGET = OpenPilot
-
-# Paths
-OPSYSTEM = ./System
-OPSYSTEMINC = $(OPSYSTEM)/inc
-OPUAVTALK = ../UAVTalk
-OPUAVTALKINC = $(OPUAVTALK)/inc
-OPUAVOBJ = ../UAVObjects
-OPUAVOBJINC = $(OPUAVOBJ)/inc
-OPTESTS  = ./Tests
-OPMODULEDIR = ../Modules
-FLIGHTLIB = ../Libraries
-FLIGHTLIBINC = $(FLIGHTLIB)/inc
-PIOS = ../PiOS.posix
-PIOSINC = $(PIOS)/inc
-PIOSPOSIX = $(PIOS)/posix
-APPLIBDIR = $(PIOSPOSIX)/Libraries
-RTOSDIR = $(APPLIBDIR)/FreeRTOS
-RTOSSRCDIR = $(RTOSDIR)/Source
-RTOSINCDIR = $(RTOSSRCDIR)/include
-DOXYGENDIR = ../Doc/Doxygen
-AHRSBOOTLOADER = ../Bootloaders/AHRS/
-AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
-PYMITE = $(FLIGHTLIB)/PyMite
-PYMITELIB = $(PYMITE)/lib
-PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
-PYMITETOOLS = $(PYMITE)/tools
-PYMITEVM = $(PYMITE)/vm
-PYMITEINC = $(PYMITEVM)
-PYMITEINC += $(PYMITEPLAT)
-PYMITEINC += $(OUTDIR)
-FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
-FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
-
-UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
-UAVOBJPYTHONSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/python
-
-# List C source files here. (C dependencies are automatically generated.)
-# use file-extension c for "c-only"-files
-
-MODNAMES = $(notdir ${MODULES})
-MODNAMES += $(notdir ${OPTMODULES})
-
-ifndef TESTAPP
-
-## PyMite files
-SRC += $(OUTDIR)/pmlib_img.c
-SRC += $(OUTDIR)/pmlib_nat.c
-SRC += $(OUTDIR)/pmlibusr_img.c
-SRC += $(OUTDIR)/pmlibusr_nat.c
-PYSRC += $(wildcard ${PYMITEVM}/*.c)
-PYSRC += $(wildcard ${PYMITEPLAT}/*.c)
-PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
-SRC += $(PYSRC)
-
-## MODULES
-SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
-SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
-SRC += ${OUTDIR}/InitMods.c
-## OPENPILOT CORE:
-SRC += ${OPMODULEDIR}/System/systemmod.c
-SRC += $(OPSYSTEM)/openpilot.c
-SRC += $(OPSYSTEM)/pios_board_posix.c
-SRC += $(OPSYSTEM)/alarms.c
-SRC += $(OPUAVTALK)/uavtalk.c
-SRC += $(OPUAVOBJ)/uavobjectmanager.c
-SRC += $(OPUAVOBJ)/eventdispatcher.c
-SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
-else
-## TESTCODE
-SRC += $(OPTESTS)/test_common.c
-SRC += $(OPTESTS)/$(TESTAPP).c
-endif
-
-
-
-## UAVOBJECTS
-ifndef TESTAPP
-#include $(UAVOBJSYNTHDIR)/Makefile.inc
-include ./UAVObjects.inc
-SRC += $(UAVOBJSRC)
-CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
-endif
-
-## PIOS Hardware (posix)
-SRC += $(PIOSPOSIX)/pios_crc.c
-SRC += $(PIOSPOSIX)/pios_sys.c
-SRC += $(PIOSPOSIX)/pios_led.c
-SRC += $(PIOSPOSIX)/pios_irq.c
-SRC += $(PIOSPOSIX)/pios_delay.c
-SRC += $(PIOSPOSIX)/pios_sdcard.c
-SRC += $(PIOSPOSIX)/pios_udp.c
-SRC += $(PIOSPOSIX)/pios_com.c
-SRC += $(PIOSPOSIX)/pios_servo.c
-SRC += $(PIOSPOSIX)/pios_wdg.c
-SRC += $(PIOSPOSIX)/pios_debug.c
-
-SRC += $(PIOSPOSIX)/pios_rcvr.c
-
-## Libraries for flight calculations
-SRC += $(FLIGHTLIB)/fifo_buffer.c
-SRC += $(FLIGHTLIB)/WorldMagModel.c
-SRC += $(FLIGHTLIB)/CoordinateConversions.c
-SRC += $(FLIGHTLIB)/taskmonitor.c
-## RTOS and RTOS Portable 
-SRC += $(RTOSSRCDIR)/list.c
-SRC += $(RTOSSRCDIR)/queue.c
-UNAME := $(shell uname)
-ifeq ($(UNAME), Linux)
-  SRC += $(RTOSSRCDIR)/tasks_linux.c
-  SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port_linux.c
-else
-  SRC += $(RTOSSRCDIR)/tasks_posix.c
-  SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port_posix.c
-endif
-SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
-
-
-
-# List C source files here which must be compiled in ARM-Mode (no -mthumb).
-# use file-extension c for "c-only"-files
-## just for testing, timer.c could be compiled in thumb-mode too
-SRCARM =
-
-# List C++ source files here.
-# use file-extension .cpp for C++-files (not .C)
-CPPSRC =
-
-# List C++ source files here which must be compiled in ARM-Mode.
-# use file-extension .cpp for C++-files (not .C)
-#CPPSRCARM = $(TARGET).cpp
-CPPSRCARM =
-
-
-# List any extra directories to look for include files here.
-#    Each directory must be seperated by a space.
-EXTRAINCDIRS  =  $(OPSYSTEM)
-EXTRAINCDIRS  += $(OPSYSTEMINC)
-EXTRAINCDIRS  += $(OPUAVTALK)
-EXTRAINCDIRS  += $(OPUAVTALKINC)
-EXTRAINCDIRS  += $(OPUAVOBJ)
-EXTRAINCDIRS  += $(OPUAVOBJINC)
-EXTRAINCDIRS  += $(UAVOBJSYNTHDIR)
-EXTRAINCDIRS  += $(PIOS)
-EXTRAINCDIRS  += $(PIOSINC)
-EXTRAINCDIRS  += $(FLIGHTLIBINC)
-EXTRAINCDIRS  += $(PIOSPOSIX)
-EXTRAINCDIRS  += $(RTOSINCDIR)
-EXTRAINCDIRS  += $(APPLIBDIR)
-EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/Posix
-EXTRAINCDIRS  += $(PYMITEINC)
-
-EXTRAINCDIRS += ${foreach MOD, ${PYMODULES} ${OPTMODULES} ${MODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
-
-
-# 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.
-ifeq ($(ENABLE_DEBUG_PINS), YES)
-CDEFS += -DPIOS_ENABLE_DEBUG_PINS
-endif
-ifeq ($(ENABLE_AUX_UART), YES)
-CDEFS += -DPIOS_ENABLE_AUX_UART
-endif
-ifeq ($(USE_BOOTLOADER), YES)
-CDEFS += -DUSE_BOOTLOADER
-endif
-
-
-
-# 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
-endif
-
-ifeq ($(DIAGNOSTICS),YES)
-CFLAGS = -DDIAGNOSTICS
-endif
-
-CFLAGS += $(CFLAGS_UAVOBJECTS)
-CFLAGS += -DARCH_POSIX
-CFLAGS += -O$(OPT)
-CFLAGS += -mtune=$(MCU)
-CFLAGS += $(CDEFS)
-CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
-
-CFLAGS += -fomit-frame-pointer
-ifeq ($(CODE_SOURCERY), YES)
-CFLAGS += -fpromote-loop-indices
-endif
-
-CFLAGS += -Wall
-CFLAGS += -Werror
-# 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  = -mtune=$(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 += -lpthread 
-LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
-LDFLAGS += -lc
-LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
-LDFLAGS += $(MATH_LIB)
-LDFLAGS += -lc -lgcc
-
-
-
-
-# Define programs and commands.
-CC      = $(TCHAIN_PREFIX)gcc
-CPP     = $(TCHAIN_PREFIX)g++
-AR      = $(TCHAIN_PREFIX)ar
-OBJCOPY = $(TCHAIN_PREFIX)objcopy
-OBJDUMP = $(TCHAIN_PREFIX)objdump
-SIZE    = $(TCHAIN_PREFIX)size
-NM      = $(TCHAIN_PREFIX)nm
-REMOVE  = $(REMOVE_CMD) -f
-PYTHON  = python
-###SHELL   = sh
-###COPY    = cp
-
-
-
-# Define Messages
-# English
-MSG_ERRORS_NONE = Errors: none
-MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
-MSG_END = ${quote}--------  end  --------${quote}
-MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
-MSG_SIZE_BEFORE = ${quote}Size before:${quote}
-MSG_SIZE_AFTER = ${quote}Size after build:${quote}
-MSG_LOAD_FILE = ${quote}Creating load file:${quote}
-MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
-MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
-MSG_LINKING = ${quote}**** Linking :${quote}
-MSG_COMPILING = ${quote}**** Compiling C :${quote}
-MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
-MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
-MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
-MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
-MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
-MSG_CLEANING = ${quote}Cleaning project:${quote}
-MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
-MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
-MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
-MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
-
-# 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)))
-
-elf: $(OUTDIR)/$(TARGET).elf
-lss: $(OUTDIR)/$(TARGET).lss
-sym: $(OUTDIR)/$(TARGET).sym
-hex: $(OUTDIR)/$(TARGET).hex
-bin: $(OUTDIR)/$(TARGET).bin
-
-# Default target.
-#all: begin gccversion sizebefore build sizeafter finished end
-#all: begin gencode gccversion build sizeafter finished end
-all: elf
-
-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
-
-# Test if quotes are needed for the echo-command
-result = ${shell echo "test"}
-ifeq (${result}, test)
-	quote = '
-else
-	quote =
-endif
-
-# Generate code for module initialization
-${OUTDIR}/InitMods.c: Makefile.posix
-	@echo ${MSG_MODINIT}
-	@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}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Start(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
-	@echo ${quote}void StartModules() {${quote} >> ${OUTDIR}/InitMods.c
-	@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Start();}${quote}  >> ${OUTDIR}/InitMods.c
-	@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
-
-# Generate code for PyMite
-${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py) 
-	@echo $(MSG_PYMITEINIT) $(call toprel, $@)
-	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
-	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
-	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
-
-# Eye candy.
-begin:
-##	@echo
-	@echo $(MSG_BEGIN)
-
-finished:
-##	@echo $(MSG_ERRORS_NONE)
-
-end:
-	@echo $(MSG_END)
-##	@echo
-
-# Display sizes of sections.
-ELFSIZE = $(SIZE) -A  $(OUTDIR)/$(TARGET).elf
-##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
-sizebefore:
-#	@if [ -f  $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
-
-sizeafter:
-#	@if [ -f  $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
-	@echo $(MSG_SIZE_AFTER)
-	$(ELFSIZE)
-
-# Display compiler version information.
-gccversion :
-	@$(CC) --version
-#	@echo $(ALLOBJ)
-
-# Program the device.
-ifeq ($(USE_BOOTLOADER), YES)
-# Program the device with OP Upload Tool".
-program: $(OUTDIR)/$(TARGET).bin
-	@echo ${quote}Programming with OP Upload Tool${quote}
-	../../ground/src/experimental/upload-build-desktop/debug/OPUploadTool -d 0 -p $(OUTDIR)/$(TARGET).bin
-else
-ifeq ($(FLASH_TOOL),OPENOCD)
-# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
-program: $(OUTDIR)/$(TARGET).elf
-	@echo ${quote}Programming with OPENOCD${quote}
-	$(OOCD_EXE) $(OOCD_CL)
-endif
-endif
-
-# Create final output file (.hex) from ELF output file.
-%.hex: %.elf
-##	@echo
-	@echo $(MSG_LOAD_FILE) $@
-	$(OBJCOPY) -O ihex $< $@
-
-# Create final output file (.bin) from ELF output file.
-%.bin: %.elf
-##	@echo
-	@echo $(MSG_LOAD_FILE) $@
-	$(OBJCOPY) -O binary $< $@
-
-# Create extended listing file/disassambly from ELF output file.
-# using objdump testing: option -C
-%.lss: %.elf
-##	@echo
-	@echo $(MSG_EXTENDED_LISTING) $@
-	$(OBJDUMP) -h -S -C -r $< > $@
-#	$(OBJDUMP) -x -S $< > $@
-
-# Create a symbol table from ELF output file.
-%.sym: %.elf
-##	@echo
-	@echo $(MSG_SYMBOL_TABLE) $@
-	$(NM) -n $< > $@
-
-# Link: create ELF output file from object files.
-.SECONDARY : $(TARGET).elf
-.PRECIOUS : $(ALLOBJ)
-%.elf:  $(ALLOBJ)
-	@echo $(MSG_LINKING) $@
-# use $(CC) for C-only projects or $(CPP) for C++-projects:
-	$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
-#	$(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
-
- 
-# Assemble: create object files from assembler source files.
-define ASSEMBLE_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_ASSEMBLING) $$<  to  $$@
-	$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
-endef
-$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
-
-# Assemble: create object files from assembler source files. ARM-only
-define ASSEMBLE_ARM_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_ASSEMBLING_ARM) $$<  to  $$@
-	$(CC) -c $$(ASFLAGS) $$< -o $$@
-endef
-$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
-
-
-# Compile: create object files from C source files.
-define COMPILE_C_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILING) $$<  to  $$@
-	$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
-endef
-$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
-
-# Compile: create object files from C source files. ARM-only
-define COMPILE_C_ARM_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILING_ARM) $$<  to  $$@
-	$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
-endef
-$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
-
-
-# Compile: create object files from C++ source files.
-define COMPILE_CPP_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILINGCPP) $$<  to  $$@
-	$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
-endef
-$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
-
-# Compile: create object files from C++ source files. ARM-only
-define COMPILE_CPP_ARM_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILINGCPP_ARM) $$<  to  $$@
-	$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
-endef
-$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
-
-
-# Compile: create assembler files from C source files. ARM/Thumb
-$(SRC:.c=.s) : %.s : %.c
-	@echo $(MSG_ASMFROMC) $< to $@
-	$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
-
-# Compile: create assembler files from C source files. ARM only
-$(SRCARM:.c=.s) : %.s : %.c
-	@echo $(MSG_ASMFROMC_ARM) $< to $@
-	$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
-
-# Generate Doxygen documents
-docs:
-	doxygen  $(DOXYGENDIR)/doxygen.cfg
-
-# Target: clean project.
-clean: begin clean_list finished end
-
-clean_list :
-##	@echo
-	@echo $(MSG_CLEANING)
-	$(REMOVE) $(OUTDIR)/$(TARGET).map
-	$(REMOVE) $(OUTDIR)/$(TARGET).elf
-	$(REMOVE) $(OUTDIR)/$(TARGET).hex
-	$(REMOVE) $(OUTDIR)/$(TARGET).bin
-	$(REMOVE) $(OUTDIR)/$(TARGET).sym
-	$(REMOVE) $(OUTDIR)/$(TARGET).lss
-	$(REMOVE) $(wildcard $(OUTDIR)/*.c)
-	$(REMOVE) $(wildcard $(OUTDIR)/*.h)
-	$(REMOVE) $(ALLOBJ)
-	$(REMOVE) $(LSTFILES)
-	$(REMOVE) $(DEPFILES)
-	$(REMOVE) $(SRC:.c=.s)
-	$(REMOVE) $(SRCARM:.c=.s)
-	$(REMOVE) $(CPPSRC:.cpp=.s)
-	$(REMOVE) $(CPPSRCARM:.cpp=.s)
-
-
-# Create output files directory
-# all known MS Windows OS define the ComSpec environment variable
-ifdef ComSpec
-$(shell md $(OUTDIR) 2>NUL)
-else
-$(shell mkdir $(OUTDIR) 2>/dev/null)
-endif
-
-# Include the dependency files.
-ifdef ComSpec
--include $(shell md $(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 begin finish end sizebefore sizeafter gccversion \
-build elf hex bin lss sym clean clean_list program
-

flight/OpenPilot/Makefile.win32

@@ -1,637 +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
- #####
-
-
-# Set developer code and compile options
-# Set to YES to compile for debugging
-DEBUG ?= YES
-
-# 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
-
-#
-USE_BOOTLOADER ?= NO
-
-
-# Set to YES when using Code Sourcery toolchain
-CODE_SOURCERY ?= NO
-
-# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
-TCHAIN_PREFIX ?= mingw32-
-
-# Remove command is different for Code Sourcery on Windows
-REMOVE_CMD ?= rm
-
-FLASH_TOOL = OPENOCD
-
-# YES enables -mthumb option to flags for source-files listed 
-# in SRC and CPPSRC
-USE_THUMB_MODE = YES
-
-# List of modules to include
-MODULES =  Telemetry Actuator Stabilization Guidance ManualControl FlightPlan
-#MODULES =  Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization
-#MODULES = Telemetry Example
-#MODULES = Telemetry MK/MKSerial
-
-#MODULES += Osd/OsdEtStd
-
-
-# MCU name, submodel and board
-# - MCU used for compiler-option (-mtune)
-# - MODEL used for linker-script name (-T) and passed as define
-# - BOARD just passed as define (optional)
-MCU      = i686
-#CHIP     = STM32F103RET
-#BOARD    = STM3210E_OP
-ifeq ($(USE_BOOTLOADER), YES)
-MODEL    = HD_BL
-
-else
-MODEL    = HD_NB
-endif
-
-# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
-OUTDIR = ../../build/sitl_win32
-
-# Target file name (without extension).
-TARGET = OpenPilot
-
-# Paths
-OPSYSTEM = ./System
-OPSYSTEMINC = $(OPSYSTEM)/inc
-OPUAVTALK = ../UAVTalk
-OPUAVTALKINC = $(OPUAVTALK)/inc
-OPUAVOBJ = ../UAVObjects
-OPUAVOBJINC = $(OPUAVOBJ)/inc
-OPTESTS  = ./Tests
-OPMODULEDIR = ../Modules
-FLIGHTLIB = ../Libraries
-FLIGHTLIBINC = ../Libraries/inc
-PIOS = ../PiOS.win32
-PIOSINC = $(PIOS)/inc
-PIOSWIN32 = $(PIOS)/win32
-APPLIBDIR = $(PIOSWIN32)/Libraries
-RTOSDIR = $(APPLIBDIR)/FreeRTOS
-RTOSSRCDIR = $(RTOSDIR)/Source
-RTOSINCDIR = $(RTOSSRCDIR)/include
-DOXYGENDIR = ../Doc/Doxygen
-PYMITE = $(FLIGHTLIB)/PyMite
-PYMITELIB = $(PYMITE)/lib
-PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
-PYMITETOOLS = $(PYMITE)/tools
-PYMITEVM = $(PYMITE)/vm
-PYMITEINC = $(PYMITEVM)
-PYMITEINC += $(PYMITEPLAT)
-PYMITEINC += $(OUTDIR)
-FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
-FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
-UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
-
-# List C source files here. (C dependencies are automatically generated.)
-# use file-extension c for "c-only"-files
-
-MODNAMES = $(notdir ${MODULES})   
-
-ifndef TESTAPP
-
-## PyMite files
-SRC += $(OUTDIR)/pmlib_img.c
-SRC += $(OUTDIR)/pmlib_nat.c
-SRC += $(OUTDIR)/pmlibusr_img.c
-SRC += $(OUTDIR)/pmlibusr_nat.c
-SRC += $(wildcard ${PYMITEVM}/*.c)
-SRC += $(wildcard ${PYMITEPLAT}/*.c)
-
-## MODULES
-SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
-SRC += ${OUTDIR}/InitMods.c
-## OPENPILOT CORE:
-SRC += ${OPMODULEDIR}/System/systemmod.c
-SRC += $(OPSYSTEM)/openpilot.c
-SRC += $(OPSYSTEM)/pios_board_posix.c
-SRC += $(OPSYSTEM)/alarms.c
-SRC += $(OPSYSTEM)/taskmonitor.c
-SRC += $(OPUAVTALK)/uavtalk.c
-SRC += $(OPUAVOBJ)/uavobjectmanager.c
-SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
-SRC += $(OPUAVOBJ)/eventdispatcher.c
-SRC += $(FLIGHTLIB)/CoordinateConversions.c
-
-else
-## TESTCODE
-SRC += $(OPTESTS)/test_common.c
-SRC += $(OPTESTS)/$(TESTAPP).c
-endif
-
-
-## UAVOBJECTS
-ifndef TESTAPP
-#include $(UAVOBJSYNTHDIR)/Makefile.inc
-include ./UAVObjects.inc
-SRC += $(UAVOBJSRC)
-CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
-endif
-
-## PIOS Hardware (win32)
-SRC += $(PIOSWIN32)/pios_sys.c
-SRC += $(PIOSWIN32)/pios_led.c
-SRC += $(PIOSWIN32)/pios_delay.c
-SRC += $(PIOSWIN32)/pios_sdcard.c
-SRC += $(PIOSWIN32)/pios_udp.c
-SRC += $(PIOSWIN32)/pios_com.c
-SRC += $(PIOSWIN32)/pios_servo.c
-SRC += $(PIOSWIN32)/pios_wdg.c
-SRC += $(PIOSWIN32)/pios_crc.c
-#
-## RTOS
-SRC += $(RTOSSRCDIR)/list.c 
-SRC += $(RTOSSRCDIR)/queue.c  
-SRC += $(RTOSSRCDIR)/tasks.c
-#
-## RTOS Portable 
-SRC += $(RTOSSRCDIR)/portable/GCC/Win32/port.c
-SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
-
-
-# List C source files here which must be compiled in ARM-Mode (no -mthumb).
-# use file-extension c for "c-only"-files
-## just for testing, timer.c could be compiled in thumb-mode too
-SRCARM = 
-
-# List C++ source files here.
-# use file-extension .cpp for C++-files (not .C)
-CPPSRC = 
-
-# List C++ source files here which must be compiled in ARM-Mode.
-# use file-extension .cpp for C++-files (not .C)
-#CPPSRCARM = $(TARGET).cpp
-CPPSRCARM = 
-
-
-# List any extra directories to look for include files here.
-#    Each directory must be seperated by a space.
-EXTRAINCDIRS  = $(PIOSINC)
-EXTRAINCDIRS  +=  $(OPSYSTEM)
-EXTRAINCDIRS  += $(OPSYSTEMINC)
-EXTRAINCDIRS  += $(OPUAVTALK)
-EXTRAINCDIRS  += $(OPUAVTALKINC)
-EXTRAINCDIRS  += $(OPUAVOBJ)
-EXTRAINCDIRS  += $(OPUAVOBJINC)
-EXTRAINCDIRS  += $(PIOS)
-EXTRAINCDIRS  += $(PIOSWIN32)
-EXTRAINCDIRS  += $(MININIDIR)
-EXTRAINCDIRS  += $(RTOSINCDIR)
-EXTRAINCDIRS  += $(APPLIBDIR)
-EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/Win32
-EXTRAINCDIRS  += $(FLIGHTLIB)
-EXTRAINCDIRS  += $(FLIGHTLIBINC)
-EXTRAINCDIRS  += $(PYMITEINC)
-EXTRAINCDIRS  += $(UAVOBJSYNTHDIR)
-
-EXTRAINCDIRS += ${foreach MOD, ${MODULES}, ${OPMODULEDIR}/${MOD}/inc} ${OPMODULEDIR}/System/inc
-
-
-# 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 = WS2_32 Winmm
-
-# 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.
-ifeq ($(ENABLE_DEBUG_PINS), YES)
-CDEFS += -DPIOS_ENABLE_DEBUG_PINS
-endif
-ifeq ($(ENABLE_AUX_UART), YES)
-CDEFS += -DPIOS_ENABLE_AUX_UART
-endif
-ifeq ($(USE_BOOTLOADER), YES)
-CDEFS += -DUSE_BOOTLOADER
-endif
-
-
-
-# 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
-endif
-
-CFLAGS += $(CFLAGS_UAVOBJECTS)
-CFLAGS += -DARCH_WIN32
-CFLAGS += -O$(OPT)
-CFLAGS += -mtune=$(MCU)
-CFLAGS += $(CDEFS)
-CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
-
-CFLAGS += -fomit-frame-pointer
-ifeq ($(CODE_SOURCERY), YES)
-CFLAGS += -fpromote-loop-indices
-endif
-
-CFLAGS += -Wall 
-CFLAGS += -Werror
-# 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  = -mtune=$(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 += -lpthread 
-LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
-#LDFLAGS += -lc
-LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
-LDFLAGS += $(MATH_LIB)
-LDFLAGS += -lgcc 
-
-
-
-
-# Define programs and commands.
-CC      = $(TCHAIN_PREFIX)gcc
-CPP     = $(TCHAIN_PREFIX)g++
-AR      = ar
-OBJCOPY = objcopy
-OBJDUMP = objdump
-SIZE    = size
-NM      = nm
-REMOVE  = $(REMOVE_CMD) -f
-PYTHON  = python
-###SHELL   = sh
-###COPY    = cp
-
-
-
-# Define Messages
-# English
-MSG_ERRORS_NONE = Errors: none
-MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
-MSG_END = ${quote}--------  end  --------${quote}
-MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
-MSG_SIZE_BEFORE = ${quote}Size before:${quote} 
-MSG_SIZE_AFTER = ${quote}Size after build:${quote}
-MSG_LOAD_FILE = ${quote}Creating load file:${quote}
-MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
-MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
-MSG_LINKING = ${quote}**** Linking :${quote}
-MSG_COMPILING = ${quote}**** Compiling C :${quote}
-MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
-MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
-MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
-MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
-MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
-MSG_CLEANING = ${quote}Cleaning project:${quote}
-MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
-MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
-MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
-MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
-
-# 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)))
-
-elf: $(OUTDIR)/$(TARGET).elf 
-lss: $(OUTDIR)/$(TARGET).lss
-sym: $(OUTDIR)/$(TARGET).sym
-hex: $(OUTDIR)/$(TARGET).hex
-bin: $(OUTDIR)/$(TARGET).bin
-exe: $(OUTDIR)/$(TARGET).exe
-
-# Default target.
-#all: begin gccversion sizebefore build sizeafter finished end
-#all: begin gccversion build sizeafter finished end
-#all: elf
-all: gencode exe
-
-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
-
-# Test if quotes are needed for the echo-command
-result = ${shell echo "test"}
-ifeq (${result}, test)
-	quote = '
-else
-	quote = 
-endif
-
-# Generate intermediate code
-gencode: ${OUTDIR}/InitMods.c $(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h
-
-${OUTDIR}/InitMods.c: Makefile.win32
-	@echo ${MSG_MODINIT}
-	@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
-	
-#Generate code for PyMite
-$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h $(OPMODULEDIR)/FlightPlan/flightplan.c: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) 
-	@echo ${MSG_PYMITEINIT}
-	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
-	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
-	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
-	
-# Eye candy.
-begin:
-##	@echo
-	@echo $(MSG_BEGIN)
-
-finished:
-##	@echo $(MSG_ERRORS_NONE)
-
-end:
-	@echo $(MSG_END)
-##	@echo
-
-# Display sizes of sections.
-ELFSIZE = $(SIZE) -A  $(OUTDIR)/$(TARGET).elf
-##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
-sizebefore:
-#	@if [ -f  $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
-
-sizeafter:
-#	@if [ -f  $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
-	@echo $(MSG_SIZE_AFTER)
-	$(ELFSIZE)
-	
-# Display compiler version information.
-gccversion : 
-	@$(CC) --version
-#	@echo $(ALLOBJ)
-
-# Program the device.
-ifeq ($(FLASH_TOOL),OPENOCD)
-# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
-program: $(OUTDIR)/$(TARGET).elf
-	@echo ${quote}Programming with OPENOCD${quote}
-	$(OOCD_EXE) $(OOCD_CL)
-endif
-
-# Create final output file (.hex) from ELF output file.
-%.hex: %.elf
-##	@echo
-	@echo $(MSG_LOAD_FILE) $@
-	$(OBJCOPY) -O ihex $< $@
-	
-# Create final output file (.bin) from ELF output file.
-%.bin: %.elf
-##	@echo
-	@echo $(MSG_LOAD_FILE) $@
-	$(OBJCOPY) -O binary $< $@
-
-# Create extended listing file/disassambly from ELF output file.
-# using objdump testing: option -C
-%.lss: %.elf
-##	@echo
-	@echo $(MSG_EXTENDED_LISTING) $@
-	$(OBJDUMP) -h -S -C -r $< > $@
-#	$(OBJDUMP) -x -S $< > $@
-
-# Create a symbol table from ELF output file.
-%.sym: %.elf
-##	@echo
-	@echo $(MSG_SYMBOL_TABLE) $@
-	$(NM) -n $< > $@
-
-# Link: create ELF output file from object files.
-.SECONDARY : $(TARGET).elf
-.PRECIOUS : $(ALLOBJ)
-%.elf:  $(ALLOBJ)
-	@echo $(MSG_LINKING) $@
-# use $(CC) for C-only projects or $(CPP) for C++-projects:
-	$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
-#	$(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
-
-# Link: create EXE output file from object files.
-.SECONDARY : $(TARGET).exe
-.PRECIOUS : $(ALLOBJ)
-%.exe:  $(ALLOBJ)
-	@echo $(MSG_LINKING) $@
-# use $(CC) for C-only projects or $(CPP) for C++-projects:
-	$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
-#	$(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
-
-# Assemble: create object files from assembler source files.
-define ASSEMBLE_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_ASSEMBLING) $$<  to  $$@
-	$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@ 
-endef
-$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src)))) 
-
-# Assemble: create object files from assembler source files. ARM-only
-define ASSEMBLE_ARM_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_ASSEMBLING_ARM) $$<  to  $$@
-	$(CC) -c $$(ASFLAGS) $$< -o $$@ 
-endef
-$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src)))) 
-
-
-# Compile: create object files from C source files.
-define COMPILE_C_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILING) $$<  to  $$@
-	$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ 
-endef
-$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src)))) 
-
-# Compile: create object files from C source files. ARM-only
-define COMPILE_C_ARM_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILING_ARM) $$<  to  $$@
-	$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@ 
-endef
-$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src)))) 
-
-
-# Compile: create object files from C++ source files.
-define COMPILE_CPP_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILINGCPP) $$<  to  $$@
-	$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@ 
-endef
-$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src)))) 
-
-# Compile: create object files from C++ source files. ARM-only
-define COMPILE_CPP_ARM_TEMPLATE
-$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
-##	@echo
-	@echo $(MSG_COMPILINGCPP_ARM) $$<  to  $$@
-	$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@ 
-endef
-$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src)))) 
-
-
-# Compile: create assembler files from C source files. ARM/Thumb
-$(SRC:.c=.s) : %.s : %.c
-	@echo $(MSG_ASMFROMC) $< to $@
-	$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
-
-# Compile: create assembler files from C source files. ARM only
-$(SRCARM:.c=.s) : %.s : %.c
-	@echo $(MSG_ASMFROMC_ARM) $< to $@
-	$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
-
-# Generate Doxygen documents
-docs:
-	doxygen  $(DOXYGENDIR)/doxygen.cfg
-	
-# Target: clean project.
-clean: begin clean_list finished end
-
-clean_list :
-##	@echo
-	@echo $(MSG_CLEANING)
-	$(REMOVE) $(OUTDIR)/$(TARGET).map
-	$(REMOVE) $(OUTDIR)/$(TARGET).elf
-	$(REMOVE) $(OUTDIR)/$(TARGET).hex
-	$(REMOVE) $(OUTDIR)/$(TARGET).bin
-	$(REMOVE) $(OUTDIR)/$(TARGET).sym
-	$(REMOVE) $(OUTDIR)/$(TARGET).lss
-	$(REMOVE) $(OUTDIR)/$(TARGET).exe
-	$(REMOVE) $(ALLOBJ)
-	$(REMOVE) $(LSTFILES)
-	$(REMOVE) $(DEPFILES)
-	$(REMOVE) $(SRC:.c=.s)
-	$(REMOVE) $(SRCARM:.c=.s)
-	$(REMOVE) $(CPPSRC:.cpp=.s)
-	$(REMOVE) $(CPPSRCARM:.cpp=.s)
-
-
-# Create output files directory
-# all known MS Windows OS define the ComSpec environment variable
-ifdef ComSpec
-$(shell md $(OUTDIR) 2>NUL)
-else
-$(shell mkdir $(OUTDIR) 2>/dev/null)
-endif
-
-# Include the dependency files.
-ifdef ComSpec
--include $(shell md $(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 begin gencode finish end sizebefore sizeafter gccversion \
-build exe elf hex bin lss sym clean clean_list program
-

flight/OpenPilot/System/inc/FreeRTOSConfig.h

@@ -1,99 +0,0 @@
-
-#ifndef FREERTOS_CONFIG_H
-#define FREERTOS_CONFIG_H
-
-/*-----------------------------------------------------------
- * Application specific definitions.
- *
- * These definitions should be adjusted for your particular hardware and
- * application requirements.
- *
- * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
- * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. 
- *
- * See http://www.freertos.org/a00110.html.
- *----------------------------------------------------------*/
-
-/**
-  * @addtogroup PIOS PIOS
-  * @{
-  * @addtogroup FreeRTOS FreeRTOS
-  * @{
-  */
-
-/* Notes: We use 5 task priorities */
-
-#define configUSE_PREEMPTION		1
-#define configUSE_IDLE_HOOK		1
-#define configUSE_TICK_HOOK		0
-#define configUSE_MALLOC_FAILED_HOOK    1
-#define configCPU_CLOCK_HZ		( ( unsigned long ) 72000000 )
-#define configTICK_RATE_HZ		( ( portTickType ) 1000 )
-#define configMAX_PRIORITIES		( ( unsigned portBASE_TYPE ) 5 )
-#define configMINIMAL_STACK_SIZE	( ( unsigned short ) 256 )
-#define configTOTAL_HEAP_SIZE		( ( size_t ) ( 41 * 1024 ) )
-#define configMAX_TASK_NAME_LEN		( 16 )
-#define configUSE_TRACE_FACILITY	0
-#define configUSE_16_BIT_TICKS		0
-#define configIDLE_SHOULD_YIELD		0
-#define configUSE_MUTEXES		1
-#define configUSE_RECURSIVE_MUTEXES	1
-#define configUSE_COUNTING_SEMAPHORES	0
-#define configUSE_ALTERNATIVE_API	0
-//#define configCHECK_FOR_STACK_OVERFLOW	2
-#define configQUEUE_REGISTRY_SIZE	10
-
-/* Co-routine definitions. */
-#define configUSE_CO_ROUTINES 		0
-#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
-
-/* Set the following definitions to 1 to include the API function, or zero
-to exclude the API function. */
-
-#define INCLUDE_vTaskPrioritySet		1
-#define INCLUDE_uxTaskPriorityGet		1
-#define INCLUDE_vTaskDelete			1
-#define INCLUDE_vTaskCleanUpResources		0
-#define INCLUDE_vTaskSuspend			1
-#define INCLUDE_vTaskDelayUntil			1
-#define INCLUDE_vTaskDelay			1
-#define INCLUDE_xTaskGetSchedulerState		1
-#define INCLUDE_xTaskGetCurrentTaskHandle	1
-#define INCLUDE_uxTaskGetStackHighWaterMark	1
-
-/* This is the raw value as per the Cortex-M3 NVIC.  Values can be 255
-(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */
-#define configKERNEL_INTERRUPT_PRIORITY 	15 << 4	/* equivalent to NVIC priority 15 */
-#define configMAX_SYSCALL_INTERRUPT_PRIORITY 	 3 << 4	/* equivalent to NVIC priority  3 */
-
-/* This is the value being used as per the ST library which permits 16
-priority values, 0 to 15.  This must correspond to the
-configKERNEL_INTERRUPT_PRIORITY setting.  Here 15 corresponds to the lowest
-NVIC value of 255. */
-#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY	15
-
-/* Enable run time stats collection */
-#if defined(DIAGNOSTICS)
-#define configCHECK_FOR_STACK_OVERFLOW	2
-
-#define configGENERATE_RUN_TIME_STATS 1
-#define INCLUDE_uxTaskGetRunTime 1
-#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
-do {\
-(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\
-(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
-} while(0)
-#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
-#else
-#define configCHECK_FOR_STACK_OVERFLOW	1
-#endif
-
-#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
-#define CHECK_IRQ_STACK
-#endif
-
-/**
-  * @}
-  */
-
-#endif /* FREERTOS_CONFIG_H */

flight/OpenPilot/System/inc/pios_board_posix.h

@@ -1,91 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pios_board.h   
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Defines board hardware for the OpenPilot Version 1.1 hardware.
- * @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_BOARD_H
-#define PIOS_BOARD_H
-
-
-
-
-//------------------------
-// PIOS_LED
-//------------------------
-//#define PIOS_LED_LED1_GPIO_PORT			GPIOC
-//#define PIOS_LED_LED1_GPIO_PIN			GPIO_Pin_12
-//#define PIOS_LED_LED1_GPIO_CLK			RCC_APB2Periph_GPIOC
-//#define PIOS_LED_LED2_GPIO_PORT			GPIOC
-//#define PIOS_LED_LED2_GPIO_PIN			GPIO_Pin_13
-//#define PIOS_LED_LED2_GPIO_CLK			RCC_APB2Periph_GPIOC
-#define PIOS_LED_NUM				2
-//#define PIOS_LED_PORTS				{ PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT }
-//#define PIOS_LED_PINS				{ PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN }
-//#define PIOS_LED_CLKS				{ PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK }
-
-
-//-------------------------
-// COM
-//
-// See also pios_board_posix.c
-//-------------------------
-//#define PIOS_USART_TX_BUFFER_SIZE		256
-#define PIOS_COM_BUFFER_SIZE 1024
-#define PIOS_COM_MAX_DEVS 256
-#define PIOS_UDP_RX_BUFFER_SIZE		PIOS_COM_BUFFER_SIZE
-
-extern uint32_t pios_com_telem_rf_id;
-extern uint32_t pios_com_telem_usb_id;
-extern uint32_t pios_com_gps_id;
-extern uint32_t pios_com_aux_id;
-extern uint32_t pios_com_spectrum_id;
-
-#define PIOS_COM_TELEM_RF                       (pios_com_telem_rf_id)
-#define PIOS_COM_TELEM_USB                      (pios_com_telem_usb_id)
-#define PIOS_COM_GPS                            (pios_com_gps_id)
-
-#ifdef PIOS_ENABLE_AUX_UART
-#define PIOS_COM_AUX                            (pios_com_aux_id)
-#define PIOS_COM_DEBUG                          (PIOS_COM_AUX
-#endif
-
-/**
- * glue macros for file IO
- * STM32 uses DOSFS for file IO
- */
-#define PIOS_FOPEN_READ(filename,file)	(file=fopen((char*)filename,"r"))==NULL
-
-#define PIOS_FOPEN_WRITE(filename,file)	(file=fopen((char*)filename,"w"))==NULL
-
-#define PIOS_FREAD(file,bufferadr,length,resultadr)	(*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length
-
-#define PIOS_FWRITE(file,bufferadr,length,resultadr)	*resultadr=fwrite((uint8_t*)bufferadr,1,length,*file)
-
-
-
-#define PIOS_FCLOSE(file)		fclose(file)
-
-#define PIOS_FUNLINK(file)		unlink((char*)filename)
-
-#endif /* PIOS_BOARD_H */

flight/OpenPilot/System/inc/pios_config_posix.h

@@ -1,78 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pios_config.h  
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      PiOS configuration header. 
- *             Central compile time config for the project.
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/* 
- * This program is free software; you can redistribute it and/or modify 
- * it under the terms of the GNU General Public License as published by 
- * the Free Software Foundation; either version 3 of the License, or 
- * (at your option) any later version.
- * 
- * This program is distributed in the hope that it will be useful, but 
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
- * for more details.
- * 
- * You should have received a copy of the GNU General Public License along 
- * with this program; if not, write to the Free Software Foundation, Inc., 
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-
-#ifndef PIOS_CONFIG_POSIX_H
-#define PIOS_CONFIG_POSIX_H
-
-
-/* Enable/Disable PiOS Modules */
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_DELAY
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_SDCARD
-#define PIOS_INCLUDE_FREERTOS
-#define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_GPS
-#define PIOS_INCLUDE_IRQ
-#define PIOS_INCLUDE_TELEMETRY_RF
-#define PIOS_INCLUDE_UDP
-#define PIOS_INCLUDE_SERVO
-#define PIOS_INCLUDE_RCVR
-
-#define PIOS_RCVR_MAX_CHANNELS			12
-#define PIOS_RCVR_MAX_DEVS              3
-
-/* Defaults for Logging */
-#define LOG_FILENAME 			"PIOS.LOG"
-#define STARTUP_LOG_ENABLED		1
-
-#define TELEM_QUEUE_SIZE                20
-#define PIOS_TELEM_STACK_SIZE           2048
-
-/* Alarm Thresholds */
-#define HEAP_LIMIT_WARNING		4000
-#define HEAP_LIMIT_CRITICAL		1000
-#define IRQSTACK_LIMIT_WARNING		150
-#define IRQSTACK_LIMIT_CRITICAL		80
-#define CPULOAD_LIMIT_WARNING		80
-#define CPULOAD_LIMIT_CRITICAL		95
-
-/* Stabilization options */
-#define PIOS_QUATERNION_STABILIZATION
-
-/* Alarm Thresholds */
-#define HEAP_LIMIT_WARNING		4000
-#define HEAP_LIMIT_CRITICAL		1000
-#define IRQSTACK_LIMIT_WARNING		150
-#define IRQSTACK_LIMIT_CRITICAL		80
-#define CPULOAD_LIMIT_WARNING		80
-#define CPULOAD_LIMIT_CRITICAL		95
-
-/* GPS options */
-#define PIOS_GPS_SETS_HOMELOCATION
-
-#endif /* PIOS_CONFIG_POSIX_H */

flight/OpenPilot/System/openpilot.c

@@ -1,351 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @brief These files are the core system files of OpenPilot.
- * They are the ground layer just above PiOS. In practice, OpenPilot actually starts
- * in the main() function of openpilot.c
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @brief This is where the OP firmware starts. Those files also define the compile-time
- * options of the firmware.
- * @{
- * @file       openpilot.c 
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Sets up and runs main OpenPilot tasks.
- * @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 "openpilot.h"
-#include "uavobjectsinit.h"
-#include "systemmod.h"
-
-/* Task Priorities */
-#define PRIORITY_TASK_HOOKS             (tskIDLE_PRIORITY + 3)
-
-/* Global Variables */
-
-/* Local Variables */
-#define INCLUDE_TEST_TASKS 0
-#if INCLUDE_TEST_TASKS
-static uint8_t sdcard_available;
-#endif
-FILEINFO File;
-char Buffer[1024];
-uint32_t Cache;
-
-/* Function Prototypes */
-#if INCLUDE_TEST_TASKS
-static void TaskTick(void *pvParameters);
-static void TaskTesting(void *pvParameters);
-static void TaskHIDTest(void *pvParameters);
-static void TaskServos(void *pvParameters);
-static void TaskSDCard(void *pvParameters);
-#endif
-int32_t CONSOLE_Parse(uint8_t port, char c);
-void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
-
-/* Prototype of PIOS_Board_Init() function */
-extern void PIOS_Board_Init(void);
-extern void Stack_Change(void);
-static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change")));
-
-/**
-* OpenPilot Main function:
-*
-* Initialize PiOS<BR>
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
-* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
-* If something goes wrong, blink LED1 and LED2 every 100ms
-*
-*/
-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();
-	
-	/* Architecture dependant Hardware and
-	 * core subsystem initialisation
-	 * (see pios_board.c for your arch)
-	 * */
-	PIOS_Board_Init();
-
-	/* Initialize modules */
-	MODULE_INITIALISE_ALL
-
-#if INCLUDE_TEST_TASKS
-	/* Create test tasks */
-	xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
-	xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
-	xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
-	xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
-#endif
-
-	/* swap the stack to use the IRQ stack (does nothing in sim mode) */
-	Stack_Change_Weak();
-
-	/* 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 */
-	while (1) {
-#if defined(PIOS_LED_HEARTBEAT)
-		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
-#endif	/* PIOS_LED_HEARTBEAT */
-		PIOS_DELAY_WaitmS(100);
-	}
-}
-
-
-#if INCLUDE_TEST_TASKS
-static void TaskTesting(void *pvParameters)
-{
-	portTickType xDelay = 250 / portTICK_RATE_MS;
-	portTickType xTimeout = 10 / portTICK_RATE_MS;
-
-	//PIOS_BMP085_Init();
-
-	for(;;)
-	{
-		/* This blocks the task until the BMP085 EOC */
-		/*
-		PIOS_BMP085_StartADC(TemperatureConv);
-		xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
-		PIOS_BMP085_ReadADC();
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetTemperature());
-
-		PIOS_BMP085_StartADC(PressureConv);
-		xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
-		PIOS_BMP085_ReadADC();
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetPressure());
-		*/
-
-#if defined(PIOS_INCLUDE_DSM)
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_DSM_Get(0), PIOS_DSM_Get(1), PIOS_DSM_Get(2), PIOS_DSM_Get(3), PIOS_DSM_Get(4), PIOS_DSM_Get(5), PIOS_DSM_Get(6), PIOS_DSM_Get(7));
-#endif
-#if defined(PIOS_INCLUDE_SBUS)
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SBus_Get(0), PIOS_SBus_Get(1), PIOS_SBus_Get(2), PIOS_SBus_Get(3), PIOS_SBus_Get(4), PIOS_SBus_Get(5), PIOS_SBus_Get(6), PIOS_SBus_Get(7));
-#endif
-#if defined(PIOS_INCLUDE_PWM)
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PWM_Get(0), PIOS_PWM_Get(1), PIOS_PWM_Get(2), PIOS_PWM_Get(3), PIOS_PWM_Get(4), PIOS_PWM_Get(5), PIOS_PWM_Get(6), PIOS_PWM_Get(7));
-#endif
-#if defined(PIOS_INCLUDE_PPM)
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PPM_Get(0), PIOS_PPM_Get(1), PIOS_PPM_Get(2), PIOS_PPM_Get(3), PIOS_PPM_Get(4), PIOS_PPM_Get(5), PIOS_PPM_Get(6), PIOS_PPM_Get(7));
-#endif
-
-		/* This blocks the task until there is something on the buffer */
-		/*xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
-		int32_t len = PIOS_COM_ReceiveBufferUsed(COM_USART1);
-		for(int32_t i = 0; i < len; i++) {
-			PIOS_COM_SendFormattedString(COM_DEBUG_USART, ">%c\r", PIOS_COM_ReceiveBuffer(COM_USART1));
-		}*/
-
-		//int32_t state = PIOS_USB_CableConnected();
-		//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "State: %d\r", state);
-
-		//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x57, (uint8_t *)50, 1);
-
-		/* Test ADC pins */
-		//temp = ((1.43 - ((Vsense / 4096) * 3.3)) / 4.3) + 25;
-		//uint32_t vsense = PIOS_ADC_PinGet(0);
-		//uint32_t Temp = (1.42 -  vsense * 3.3 / 4096) * 1000 / 4.35 + 25;
-		//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "Temp: %d, CS_I: %d, CS_V: %d, 5v: %d\r", PIOS_ADC_PinGet(0), PIOS_ADC_PinGet(1), PIOS_ADC_PinGet(2), PIOS_ADC_PinGet(3));
-		//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "AUX1: %d, AUX2: %d, AUX3: %d\r", PIOS_ADC_PinGet(4), PIOS_ADC_PinGet(5), PIOS_ADC_PinGet(6));
-
-		vTaskDelay(xDelay);
-	}
-}
-#endif
-
-#if INCLUDE_TEST_TASKS
-static void TaskHIDTest(void *pvParameters)
-{
-	uint8_t byte;
-	uint8_t line_buffer[128];
-	uint16_t line_ix = 0;
-
-	for(;;)
-	{
-		/* HID Loopback Test */
-#if 0
-		if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) != 0) {
-			byte = PIOS_COM_ReceiveBuffer(COM_USB_HID);
-			if(byte == '\r' || byte == '\n' || byte == 0) {
-				PIOS_COM_SendFormattedString(COM_USB_HID, "RX: %s\r", line_buffer);
-				PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
-				line_ix = 0;
-			} else if(line_ix < sizeof(line_buffer)) {
-				line_buffer[line_ix++] = byte;
-				line_buffer[line_ix] = 0;
-			}
-		}
-#endif
-
-		/* HID Loopback Test */
-		if(PIOS_COM_ReceiveBufferUsed(COM_USART2) != 0) {
-			byte = PIOS_COM_ReceiveBuffer(COM_USART2);
-#if 0
-			if(byte == '\r' || byte == '\n' || byte == 0) {
-				PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
-				line_ix = 0;
-			} else if(line_ix < sizeof(line_buffer)) {
-				line_buffer[line_ix++] = byte;
-				line_buffer[line_ix] = 0;
-			}
-#endif
-			PIOS_COM_SendChar(COM_DEBUG_USART, (char)byte);
-		}
-	}
-}
-#endif
-
-#if INCLUDE_TEST_TASKS
-static void TaskServos(void *pvParameters)
-{
-	/* For testing servo outputs */
-	portTickType xDelay;
-
-	/* Used to test servos, cycles all servos from one side to the other */
-	for(;;) {
-		/*xDelay = 250 / portTICK_RATE_MS;
-		PIOS_Servo_Set(0, 2000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(1, 2000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(2, 2000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(3, 2000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(4, 2000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(5, 2000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(6, 2000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(7, 2000);
-		vTaskDelay(xDelay);
-
-		PIOS_Servo_Set(7, 1000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(6, 1000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(5, 1000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(4, 1000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(3, 1000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(2, 1000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(1, 1000);
-		vTaskDelay(xDelay);
-		PIOS_Servo_Set(0, 1000);
-		vTaskDelay(xDelay);*/
-
-		xDelay = 1 / portTICK_RATE_MS;
-		for(int i = 1000; i < 2000; i++) {
-			PIOS_Servo_Set(0, i);
-			PIOS_Servo_Set(1, i);
-			PIOS_Servo_Set(2, i);
-			PIOS_Servo_Set(3, i);
-			PIOS_Servo_Set(4, i);
-			PIOS_Servo_Set(5, i);
-			PIOS_Servo_Set(6, i);
-			PIOS_Servo_Set(7, i);
-			vTaskDelay(xDelay);
-		}
-		for(int i = 2000; i > 1000; i--) {
-			PIOS_Servo_Set(0, i);
-			PIOS_Servo_Set(1, i);
-			PIOS_Servo_Set(2, i);
-			PIOS_Servo_Set(3, i);
-			PIOS_Servo_Set(4, i);
-			PIOS_Servo_Set(5, i);
-			PIOS_Servo_Set(6, i);
-			PIOS_Servo_Set(7, i);
-			vTaskDelay(xDelay);
-		}
-	}
-}
-#endif
-
-#if INCLUDE_TEST_TASKS
-static void TaskSDCard(void *pvParameters)
-{
-	uint16_t second_delay_ctr = 0;
-	portTickType xLastExecutionTime;
-
-	/* Initialise the xLastExecutionTime variable on task entry */
-	xLastExecutionTime = xTaskGetTickCount();
-
-	for(;;) {
-		vTaskDelayUntil(&xLastExecutionTime, 1 / portTICK_RATE_MS);
-
-		/* Each second: */
-		/* Check if SD card is available */
-		/* High-speed access if SD card was previously available */
-		if(++second_delay_ctr >= 1000) {
-			second_delay_ctr = 0;
-
-			uint8_t prev_sdcard_available = sdcard_available;
-			sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available);
-
-			if(sdcard_available && !prev_sdcard_available) {
-				/* SD Card has been connected! */
-				/* Switch to mass storage device */
-				MSD_Init(0);
-			} else if(!sdcard_available && prev_sdcard_available) {
-				/* Re-init USB for HID */
-				PIOS_USB_Init(1);
-				/* SD Card disconnected! */
-			}
-		}
-
-		/* Each millisecond: */
-		/* Handle USB access if device is available */
-		if(sdcard_available) {
-			MSD_Periodic_mS();
-		}
-	}
-}
-#endif
-/**
- * @}
- * @}
- */
-

flight/OpenPilot/System/pios_board.c

@@ -1,440 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @{
- *
- * @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 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
- */
-
-
-/* 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>
-#include <hwsettings.h>
-#include <manualcontrolsettings.h>
-
-//#define I2C_DEBUG_PIN			0
-//#define USART_GPS_DEBUG_PIN		1
-
-/* One slot per selectable receiver group.
- *  eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
- * NOTE: No slot in this map for NONE.
- */
-uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
-
-#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 96
-
-#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
-#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
-
-#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_aux_id;
-uint32_t pios_com_dsm_id;
-
-#include "ahrs_spi_comm.h"
-
-/**
- * PIOS_Board_Init()
- * initializes all the core subsystems on this specific hardware
- * called from System/openpilot.c
- */
-void PIOS_Board_Init(void) {
-
-	/* Remap AFIO pin */
-	//GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
-
-#ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS
-	PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
-#endif	/* PIOS_DEBUG_ENABLE_DEBUG_PINS */
-
-	/* Delay system */
-	PIOS_DELAY_Init();	
-	
-#if defined(PIOS_INCLUDE_SPI)	
-	/* Set up the SPI interface to the SD card */
-	if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) {
-		PIOS_Assert(0);
-	}
-
-	/* Enable and mount the SDCard */
-	PIOS_SDCARD_Init(pios_spi_sdcard_id);
-	PIOS_SDCARD_MountFS(0);
-#endif /* PIOS_INCLUDE_SPI */
-
-	/* Initialize UAVObject libraries */
-	EventDispatcherInitialize();
-	UAVObjInitialize();
-
-#if defined(PIOS_INCLUDE_RTC)
-	/* Initialize the real-time clock and its associated tick */
-	PIOS_RTC_Init(&pios_rtc_main_cfg);
-#endif
-
-#if defined(PIOS_INCLUDE_LED)
-	PIOS_LED_Init(&pios_led_cfg);
-#endif	/* PIOS_INCLUDE_LED */
-
-	HwSettingsInitialize();
-
-	PIOS_WDG_Init();
-
-	/* Initialize the alarms library */
-	AlarmsInitialize();
-
-	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();
-
-	/* Set up pulse timers */
-	PIOS_TIM_InitClock(&tim_1_cfg);
-	PIOS_TIM_InitClock(&tim_3_cfg);
-	PIOS_TIM_InitClock(&tim_5_cfg);
-	PIOS_TIM_InitClock(&tim_4_cfg);
-	PIOS_TIM_InitClock(&tim_8_cfg);
-
-	/* Prepare the AHRS Comms upper layer protocol */
-	AhrsInitComms();
-
-	/* Set up the SPI interface to the AHRS */
-	if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
-		PIOS_Assert(0);
-	}
-
-	/* Bind the AHRS comms layer to the AHRS SPI link */
-	AhrsConnect(pios_spi_ahrs_id);
-
-#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;
-
-	uint8_t hwsettings_usb_devicetype;
-	HwSettingsUSB_DeviceTypeGet(&hwsettings_usb_devicetype);
-
-	switch (hwsettings_usb_devicetype) {
-	case HWSETTINGS_USB_DEVICETYPE_HIDONLY:
-		if (PIOS_USB_DESC_HID_ONLY_Init()) {
-			PIOS_Assert(0);
-		}
-		usb_hid_present = true;
-		break;
-	case HWSETTINGS_USB_DEVICETYPE_HIDVCP:
-		if (PIOS_USB_DESC_HID_CDC_Init()) {
-			PIOS_Assert(0);
-		}
-		usb_hid_present = true;
-		usb_cdc_present = true;
-		break;
-	case HWSETTINGS_USB_DEVICETYPE_VCPONLY:
-		break;
-	default:
-		PIOS_Assert(0);
-	}
-
-	uint32_t pios_usb_id;
-	PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
-
-#if defined(PIOS_INCLUDE_USB_CDC)
-	/* Configure the USB VCP port */
-	uint8_t hwsettings_usb_vcpport;
-	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_op_mainport;
-	HwSettingsOP_MainPortGet(&hwsettings_op_mainport);
-
-	switch (hwsettings_op_mainport) {
-	case HWSETTINGS_OP_MAINPORT_DISABLED:
-		break;
-	case HWSETTINGS_OP_MAINPORT_TELEMETRY:
-#if defined(PIOS_INCLUDE_TELEMETRY_RF)
-		{
-			uint32_t pios_usart_telem_rf_id;
-			if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_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,
-							  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
-							  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
-				PIOS_Assert(0);
-			}
-		}
-#endif /* PIOS_INCLUDE_TELEMETRY_RF */
-		break;
-	}
-
-	/* Configure the flexi port */
-	uint8_t hwsettings_op_flexiport;
-	HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport);
-
-	switch (hwsettings_op_flexiport) {
-	case HWSETTINGS_OP_FLEXIPORT_DISABLED:
-		break;
-	case HWSETTINGS_OP_FLEXIPORT_GPS:
-#if defined(PIOS_INCLUDE_GPS)
-		{
-			uint32_t pios_usart_gps_id;
-			if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_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,
-						rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
-						NULL, 0)) {
-				PIOS_Assert(0);
-			}
-		}
-#endif	/* PIOS_INCLUDE_GPS */
-		break;
-	}
-
-#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
-	PIOS_Servo_Init(&pios_servo_cfg);
-#endif	/* PIOS_DEBUG_ENABLE_DEBUG_PINS */
-
-	PIOS_ADC_Init();
-	PIOS_GPIO_Init();
-
-	/* Configure the rcvr port */
-	uint8_t hwsettings_rcvrport;
-	HwSettingsOP_RcvrPortGet(&hwsettings_rcvrport);
-
-
-	switch (hwsettings_rcvrport) {
-	case HWSETTINGS_OP_RCVRPORT_DISABLED:
-		break;
-	case HWSETTINGS_OP_RCVRPORT_DEBUG:
-		/* Not supported yet */
-		break;
-	case HWSETTINGS_OP_RCVRPORT_DSM2:
-	case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
-	case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
-#if defined(PIOS_INCLUDE_DSM)
-		{
-			enum pios_dsm_proto proto;
-			switch (hwsettings_rcvrport) {
-			case HWSETTINGS_OP_RCVRPORT_DSM2:
-				proto = PIOS_DSM_PROTO_DSM2;
-				break;
-			case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
-				proto = PIOS_DSM_PROTO_DSMX10BIT;
-				break;
-			case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
-				proto = PIOS_DSM_PROTO_DSMX11BIT;
-				break;
-			default:
-				PIOS_Assert(0);
-				break;
-			}
-
-			uint32_t pios_usart_dsm_id;
-			if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_cfg)) {
-				PIOS_Assert(0);
-			}
-
-			uint32_t pios_dsm_id;
-			if (PIOS_DSM_Init(&pios_dsm_id,
-					  &pios_dsm_cfg,
-					  &pios_usart_com_driver,
-					  pios_usart_dsm_id,
-					  proto, 0)) {
-				PIOS_Assert(0);
-			}
-
-			uint32_t pios_dsm_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
-				PIOS_Assert(0);
-			}
-			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id;
-		}
-#endif
-		break;
-	case HWSETTINGS_OP_RCVRPORT_PWM:
-#if defined(PIOS_INCLUDE_PWM)
-		{
-			uint32_t pios_pwm_id;
-			PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg);
-
-			uint32_t pios_pwm_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
-				PIOS_Assert(0);
-			}
-			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
-		}
-#endif	/* PIOS_INCLUDE_PWM */
-		break;
-	case HWSETTINGS_OP_RCVRPORT_PPM:
-#if defined(PIOS_INCLUDE_PPM)
-		{
-			uint32_t pios_ppm_id;
-			PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
-
-			uint32_t pios_ppm_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
-				PIOS_Assert(0);
-			}
-			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
-		}
-#endif	/* PIOS_INCLUDE_PPM */
-		break;
-	}
-
-#if defined(PIOS_INCLUDE_I2C)
-	if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
-		PIOS_Assert(0);
-	}
-#endif	/* PIOS_INCLUDE_I2C */
-
-	/* Make sure we have at least one telemetry link configured or else fail initialization */
-	PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
-}
-
-/**
- * @}
- */

flight/OpenPilot/System/pios_board_posix.c

@@ -1,197 +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 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 <pios_udp_priv.h>
-#include <pios_com_priv.h>
-#include <openpilot.h>
-#include <uavobjectsinit.h>
-
-#include "hwsettings.h"
-#include "attituderaw.h"
-#include "attitudeactual.h"
-#include "positionactual.h"
-#include "velocityactual.h"
-#include "manualcontrolsettings.h"
-
-#if defined(PIOS_INCLUDE_RCVR)
-#include "pios_rcvr_priv.h"
-
-/* One slot per selectable receiver group.
- *  eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
- * NOTE: No slot in this map for NONE.
- */
-uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
-
-#endif /* PIOS_INCLUDE_RCVR */
-
-
-void Stack_Change() {
-}
-
-void Stack_Change_Weak() {
-}
-
-
-const struct pios_udp_cfg pios_udp_telem_cfg = {
-  .ip = "0.0.0.0",
-  .port = 9000,
-};
-const struct pios_udp_cfg pios_udp_gps_cfg = {
-  .ip = "0.0.0.0",
-  .port = 9001,
-};
-const struct pios_udp_cfg pios_udp_debug_cfg = {
-  .ip = "0.0.0.0",
-  .port = 9002,
-};
-
-#ifdef PIOS_COM_AUX
-/*
- * AUX USART
- */
-const struct pios_udp_cfg pios_udp_aux_cfg = {
-  .ip = "0.0.0.0",
-  .port = 9003,
-};
-#endif
-
-#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 96
-
-/*
- * Board specific number of devices.
- */
-/*
-struct pios_udp_dev pios_udp_devs[] = {
-#define PIOS_UDP_TELEM  0
-  {
-    .cfg = &pios_udp0_cfg,
-  },
-#define PIOS_UDP_GPS    1
-  {
-    .cfg = &pios_udp1_cfg,
-  },
-#define PIOS_UDP_LOCAL    2
-  {
-    .cfg = &pios_udp2_cfg,
-  },
-#ifdef PIOS_COM_AUX
-#define PIOS_UDP_AUX    3
-  {
-    .cfg = &pios_udp3_cfg,
-  },
-#endif
-};
-
-uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
-*/
-/*
- * COM devices
- */
-
-/*
- * Board specific number of devices.
- */
-extern const struct pios_com_driver pios_serial_com_driver;
-extern const struct pios_com_driver pios_udp_com_driver;
-
-uint32_t pios_com_telem_rf_id;
-uint32_t pios_com_telem_usb_id;
-uint32_t pios_com_gps_id;
-uint32_t pios_com_aux_id;
-uint32_t pios_com_spectrum_id;
-
-/**
- * PIOS_Board_Init()
- * initializes all the core systems on this specific hardware
- * called from System/openpilot.c
- */
-void PIOS_Board_Init(void) {
-
-	/* Delay system */
-	PIOS_DELAY_Init();
-
-	/* Initialize UAVObject libraries */
-	EventDispatcherInitialize();
-	UAVObjInitialize();
-	UAVObjectsInitializeAll();
-
-	/* Initialize the alarms library */
-	AlarmsInitialize();
-
-	/* Initialize the task monitor library */
-	TaskMonitorInitialize();
-
-#if defined(PIOS_INCLUDE_COM)
-#if defined(PIOS_INCLUDE_TELEMETRY_RF)
-	{
-		uint32_t pios_udp_telem_rf_id;
-		if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_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_udp_com_driver, pios_udp_telem_rf_id,
-						  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
-						  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
-			PIOS_Assert(0);
-		}
-	}
-#endif /* PIOS_INCLUDE_TELEMETRY_RF */
-
-#if defined(PIOS_INCLUDE_GPS)
-	{
-		uint32_t pios_udp_gps_id;
-		if (PIOS_UDP_Init(&pios_udp_gps_id, &pios_udp_gps_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_udp_com_driver, pios_udp_gps_id,
-				  rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
-				  NULL, 0)) {
-			PIOS_Assert(0);
-		}
-	}
-#endif	/* PIOS_INCLUDE_GPS */
-#endif
-
-	// Initialize these here as posix has no AHRSComms
-	AttitudeRawInitialize();
-	AttitudeActualInitialize();
-	VelocityActualInitialize();
-	PositionActualInitialize();
-	HwSettingsInitialize();
-
-}
-
-/**
- * @}
- */

flight/OpenPilot/Tests/test_BMP085.c

@@ -1,100 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       openpilot.c 
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Sets up and runs main OpenPilot tasks.
- * @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 "openpilot.h"
-
-/* Task Priorities */
-
-/* Global Variables */
-
-/* Local Variables */
-
-/* Function Prototypes */
-static void TaskTesting(void *pvParameters);
-
-/**
-* Test Main function
-*/
-int main()
-{
-	// Init
-	PIOS_SYS_Init();
-	PIOS_DELAY_Init();
-	PIOS_COM_Init();
-	PIOS_ADC_Init();
-	PIOS_I2C_Init();
-	xTaskCreate(TaskTesting, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
-
-	// Start the FreeRTOS scheduler
-	vTaskStartScheduler();
-
-	// Should not get here
-	PIOS_DEBUG_Assert(0);
-
-	return 0;
-}
-
-static void TaskTesting(void *pvParameters)
-{
-	portTickType xDelay = 500 / portTICK_RATE_MS;
-	portTickType xTimeout = 10 / portTICK_RATE_MS;
-
-	PIOS_BMP085_Init();
-
-	for(;;)
-	{
-		PIOS_LED_Toggle(LED2);
-
-		int16_t temp;
-		int32_t pressure;
-
-		PIOS_BMP085_StartADC(TemperatureConv);
-		xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
-		PIOS_BMP085_ReadADC();
-
-		PIOS_BMP085_StartADC(PressureConv);
-		xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
-		PIOS_BMP085_ReadADC();
-
-		temp = PIOS_BMP085_GetTemperature();
-		pressure = PIOS_BMP085_GetPressure();
-
-		PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_TELEM_RF, "%3u,%4u\r", temp, pressure);
-
-		vTaskDelay(xDelay);
-	}
-}
-
-/**
-* Idle hook function
-*/
-void vApplicationIdleHook(void)
-{
-	/* Called when the scheduler has no tasks to run */
-
-
-}

flight/OpenPilot/Tests/test_cpuload.c

@@ -1,89 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       test_cpuload.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Calibration for the CPU load calculation
- * @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
- */
-
-/**
- * This file is used to calculate the number of counts we get when
- * the CPU is fully idle (no load). This is used by systemmod.c to
- * calculate the CPU load. This calibration should be run whenever
- * changes are made in the FreeRTOS configuration or on the compiler
- * optimisation parameters.
- */
-
-#include "openpilot.h"
-
-// Local constants
-#define BENCHMARK_DURATION_MS 10000
-
-// Local functions
-static void testTask(void *pvParameters);
-
-// Variables
-static uint32_t idleCounter = 0;
-static uint32_t idleCounterClear = 0;
-
-int main()
-{
-	PIOS_SYS_Init();
-
-	// Create test task
-	xTaskCreate(testTask, (signed portCHAR *)"Test", 1000 , NULL, 1, NULL);
-
-	// Start the FreeRTOS scheduler
-	vTaskStartScheduler();
-	return 0;
-}
-
-static void testTask(void *pvParameters)
-{
-	uint32_t countsPerSecond = 0;
-
-	while (1)
-	{
-		// Delay until enough required test duration
-		vTaskDelay( BENCHMARK_DURATION_MS / portTICK_RATE_MS );
-
-		// Calculate counts per second, set breakpoint here
-		countsPerSecond = idleCounter / (BENCHMARK_DURATION_MS/1000);
-
-		// Reset and start again - do not clear idleCounter directly!
-		// SET BREAKPOINT HERE and read the countsPerSecond variable
-		// this should be used to update IDLE_COUNTS_PER_SEC_AT_NO_LOAD in systemmod.c
-		idleCounterClear = 1;
-	}
-}
-
-void vApplicationIdleHook(void)
-{
-	/* Called when the scheduler has no tasks to run */
-	if (idleCounterClear == 0)
-	{
-		++idleCounter;
-	}
-	else
-	{
-		idleCounter = 0;
-		idleCounterClear = 0;
-	}
-}