rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merged latest origin/next.

Browse Source
This commit is contained in:
Brian Webb 2012-05-05 09:40:44 -07:00
parent 44d95003f3
commit e83fec0463
214 changed files with 4159 additions and 32031 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

230
Makefile
View File

@ -72,6 +72,7 @@ help:
@echo " arm_sdk_install - Install the Code Sourcery ARM gcc toolchain" @echo " arm_sdk_install - Install the Code Sourcery ARM gcc toolchain"
@echo " openocd_install - Install the OpenOCD JTAG daemon" @echo " openocd_install - Install the OpenOCD JTAG daemon"
@echo " stm32flash_install - Install the stm32flash tool for unbricking boards" @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
@echo " [Big Hammer]" @echo " [Big Hammer]"
@echo " all - Generate UAVObjects, build openpilot firmware and gcs" @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 qt_sdk_install: qt_sdk_clean
# download the source only if it's newer than what we already have # download the source only if it's newer than what we already have
$(V1) wget -N --content-disposition -P "$(DL_DIR)" "$(QT_SDK_URL)" $(V1) wget -N --content-disposition -P "$(DL_DIR)" "$(QT_SDK_URL)"
# tell the user exactly which path they should select in the GUI # tell the user exactly which path they should select in the GUI
$(V1) echo "*** NOTE NOTE NOTE ***" $(V1) echo "*** NOTE NOTE NOTE ***"
$(V1) echo "*" $(V1) echo "*"
@ -209,36 +209,173 @@ arm_sdk_clean:
$(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR) $(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR)
# Set up openocd tools # 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 .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_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 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 openocd_install: openocd_clean
# download the source only if it's newer than what we already have # download the source only if it's newer than what we already have
$(V1) wget -N -P "$(DL_DIR)" --trust-server-name "$(OPENOCD_URL)" $(V1) wget -N -P "$(DL_DIR)" --trust-server-name "$(OPENOCD_URL)"
# extract the source # 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 # build and install
$(V1) mkdir -p "$(OPENOCD_DIR)" $(V1) mkdir -p "$(OPENOCD_DIR)"
$(V1) ( \ $(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 ; \ ./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) ; \
$(MAKE) install ; \ $(MAKE) install ; \
) )
# delete the extracted source when we're done # 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 .PHONY: openocd_clean
openocd_clean: openocd_clean:
$(V0) @echo " CLEAN $(OPENOCD_DIR)"
$(V1) [ ! -d "$(OPENOCD_DIR)" ] || $(RM) -r "$(OPENOCD_DIR)" $(V1) [ ! -d "$(OPENOCD_DIR)" ] || $(RM) -r "$(OPENOCD_DIR)"
STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash
@ -260,6 +397,38 @@ stm32flash_clean:
$(V0) @echo " CLEAN $(STM32FLASH_DIR)" $(V0) @echo " CLEAN $(STM32FLASH_DIR)"
$(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -r "$(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 # Set up paths to tools
@ -429,32 +598,53 @@ bu_$(1)_clean:
$(V1) $(RM) -fr $(BUILD_DIR)/bu_$(1) $(V1) $(RM) -fr $(BUILD_DIR)/bu_$(1)
endef 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) # $(1) = Canonical board name all in lower case (e.g. coptercontrol)
define BOARD_PHONY_TEMPLATE define BOARD_PHONY_TEMPLATE
.PHONY: all_$(1) .PHONY: all_$(1)
all_$(1): $$(filter fw_$(1), $$(FW_TARGETS)) all_$(1): $$(filter fw_$(1), $$(FW_TARGETS))
all_$(1): $$(filter bl_$(1), $$(BL_TARGETS)) all_$(1): $$(filter bl_$(1), $$(BL_TARGETS))
all_$(1): $$(filter bu_$(1), $$(BU_TARGETS)) all_$(1): $$(filter bu_$(1), $$(BU_TARGETS))
all_$(1): $$(filter ef_$(1), $$(EF_TARGETS))
.PHONY: all_$(1)_clean .PHONY: all_$(1)_clean
all_$(1)_clean: $$(addsuffix _clean, $$(filter fw_$(1), $$(FW_TARGETS))) 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 bl_$(1), $$(BL_TARGETS)))
all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS))) all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS)))
endef endef
ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins ALL_BOARDS := coptercontrol pipxtreme revolution
# Friendly names of each board (used to find source tree) # Friendly names of each board (used to find source tree)
openpilot_friendly := OpenPilot
coptercontrol_friendly := CopterControl coptercontrol_friendly := CopterControl
pipxtreme_friendly := PipXtreme 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 # Start out assuming that we'll build fw, bl and bu for all boards
FW_BOARDS := $(ALL_BOARDS) FW_BOARDS := $(ALL_BOARDS)
BL_BOARDS := $(ALL_BOARDS) BL_BOARDS := $(ALL_BOARDS)
BU_BOARDS := $(ALL_BOARDS) BU_BOARDS := $(ALL_BOARDS)
EF_BOARDS := $(ALL_BOARDS)
# FIXME: The INS build doesn't have a bootloader or bootloader # FIXME: The INS build doesn't have a bootloader or bootloader
# updater yet so we need to filter them out to prevent errors. # updater yet so we need to filter them out to prevent errors.
@ -465,6 +655,7 @@ BU_BOARDS := $(filter-out ins, $(BU_BOARDS))
FW_TARGETS := $(addprefix fw_, $(FW_BOARDS)) FW_TARGETS := $(addprefix fw_, $(FW_BOARDS))
BL_TARGETS := $(addprefix bl_, $(BL_BOARDS)) BL_TARGETS := $(addprefix bl_, $(BL_BOARDS))
BU_TARGETS := $(addprefix bu_, $(BU_BOARDS)) BU_TARGETS := $(addprefix bu_, $(BU_BOARDS))
EF_TARGETS := $(addprefix ef_, $(EF_BOARDS))
.PHONY: all_fw all_fw_clean .PHONY: all_fw all_fw_clean
all_fw: $(addsuffix _opfw, $(FW_TARGETS)) all_fw: $(addsuffix _opfw, $(FW_TARGETS))
@ -478,9 +669,13 @@ all_bl_clean: $(addsuffix _clean, $(BL_TARGETS))
all_bu: $(addsuffix _opfw, $(BU_TARGETS)) all_bu: $(addsuffix _opfw, $(BU_TARGETS))
all_bu_clean: $(addsuffix _clean, $(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 .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 # Expand the groups of targets for each board
$(foreach board, $(ALL_BOARDS), $(eval $(call BOARD_PHONY_TEMPLATE,$(board)))) $(foreach board, $(ALL_BOARDS), $(eval $(call BOARD_PHONY_TEMPLATE,$(board))))
@ -494,6 +689,9 @@ $(foreach board, $(ALL_BOARDS), $(eval $(call FW_TEMPLATE,$(board),$($(board)_fr
# Expand the bootloader rules # Expand the bootloader rules
$(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_friendly)))) $(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_friendly))))
# Expand the entire-flash rules
$(foreach board, $(ALL_BOARDS), $(eval $(call EF_TEMPLATE,$(board),$($(board)_friendly))))
.PHONY: sim_posix .PHONY: sim_posix
sim_posix: sim_posix_elf sim_posix: sim_posix_elf

424
flight/AHRS/Makefile
View File

@ -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

1265
flight/AHRS/ahrs.c
View File

@ -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);
}
}
/**
* @}
*/

61
flight/AHRS/ahrs_timer.c
View File

@ -1,61 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_TIMER AHRS Timer
* @brief Sets up a simple timer that can be polled to estimate idle time
* @{
*
*
* @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
*/
#include "ahrs_timer.h"
void timer_start()
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,
ENABLE);
PWR_BackupAccessCmd(ENABLE);
RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128);
RCC_RTCCLKCmd(ENABLE);
RTC_WaitForLastTask();
RTC_WaitForSynchro();
RTC_WaitForLastTask();
RTC_SetPrescaler(0); // counting at 8e6 / 128
RTC_WaitForLastTask();
RTC_SetCounter(0);
RTC_WaitForLastTask();
}
uint32_t timer_count()
{
return RTC_GetCounter();
}
uint32_t timer_rate()
{
return TIMER_RATE;
}

115
flight/AHRS/inc/ahrs.h
View File

@ -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 */

94
flight/AHRS/inc/ahrs_fsm.h
View File

@ -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 */

45
flight/AHRS/inc/ahrs_timer.h
View File

@ -1,45 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS Control
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_TIMER
* @brief Sets up a simple timer that can be polled to estimate idle time
* @{
*
*
* @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
*/
#ifndef AHRS_TIMER
#define AHRS_TIMER
#include <pios.h>
#define TIMER_RATE (8e6 / 128)
void timer_start();
uint32_t timer_count();
uint32_t timer_rate();
#endif

93
flight/AHRS/inc/insgps.h
View File

@ -1,93 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @file insgps.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the INSGPS exposed functionality.
*
* @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 INSGPS_H_
#define INSGPS_H_
#include "stdint.h"
/**
* @addtogroup Constants
* @{
*/
#define POS_SENSORS 0x007
#define HORIZ_SENSORS 0x018
#define VERT_SENSORS 0x020
#define MAG_SENSORS 0x1C0
#define BARO_SENSOR 0x200
#define FULL_SENSORS 0x3FF
/**
* @}
*/
// Exposed Function Prototypes
void INSGPSInit();
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
void INSSetPosVelVar(float PosVar, float VelVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]);
void INSPosVelReset(float pos[3], float vel[3]);
void MagCorrection(float mag_data[3]);
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt);
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
void VelBaroCorrection(float Vel[3], float BaroAlt);
uint16_t ins_get_num_states();
// Nav structure containing current solution
struct NavStruct {
float Pos[3]; // Position in meters and relative to a local NED frame
float Vel[3]; // Velocity in meters and in NED
float q[4]; // unit quaternion rotation relative to NED
float gyro_bias[3];
float accel_bias[3];
} Nav;
/**
* @}
* @}
*/
#endif /* EKF_H_ */

46
flight/AHRS/inc/pios_config.h
View File

@ -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 */

1644
flight/AHRS/insgps13state.c
View File

@ -1,1644 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @file insgps.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief An INS/GPS algorithm implemented with an EKF.
*
* @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 "insgps.h"
#include <math.h>
#include <stdint.h>
// constants/macros/typdefs
#define NUMX 13 // number of states, X is the state vector
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector
#if defined(GENERAL_COV)
// This might trick people so I have a note here. There is a slower but bigger version of the
// code here but won't fit when debugging disabled (requires -Os)
#define COVARIANCE_PREDICTION_GENERAL
#endif
// Private functions
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW]);
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
// global to init to zero and maintain zero elements
float Be[3]; // local magnetic unit vector in NED frame
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
float K[NUMX][NUMV]; // feedback gain matrix
// ************* Exposed Functions ****************
// *************************************************
uint16_t ins_get_num_states()
{
return NUMX;
}
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1;
Be[1] = 0;
Be[2] = 0; // 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[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-5; // 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[6] = 1;
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9; // 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[2] = 0.036; // 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[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
R[9] = .05; // High freq altimeter noise variance (m^2)
}
void INSResetP(float PDiag[NUMX])
{
uint8_t i,j;
// if PDiag[i] nonzero then clear row and column and set diagonal element
for (i=0;i<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0;
P[i][i]=PDiag[i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
{
/* Note: accel_bias not used in 13 state INS */
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
X[6] = q[0];
X[7] = q[1];
X[8] = q[2];
X[9] = q[3];
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSPosVelReset(float pos[3], float vel[3])
{
for (int i = 0; i < 6; i++) {
for(int j = i; j < NUMX; j++) {
P[i][j] = 0; // zero the first 6 rows and columns
P[j][i] = 0;
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar, float VelVar)
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = VelVar;
R[4] = VelVar;
// R[5] = PosVar; // Don't change vertical velocity, not measured
}
void INSSetGyroBias(float gyro_bias[3])
{
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSSetAccelVar(float accel_var[3])
{
Q[3] = accel_var[0];
Q[4] = accel_var[1];
Q[5] = accel_var[2];
}
void INSSetGyroVar(float gyro_var[3])
{
Q[0] = gyro_var[0];
Q[1] = gyro_var[1];
Q[2] = gyro_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2];
}
void INSSetMagNorth(float B[3])
{
Be[0] = B[0];
Be[1] = B[1];
Be[2] = B[2];
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
float qmag;
// rate gyro inputs in units of rad/s
U[0] = gyro_data[0];
U[1] = gyro_data[1];
U[2] = gyro_data[2];
// accelerometer inputs in units of m/s
U[3] = accel_data[0];
U[4] = accel_data[1];
U[5] = accel_data[2];
// 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]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
//CovariancePrediction(F,G,Q,dT,P);
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
}
void INSCovariancePrediction(float dT)
{
CovariancePrediction(F, G, Q, dT, P);
}
float zeros[3] = { 0, 0, 0 };
void MagCorrection(float mag_data[3])
{
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
}
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, zeros, Vel, BaroAlt,
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
BARO_SENSOR);
}
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(zeros, Pos, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt)
{
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
}
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
{
INSCorrection(mag_data, Pos, Vel, zeros[0],
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
}
void VelBaroCorrection(float Vel[3], float BaroAlt)
{
INSCorrection(zeros, zeros, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed)
{
float Z[10], Y[10];
float Bmag, qmag;
// GPS Position in meters and in local NED frame
Z[0] = Pos[0];
Z[1] = Pos[1];
Z[2] = Pos[2];
// GPS Velocity in meters and in local NED frame
Z[3] = Vel[0];
Z[4] = Vel[1];
Z[5] = Vel[2];
// 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] +
mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] / Bmag;
Z[7] = mag_data[1] / Bmag;
Z[8] = mag_data[2] / Bmag;
// barometric altimeter in meters and in local NED frame
Z[9] = BaroAlt;
// EKF correction step
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]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
}
// ************* CovariancePrediction *************
// Does the prediction step of the Kalman filter for the covariance matrix
// Output, Pnew, overwrites P, the input covariance
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
// Q is the discrete time covariance of process noise
// Q is vector of the diagonal for a square matrix with
// dimensions equal to the number of disturbance noise variables
// The General Method is very inefficient,not taking advantage of the sparse F and G
// The first Method is very specific to this implementation
// ************************************************
#ifdef COVARIANCE_PREDICTION_GENERAL
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float Dummy[NUMX][NUMX], dTsq;
uint8_t i, j, k;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
dTsq = dT * dT;
for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
for (j = 0; j < NUMX; j++) {
Dummy[i][j] = P[i][j] / dT;
for (k = 0; k < NUMX; k++)
Dummy[i][j] += F[i][k] * P[k][j];
}
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
P[i][j] = Dummy[i][j] / dT;
for (k = 0; k < NUMX; k++)
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
for (k = 0; k < NUMW; k++)
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
}
}
#else
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float D[NUMX][NUMX], T, Tsq;
uint8_t i, j;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
T = dT;
Tsq = dT * dT;
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
for (j = i; j < NUMX; j++)
D[i][j] = P[i][j];
// Brute force calculation of the elements of P
P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0];
P[0][1] = P[1][0] =
D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1];
P[0][2] = P[2][0] =
D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2];
P[0][3] = P[3][0] =
(F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] +
F[3][9] * D[3][9]) * Tsq + (D[3][3] + F[3][6] * D[0][6] +
F[3][7] * D[0][7] +
F[3][8] * D[0][8] +
F[3][9] * D[0][9]) * T + D[0][3];
P[0][4] = P[4][0] =
(F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] +
F[4][9] * D[3][9]) * Tsq + (D[3][4] + F[4][6] * D[0][6] +
F[4][7] * D[0][7] +
F[4][8] * D[0][8] +
F[4][9] * D[0][9]) * T + D[0][4];
P[0][5] = P[5][0] =
(F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] +
F[5][9] * D[3][9]) * Tsq + (D[3][5] + F[5][6] * D[0][6] +
F[5][7] * D[0][7] +
F[5][8] * D[0][8] +
F[5][9] * D[0][9]) * T + D[0][5];
P[0][6] = P[6][0] =
(F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] +
F[6][10] * D[3][10] + F[6][11] * D[3][11] +
F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] +
F[6][8] * D[0][8] +
F[6][9] * D[0][9] +
F[6][10] * D[0][10] +
F[6][11] * D[0][11] +
F[6][12] * D[0][12]) * T +
D[0][6];
P[0][7] = P[7][0] =
(F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] +
F[7][10] * D[3][10] + F[7][11] * D[3][11] +
F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] +
F[7][8] * D[0][8] +
F[7][9] * D[0][9] +
F[7][10] * D[0][10] +
F[7][11] * D[0][11] +
F[7][12] * D[0][12]) * T +
D[0][7];
P[0][8] = P[8][0] =
(F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] +
F[8][10] * D[3][10] + F[8][11] * D[3][11] +
F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] +
F[8][7] * D[0][7] +
F[8][9] * D[0][9] +
F[8][10] * D[0][10] +
F[8][11] * D[0][11] +
F[8][12] * D[0][12]) * T +
D[0][8];
P[0][9] = P[9][0] =
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
F[9][10] * D[3][10] + F[9][11] * D[3][11] +
F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] +
F[9][7] * D[0][7] +
F[9][8] * D[0][8] +
F[9][10] * D[0][10] +
F[9][11] * D[0][11] +
F[9][12] * D[0][12]) * T +
D[0][9];
P[0][10] = P[10][0] = D[3][10] * T + D[0][10];
P[0][11] = P[11][0] = D[3][11] * T + D[0][11];
P[0][12] = P[12][0] = D[3][12] * T + D[0][12];
P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1];
P[1][2] = P[2][1] =
D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2];
P[1][3] = P[3][1] =
(F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] +
F[3][9] * D[4][9]) * Tsq + (D[3][4] + F[3][6] * D[1][6] +
F[3][7] * D[1][7] +
F[3][8] * D[1][8] +
F[3][9] * D[1][9]) * T + D[1][3];
P[1][4] = P[4][1] =
(F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] +
F[4][9] * D[4][9]) * Tsq + (D[4][4] + F[4][6] * D[1][6] +
F[4][7] * D[1][7] +
F[4][8] * D[1][8] +
F[4][9] * D[1][9]) * T + D[1][4];
P[1][5] = P[5][1] =
(F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] +
F[5][9] * D[4][9]) * Tsq + (D[4][5] + F[5][6] * D[1][6] +
F[5][7] * D[1][7] +
F[5][8] * D[1][8] +
F[5][9] * D[1][9]) * T + D[1][5];
P[1][6] = P[6][1] =
(F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] +
F[6][10] * D[4][10] + F[6][11] * D[4][11] +
F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] +
F[6][8] * D[1][8] +
F[6][9] * D[1][9] +
F[6][10] * D[1][10] +
F[6][11] * D[1][11] +
F[6][12] * D[1][12]) * T +
D[1][6];
P[1][7] = P[7][1] =
(F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] +
F[7][10] * D[4][10] + F[7][11] * D[4][11] +
F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] +
F[7][8] * D[1][8] +
F[7][9] * D[1][9] +
F[7][10] * D[1][10] +
F[7][11] * D[1][11] +
F[7][12] * D[1][12]) * T +
D[1][7];
P[1][8] = P[8][1] =
(F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] +
F[8][10] * D[4][10] + F[8][11] * D[4][11] +
F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] +
F[8][7] * D[1][7] +
F[8][9] * D[1][9] +
F[8][10] * D[1][10] +
F[8][11] * D[1][11] +
F[8][12] * D[1][12]) * T +
D[1][8];
P[1][9] = P[9][1] =
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
F[9][10] * D[4][10] + F[9][11] * D[4][11] +
F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] +
F[9][7] * D[1][7] +
F[9][8] * D[1][8] +
F[9][10] * D[1][10] +
F[9][11] * D[1][11] +
F[9][12] * D[1][12]) * T +
D[1][9];
P[1][10] = P[10][1] = D[4][10] * T + D[1][10];
P[1][11] = P[11][1] = D[4][11] * T + D[1][11];
P[1][12] = P[12][1] = D[4][12] * T + D[1][12];
P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2];
P[2][3] = P[3][2] =
(F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] +
F[3][9] * D[5][9]) * Tsq + (D[3][5] + F[3][6] * D[2][6] +
F[3][7] * D[2][7] +
F[3][8] * D[2][8] +
F[3][9] * D[2][9]) * T + D[2][3];
P[2][4] = P[4][2] =
(F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] +
F[4][9] * D[5][9]) * Tsq + (D[4][5] + F[4][6] * D[2][6] +
F[4][7] * D[2][7] +
F[4][8] * D[2][8] +
F[4][9] * D[2][9]) * T + D[2][4];
P[2][5] = P[5][2] =
(F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] +
F[5][9] * D[5][9]) * Tsq + (D[5][5] + F[5][6] * D[2][6] +
F[5][7] * D[2][7] +
F[5][8] * D[2][8] +
F[5][9] * D[2][9]) * T + D[2][5];
P[2][6] = P[6][2] =
(F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] +
F[6][10] * D[5][10] + F[6][11] * D[5][11] +
F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] +
F[6][8] * D[2][8] +
F[6][9] * D[2][9] +
F[6][10] * D[2][10] +
F[6][11] * D[2][11] +
F[6][12] * D[2][12]) * T +
D[2][6];
P[2][7] = P[7][2] =
(F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] +
F[7][10] * D[5][10] + F[7][11] * D[5][11] +
F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] +
F[7][8] * D[2][8] +
F[7][9] * D[2][9] +
F[7][10] * D[2][10] +
F[7][11] * D[2][11] +
F[7][12] * D[2][12]) * T +
D[2][7];
P[2][8] = P[8][2] =
(F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] +
F[8][10] * D[5][10] + F[8][11] * D[5][11] +
F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] +
F[8][7] * D[2][7] +
F[8][9] * D[2][9] +
F[8][10] * D[2][10] +
F[8][11] * D[2][11] +
F[8][12] * D[2][12]) * T +
D[2][8];
P[2][9] = P[9][2] =
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
F[9][10] * D[5][10] + F[9][11] * D[5][11] +
F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] +
F[9][7] * D[2][7] +
F[9][8] * D[2][8] +
F[9][10] * D[2][10] +
F[9][11] * D[2][11] +
F[9][12] * D[2][12]) * T +
D[2][9];
P[2][10] = P[10][2] = D[5][10] * T + D[2][10];
P[2][11] = P[11][2] = D[5][11] * T + D[2][11];
P[2][12] = P[12][2] = D[5][12] * T + D[2][12];
P[3][3] =
(Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] +
Q[5] * G[3][5] * G[3][5] + F[3][9] * (F[3][9] * D[9][9] +
F[3][6] * D[6][9] +
F[3][7] * D[7][9] +
F[3][8] * D[8][9]) +
F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] +
2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9]) * T + D[3][3];
P[3][4] = P[4][3] =
(F[4][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[4][6] * (F[3][6] * D[6][6] +
F[3][7] * D[6][7] +
F[3][8] * D[6][8] +
F[3][9] * D[6][9]) +
F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] +
G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] +
F[4][6] * D[3][6] +
F[3][7] * D[4][7] +
F[4][7] * D[3][7] +
F[3][8] * D[4][8] +
F[4][8] * D[3][8] +
F[3][9] * D[4][9] +
F[4][9] * D[3][9]) * T +
D[3][4];
P[3][5] = P[5][3] =
(F[5][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[5][6] * (F[3][6] * D[6][6] +
F[3][7] * D[6][7] +
F[3][8] * D[6][8] +
F[3][9] * D[6][9]) +
F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] +
G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] +
F[5][6] * D[3][6] +
F[3][7] * D[5][7] +
F[5][7] * D[3][7] +
F[3][8] * D[5][8] +
F[5][8] * D[3][8] +
F[3][9] * D[5][9] +
F[5][9] * D[3][9]) * T +
D[3][5];
P[3][6] = P[6][3] =
(F[6][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[6][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[6][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[6][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] +
F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] +
F[6][9] * D[3][9] + F[6][10] * D[3][10] +
F[6][11] * D[3][11] + F[6][12] * D[3][12]) * T + D[3][6];
P[3][7] = P[7][3] =
(F[7][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[7][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[7][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[7][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] +
F[7][9] * D[3][9] + F[7][10] * D[3][10] +
F[7][11] * D[3][11] + F[7][12] * D[3][12]) * T + D[3][7];
P[3][8] = P[8][3] =
(F[8][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[8][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[8][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[8][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9])) * Tsq +
(F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] +
F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] +
F[8][9] * D[3][9] + F[8][10] * D[3][10] +
F[8][11] * D[3][11] + F[8][12] * D[3][12]) * T + D[3][8];
P[3][9] = P[9][3] =
(F[9][10] *
(F[3][9] * D[9][10] + F[3][6] * D[6][10] +
F[3][7] * D[7][10] + F[3][8] * D[8][10]) +
F[9][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[9][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
F[3][9] * D[9][9] + F[9][10] * D[3][10] +
F[9][11] * D[3][11] + F[9][12] * D[3][12] +
F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) * T + D[3][9];
P[3][10] = P[10][3] =
(F[3][9] * D[9][10] + F[3][6] * D[6][10] + F[3][7] * D[7][10] +
F[3][8] * D[8][10]) * T + D[3][10];
P[3][11] = P[11][3] =
(F[3][9] * D[9][11] + F[3][6] * D[6][11] + F[3][7] * D[7][11] +
F[3][8] * D[8][11]) * T + D[3][11];
P[3][12] = P[12][3] =
(F[3][9] * D[9][12] + F[3][6] * D[6][12] + F[3][7] * D[7][12] +
F[3][8] * D[8][12]) * T + D[3][12];
P[4][4] =
(Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] +
Q[5] * G[4][5] * G[4][5] + F[4][9] * (F[4][9] * D[9][9] +
F[4][6] * D[6][9] +
F[4][7] * D[7][9] +
F[4][8] * D[8][9]) +
F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] +
2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9]) * T + D[4][4];
P[4][5] = P[5][4] =
(F[5][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[5][6] * (F[4][6] * D[6][6] +
F[4][7] * D[6][7] +
F[4][8] * D[6][8] +
F[4][9] * D[6][9]) +
F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9]) +
G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] +
G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] +
F[5][6] * D[4][6] +
F[4][7] * D[5][7] +
F[5][7] * D[4][7] +
F[4][8] * D[5][8] +
F[5][8] * D[4][8] +
F[4][9] * D[5][9] +
F[5][9] * D[4][9]) * T +
D[4][5];
P[4][6] = P[6][4] =
(F[6][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[6][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[6][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[6][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] +
F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] +
F[6][9] * D[4][9] + F[6][10] * D[4][10] +
F[6][11] * D[4][11] + F[6][12] * D[4][12]) * T + D[4][6];
P[4][7] = P[7][4] =
(F[7][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[7][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[7][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[7][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] +
F[7][9] * D[4][9] + F[7][10] * D[4][10] +
F[7][11] * D[4][11] + F[7][12] * D[4][12]) * T + D[4][7];
P[4][8] = P[8][4] =
(F[8][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[8][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[8][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[8][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9])) * Tsq +
(F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] +
F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] +
F[8][9] * D[4][9] + F[8][10] * D[4][10] +
F[8][11] * D[4][11] + F[8][12] * D[4][12]) * T + D[4][8];
P[4][9] = P[9][4] =
(F[9][10] *
(F[4][9] * D[9][10] + F[4][6] * D[6][10] +
F[4][7] * D[7][10] + F[4][8] * D[8][10]) +
F[9][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[9][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
F[4][9] * D[9][9] + F[9][10] * D[4][10] +
F[9][11] * D[4][11] + F[9][12] * D[4][12] +
F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) * T + D[4][9];
P[4][10] = P[10][4] =
(F[4][9] * D[9][10] + F[4][6] * D[6][10] + F[4][7] * D[7][10] +
F[4][8] * D[8][10]) * T + D[4][10];
P[4][11] = P[11][4] =
(F[4][9] * D[9][11] + F[4][6] * D[6][11] + F[4][7] * D[7][11] +
F[4][8] * D[8][11]) * T + D[4][11];
P[4][12] = P[12][4] =
(F[4][9] * D[9][12] + F[4][6] * D[6][12] + F[4][7] * D[7][12] +
F[4][8] * D[8][12]) * T + D[4][12];
P[5][5] =
(Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] +
Q[5] * G[5][5] * G[5][5] + F[5][9] * (F[5][9] * D[9][9] +
F[5][6] * D[6][9] +
F[5][7] * D[7][9] +
F[5][8] * D[8][9]) +
F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] +
2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9]) * T + D[5][5];
P[5][6] = P[6][5] =
(F[6][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[6][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[6][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[6][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] +
F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] +
F[6][9] * D[5][9] + F[6][10] * D[5][10] +
F[6][11] * D[5][11] + F[6][12] * D[5][12]) * T + D[5][6];
P[5][7] = P[7][5] =
(F[7][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[7][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[7][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[7][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] +
F[7][9] * D[5][9] + F[7][10] * D[5][10] +
F[7][11] * D[5][11] + F[7][12] * D[5][12]) * T + D[5][7];
P[5][8] = P[8][5] =
(F[8][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[8][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[8][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[8][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9])) * Tsq +
(F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] +
F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] +
F[8][9] * D[5][9] + F[8][10] * D[5][10] +
F[8][11] * D[5][11] + F[8][12] * D[5][12]) * T + D[5][8];
P[5][9] = P[9][5] =
(F[9][10] *
(F[5][9] * D[9][10] + F[5][6] * D[6][10] +
F[5][7] * D[7][10] + F[5][8] * D[8][10]) +
F[9][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[9][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
F[5][9] * D[9][9] + F[9][10] * D[5][10] +
F[9][11] * D[5][11] + F[9][12] * D[5][12] +
F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) * T + D[5][9];
P[5][10] = P[10][5] =
(F[5][9] * D[9][10] + F[5][6] * D[6][10] + F[5][7] * D[7][10] +
F[5][8] * D[8][10]) * T + D[5][10];
P[5][11] = P[11][5] =
(F[5][9] * D[9][11] + F[5][6] * D[6][11] + F[5][7] * D[7][11] +
F[5][8] * D[8][11]) * T + D[5][11];
P[5][12] = P[12][5] =
(F[5][9] * D[9][12] + F[5][6] * D[6][12] + F[5][7] * D[7][12] +
F[5][8] * D[8][12]) * T + D[5][12];
P[6][6] =
(Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] +
Q[2] * G[6][2] * G[6][2] + F[6][9] * (F[6][9] * D[9][9] +
F[6][10] * D[9][10] +
F[6][11] * D[9][11] +
F[6][12] * D[9][12] +
F[6][7] * D[7][9] +
F[6][8] * D[8][9]) +
F[6][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[6][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[6][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12])) * Tsq +
(2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] +
2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] +
2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T +
D[6][6];
P[6][7] = P[7][6] =
(F[7][9] *
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
F[7][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[7][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[7][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] +
G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] +
F[6][7] * D[7][7] +
F[6][8] * D[7][8] +
F[7][8] * D[6][8] +
F[6][9] * D[7][9] +
F[7][9] * D[6][9] +
F[6][10] * D[7][10] +
F[7][10] * D[6][10] +
F[6][11] * D[7][11] +
F[7][11] * D[6][11] +
F[6][12] * D[7][12] +
F[7][12] * D[6][12]) * T +
D[6][7];
P[6][8] = P[8][6] =
(F[8][9] *
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
F[8][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[8][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[8][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] +
G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] +
F[8][6] * D[6][6] +
F[8][7] * D[6][7] +
F[6][8] * D[8][8] +
F[6][9] * D[8][9] +
F[8][9] * D[6][9] +
F[6][10] * D[8][10] +
F[8][10] * D[6][10] +
F[6][11] * D[8][11] +
F[8][11] * D[6][11] +
F[6][12] * D[8][12] +
F[8][12] * D[6][12]) * T +
D[6][8];
P[6][9] = P[9][6] =
(F[9][10] *
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[9][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[9][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
G[9][0] * G[6][0] * Q[0] + G[9][1] * G[6][1] * Q[1] +
G[9][2] * G[6][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] +
F[9][7] * D[6][7] +
F[9][8] * D[6][8] +
F[6][9] * D[9][9] +
F[9][10] * D[6][10] +
F[6][10] * D[9][10] +
F[9][11] * D[6][11] +
F[6][11] * D[9][11] +
F[9][12] * D[6][12] +
F[6][12] * D[9][12] +
F[6][7] * D[7][9] +
F[6][8] * D[8][9]) * T +
D[6][9];
P[6][10] = P[10][6] =
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) * T + D[6][10];
P[6][11] = P[11][6] =
(F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) * T + D[6][11];
P[6][12] = P[12][6] =
(F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) * T + D[6][12];
P[7][7] =
(Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] +
Q[2] * G[7][2] * G[7][2] + F[7][9] * (F[7][9] * D[9][9] +
F[7][10] * D[9][10] +
F[7][11] * D[9][11] +
F[7][12] * D[9][12] +
F[7][6] * D[6][9] +
F[7][8] * D[8][9]) +
F[7][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[7][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[7][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
F[7][11] * D[8][11] + F[7][12] * D[8][12])) * Tsq +
(2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] +
2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] +
2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T +
D[7][7];
P[7][8] = P[8][7] =
(F[8][9] *
(F[7][9] * D[9][9] + F[7][10] * D[9][10] +
F[7][11] * D[9][11] + F[7][12] * D[9][12] +
F[7][6] * D[6][9] + F[7][8] * D[8][9]) +
F[8][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[8][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[8][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] +
G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] +
F[8][6] * D[6][7] +
F[8][7] * D[7][7] +
F[7][8] * D[8][8] +
F[7][9] * D[8][9] +
F[8][9] * D[7][9] +
F[7][10] * D[8][10] +
F[8][10] * D[7][10] +
F[7][11] * D[8][11] +
F[8][11] * D[7][11] +
F[7][12] * D[8][12] +
F[8][12] * D[7][12]) * T +
D[7][8];
P[7][9] = P[9][7] =
(F[9][10] *
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[9][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[9][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
F[7][11] * D[8][11] + F[7][12] * D[8][12]) +
G[9][0] * G[7][0] * Q[0] + G[9][1] * G[7][1] * Q[1] +
G[9][2] * G[7][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] +
F[9][7] * D[7][7] +
F[9][8] * D[7][8] +
F[7][9] * D[9][9] +
F[9][10] * D[7][10] +
F[7][10] * D[9][10] +
F[9][11] * D[7][11] +
F[7][11] * D[9][11] +
F[9][12] * D[7][12] +
F[7][12] * D[9][12] +
F[7][6] * D[6][9] +
F[7][8] * D[8][9]) * T +
D[7][9];
P[7][10] = P[10][7] =
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) * T + D[7][10];
P[7][11] = P[11][7] =
(F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) * T + D[7][11];
P[7][12] = P[12][7] =
(F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) * T + D[7][12];
P[8][8] =
(Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] +
Q[2] * G[8][2] * G[8][2] + F[8][9] * (F[8][9] * D[9][9] +
F[8][10] * D[9][10] +
F[8][11] * D[9][11] +
F[8][12] * D[9][12] +
F[8][6] * D[6][9] +
F[8][7] * D[7][9]) +
F[8][10] * (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
F[8][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
F[8][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
F[8][11] * D[7][11] + F[8][12] * D[7][12])) * Tsq +
(2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] +
2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] +
2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T +
D[8][8];
P[8][9] = P[9][8] =
(F[9][10] *
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
F[9][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
F[9][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
F[8][11] * D[7][11] + F[8][12] * D[7][12]) +
F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] +
F[8][9] * D[8][9] + F[8][10] * D[8][10] +
F[8][11] * D[8][11] + F[8][12] * D[8][12]) +
G[9][0] * G[8][0] * Q[0] + G[9][1] * G[8][1] * Q[1] +
G[9][2] * G[8][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] +
F[9][7] * D[7][8] +
F[9][8] * D[8][8] +
F[8][9] * D[9][9] +
F[9][10] * D[8][10] +
F[8][10] * D[9][10] +
F[9][11] * D[8][11] +
F[8][11] * D[9][11] +
F[9][12] * D[8][12] +
F[8][12] * D[9][12] +
F[8][6] * D[6][9] +
F[8][7] * D[7][9]) * T +
D[8][9];
P[8][10] = P[10][8] =
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) * T + D[8][10];
P[8][11] = P[11][8] =
(F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) * T + D[8][11];
P[8][12] = P[12][8] =
(F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) * T + D[8][12];
P[9][9] =
(Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] +
Q[2] * G[9][2] * G[9][2] + F[9][10] * (F[9][10] * D[10][10] +
F[9][11] * D[10][11] +
F[9][12] * D[10][12] +
F[9][6] * D[6][10] +
F[9][7] * D[7][10] +
F[9][8] * D[8][10]) +
F[9][11] * (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
F[9][7] * D[7][11] + F[9][8] * D[8][11]) +
F[9][12] * (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
F[9][7] * D[7][12] + F[9][8] * D[8][12]) +
F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] +
F[9][8] * D[6][8] + F[9][10] * D[6][10] +
F[9][11] * D[6][11] + F[9][12] * D[6][12]) +
F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] +
F[9][8] * D[7][8] + F[9][10] * D[7][10] +
F[9][11] * D[7][11] + F[9][12] * D[7][12]) +
F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] +
F[9][8] * D[8][8] + F[9][10] * D[8][10] +
F[9][11] * D[8][11] + F[9][12] * D[8][12])) * Tsq +
(2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] +
2 * F[9][12] * D[9][12] + 2 * F[9][6] * D[6][9] +
2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9]) * T + D[9][9];
P[9][10] = P[10][9] =
(F[9][10] * D[10][10] + F[9][11] * D[10][11] +
F[9][12] * D[10][12] + F[9][6] * D[6][10] +
F[9][7] * D[7][10] + F[9][8] * D[8][10]) * T + D[9][10];
P[9][11] = P[11][9] =
(F[9][10] * D[10][11] + F[9][11] * D[11][11] +
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
F[9][7] * D[7][11] + F[9][8] * D[8][11]) * T + D[9][11];
P[9][12] = P[12][9] =
(F[9][10] * D[10][12] + F[9][11] * D[11][12] +
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
F[9][7] * D[7][12] + F[9][8] * D[8][12]) * T + D[9][12];
P[10][10] = Q[6] * Tsq + D[10][10];
P[10][11] = P[11][10] = D[10][11];
P[10][12] = P[12][10] = D[10][12];
P[11][11] = Q[7] * Tsq + D[11][11];
P[11][12] = P[12][11] = D[11][12];
P[12][12] = Q[8] * Tsq + D[12][12];
}
#endif
// ************* SerialUpdate *******************
// Does the update step of the Kalman filter for the covariance and estimate
// Outputs are Xnew & Pnew, and are written over P and X
// Z is actual measurement, Y is predicted measurement
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
// where K=P*H'*inv[H*P*H'+R]
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
// i.e. the measurment noises are uncorrelated.
// It therefore uses a serial update that requires no matrix inversion by
// processing the measurements one at a time.
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
// The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update.
// ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed)
{
float HP[NUMX], HPHR, Error;
uint8_t i, j, k, m;
for (m = 0; m < NUMV; m++) {
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0;
for (k = 0; k < NUMX; k++)
HP[j] += H[m][k] * P[k][j];
}
HPHR = R[m]; // Find HPHR = H*P*H' + R
for (k = 0; k < NUMX; k++)
HPHR += HP[k] * H[m][k];
for (k = 0; k < NUMX; k++)
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++)
P[i][j] = P[j][i] =
P[i][j] - K[i][m] * HP[j];
}
Error = Z[m] - Y[m];
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
X[i] = X[i] + K[i][m] * Error;
}
}
}
// ************* RungeKutta **********************
// Does a 4th order Runge Kutta numerical integration step
// Output, Xnew, is written over X
// NOTE the algorithm assumes time invariant state equations and
// constant inputs over integration step
// ************************************************
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{
float dT2 =
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i = 0; i < NUMX; i++)
Xlast[i] = X[i]; // make a working copy
StateEq(X, U, K1); // k1 = f(x,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K1[i];
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K2[i];
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT * K3[i];
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i = 0; i < NUMX; i++)
X[i] =
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
K4[i]) / 6;
}
// ************* Model Specific Stuff ***************************
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
//
// State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
// Deterministic Inputs = [AngularVel Accel]
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
//
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
// Inputs to Measurement = [EarthFrameMagField]
//
// Notes: Pos and Vel in earth frame
// AngularVel and Accel in body frame
// MagFields are unit vectors
// Xdot is output of StateEq()
// F and G are outputs of LinearizeFG(), all elements not set should be zero
// y is output of OutputEq()
// H is output of LinearizeH(), all elements not set should be zero
// ************************************************
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
Xdot[0] = X[3];
Xdot[1] = X[4];
Xdot[2] = X[5];
// Vdot = Reb*a
Xdot[3] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
q0 * q3) *
ay + 2 * (q1 * q3 + q0 * q2) * az;
Xdot[4] =
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
q3 * q3) * ay + 2 * (q2 * q3 -
q0 * q1) *
az;
Xdot[5] =
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
// best guess is that bias stays constant
Xdot[10] = Xdot[11] = Xdot[12] = 0;
}
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
F[0][3] = F[1][4] = F[2][5] = 1;
// dVdot/dq
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
// dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
// F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
// F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
// F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dq
F[6][6] = 0;
F[6][7] = -wx / 2;
F[6][8] = -wy / 2;
F[6][9] = -wz / 2;
F[7][6] = wx / 2;
F[7][7] = 0;
F[7][8] = wz / 2;
F[7][9] = -wy / 2;
F[8][6] = wy / 2;
F[8][7] = -wz / 2;
F[8][8] = 0;
F[8][9] = wx / 2;
F[9][6] = wz / 2;
F[9][7] = wy / 2;
F[9][8] = -wx / 2;
F[9][9] = 0;
// dqdot/dwbias
F[6][10] = q1 / 2;
F[6][11] = q2 / 2;
F[6][12] = q3 / 2;
F[7][10] = -q0 / 2;
F[7][11] = q3 / 2;
F[7][12] = -q2 / 2;
F[8][10] = -q3 / 2;
F[8][11] = -q0 / 2;
F[8][12] = q1 / 2;
F[9][10] = q2 / 2;
F[9][11] = -q1 / 2;
F[9][12] = -q0 / 2;
// dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
G[3][4] = 2 * (-q1 * q2 + q0 * q3);
G[3][5] = -2 * (q1 * q3 + q0 * q2);
G[4][3] = -2 * (q1 * q2 + q0 * q3);
G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
G[4][5] = 2 * (-q2 * q3 + q0 * q1);
G[5][3] = 2 * (-q1 * q3 + q0 * q2);
G[5][4] = -2 * (q2 * q3 + q0 * q1);
G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
// dqdot/dnw
G[6][0] = q1 / 2;
G[6][1] = q2 / 2;
G[6][2] = q3 / 2;
G[7][0] = -q0 / 2;
G[7][1] = q3 / 2;
G[7][2] = -q2 / 2;
G[8][0] = -q3 / 2;
G[8][1] = -q0 / 2;
G[8][2] = q1 / 2;
G[9][0] = q2 / 2;
G[9][1] = -q1 / 2;
G[9][2] = -q0 / 2;
// dwbias = random walk noise
G[10][6] = G[11][7] = G[12][8] = 1;
// dabias = random walk noise
// G[13][9]=G[14][10]=G[15][11]=1; // NO BIAS STATES ON ACCELS
}
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// first six outputs are P and V
Y[0] = X[0];
Y[1] = X[1];
Y[2] = X[2];
Y[3] = X[3];
Y[4] = X[4];
Y[5] = X[5];
// Bb=Rbe*Be
Y[6] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
q0 * q2) * Be[2];
Y[7] =
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
q2 * q2 - q3 * q3) * Be[1] +
2 * (q2 * q3 + q0 * q1) * Be[2];
Y[8] =
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
q0 * q1) * Be[1] +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
// Alt = -Pz
Y[9] = -X[2];
}
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// dP/dP=I;
H[0][0] = H[1][1] = H[2][2] = 1;
// dV/dV=I;
H[3][3] = H[4][4] = H[5][5] = 1;
// dBb/dq
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
// dAlt/dPz = -1
H[9][2] = -1;
}
/**
* @}
* @}
*/

866
flight/AHRS/insgps16state.c
View File

@ -1,866 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @file insgps.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief An INS/GPS algorithm implemented with an EKF.
*
* @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 "insgps.h"
#include <math.h>
#include <stdint.h>
// constants/macros/typdefs
#define NUMX 16 // number of states, X is the state vector
#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector
#if defined(GENERAL_COV)
// This might trick people so I have a note here. There is a slower but bigger version of the
// code here but won't fit when debugging disabled (requires -Os)
#define COVARIANCE_PREDICTION_GENERAL
#endif
// Private functions
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW]);
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
// global to init to zero and maintain zero elements
float Be[3]; // local magnetic unit vector in NED frame
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
float K[NUMX][NUMV]; // feedback gain matrix
// ************* Exposed Functions ****************
// *************************************************
uint16_t ins_get_num_states()
{
return NUMX;
}
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1;
Be[1] = 0;
Be[2] = 0; // 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[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-5; // 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[6] = 1;
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // 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[9] = Q[10] = Q[11] = 2e-20; // accel bias random walk variance (m/s^3)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // 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[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
R[9] = .05; // High freq altimeter noise variance (m^2)
}
void INSResetP(float PDiag[NUMX])
{
uint8_t i,j;
// if PDiag[i] nonzero then clear row and column and set diagonal element
for (i=0;i<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0;
P[i][i]=PDiag[i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
{
Nav.Pos[0] = X[0] = pos[0];
Nav.Pos[1] = X[1] = pos[1];
Nav.Pos[2] = X[2] = pos[2];
Nav.Vel[0] = X[3] = vel[0];
Nav.Vel[1] = X[4] = vel[1];
Nav.Vel[2] = X[5] = vel[2];
Nav.q[0] = X[6] = q[0];
Nav.q[1] = X[7] = q[1];
Nav.q[2] = X[8] = q[2];
Nav.q[3] = X[9] = q[3];
Nav.gyro_bias[0] = X[10] = gyro_bias[0];
Nav.gyro_bias[1] = X[11] = gyro_bias[1];
Nav.gyro_bias[2] = X[12] = gyro_bias[2];
Nav.accel_bias[0] = X[13] = accel_bias[0];
Nav.accel_bias[1] = X[14] = accel_bias[1];
Nav.accel_bias[2] = X[15] = accel_bias[2];
}
void INSPosVelReset(float pos[3], float vel[3])
{
for (int i = 0; i < 6; i++) {
for(int j = i; j < NUMX; j++) {
P[i][j] = 0; // zero the first 6 rows and columns
P[j][i] = 0;
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar, float VelVar)
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = VelVar;
R[4] = VelVar;
// R[5] = PosVar; // Don't change vertical velocity, not measured
}
void INSSetGyroBias(float gyro_bias[3])
{
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSSetAccelVar(float accel_var[3])
{
Q[3] = accel_var[0];
Q[4] = accel_var[1];
Q[5] = accel_var[2];
}
void INSSetGyroVar(float gyro_var[3])
{
Q[0] = gyro_var[0];
Q[1] = gyro_var[1];
Q[2] = gyro_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2];
}
void INSSetMagNorth(float B[3])
{
Be[0] = B[0];
Be[1] = B[1];
Be[2] = B[2];
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
float qmag;
// rate gyro inputs in units of rad/s
U[0] = gyro_data[0];
U[1] = gyro_data[1];
U[2] = gyro_data[2];
// accelerometer inputs in units of m/s
U[3] = accel_data[0];
U[4] = accel_data[1];
U[5] = accel_data[2];
// 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]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
//CovariancePrediction(F,G,Q,dT,P);
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
}
void INSCovariancePrediction(float dT)
{
CovariancePrediction(F, G, Q, dT, P);
}
float zeros[3] = { 0, 0, 0 };
void MagCorrection(float mag_data[3])
{
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
}
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, zeros, Vel, BaroAlt,
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
BARO_SENSOR);
}
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(zeros, Pos, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt)
{
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
}
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
{
INSCorrection(mag_data, Pos, Vel, zeros[0],
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
}
void VelBaroCorrection(float Vel[3], float BaroAlt)
{
INSCorrection(zeros, zeros, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed)
{
float Z[10], Y[10];
float Bmag, qmag;
// GPS Position in meters and in local NED frame
Z[0] = Pos[0];
Z[1] = Pos[1];
Z[2] = Pos[2];
// GPS Velocity in meters and in local NED frame
Z[3] = Vel[0];
Z[4] = Vel[1];
Z[5] = Vel[2];
// 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] +
mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] / Bmag;
Z[7] = mag_data[1] / Bmag;
Z[8] = mag_data[2] / Bmag;
// barometric altimeter in meters and in local NED frame
Z[9] = BaroAlt;
// EKF correction step
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]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
Nav.accel_bias[0] = X[13];
Nav.accel_bias[1] = X[14];
Nav.accel_bias[2] = X[15];
}
// ************* CovariancePrediction *************
// Does the prediction step of the Kalman filter for the covariance matrix
// Output, Pnew, overwrites P, the input covariance
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
// Q is the discrete time covariance of process noise
// Q is vector of the diagonal for a square matrix with
// dimensions equal to the number of disturbance noise variables
// The General Method is very inefficient,not taking advantage of the sparse F and G
// The first Method is very specific to this implementation
// ************************************************
#ifdef COVARIANCE_PREDICTION_GENERAL
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float Dummy[NUMX][NUMX], dTsq;
uint8_t i, j, k;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
dTsq = dT * dT;
for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
for (j = 0; j < NUMX; j++) {
Dummy[i][j] = P[i][j] / dT;
for (k = 0; k < NUMX; k++)
Dummy[i][j] += F[i][k] * P[k][j];
}
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
P[i][j] = Dummy[i][j] / dT;
for (k = 0; k < NUMX; k++)
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
for (k = 0; k < NUMW; k++)
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
}
}
#else
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float D[NUMX][NUMX], T, Tsq;
uint8_t i, j;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
T = dT;
Tsq = dT * dT;
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
for (j = i; j < NUMX; j++)
D[i][j] = P[i][j];
// Brute force calculation of the elements of P
P[0][0] = D[3][3]*Tsq + (2*D[0][3])*T + D[0][0];
P[0][1] = P[1][0] = D[3][4]*Tsq + (D[0][4] + D[1][3])*T + D[0][1];
P[0][2] = P[2][0] = D[3][5]*Tsq + (D[0][5] + D[2][3])*T + D[0][2];
P[0][3] = P[3][0] = (F[3][6]*D[3][6] + F[3][7]*D[3][7] + F[3][8]*D[3][8] + F[3][9]*D[3][9] + F[3][13]*D[3][13] + F[3][14]*D[3][14] + F[3][15]*D[3][15])*Tsq + (D[3][3] + F[3][6]*D[0][6] + F[3][7]*D[0][7] + F[3][8]*D[0][8] + F[3][9]*D[0][9] + F[3][13]*D[0][13] + F[3][14]*D[0][14] + F[3][15]*D[0][15])*T + D[0][3];
P[0][4] = P[4][0] = (F[4][6]*D[3][6] + F[4][7]*D[3][7] + F[4][8]*D[3][8] + F[4][9]*D[3][9] + F[4][13]*D[3][13] + F[4][14]*D[3][14] + F[4][15]*D[3][15])*Tsq + (D[3][4] + F[4][6]*D[0][6] + F[4][7]*D[0][7] + F[4][8]*D[0][8] + F[4][9]*D[0][9] + F[4][13]*D[0][13] + F[4][14]*D[0][14] + F[4][15]*D[0][15])*T + D[0][4];
P[0][5] = P[5][0] = (F[5][6]*D[3][6] + F[5][7]*D[3][7] + F[5][8]*D[3][8] + F[5][9]*D[3][9] + F[5][13]*D[3][13] + F[5][14]*D[3][14] + F[5][15]*D[3][15])*Tsq + (D[3][5] + F[5][6]*D[0][6] + F[5][7]*D[0][7] + F[5][8]*D[0][8] + F[5][9]*D[0][9] + F[5][13]*D[0][13] + F[5][14]*D[0][14] + F[5][15]*D[0][15])*T + D[0][5];
P[0][6] = P[6][0] = (F[6][7]*D[3][7] + F[6][8]*D[3][8] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*Tsq + (D[3][6] + F[6][7]*D[0][7] + F[6][8]*D[0][8] + F[6][9]*D[0][9] + F[6][10]*D[0][10] + F[6][11]*D[0][11] + F[6][12]*D[0][12])*T + D[0][6];
P[0][7] = P[7][0] = (F[7][6]*D[3][6] + F[7][8]*D[3][8] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*Tsq + (D[3][7] + F[7][6]*D[0][6] + F[7][8]*D[0][8] + F[7][9]*D[0][9] + F[7][10]*D[0][10] + F[7][11]*D[0][11] + F[7][12]*D[0][12])*T + D[0][7];
P[0][8] = P[8][0] = (F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*Tsq + (D[3][8] + F[8][6]*D[0][6] + F[8][7]*D[0][7] + F[8][9]*D[0][9] + F[8][10]*D[0][10] + F[8][11]*D[0][11] + F[8][12]*D[0][12])*T + D[0][8];
P[0][9] = P[9][0] = (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12])*Tsq + (D[3][9] + F[9][6]*D[0][6] + F[9][7]*D[0][7] + F[9][8]*D[0][8] + F[9][10]*D[0][10] + F[9][11]*D[0][11] + F[9][12]*D[0][12])*T + D[0][9];
P[0][10] = P[10][0] = D[3][10]*T + D[0][10];
P[0][11] = P[11][0] = D[3][11]*T + D[0][11];
P[0][12] = P[12][0] = D[3][12]*T + D[0][12];
P[0][13] = P[13][0] = D[3][13]*T + D[0][13];
P[0][14] = P[14][0] = D[3][14]*T + D[0][14];
P[0][15] = P[15][0] = D[3][15]*T + D[0][15];
P[1][1] = D[4][4]*Tsq + (2*D[1][4])*T + D[1][1];
P[1][2] = P[2][1] = D[4][5]*Tsq + (D[1][5] + D[2][4])*T + D[1][2];
P[1][3] = P[3][1] = (F[3][6]*D[4][6] + F[3][7]*D[4][7] + F[3][8]*D[4][8] + F[3][9]*D[4][9] + F[3][13]*D[4][13] + F[3][14]*D[4][14] + F[3][15]*D[4][15])*Tsq + (D[3][4] + F[3][6]*D[1][6] + F[3][7]*D[1][7] + F[3][8]*D[1][8] + F[3][9]*D[1][9] + F[3][13]*D[1][13] + F[3][14]*D[1][14] + F[3][15]*D[1][15])*T + D[1][3];
P[1][4] = P[4][1] = (F[4][6]*D[4][6] + F[4][7]*D[4][7] + F[4][8]*D[4][8] + F[4][9]*D[4][9] + F[4][13]*D[4][13] + F[4][14]*D[4][14] + F[4][15]*D[4][15])*Tsq + (D[4][4] + F[4][6]*D[1][6] + F[4][7]*D[1][7] + F[4][8]*D[1][8] + F[4][9]*D[1][9] + F[4][13]*D[1][13] + F[4][14]*D[1][14] + F[4][15]*D[1][15])*T + D[1][4];
P[1][5] = P[5][1] = (F[5][6]*D[4][6] + F[5][7]*D[4][7] + F[5][8]*D[4][8] + F[5][9]*D[4][9] + F[5][13]*D[4][13] + F[5][14]*D[4][14] + F[5][15]*D[4][15])*Tsq + (D[4][5] + F[5][6]*D[1][6] + F[5][7]*D[1][7] + F[5][8]*D[1][8] + F[5][9]*D[1][9] + F[5][13]*D[1][13] + F[5][14]*D[1][14] + F[5][15]*D[1][15])*T + D[1][5];
P[1][6] = P[6][1] = (F[6][7]*D[4][7] + F[6][8]*D[4][8] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*Tsq + (D[4][6] + F[6][7]*D[1][7] + F[6][8]*D[1][8] + F[6][9]*D[1][9] + F[6][10]*D[1][10] + F[6][11]*D[1][11] + F[6][12]*D[1][12])*T + D[1][6];
P[1][7] = P[7][1] = (F[7][6]*D[4][6] + F[7][8]*D[4][8] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*Tsq + (D[4][7] + F[7][6]*D[1][6] + F[7][8]*D[1][8] + F[7][9]*D[1][9] + F[7][10]*D[1][10] + F[7][11]*D[1][11] + F[7][12]*D[1][12])*T + D[1][7];
P[1][8] = P[8][1] = (F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*Tsq + (D[4][8] + F[8][6]*D[1][6] + F[8][7]*D[1][7] + F[8][9]*D[1][9] + F[8][10]*D[1][10] + F[8][11]*D[1][11] + F[8][12]*D[1][12])*T + D[1][8];
P[1][9] = P[9][1] = (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12])*Tsq + (D[4][9] + F[9][6]*D[1][6] + F[9][7]*D[1][7] + F[9][8]*D[1][8] + F[9][10]*D[1][10] + F[9][11]*D[1][11] + F[9][12]*D[1][12])*T + D[1][9];
P[1][10] = P[10][1] = D[4][10]*T + D[1][10];
P[1][11] = P[11][1] = D[4][11]*T + D[1][11];
P[1][12] = P[12][1] = D[4][12]*T + D[1][12];
P[1][13] = P[13][1] = D[4][13]*T + D[1][13];
P[1][14] = P[14][1] = D[4][14]*T + D[1][14];
P[1][15] = P[15][1] = D[4][15]*T + D[1][15];
P[2][2] = D[5][5]*Tsq + (2*D[2][5])*T + D[2][2];
P[2][3] = P[3][2] = (F[3][6]*D[5][6] + F[3][7]*D[5][7] + F[3][8]*D[5][8] + F[3][9]*D[5][9] + F[3][13]*D[5][13] + F[3][14]*D[5][14] + F[3][15]*D[5][15])*Tsq + (D[3][5] + F[3][6]*D[2][6] + F[3][7]*D[2][7] + F[3][8]*D[2][8] + F[3][9]*D[2][9] + F[3][13]*D[2][13] + F[3][14]*D[2][14] + F[3][15]*D[2][15])*T + D[2][3];
P[2][4] = P[4][2] = (F[4][6]*D[5][6] + F[4][7]*D[5][7] + F[4][8]*D[5][8] + F[4][9]*D[5][9] + F[4][13]*D[5][13] + F[4][14]*D[5][14] + F[4][15]*D[5][15])*Tsq + (D[4][5] + F[4][6]*D[2][6] + F[4][7]*D[2][7] + F[4][8]*D[2][8] + F[4][9]*D[2][9] + F[4][13]*D[2][13] + F[4][14]*D[2][14] + F[4][15]*D[2][15])*T + D[2][4];
P[2][5] = P[5][2] = (F[5][6]*D[5][6] + F[5][7]*D[5][7] + F[5][8]*D[5][8] + F[5][9]*D[5][9] + F[5][13]*D[5][13] + F[5][14]*D[5][14] + F[5][15]*D[5][15])*Tsq + (D[5][5] + F[5][6]*D[2][6] + F[5][7]*D[2][7] + F[5][8]*D[2][8] + F[5][9]*D[2][9] + F[5][13]*D[2][13] + F[5][14]*D[2][14] + F[5][15]*D[2][15])*T + D[2][5];
P[2][6] = P[6][2] = (F[6][7]*D[5][7] + F[6][8]*D[5][8] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*Tsq + (D[5][6] + F[6][7]*D[2][7] + F[6][8]*D[2][8] + F[6][9]*D[2][9] + F[6][10]*D[2][10] + F[6][11]*D[2][11] + F[6][12]*D[2][12])*T + D[2][6];
P[2][7] = P[7][2] = (F[7][6]*D[5][6] + F[7][8]*D[5][8] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*Tsq + (D[5][7] + F[7][6]*D[2][6] + F[7][8]*D[2][8] + F[7][9]*D[2][9] + F[7][10]*D[2][10] + F[7][11]*D[2][11] + F[7][12]*D[2][12])*T + D[2][7];
P[2][8] = P[8][2] = (F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*Tsq + (D[5][8] + F[8][6]*D[2][6] + F[8][7]*D[2][7] + F[8][9]*D[2][9] + F[8][10]*D[2][10] + F[8][11]*D[2][11] + F[8][12]*D[2][12])*T + D[2][8];
P[2][9] = P[9][2] = (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12])*Tsq + (D[5][9] + F[9][6]*D[2][6] + F[9][7]*D[2][7] + F[9][8]*D[2][8] + F[9][10]*D[2][10] + F[9][11]*D[2][11] + F[9][12]*D[2][12])*T + D[2][9];
P[2][10] = P[10][2] = D[5][10]*T + D[2][10];
P[2][11] = P[11][2] = D[5][11]*T + D[2][11];
P[2][12] = P[12][2] = D[5][12]*T + D[2][12];
P[2][13] = P[13][2] = D[5][13]*T + D[2][13];
P[2][14] = P[14][2] = D[5][14]*T + D[2][14];
P[2][15] = P[15][2] = D[5][15]*T + D[2][15];
P[3][3] = (Q[3]*G[3][3]*G[3][3] + Q[4]*G[3][4]*G[3][4] + Q[5]*G[3][5]*G[3][5] + F[3][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[3][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[3][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[3][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[3][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[3][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[3][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (2*F[3][6]*D[3][6] + 2*F[3][7]*D[3][7] + 2*F[3][8]*D[3][8] + 2*F[3][9]*D[3][9] + 2*F[3][13]*D[3][13] + 2*F[3][14]*D[3][14] + 2*F[3][15]*D[3][15])*T + D[3][3];
P[3][4] = P[4][3] = (F[4][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[4][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[4][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[4][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[4][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[4][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[4][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[4][3]*Q[3] + G[3][4]*G[4][4]*Q[4] + G[3][5]*G[4][5]*Q[5])*Tsq + (F[3][6]*D[4][6] + F[4][6]*D[3][6] + F[3][7]*D[4][7] + F[4][7]*D[3][7] + F[3][8]*D[4][8] + F[4][8]*D[3][8] + F[3][9]*D[4][9] + F[4][9]*D[3][9] + F[3][13]*D[4][13] + F[4][13]*D[3][13] + F[3][14]*D[4][14] + F[4][14]*D[3][14] + F[3][15]*D[4][15] + F[4][15]*D[3][15])*T + D[3][4];
P[3][5] = P[5][3] = (F[5][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[5][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[5][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[5][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[5][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[5][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[5][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[5][3]*Q[3] + G[3][4]*G[5][4]*Q[4] + G[3][5]*G[5][5]*Q[5])*Tsq + (F[3][6]*D[5][6] + F[5][6]*D[3][6] + F[3][7]*D[5][7] + F[5][7]*D[3][7] + F[3][8]*D[5][8] + F[5][8]*D[3][8] + F[3][9]*D[5][9] + F[5][9]*D[3][9] + F[3][13]*D[5][13] + F[5][13]*D[3][13] + F[3][14]*D[5][14] + F[5][14]*D[3][14] + F[3][15]*D[5][15] + F[5][15]*D[3][15])*T + D[3][5];
P[3][6] = P[6][3] = (F[6][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[6][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[6][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[6][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[6][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[6][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[6][7]*D[3][7] + F[3][8]*D[6][8] + F[6][8]*D[3][8] + F[3][9]*D[6][9] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15])*T + D[3][6];
P[3][7] = P[7][3] = (F[7][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[7][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[7][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[7][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[7][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[7][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][7] + F[7][6]*D[3][6] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[7][8]*D[3][8] + F[3][9]*D[7][9] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15])*T + D[3][7];
P[3][8] = P[8][3] = (F[8][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[8][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[8][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[8][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[8][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[8][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]))*Tsq + (F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15])*T + D[3][8];
P[3][9] = P[9][3] = (F[9][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[9][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[9][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[9][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[9][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[9][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[3][9]*D[9][9] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9])*T + D[3][9];
P[3][10] = P[10][3] = (F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10])*T + D[3][10];
P[3][11] = P[11][3] = (F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11])*T + D[3][11];
P[3][12] = P[12][3] = (F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12])*T + D[3][12];
P[3][13] = P[13][3] = (F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13])*T + D[3][13];
P[3][14] = P[14][3] = (F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14])*T + D[3][14];
P[3][15] = P[15][3] = (F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15])*T + D[3][15];
P[4][4] = (Q[3]*G[4][3]*G[4][3] + Q[4]*G[4][4]*G[4][4] + Q[5]*G[4][5]*G[4][5] + F[4][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[4][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[4][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[4][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[4][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[4][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[4][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (2*F[4][6]*D[4][6] + 2*F[4][7]*D[4][7] + 2*F[4][8]*D[4][8] + 2*F[4][9]*D[4][9] + 2*F[4][13]*D[4][13] + 2*F[4][14]*D[4][14] + 2*F[4][15]*D[4][15])*T + D[4][4];
P[4][5] = P[5][4] = (F[5][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[5][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[5][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[5][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[5][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[5][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[5][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + G[4][3]*G[5][3]*Q[3] + G[4][4]*G[5][4]*Q[4] + G[4][5]*G[5][5]*Q[5])*Tsq + (F[4][6]*D[5][6] + F[5][6]*D[4][6] + F[4][7]*D[5][7] + F[5][7]*D[4][7] + F[4][8]*D[5][8] + F[5][8]*D[4][8] + F[4][9]*D[5][9] + F[5][9]*D[4][9] + F[4][13]*D[5][13] + F[5][13]*D[4][13] + F[4][14]*D[5][14] + F[5][14]*D[4][14] + F[4][15]*D[5][15] + F[5][15]*D[4][15])*T + D[4][5];
P[4][6] = P[6][4] = (F[6][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[6][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[6][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[6][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[6][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[6][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[6][7]*D[4][7] + F[4][8]*D[6][8] + F[6][8]*D[4][8] + F[4][9]*D[6][9] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15])*T + D[4][6];
P[4][7] = P[7][4] = (F[7][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[7][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[7][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[7][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[7][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[7][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][7] + F[7][6]*D[4][6] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[7][8]*D[4][8] + F[4][9]*D[7][9] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15])*T + D[4][7];
P[4][8] = P[8][4] = (F[8][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[8][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[8][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[8][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[8][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[8][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]))*Tsq + (F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15])*T + D[4][8];
P[4][9] = P[9][4] = (F[9][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[9][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[9][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[9][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[9][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[9][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[4][9]*D[9][9] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9])*T + D[4][9];
P[4][10] = P[10][4] = (F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10])*T + D[4][10];
P[4][11] = P[11][4] = (F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11])*T + D[4][11];
P[4][12] = P[12][4] = (F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12])*T + D[4][12];
P[4][13] = P[13][4] = (F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13])*T + D[4][13];
P[4][14] = P[14][4] = (F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14])*T + D[4][14];
P[4][15] = P[15][4] = (F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15])*T + D[4][15];
P[5][5] = (Q[3]*G[5][3]*G[5][3] + Q[4]*G[5][4]*G[5][4] + Q[5]*G[5][5]*G[5][5] + F[5][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[5][13]*(F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13]) + F[5][14]*(F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14]) + F[5][15]*(F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15]) + F[5][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[5][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[5][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (2*F[5][6]*D[5][6] + 2*F[5][7]*D[5][7] + 2*F[5][8]*D[5][8] + 2*F[5][9]*D[5][9] + 2*F[5][13]*D[5][13] + 2*F[5][14]*D[5][14] + 2*F[5][15]*D[5][15])*T + D[5][5];
P[5][6] = P[6][5] = (F[6][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[6][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[6][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[6][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[6][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[6][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[6][7]*D[5][7] + F[5][8]*D[6][8] + F[6][8]*D[5][8] + F[5][9]*D[6][9] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15])*T + D[5][6];
P[5][7] = P[7][5] = (F[7][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[7][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[7][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[7][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[7][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[7][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][7] + F[7][6]*D[5][6] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[7][8]*D[5][8] + F[5][9]*D[7][9] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15])*T + D[5][7];
P[5][8] = P[8][5] = (F[8][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[8][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[8][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[8][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[8][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[8][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]))*Tsq + (F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15])*T + D[5][8];
P[5][9] = P[9][5] = (F[9][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[9][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[9][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[9][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[9][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[9][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[5][9]*D[9][9] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9])*T + D[5][9];
P[5][10] = P[10][5] = (F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10])*T + D[5][10];
P[5][11] = P[11][5] = (F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11])*T + D[5][11];
P[5][12] = P[12][5] = (F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12])*T + D[5][12];
P[5][13] = P[13][5] = (F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13])*T + D[5][13];
P[5][14] = P[14][5] = (F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14])*T + D[5][14];
P[5][15] = P[15][5] = (F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15])*T + D[5][15];
P[6][6] = (Q[0]*G[6][0]*G[6][0] + Q[1]*G[6][1]*G[6][1] + Q[2]*G[6][2]*G[6][2] + F[6][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[6][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[6][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[6][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[6][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[6][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]))*Tsq + (2*F[6][7]*D[6][7] + 2*F[6][8]*D[6][8] + 2*F[6][9]*D[6][9] + 2*F[6][10]*D[6][10] + 2*F[6][11]*D[6][11] + 2*F[6][12]*D[6][12])*T + D[6][6];
P[6][7] = P[7][6] = (F[7][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[7][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[7][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[7][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[7][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[7][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[6][0]*G[7][0]*Q[0] + G[6][1]*G[7][1]*Q[1] + G[6][2]*G[7][2]*Q[2])*Tsq + (F[7][6]*D[6][6] + F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[7][8]*D[6][8] + F[6][9]*D[7][9] + F[7][9]*D[6][9] + F[6][10]*D[7][10] + F[7][10]*D[6][10] + F[6][11]*D[7][11] + F[7][11]*D[6][11] + F[6][12]*D[7][12] + F[7][12]*D[6][12])*T + D[6][7];
P[6][8] = P[8][6] = (F[8][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[8][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[8][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[8][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[8][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[8][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + G[6][0]*G[8][0]*Q[0] + G[6][1]*G[8][1]*Q[1] + G[6][2]*G[8][2]*Q[2])*Tsq + (F[6][7]*D[7][8] + F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[8][9]*D[6][9] + F[6][10]*D[8][10] + F[8][10]*D[6][10] + F[6][11]*D[8][11] + F[8][11]*D[6][11] + F[6][12]*D[8][12] + F[8][12]*D[6][12])*T + D[6][8];
P[6][9] = P[9][6] = (F[9][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[9][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[9][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[9][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[9][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[9][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[9][0]*G[6][0]*Q[0] + G[9][1]*G[6][1]*Q[1] + G[9][2]*G[6][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][9]*D[9][9] + F[9][10]*D[6][10] + F[6][10]*D[9][10] + F[9][11]*D[6][11] + F[6][11]*D[9][11] + F[9][12]*D[6][12] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9])*T + D[6][9];
P[6][10] = P[10][6] = (F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10])*T + D[6][10];
P[6][11] = P[11][6] = (F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11])*T + D[6][11];
P[6][12] = P[12][6] = (F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12])*T + D[6][12];
P[6][13] = P[13][6] = (F[6][9]*D[9][13] + F[6][10]*D[10][13] + F[6][11]*D[11][13] + F[6][12]*D[12][13] + F[6][7]*D[7][13] + F[6][8]*D[8][13])*T + D[6][13];
P[6][14] = P[14][6] = (F[6][9]*D[9][14] + F[6][10]*D[10][14] + F[6][11]*D[11][14] + F[6][12]*D[12][14] + F[6][7]*D[7][14] + F[6][8]*D[8][14])*T + D[6][14];
P[6][15] = P[15][6] = (F[6][9]*D[9][15] + F[6][10]*D[10][15] + F[6][11]*D[11][15] + F[6][12]*D[12][15] + F[6][7]*D[7][15] + F[6][8]*D[8][15])*T + D[6][15];
P[7][7] = (Q[0]*G[7][0]*G[7][0] + Q[1]*G[7][1]*G[7][1] + Q[2]*G[7][2]*G[7][2] + F[7][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[7][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[7][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[7][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[7][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[7][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]))*Tsq + (2*F[7][6]*D[6][7] + 2*F[7][8]*D[7][8] + 2*F[7][9]*D[7][9] + 2*F[7][10]*D[7][10] + 2*F[7][11]*D[7][11] + 2*F[7][12]*D[7][12])*T + D[7][7];
P[7][8] = P[8][7] = (F[8][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[8][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[8][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[8][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[8][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[8][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + G[7][0]*G[8][0]*Q[0] + G[7][1]*G[8][1]*Q[1] + G[7][2]*G[8][2]*Q[2])*Tsq + (F[7][6]*D[6][8] + F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[8][9]*D[7][9] + F[7][10]*D[8][10] + F[8][10]*D[7][10] + F[7][11]*D[8][11] + F[8][11]*D[7][11] + F[7][12]*D[8][12] + F[8][12]*D[7][12])*T + D[7][8];
P[7][9] = P[9][7] = (F[9][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[9][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[9][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[9][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[9][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[9][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + G[9][0]*G[7][0]*Q[0] + G[9][1]*G[7][1]*Q[1] + G[9][2]*G[7][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][9]*D[9][9] + F[9][10]*D[7][10] + F[7][10]*D[9][10] + F[9][11]*D[7][11] + F[7][11]*D[9][11] + F[9][12]*D[7][12] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9])*T + D[7][9];
P[7][10] = P[10][7] = (F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10])*T + D[7][10];
P[7][11] = P[11][7] = (F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11])*T + D[7][11];
P[7][12] = P[12][7] = (F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12])*T + D[7][12];
P[7][13] = P[13][7] = (F[7][9]*D[9][13] + F[7][10]*D[10][13] + F[7][11]*D[11][13] + F[7][12]*D[12][13] + F[7][6]*D[6][13] + F[7][8]*D[8][13])*T + D[7][13];
P[7][14] = P[14][7] = (F[7][9]*D[9][14] + F[7][10]*D[10][14] + F[7][11]*D[11][14] + F[7][12]*D[12][14] + F[7][6]*D[6][14] + F[7][8]*D[8][14])*T + D[7][14];
P[7][15] = P[15][7] = (F[7][9]*D[9][15] + F[7][10]*D[10][15] + F[7][11]*D[11][15] + F[7][12]*D[12][15] + F[7][6]*D[6][15] + F[7][8]*D[8][15])*T + D[7][15];
P[8][8] = (Q[0]*G[8][0]*G[8][0] + Q[1]*G[8][1]*G[8][1] + Q[2]*G[8][2]*G[8][2] + F[8][9]*(F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[8][11]*D[9][11] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9]) + F[8][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[8][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[8][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[8][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[8][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]))*Tsq + (2*F[8][6]*D[6][8] + 2*F[8][7]*D[7][8] + 2*F[8][9]*D[8][9] + 2*F[8][10]*D[8][10] + 2*F[8][11]*D[8][11] + 2*F[8][12]*D[8][12])*T + D[8][8];
P[8][9] = P[9][8] = (F[9][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[9][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[9][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[9][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[9][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[9][8]*(F[8][6]*D[6][8] + F[8][7]*D[7][8] + F[8][9]*D[8][9] + F[8][10]*D[8][10] + F[8][11]*D[8][11] + F[8][12]*D[8][12]) + G[9][0]*G[8][0]*Q[0] + G[9][1]*G[8][1]*Q[1] + G[9][2]*G[8][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][9]*D[9][9] + F[9][10]*D[8][10] + F[8][10]*D[9][10] + F[9][11]*D[8][11] + F[8][11]*D[9][11] + F[9][12]*D[8][12] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9])*T + D[8][9];
P[8][10] = P[10][8] = (F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10])*T + D[8][10];
P[8][11] = P[11][8] = (F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11])*T + D[8][11];
P[8][12] = P[12][8] = (F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12])*T + D[8][12];
P[8][13] = P[13][8] = (F[8][9]*D[9][13] + F[8][10]*D[10][13] + F[8][11]*D[11][13] + F[8][12]*D[12][13] + F[8][6]*D[6][13] + F[8][7]*D[7][13])*T + D[8][13];
P[8][14] = P[14][8] = (F[8][9]*D[9][14] + F[8][10]*D[10][14] + F[8][11]*D[11][14] + F[8][12]*D[12][14] + F[8][6]*D[6][14] + F[8][7]*D[7][14])*T + D[8][14];
P[8][15] = P[15][8] = (F[8][9]*D[9][15] + F[8][10]*D[10][15] + F[8][11]*D[11][15] + F[8][12]*D[12][15] + F[8][6]*D[6][15] + F[8][7]*D[7][15])*T + D[8][15];
P[9][9] = (Q[0]*G[9][0]*G[9][0] + Q[1]*G[9][1]*G[9][1] + Q[2]*G[9][2]*G[9][2] + F[9][10]*(F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10]) + F[9][11]*(F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11]) + F[9][12]*(F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12]) + F[9][6]*(F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[9][10]*D[6][10] + F[9][11]*D[6][11] + F[9][12]*D[6][12]) + F[9][7]*(F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[9][10]*D[7][10] + F[9][11]*D[7][11] + F[9][12]*D[7][12]) + F[9][8]*(F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[9][10]*D[8][10] + F[9][11]*D[8][11] + F[9][12]*D[8][12]))*Tsq + (2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12] + 2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9])*T + D[9][9];
P[9][10] = P[10][9] = (F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10])*T + D[9][10];
P[9][11] = P[11][9] = (F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11])*T + D[9][11];
P[9][12] = P[12][9] = (F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12])*T + D[9][12];
P[9][13] = P[13][9] = (F[9][10]*D[10][13] + F[9][11]*D[11][13] + F[9][12]*D[12][13] + F[9][6]*D[6][13] + F[9][7]*D[7][13] + F[9][8]*D[8][13])*T + D[9][13];
P[9][14] = P[14][9] = (F[9][10]*D[10][14] + F[9][11]*D[11][14] + F[9][12]*D[12][14] + F[9][6]*D[6][14] + F[9][7]*D[7][14] + F[9][8]*D[8][14])*T + D[9][14];
P[9][15] = P[15][9] = (F[9][10]*D[10][15] + F[9][11]*D[11][15] + F[9][12]*D[12][15] + F[9][6]*D[6][15] + F[9][7]*D[7][15] + F[9][8]*D[8][15])*T + D[9][15];
P[10][10] = Q[6]*Tsq + D[10][10];
P[10][11] = P[11][10] = D[10][11];
P[10][12] = P[12][10] = D[10][12];
P[10][13] = P[13][10] = D[10][13];
P[10][14] = P[14][10] = D[10][14];
P[10][15] = P[15][10] = D[10][15];
P[11][11] = Q[7]*Tsq + D[11][11];
P[11][12] = P[12][11] = D[11][12];
P[11][13] = P[13][11] = D[11][13];
P[11][14] = P[14][11] = D[11][14];
P[11][15] = P[15][11] = D[11][15];
P[12][12] = Q[8]*Tsq + D[12][12];
P[12][13] = P[13][12] = D[12][13];
P[12][14] = P[14][12] = D[12][14];
P[12][15] = P[15][12] = D[12][15];
P[13][13] = Q[9]*Tsq + D[13][13];
P[13][14] = P[14][13] = D[13][14];
P[13][15] = P[15][13] = D[13][15];
P[14][14] = Q[10]*Tsq + D[14][14];
P[14][15] = P[15][14] = D[14][15];
P[15][15] = Q[11]*Tsq + D[14][15];
}
#endif
// ************* SerialUpdate *******************
// Does the update step of the Kalman filter for the covariance and estimate
// Outputs are Xnew & Pnew, and are written over P and X
// Z is actual measurement, Y is predicted measurement
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
// where K=P*H'*inv[H*P*H'+R]
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
// i.e. the measurment noises are uncorrelated.
// It therefore uses a serial update that requires no matrix inversion by
// processing the measurements one at a time.
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
// The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update.
// ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed)
{
float HP[NUMX], HPHR, Error;
uint8_t i, j, k, m;
for (m = 0; m < NUMV; m++) {
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0;
for (k = 0; k < NUMX; k++)
HP[j] += H[m][k] * P[k][j];
}
HPHR = R[m]; // Find HPHR = H*P*H' + R
for (k = 0; k < NUMX; k++)
HPHR += HP[k] * H[m][k];
for (k = 0; k < NUMX; k++)
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++)
P[i][j] = P[j][i] =
P[i][j] - K[i][m] * HP[j];
}
Error = Z[m] - Y[m];
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
X[i] = X[i] + K[i][m] * Error;
}
}
}
// ************* RungeKutta **********************
// Does a 4th order Runge Kutta numerical integration step
// Output, Xnew, is written over X
// NOTE the algorithm assumes time invariant state equations and
// constant inputs over integration step
// ************************************************
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{
float dT2 =
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i = 0; i < NUMX; i++)
Xlast[i] = X[i]; // make a working copy
StateEq(X, U, K1); // k1 = f(x,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K1[i];
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K2[i];
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT * K3[i];
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i = 0; i < NUMX; i++)
X[i] =
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
K4[i]) / 6;
}
// ************* Model Specific Stuff ***************************
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
//
// State Variables = [Pos Vel Quaternion GyroBias AccelBias]
// Deterministic Inputs = [AngularVel Accel]
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise]
//
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
// Inputs to Measurement = [EarthFrameMagField]
//
// Notes: Pos and Vel in earth frame
// AngularVel and Accel in body frame
// MagFields are unit vectors
// Xdot is output of StateEq()
// F and G are outputs of LinearizeFG(), all elements not set should be zero
// y is output of OutputEq()
// H is output of LinearizeH(), all elements not set should be zero
// ************************************************
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
ax=U[3]-X[13];
ay=U[4]-X[14];
az=U[5]-X[15]; // subtract the biases on accels
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
Xdot[0] = X[3];
Xdot[1] = X[4];
Xdot[2] = X[5];
// Vdot = Reb*a
Xdot[3] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
q0 * q3) *
ay + 2 * (q1 * q3 + q0 * q2) * az;
Xdot[4] =
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
q3 * q3) * ay + 2 * (q2 * q3 -
q0 * q1) *
az;
Xdot[5] =
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
// best guess is that bias stays constant
Xdot[10] = Xdot[11] = Xdot[12] = 0;
Xdot[13] = Xdot[14] = Xdot[15] = 0;
}
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
F[0][3] = F[1][4] = F[2][5] = 1;
// dVdot/dq
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
// dVdot/dabias & dVdot/dna
F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dq
F[6][6] = 0;
F[6][7] = -wx / 2;
F[6][8] = -wy / 2;
F[6][9] = -wz / 2;
F[7][6] = wx / 2;
F[7][7] = 0;
F[7][8] = wz / 2;
F[7][9] = -wy / 2;
F[8][6] = wy / 2;
F[8][7] = -wz / 2;
F[8][8] = 0;
F[8][9] = wx / 2;
F[9][6] = wz / 2;
F[9][7] = wy / 2;
F[9][8] = -wx / 2;
F[9][9] = 0;
// dqdot/dwbias
F[6][10] = q1 / 2;
F[6][11] = q2 / 2;
F[6][12] = q3 / 2;
F[7][10] = -q0 / 2;
F[7][11] = q3 / 2;
F[7][12] = -q2 / 2;
F[8][10] = -q3 / 2;
F[8][11] = -q0 / 2;
F[8][12] = q1 / 2;
F[9][10] = q2 / 2;
F[9][11] = -q1 / 2;
F[9][12] = -q0 / 2;
// dVdot/dna - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
//G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3); G[3][5]=-2*(q1*q3+q0*q2);
//G[4][3]=-2*(q1*q2+q0*q3); G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; G[4][5]=2*(-q2*q3+q0*q1);
//G[5][3]=2*(-q1*q3+q0*q2); G[5][4]=-2*(q2*q3+q0*q1); G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dnw
G[6][0] = q1 / 2;
G[6][1] = q2 / 2;
G[6][2] = q3 / 2;
G[7][0] = -q0 / 2;
G[7][1] = q3 / 2;
G[7][2] = -q2 / 2;
G[8][0] = -q3 / 2;
G[8][1] = -q0 / 2;
G[8][2] = q1 / 2;
G[9][0] = q2 / 2;
G[9][1] = -q1 / 2;
G[9][2] = -q0 / 2;
// dwbias = random walk noise
G[10][6] = G[11][7] = G[12][8] = 1;
// dabias = random walk noise
G[13][9] = G[14][10] = G[15][11] = 1;
}
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// first six outputs are P and V
Y[0] = X[0];
Y[1] = X[1];
Y[2] = X[2];
Y[3] = X[3];
Y[4] = X[4];
Y[5] = X[5];
// Bb=Rbe*Be
Y[6] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
q0 * q2) * Be[2];
Y[7] =
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
q2 * q2 - q3 * q3) * Be[1] +
2 * (q2 * q3 + q0 * q1) * Be[2];
Y[8] =
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
q0 * q1) * Be[1] +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
// Alt = -Pz
Y[9] = -X[2];
}
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// dP/dP=I;
H[0][0] = H[1][1] = H[2][2] = 1;
// dV/dV=I;
H[3][3] = H[4][4] = H[5][5] = 1;
// dBb/dq
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
// dAlt/dPz = -1
H[9][2] = -1;
}
/**
* @}
* @}
*/

118
flight/AHRS/pios_board.c
View File

@ -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
}

37
flight/AHRS/test.c
View File

@ -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;
}

422
flight/Bootloaders/AHRS/Makefile
View File

@ -1,422 +0,0 @@
#####
# Project: OpenPilot AHRS_BOOTLOADER
#
#
# Makefile for OpenPilot AHRS_BOOTLOADER project
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := bl_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
else
REMOVE_CMD = rm
endif
FLASH_TOOL = OPENOCD
# Paths
AHRS_BL = ./
AHRS_BLINC = $(AHRS_BL)/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
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 += pios_board.c
SRC += bl_fsm.c
#SRC += insgps.c
#SRC += $(FLIGHTLIB)/CoordinateConversions.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_gpio.c
SRC += $(PIOSSTM32F10X)/pios_spi.c
## 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)/printf-stdarg.c
SRC += $(PIOSCOMMON)/pios_bl_helper.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
## 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 += $(AHRS_BLINC)
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)
# Provide (only) the bootloader with board-specific defines
BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS =
endif
CFLAGS += -g$(DEBUGF)
CFLAGS += -O$(OPT)
ifeq ($(DEBUG),NO)
CFLAGS += -fdata-sections -ffunction-sections
endif
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(BLONLY_CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
CFLAGS += -Werror
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
ifeq ($(DEBUG),NO)
LDFLAGS += -Wl,-static
endif
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
# 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
# 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

34
flight/Bootloaders/AHRS/ahrs_slave_test.c
View File

@ -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);
}

65
flight/Bootloaders/AHRS/ahrs_spi_program.c
View File

@ -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);
}

131
flight/Bootloaders/AHRS/ahrs_spi_program_master.c
View File

@ -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);
}

137
flight/Bootloaders/AHRS/ahrs_spi_program_slave.c
View File

@ -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;
}
}

591
flight/Bootloaders/AHRS/bl_fsm.c
View File

@ -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);
}
}

52
flight/Bootloaders/AHRS/inc/ahrs_bl.h
View File

@ -1,52 +0,0 @@
/**
******************************************************************************
*
* @file ahrs_bl.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main AHRS_BL header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_BL_H
#define AHRS_BL_H
/* PIOS Includes */
#include <pios.h>
/** Start programming
returns: true if FLASH erased and ready to program
*/
bool
StartProgramming(void);
/** Write a block to FLASH
buffer contains the data to be written
returns: true if FLASH programmed correctly
*/
bool
WriteData(uint32_t offset, uint8_t *buffer, uint32_t size);
/** Read a block from FLASH
returns: true if FLASH read correctly.
Buffer is set to the read data
*/
bool
ReadData(uint32_t offset, uint8_t *buffer, uint32_t size);
#endif /* AHRS_BL_H */

65
flight/Bootloaders/AHRS/inc/ahrs_spi_program.h
View File

@ -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

66
flight/Bootloaders/AHRS/inc/ahrs_spi_program_master.h
View File

@ -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

35
flight/Bootloaders/AHRS/inc/ahrs_spi_program_slave.h
View File

@ -1,35 +0,0 @@
/**
******************************************************************************
*
* @file ahrs_spi_program_slave.h
* @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
*/
#ifndef AHRS_SPI_PROGRAM_SLAVE_H
#define AHRS_SPI_PROGRAM_SLAVE_H
/** Check if OpenPilot is trying to program AHRS
* If so, it will program the FLASH then return
* If not it just returns.
*/
void AhrsProgramReceive(uint32_t spi_id);
#endif //AHRS_PROGRAM_SLAVE_H

97
flight/Bootloaders/AHRS/inc/bl_fsm.h
View File

@ -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 */

41
flight/Bootloaders/AHRS/inc/pios_config.h
View File

@ -1,41 +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_DELAY
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_IAP
#endif /* PIOS_CONFIG_H */

268
flight/Bootloaders/AHRS/main.c
View File

@ -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;
}
}

57
flight/Bootloaders/AHRS/pios_board.c
View File

@ -1,57 +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>
#include "bl_fsm.h" /* lfsm_* */
static bool board_init_complete = false;
void PIOS_Board_Init() {
if (board_init_complete) {
return;
}
#if defined(PIOS_INCLUDE_LED)
PIOS_LED_Init(&pios_led_cfg);
#endif /* PIOS_INCLUDE_LED */
/* Set up the SPI interface to the OP board */
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
PIOS_DEBUG_Assert(0);
}
lfsm_attach(pios_spi_op_id);
lfsm_init();
board_init_complete = true;
}

5
flight/Bootloaders/BootloaderUpdater/pios_board.c
View File

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

6
flight/Bootloaders/CopterControl/Makefile
View File

@ -104,6 +104,8 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
SRC += $(PIOSSTM32F10X)/pios_irq.c SRC += $(PIOSSTM32F10X)/pios_irq.c
SRC += $(PIOSSTM32F10X)/pios_debug.c SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.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) # PIOS USB related files (seperated to make code maintenance more easy)
SRC += $(PIOSSTM32F10X)/pios_usb.c SRC += $(PIOSSTM32F10X)/pios_usb.c
@ -117,8 +119,6 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
## PIOS Hardware (Common) ## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_board_info.c SRC += $(PIOSCOMMON)/pios_board_info.c
SRC += $(PIOSCOMMON)/pios_com_msg.c SRC += $(PIOSCOMMON)/pios_com_msg.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(PIOSCOMMON)/printf-stdarg.c
## Libraries for flight calculations ## Libraries for flight calculations
@ -414,7 +414,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
# Add jtag targets (program and wipe) # 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 .PHONY: elf lss sym hex bin bino
elf: $(OUTDIR)/$(TARGET).elf elf: $(OUTDIR)/$(TARGET).elf

9
flight/Bootloaders/CopterControl/main.c
View File

@ -70,14 +70,13 @@ uint8_t processRX();
void jump_to_app(); void jump_to_app();
int main() { int main() {
PIOS_SYS_Init(); PIOS_SYS_Init();
if (BSL_HOLD_STATE == 0) PIOS_Board_Init();
USB_connected = TRUE;
PIOS_IAP_Init(); PIOS_IAP_Init();
USB_connected = PIOS_USB_CheckAvailable(0);
if (PIOS_IAP_CheckRequest() == TRUE) { if (PIOS_IAP_CheckRequest() == TRUE) {
PIOS_Board_Init();
PIOS_DELAY_WaitmS(1000); PIOS_DELAY_WaitmS(1000);
User_DFU_request = TRUE; User_DFU_request = TRUE;
PIOS_IAP_ClearRequest(); PIOS_IAP_ClearRequest();

31
flight/Bootloaders/CopterControl/pios_board.c
View File

@ -31,7 +31,7 @@
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
*/ */
#include "board_hw_defs.c" #include "board_hw_defs.c"
#include <pios_board_info.h>
#include <pios.h> #include <pios.h>
uint32_t pios_com_telem_usb_id; uint32_t pios_com_telem_usb_id;
@ -59,8 +59,20 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */ /* Initialize the PiOS library */
PIOS_GPIO_Init(); PIOS_GPIO_Init();
const struct pios_board_info * bdinfo = &pios_board_info_blob;
#if defined(PIOS_INCLUDE_LED) #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 */ #endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_USB) #if defined(PIOS_INCLUDE_USB)
@ -71,8 +83,15 @@ void PIOS_Board_Init(void) {
PIOS_USB_DESC_HID_ONLY_Init(); PIOS_USB_DESC_HID_ONLY_Init();
uint32_t pios_usb_id; 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) #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
uint32_t pios_usb_hid_id; uint32_t pios_usb_hid_id;
@ -90,3 +109,7 @@ void PIOS_Board_Init(void) {
board_init_complete = true; board_init_complete = true;
} }
void PIOS_ADC_DMA_Handler()
{
}

491
flight/Bootloaders/OpenPilot/Makefile
View File

@ -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

115
flight/Bootloaders/OpenPilot/inc/common.h
View File

@ -1,115 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotBL OpenPilot BootLoader
* @brief These files contain the code to the OpenPilot MB Bootloader.
*
* @{
* @file common.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This file contains various common defines for the 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
*/
#ifndef COMMON_H_
#define COMMON_H_
//#include "board.h"
typedef enum {
start, keepgoing,
} DownloadAction;
/**************************************************/
/* OP_DFU states */
/**************************************************/
typedef enum {
DFUidle, //0
uploading, //1
wrong_packet_received, //2
too_many_packets, //3
too_few_packets, //4
Last_operation_Success, //5
downloading, //6
BLidle, //7
Last_operation_failed, //8
uploadingStarting, //9
outsideDevCapabilities, //10
CRC_Fail,//11
failed_jump,
//12
} DFUStates;
/**************************************************/
/* OP_DFU commands */
/**************************************************/
typedef enum {
Reserved, //0
Req_Capabilities, //1
Rep_Capabilities, //2
EnterDFU, //3
JumpFW, //4
Reset, //5
Abort_Operation, //6
Upload, //7
Op_END, //8
Download_Req, //9
Download, //10
Status_Request, //11
Status_Rep
//12
} DFUCommands;
typedef enum {
High_Density, Medium_Density
} DeviceType;
/**************************************************/
/* OP_DFU transfer types */
/**************************************************/
typedef enum {
FW, //0
Descript
//2
} DFUTransfer;
/**************************************************/
/* OP_DFU transfer port */
/**************************************************/
typedef enum {
Usb, //0
Serial
//2
} DFUPort;
/**************************************************/
/* OP_DFU programable programable HW types */
/**************************************************/
typedef enum {
Self_flash, //0
Remote_flash_via_spi
//1
} DFUProgType;
/**************************************************/
/* OP_DFU programable sources */
/**************************************************/
#define USB 0
#define SPI 1
#define DownloadDelay 100000
#define MAX_DEL_RETRYS 3
#define MAX_WRI_RETRYS 3
#endif /* COMMON_H_ */

60
flight/Bootloaders/OpenPilot/inc/op_dfu.h
View File

@ -1,60 +0,0 @@
/**
******************************************************************************
*
* @file op_dfu.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
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __OP_DFU_H
#define __OP_DFU_H
#include "common.h"
/* Includes ------------------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
typedef struct {
uint8_t programmingType;
uint8_t readWriteFlags;
uint32_t startOfUserCode;
uint32_t sizeOfCode;
uint8_t sizeOfDescription;
uint8_t BL_Version;
uint16_t devID;
DeviceType devType;
uint32_t FW_Crc;
} Device;
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported define -----------------------------------------------------------*/
#define COMMAND 0
#define COUNT 1
#define DATA 5
/* Exported functions ------------------------------------------------------- */
void processComand(uint8_t *Receive_Buffer);
uint32_t baseOfAdressType(uint8_t type);
uint8_t isBiggerThanAvailable(uint8_t type, uint32_t size);
void OPDfuIni(uint8_t discover);
void DataDownload(DownloadAction);
bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type);
#endif /* __OP_DFU_H */
/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/

58
flight/Bootloaders/OpenPilot/inc/pios_config.h
View File

@ -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 */
/**
* @}
* @}
*/

51
flight/Bootloaders/OpenPilot/inc/pios_usb_board_data.h
View File

@ -1,51 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB definitions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 2
#include "pios_usb_defs.h" /* struct usb_* */
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPENPILOT_MAIN
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPENPILOT_MAIN, USB_OP_BOARD_MODE_BL)
/*
* The bootloader uses a simplified report structure
* BL: <REPORT_ID><DATA>...<DATA>
* FW: <REPORT_ID><LENGTH><DATA>...<DATA>
* This define changes the behaviour in pios_usb_hid.c
*/
#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
#endif /* PIOS_USB_BOARD_DATA_H */

117
flight/Bootloaders/OpenPilot/inc/ssp.h
View File

@ -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

284
flight/Bootloaders/OpenPilot/main.c
View File

@ -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();
}

586
flight/Bootloaders/OpenPilot/op_dfu.c
View File

@ -1,586 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotBL OpenPilot BootLoader
* @brief These files contain the code to the OpenPilot MB Bootloader.
*
* @{
* @file op_dfu.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This file contains the DFU commands handling code
* @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
*/
/* Includes ------------------------------------------------------------------*/
#include "pios.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;
DFUProgType currentProgrammingDestination; //flash, flash_trough spi
uint8_t currentDeviceCanRead;
uint8_t currentDeviceCanWrite;
Device currentDevice;
uint8_t Buffer[64];
uint8_t echoBuffer[64];
uint8_t SendBuffer[64];
uint8_t Command = 0;
uint8_t EchoReqFlag = 0;
uint8_t EchoAnsFlag = 0;
uint8_t StartFlag = 0;
uint32_t Aditionals = 0;
uint32_t SizeOfTransfer = 0;
uint32_t Expected_CRC = 0;
uint8_t SizeOfLastPacket = 0;
uint32_t Next_Packet = 0;
uint8_t TransferType;
uint32_t Count = 0;
uint32_t Data;
uint8_t Data0;
uint8_t Data1;
uint8_t Data2;
uint8_t Data3;
uint8_t offset = 0;
uint32_t aux;
//Download vars
uint32_t downSizeOfLastPacket = 0;
uint32_t downPacketTotal = 0;
uint32_t downPacketCurrent = 0;
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);
uint32_t CalcFirmCRC(void);
void DataDownload(DownloadAction action) {
if ((DeviceState == downloading)) {
uint8_t packetSize;
uint32_t offset;
uint32_t partoffset;
SendBuffer[0] = 0x01;
SendBuffer[1] = Download;
SendBuffer[2] = downPacketCurrent >> 24;
SendBuffer[3] = downPacketCurrent >> 16;
SendBuffer[4] = downPacketCurrent >> 8;
SendBuffer[5] = downPacketCurrent;
if (downPacketCurrent == downPacketTotal - 1) {
packetSize = downSizeOfLastPacket;
} else {
packetSize = 14;
}
for (uint8_t x = 0; x < packetSize; ++x) {
partoffset = (downPacketCurrent * 14 * 4) + (x * 4);
offset = baseOfAdressType(downType) + partoffset;
if (!flash_read(SendBuffer + (6 + x * 4), offset,
currentProgrammingDestination)) {
DeviceState = Last_operation_failed;
}
}
downPacketCurrent = downPacketCurrent + 1;
if (downPacketCurrent > downPacketTotal - 1) {
DeviceState = Last_operation_Success;
Aditionals = (uint32_t) Download;
}
sendData(SendBuffer + 1, 63);
}
}
void processComand(uint8_t *xReceive_Buffer) {
Command = xReceive_Buffer[COMMAND];
#ifdef DEBUG_SSP
char str[63]= {0};
sprintf(str,"Received COMMAND:%d|",Command);
PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
#endif
EchoReqFlag = (Command >> 7);
EchoAnsFlag = (Command >> 6) & 0x01;
StartFlag = (Command >> 5) & 0x01;
Count = xReceive_Buffer[COUNT] << 24;
Count += xReceive_Buffer[COUNT + 1] << 16;
Count += xReceive_Buffer[COUNT + 2] << 8;
Count += xReceive_Buffer[COUNT + 3];
Data = xReceive_Buffer[DATA] << 24;
Data += xReceive_Buffer[DATA + 1] << 16;
Data += xReceive_Buffer[DATA + 2] << 8;
Data += xReceive_Buffer[DATA + 3];
Data0 = xReceive_Buffer[DATA];
Data1 = xReceive_Buffer[DATA + 1];
Data2 = xReceive_Buffer[DATA + 2];
Data3 = xReceive_Buffer[DATA + 3];
Command = Command & 0b00011111;
if (EchoReqFlag == 1) {
memcpy(echoBuffer, Buffer, 64);
}
switch (Command) {
case EnterDFU:
if (((DeviceState == BLidle) && (Data0 < numberOfDevices))
|| (DeviceState == DFUidle)) {
if (Data0 > 0)
OPDfuIni(TRUE);
DeviceState = DFUidle;
currentProgrammingDestination = devicesTable[Data0].programmingType;
currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01;
currentDeviceCanWrite = devicesTable[Data0].readWriteFlags >> 1
& 0x01;
currentDevice = devicesTable[Data0];
uint8_t result = 0;
switch (currentProgrammingDestination) {
case Self_flash:
result = PIOS_BL_HELPER_FLASH_Ini();
break;
case Remote_flash_via_spi:
result = TRUE;
break;
default:
DeviceState = Last_operation_failed;
Aditionals = (uint16_t) Command;
}
if (result != 1) {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
}
break;
case Upload:
if ((DeviceState == DFUidle) || (DeviceState == uploading)) {
if ((StartFlag == 1) && (Next_Packet == 0)) {
TransferType = Data0;
SizeOfTransfer = Count;
Next_Packet = 1;
Expected_CRC = Data2 << 24;
Expected_CRC += Data3 << 16;
Expected_CRC += xReceive_Buffer[DATA + 4] << 8;
Expected_CRC += xReceive_Buffer[DATA + 5];
SizeOfLastPacket = Data1;
if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1)
* 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;
for (int i = 0; i < 5; ++i) {
PIOS_DELAY_WaitmS(1000);
PIOS_OPAHRS_bl_resync();
if (PIOS_OPAHRS_bl_FwupStatus(&rsp)
== OPAHRS_RESULT_OK) {
if (rsp.payload.user.v.rsp.fwup_status.status
== started) {
result = TRUE;
break;
} else {
result = FALSE;
break;
}
}
}
break;
default:
break;
}
}
if (result != 1) {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
} else {
DeviceState = uploading;
}
}
} else if ((StartFlag != 1) && (Next_Packet != 0)) {
if (Count > SizeOfTransfer) {
DeviceState = too_many_packets;
Aditionals = Count;
} else if (Count == Next_Packet - 1) {
uint8_t numberOfWords = 14;
if (Count == SizeOfTransfer - 1)//is this the last packet?
{
numberOfWords = SizeOfLastPacket;
}
struct opahrs_msg_v0 rsp;
struct opahrs_msg_v0 req;
uint8_t result = 0;
switch (currentProgrammingDestination) {
case Self_flash:
for (uint8_t x = 0; x < numberOfWords; ++x) {
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];
aux = baseOfAdressType(TransferType) + (uint32_t)(
Count * 14 * 4 + x * 4);
result = 0;
for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
if (result == 0) {
result = (FLASH_ProgramWord(aux, Data)
== FLASH_COMPLETE) ? 1 : 0;
}
}
}
break;
case Remote_flash_via_spi:
for (uint8_t x = 0; x < numberOfWords; ++x) {
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;
break;
}
if (result != 1) {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
++Next_Packet;
} else {
DeviceState = wrong_packet_received;
Aditionals = Count;
}
} else {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
}
break;
case Req_Capabilities:
OPDfuIni(TRUE);
Buffer[0] = 0x01;
Buffer[1] = Rep_Capabilities;
if (Data0 == 0) {
Buffer[2] = 0;
Buffer[3] = 0;
Buffer[4] = 0;
Buffer[5] = 0;
Buffer[6] = 0;
Buffer[7] = numberOfDevices;
uint16_t WRFlags = 0;
for (int x = 0; x < numberOfDevices; ++x) {
WRFlags = ((devicesTable[x].readWriteFlags << (x * 2))
| WRFlags);
}
Buffer[8] = WRFlags >> 8;
Buffer[9] = WRFlags;
} else {
Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24;
Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16;
Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8;
Buffer[5] = devicesTable[Data0 - 1].sizeOfCode;
Buffer[6] = Data0;
Buffer[7] = devicesTable[Data0 - 1].BL_Version;
Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription;
Buffer[9] = devicesTable[Data0 - 1].devID;
Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24;
Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16;
Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8;
Buffer[13] = devicesTable[Data0 - 1].FW_Crc;
Buffer[14] = devicesTable[Data0 - 1].devID >> 8;
Buffer[15] = devicesTable[Data0 - 1].devID;
}
sendData(Buffer + 1, 63);
break;
case JumpFW:
if (numberOfDevices > 1) {
struct opahrs_msg_v0 rsp;
PIOS_OPAHRS_bl_boot(0);
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;
}
break;
case Reset:
PIOS_SYS_Reset();
break;
case Abort_Operation:
Next_Packet = 0;
DeviceState = DFUidle;
break;
case Op_END:
if (DeviceState == uploading) {
if (Next_Packet - 1 == SizeOfTransfer) {
Next_Packet = 0;
if ((TransferType != FW) || (Expected_CRC == CalcFirmCRC())) {
DeviceState = Last_operation_Success;
} else {
DeviceState = CRC_Fail;
}
}
if (Next_Packet - 1 < SizeOfTransfer) {
Next_Packet = 0;
DeviceState = too_few_packets;
}
}
break;
case Download_Req:
#ifdef DEBUG_SSP
sprintf(str,"COMMAND:DOWNLOAD_REQ 1 Status=%d|",DeviceState);
PIOS_COM_SendString(PIOS_COM_TELEM_USB,str);
#endif
if (DeviceState == DFUidle) {
#ifdef DEBUG_SSP
PIOS_COM_SendString(PIOS_COM_TELEM_USB,"COMMAND:DOWNLOAD_REQ 1|");
#endif
downType = Data0;
downPacketTotal = Count;
downSizeOfLastPacket = Data1;
if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14
+ downSizeOfLastPacket) == 1) {
DeviceState = outsideDevCapabilities;
Aditionals = (uint32_t) Command;
} else {
downPacketCurrent = 0;
DeviceState = downloading;
}
} else {
DeviceState = Last_operation_failed;
Aditionals = (uint32_t) Command;
}
break;
case Status_Request:
Buffer[0] = 0x01;
Buffer[1] = Status_Rep;
if (DeviceState == wrong_packet_received) {
Buffer[2] = Aditionals >> 24;
Buffer[3] = Aditionals >> 16;
Buffer[4] = Aditionals >> 8;
Buffer[5] = Aditionals;
} else {
Buffer[2] = 0;
Buffer[3] = ((uint16_t) Aditionals) >> 8;
Buffer[4] = ((uint16_t) Aditionals);
Buffer[5] = 0;
}
Buffer[6] = DeviceState;
Buffer[7] = 0;
Buffer[8] = 0;
Buffer[9] = 0;
sendData(Buffer + 1, 63);
if (DeviceState == Last_operation_Success) {
DeviceState = DFUidle;
}
break;
case Status_Rep:
break;
}
if (EchoReqFlag == 1) {
echoBuffer[1] = echoBuffer[1] | EchoAnsFlag;
sendData(echoBuffer + 1, 63);
}
return;
}
void OPDfuIni(uint8_t discover) {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
Device dev;
dev.programmingType = Self_flash;
dev.readWriteFlags = (BOARD_READABLE | (BOARD_WRITABLE << 1));
dev.startOfUserCode = bdinfo->fw_base;
dev.sizeOfCode = bdinfo->fw_size;
dev.sizeOfDescription = bdinfo->desc_size;
dev.BL_Version = bdinfo->bl_rev;
dev.FW_Crc = CalcFirmCRC();
dev.devID = (bdinfo->board_type << 8) | (bdinfo->board_rev);
dev.devType = bdinfo->hw_type;
numberOfDevices = 1;
devicesTable[0] = dev;
if (discover) {
uint8_t found_spi_device = FALSE;
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) {
case FW:
return currentDevice.startOfUserCode;
break;
case Descript:
return currentDevice.startOfUserCode + currentDevice.sizeOfCode;
break;
default:
return 0;
}
}
uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) {
switch (type) {
case FW:
return (size > currentDevice.sizeOfCode) ? 1 : 0;
break;
case Descript:
return (size > currentDevice.sizeOfDescription) ? 1 : 0;
break;
default:
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:
return 0;
break;
}
}
void sendData(uint8_t * buf, uint16_t size) {
if (ProgPort == Usb) {
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;
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;
break;
default:
return FALSE;
}
}

128
flight/Bootloaders/OpenPilot/pios_board.c
View File

@ -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;
}
/**
* @}
*/

119
flight/Bootloaders/OpenPilot/pios_usb_board_data.c
View File

@ -1,119 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB definitions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios_usb_board_data.h" /* struct usb_*, USB_* */
#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
static const uint8_t usb_product_id[20] = {
sizeof(usb_product_id),
USB_DESC_TYPE_STRING,
'O', 0,
'p', 0,
'e', 0,
'n', 0,
'P', 0,
'i', 0,
'l', 0,
'o', 0,
't', 0,
};
static uint8_t usb_serial_number[52] = {
sizeof(usb_serial_number),
USB_DESC_TYPE_STRING,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0,
0, 0
};
static const struct usb_string_langid usb_lang_id = {
.bLength = sizeof(usb_lang_id),
.bDescriptorType = USB_DESC_TYPE_STRING,
.bLangID = htousbs(USB_LANGID_ENGLISH_UK),
};
static const uint8_t usb_vendor_id[28] = {
sizeof(usb_vendor_id),
USB_DESC_TYPE_STRING,
'o', 0,
'p', 0,
'e', 0,
'n', 0,
'p', 0,
'i', 0,
'l', 0,
'o', 0,
't', 0,
'.', 0,
'o', 0,
'r', 0,
'g', 0
};
int32_t PIOS_USB_BOARD_DATA_Init(void)
{
/* Load device serial number into serial number string */
uint8_t sn[25];
PIOS_SYS_SerialNumberGet((char *)sn);
for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) {
usb_serial_number[2 + 2 * i] = sn[i];
}
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id));
PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id));
return 0;
}

821
flight/Bootloaders/OpenPilot/ssp.c
View File

@ -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;
}

91
flight/Bootloaders/OpenPilot/ssp_timer.c
View File

@ -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;
}

4
flight/Bootloaders/PipXtreme/Makefile
View File

@ -103,6 +103,8 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
SRC += $(PIOSSTM32F10X)/pios_irq.c SRC += $(PIOSSTM32F10X)/pios_irq.c
SRC += $(PIOSSTM32F10X)/pios_debug.c SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.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) # PIOS USB related files (seperated to make code maintenance more easy)
SRC += $(PIOSSTM32F10X)/pios_usb.c SRC += $(PIOSSTM32F10X)/pios_usb.c
@ -416,7 +418,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
# Add jtag targets (program and wipe) # 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 .PHONY: elf lss sym hex bin bino
elf: $(OUTDIR)/$(TARGET).elf elf: $(OUTDIR)/$(TARGET).elf

20
flight/CopterControl/Makefile
View File

@ -39,6 +39,7 @@ DEBUG ?= NO
# Include objects that are just nice information to show # Include objects that are just nice information to show
DIAGNOSTICS ?= NO DIAGNOSTICS ?= NO
DIAG_TASKS ?= NO
# Set to YES to build a FW version that will erase all flash memory # Set to YES to build a FW version that will erase all flash memory
ERASE_FLASH ?= NO ERASE_FLASH ?= NO
@ -49,7 +50,7 @@ ENABLE_DEBUG_PINS ?= NO
ENABLE_AUX_UART ?= NO ENABLE_AUX_UART ?= NO
# Set to YES when using Code Sourcery toolchain # Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES CODE_SOURCERY ?= NO
# Remove command is different for Code Sourcery on Windows # Remove command is different for Code Sourcery on Windows
ifeq ($(CODE_SOURCERY), YES) ifeq ($(CODE_SOURCERY), YES)
@ -185,7 +186,8 @@ SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
SRC += $(OPUAVSYNTHDIR)/attituderaw.c SRC += $(OPUAVSYNTHDIR)/accels.c
SRC += $(OPUAVSYNTHDIR)/gyros.c
SRC += $(OPUAVSYNTHDIR)/attitudeactual.c SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c SRC += $(OPUAVSYNTHDIR)/i2cstats.c
@ -227,8 +229,9 @@ SRC += $(PIOSSTM32F10X)/pios_gpio.c
SRC += $(PIOSSTM32F10X)/pios_exti.c SRC += $(PIOSSTM32F10X)/pios_exti.c
SRC += $(PIOSSTM32F10X)/pios_rtc.c SRC += $(PIOSSTM32F10X)/pios_rtc.c
SRC += $(PIOSSTM32F10X)/pios_wdg.c SRC += $(PIOSSTM32F10X)/pios_wdg.c
SRC += $(PIOSSTM32F10X)/pios_iap.c
SRC += $(PIOSSTM32F10X)/pios_tim.c SRC += $(PIOSSTM32F10X)/pios_tim.c
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
# PIOS USB related files (separated to make code maintenance more easy) # PIOS USB related files (separated to make code maintenance more easy)
SRC += $(PIOSSTM32F10X)/pios_usb.c SRC += $(PIOSSTM32F10X)/pios_usb.c
@ -244,13 +247,12 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
## PIOS Hardware (Common) ## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_crc.c SRC += $(PIOSCOMMON)/pios_crc.c
SRC += $(PIOSCOMMON)/pios_flashfs_objlist.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_adxl345.c
SRC += $(PIOSCOMMON)/pios_mpu6000.c
SRC += $(PIOSCOMMON)/pios_com.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_rcvr.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.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))) $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
# Add jtag targets (program and wipe) # 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 .PHONY: elf lss sym hex bin bino opfw
elf: $(OUTDIR)/$(TARGET).elf elf: $(OUTDIR)/$(TARGET).elf

2
flight/CopterControl/System/coptercontrol.c
View File

@ -71,7 +71,7 @@ int main()
PIOS_Board_Init(); PIOS_Board_Init();
#ifdef ERASE_FLASH #ifdef ERASE_FLASH
PIOS_Flash_W25X_EraseChip(); PIOS_Flash_Jedec_EraseChip();
#if defined(PIOS_LED_HEARTBEAT) #if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Off(PIOS_LED_HEARTBEAT); PIOS_LED_Off(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */ #endif /* PIOS_LED_HEARTBEAT */

2
flight/CopterControl/System/inc/FreeRTOSConfig.h
View File

@ -31,7 +31,7 @@
#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) #define configTICK_RATE_HZ ( ( portTickType ) 1000 )
#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) #define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 )
#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 48 ) #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 configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 0 #define configUSE_TRACE_FACILITY 0
#define configUSE_16_BIT_TICKS 0 #define configUSE_16_BIT_TICKS 0

12
flight/CopterControl/System/inc/pios_config.h
View File

@ -36,10 +36,10 @@
/* Enable/Disable PiOS Modules */ /* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY #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_IRQ
#define PIOS_INCLUDE_LED #define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_IAP
@ -77,8 +77,8 @@
#define PIOS_INCLUDE_ADXL345 #define PIOS_INCLUDE_ADXL345
#define PIOS_INCLUDE_FLASH #define PIOS_INCLUDE_FLASH
#define PIOS_INCLUDE_MPU6000
#define PIOS_INCLUDE_BMP085 #define PIOS_MPU6000_ACCEL
/* A really shitty setting saving implementation */ /* A really shitty setting saving implementation */
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS

173
flight/CopterControl/System/pios_board.c
View File

@ -43,6 +43,7 @@
#include <manualcontrolsettings.h> #include <manualcontrolsettings.h>
#include <gcsreceiver.h> #include <gcsreceiver.h>
/* One slot per selectable receiver group. /* One slot per selectable receiver group.
* eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
* NOTE: No slot in this map for NONE. * 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_gps_id;
uint32_t pios_com_bridge_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() * PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware * initializes all the core subsystems on this specific hardware
* called from System/openpilot.c * called from System/openpilot.c
*/ */
int32_t init_test;
void PIOS_Board_Init(void) { void PIOS_Board_Init(void) {
/* Delay system */ /* Delay system */
PIOS_DELAY_Init(); 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 */ /* 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 */ /* Initialize UAVObject libraries */
EventDispatcherInitialize(); EventDispatcherInitialize();
@ -95,10 +211,6 @@ void PIOS_Board_Init(void) {
PIOS_RTC_Init(&pios_rtc_main_cfg); PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif #endif
#if defined(PIOS_INCLUDE_LED)
PIOS_LED_Init(&pios_led_cfg);
#endif /* PIOS_INCLUDE_LED */
HwSettingsInitialize(); HwSettingsInitialize();
#ifndef ERASE_FLASH #ifndef ERASE_FLASH
@ -162,7 +274,17 @@ void PIOS_Board_Init(void) {
} }
uint32_t pios_usb_id; 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) #if defined(PIOS_INCLUDE_USB_CDC)
@ -575,8 +697,35 @@ void PIOS_Board_Init(void) {
#else #else
PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ #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(); PIOS_GPIO_Init();
/* Make sure we have at least one telemetry link configured or else fail initialization */ /* Make sure we have at least one telemetry link configured or else fail initialization */

2
flight/Doc/Doxygen/doxygen.cfg
View File

@ -574,7 +574,7 @@ WARN_LOGFILE =
# directories like "/usr/src/myproject". Separate the files or directories # directories like "/usr/src/myproject". Separate the files or directories
# with spaces. # 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 # 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 # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is

431
flight/INS/Makefile
View File

@ -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

129
flight/INS/inc/ins.h
View File

@ -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 */
/**
* @}
* @}
*/

62
flight/INS/inc/pios_config.h
View File

@ -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 */
/**
* @}
* @}
*/

218
flight/INS/ins.c
View File

@ -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;
}
/**
* @}
*/

130
flight/INS/pios_board.c
View File

@ -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 */
}
/**
* @}
* @}
*/

43
flight/Libraries/CoordinateConversions.c
View File

@ -31,16 +31,17 @@
#include <stdint.h> #include <stdint.h>
#include "CoordinateConversions.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 ************ // ****** 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]); sinLat = sin(DEG2RAD * LLA[0]);
sinLon = sin(DEG2RAD * LLA[1]); sinLon = sin(DEG2RAD * LLA[1]);
@ -55,7 +56,7 @@ void LLA2ECEF(double LLA[3], double ECEF[3])
} }
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* // ****** 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. * 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] * 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; uint16_t iter;
#define MAX_ITER 10 // should not take more than 5 for valid coordinates #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 #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 ******** // ****** 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; 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 q2s = q[2] * q[2];
float q3s = q[3] * q[3]; 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; 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; R33 = q0s - q1s - q2s + q3s;
rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 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 ******** // ****** 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]; float diff[3];
LLA2ECEF(LLA, ECEF); 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 ******** // ****** 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]; float diff[3];
@ -239,7 +240,7 @@ void R2Quaternion(float R[3][3], float q[4])
index = i; index = i;
} }
} }
mag = 2*sqrt(mag); mag = 2*sqrtf(mag);
if (index == 0) { if (index == 0) {
q[0] = mag/4; q[0] = mag/4;
@ -373,7 +374,7 @@ void CrossProduct(const float v1[3], const float v2[3], float result[3])
// ****** Vector Magnitude ******** // ****** Vector Magnitude ********
float VectorMagnitude(const float v[3]) 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]));
} }
/** /**

35
flight/Libraries/ahrs_comm_objects.c
View File

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

16
flight/Libraries/ahrs_spi_comm.c
View File

@ -33,7 +33,7 @@
#include "pios_spi.h" #include "pios_spi.h"
#include "pios_irq.h" #include "pios_irq.h"
#include "ahrs_spi_program_slave.h" #include "ahrs_spi_program_slave.h"
#include "STM32103CB_AHRS.h" //#include "STM32103CB_AHRS.h"
#endif #endif
/*transmit and receive packet magic numbers. /*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. //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 //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 //Number of transmissions + 1 before we expect to see the data acknowledge
//This is controlled by the SPI hardware. //This is controlled by the SPI hardware.
@ -298,13 +298,11 @@ int32_t AhrsConnectCallBack(AhrsObjHandle obj, AhrsEventCallback cb)
return (0); 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) void SendPacket(void)
{ {
#ifndef IN_AHRS
PIOS_SPI_RC_PinSet(opahrs_spi_id, 0); 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 //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); PIOS_SPI_TransferBlock(opahrs_spi_id, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
} }

532
flight/Libraries/fifo_buffer.c
View File

@ -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;
} }
// ***************************************************************************** // *****************************************************************************

10
flight/Libraries/inc/CoordinateConversions.h
View File

@ -31,12 +31,12 @@
#define COORDINATECONVERSIONS_H_ #define COORDINATECONVERSIONS_H_
// ****** convert Lat,Lon,Alt to ECEF ************ // ****** 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!) ********* // ****** 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 // ****** find rotation matrix from rotation vector
void Rv2Rot(float Rv[3], float R[3][3]); 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]); void Quaternion2R(float q[4], float Rbe[3][3]);
// ****** Express LLA in a local NED Base Frame ******** // ****** 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 ******** // ****** 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 ******** // ****** convert Rotation Matrix to Quaternion ********
// ****** if R converts from e to b, q is rotation from e to b **** // ****** if R converts from e to b, q is rotation from e to b ****

20
flight/Libraries/inc/ahrs_comm_objects.h
View File

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

2
flight/Libraries/inc/ahrs_spi_comm.h
View File

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

130
flight/Libraries/inc/fifo_buffer.h
View File

@ -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

5
flight/Libraries/packet_handler.c
View File

@ -419,15 +419,14 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
if(data->data_handler) if(data->data_handler)
data->data_handler(p->data, p->header.data_size); data->data_handler(p->data, p->header.data_size);
// Release the packet.
PHReleaseTXPacket(h, p);
break; break;
default: default:
break; break;
} }
// Release the packet.
PHReleaseRXPacket(h, p);
return 1; return 1;
} }

24
flight/Modules/AHRSComms/ahrs_comms.c
View File

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

59
flight/Modules/Actuator/actuator.c
View File

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

12
flight/Modules/Altitude/altitude.c
View File

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

273
flight/Modules/Attitude/attitude.c
View File

@ -50,19 +50,21 @@
#include "pios.h" #include "pios.h"
#include "attitude.h" #include "attitude.h"
#include "attituderaw.h" #include "gyros.h"
#include "accels.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attitudesettings.h" #include "attitudesettings.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include "pios_flash_w25x.h" #include <pios_board_info.h>
// Private constants // Private constants
#define STACK_SIZE_BYTES 540 #define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY+3) #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 GYRO_NEUTRAL 1665
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI) #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 float gyro_correct_int[3] = {0,0,0};
static xQueueHandle gyro_queue; 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 void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0; static float accelKi = 0;
@ -124,8 +127,9 @@ int32_t AttitudeStart(void)
int32_t AttitudeInitialize(void) int32_t AttitudeInitialize(void)
{ {
AttitudeActualInitialize(); AttitudeActualInitialize();
AttitudeRawInitialize();
AttitudeSettingsInitialize(); AttitudeSettingsInitialize();
AccelsInitialize();
GyrosInitialize();
// Initialize quaternion // Initialize quaternion
AttitudeActualData attitude; AttitudeActualData attitude;
@ -151,14 +155,6 @@ int32_t AttitudeInitialize(void)
trim_requested = false; 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); AttitudeSettingsConnectCallback(&settingsUpdatedCb);
return 0; return 0;
@ -169,23 +165,42 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
/** /**
* Module thread, should not return. * Module thread, should not return.
*/ */
int32_t accel_test;
int32_t gyro_test;
static void AttitudeTask(void *parameters) static void AttitudeTask(void *parameters)
{ {
uint8_t init = 0; uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); 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 // Set critical error and wait until the accel is producing data
while(PIOS_ADXL345_FifoElements() == 0) { while(PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); 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 // Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle()); settingsUpdatedCb(AttitudeSettingsHandle());
@ -216,27 +231,33 @@ static void AttitudeTask(void *parameters)
} }
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); 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); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else { else {
// Only update attitude when sensor data is good // Only update attitude when sensor data is good
updateAttitude(&attitudeRaw); updateAttitude(&accels, &gyros);
AttitudeRawSet(&attitudeRaw);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
} }
} }
} }
float gyros_passed[3];
/** /**
* Get an update from the sensors * Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here * @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not * @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; struct pios_adxl345_data accel_data;
float gyro[4]; float gyro[4];
@ -252,9 +273,9 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
return -1; return -1;
// First sample is temperature // 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 x = 0;
int32_t y = 0; int32_t y = 0;
@ -268,26 +289,25 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
y += -accel_data.y; y += -accel_data.y;
z += -accel_data.z; z += -accel_data.z;
} while ( (i < 32) && (samples_remaining > 0) ); } 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}; float accel[3] = {(float) x / i, (float) y / i, (float) z / i};
if(rotate) { if(rotate) {
// TODO: rotate sensors too so stabilization is well behaved // TODO: rotate sensors too so stabilization is well behaved
float vec_out[3]; float vec_out[3];
rot_mult(R, accel, vec_out); 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 { } 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) { if (trim_requested) {
@ -301,33 +321,100 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) { if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
trim_samples++; trim_samples++;
// Store the digitally scaled version since that is what we use for bias // 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 // 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) { if(bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias // 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 // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly) // 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; 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; float dT;
portTickType thisSysTime = xTaskGetTickCount(); portTickType thisSysTime = xTaskGetTickCount();
@ -337,48 +424,46 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory // 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 { // scoping variables to save memory
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s // Also accounts for the fact that gyros are in deg/s
float qdot[4]; 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 // Take a time step
q[0] = q[0] + qdot[0]; q[0] = q[0] + qdot[0];
@ -395,7 +480,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
} }
// Renomalize // 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[0] = q[0] / qmag;
q[1] = q[1] / qmag; q[1] = q[1] / qmag;
q[2] = q[2] / qmag; q[2] = q[2] / qmag;

13
flight/Modules/FirmwareIAP/firmwareiap.c
View File

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

1
flight/Modules/GPS/GPS.c
View File

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

23
flight/Modules/GPS/NMEA.c
View File

@ -216,7 +216,7 @@ static float NMEA_real_to_float(char *nmea_real)
} }
/* Convert to float */ /* 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 */ /* 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) { switch (units) {
case 0: case 0:
/* no digits, value is zero so no scaling */ /* no digits, value is zero so no scaling */
break; break;
case 1: /* m */ case 1: /* m */
num_m *= 1e6; /* m000000 */ num_m *= 1000000; /* m000000 */
break; break;
case 2: /* mm */ case 2: /* mm */
num_m *= 1e5; /* mm00000 */ num_m *= 100000; /* mm00000 */
break; break;
case 3: /* mmm */ case 3: /* mmm */
num_m *= 1e4; /* mmm0000 */ num_m *= 10000; /* mmm0000 */
break; break;
case 4: /* mmmm */ case 4: /* mmmm */
num_m *= 1e3; /* mmmm000 */ num_m *= 1000; /* mmmm000 */
break; break;
case 5: /* mmmmm */ case 5: /* mmmmm */
num_m *= 1e2; /* mmmmm00 */ num_m *= 100; /* mmmmm00 */
break; break;
case 6: /* mmmmmm */ case 6: /* mmmmmm */
num_m *= 1e1; /* mmmmmm0 */ num_m *= 10; /* mmmmmm0 */
break; break;
default: default:
/* unhandled format */ /* unhandled format */
num_m = 0; num_m = 0.0f;
break; 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) if (negative)
*latlon *= -1; *latlon *= -1;

6
flight/Modules/Guidance/guidance.c
View File

@ -192,9 +192,9 @@ static void guidanceTask(void *parameters)
NedAccelData accelData; NedAccelData accelData;
NedAccelGet(&accelData); NedAccelGet(&accelData);
// Convert from m/s to cm/s // 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); NedAccelSet(&accelData);

1
flight/Modules/ManualControl/inc/manualcontrol.h
View File

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

66
flight/Modules/ManualControl/manualcontrol.c
View File

@ -44,20 +44,23 @@
#include "flightstatus.h" #include "flightstatus.h"
#include "accessorydesired.h" #include "accessorydesired.h"
#include "receiveractivity.h" #include "receiveractivity.h"
#include "altitudeholddesired.h"
#include "positionactual.h"
#include "baroaltitude.h"
// Private constants // Private constants
#if defined(PIOS_MANUAL_STACK_SIZE) #if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else #else
#define STACK_SIZE_BYTES 824 #define STACK_SIZE_BYTES 1024
#endif #endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_PERIOD_MS 20 #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_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) //safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150 #define CONNECTION_OFFSET 150
@ -79,6 +82,7 @@ static portTickType lastSysTime;
// Private functions // Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void altitudeHoldDesired(ManualControlCommandData * cmd);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode); static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void setArmedIfChanged(uint8_t val); static void setArmedIfChanged(uint8_t val);
@ -383,7 +387,13 @@ static void manualControlTask(void *parameters)
updateStabilizationDesired(&cmd, &settings); updateStabilizationDesired(&cmd, &settings);
break; break;
case FLIGHTMODE_GUIDANCE: case FLIGHTMODE_GUIDANCE:
// TODO: Implement switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
altitudeHoldDesired(&cmd);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
}
break; break;
} }
} }
@ -596,6 +606,52 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
StabilizationDesiredSet(&stabilization); 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. * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/ */

2
flight/Modules/Osd/OsdEtStd/OsdEtStd.c
View File

@ -248,7 +248,7 @@ static bool Read(uint32_t start, uint8_t length, uint8_t * buffer)
cmd[3] = (uint8_t) (start >> 8); cmd[3] = (uint8_t) (start >> 8);
cmd[4] = length; 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) static bool Write(uint32_t start, uint8_t length, const uint8_t * buffer)

7
flight/Modules/PipXtreme/pipxtrememod.c
View File

@ -108,7 +108,7 @@ int32_t PipXtremeModInitialize(void)
const struct pios_board_info * bdinfo = &pios_board_info_blob; const struct pios_board_info * bdinfo = &pios_board_info_blob;
pipxStatus.BoardType= bdinfo->board_type; pipxStatus.BoardType= bdinfo->board_type;
PIOS_BL_HELPER_FLASH_Read_Description(pipxStatus.Description, PIPXSTATUS_DESCRIPTION_NUMELEM); //PIOS_BL_HELPER_FLASH_Read_Description(pipxStatus.Description, PIPXSTATUS_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(pipxStatus.CPUSerial); PIOS_SYS_SerialNumberGetBinary(pipxStatus.CPUSerial);
pipxStatus.BoardRevision= bdinfo->board_rev; pipxStatus.BoardRevision= bdinfo->board_rev;
@ -141,11 +141,6 @@ static void systemTask(void *parameters)
PIOS_SYS_Reset(); PIOS_SYS_Reset();
} }
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars // Initialize vars
idleCounter = 0; idleCounter = 0;
idleCounterClear = 0; idleCounterClear = 0;

59
flight/Modules/RadioComBridge/RadioComBridge.c
View File

@ -57,11 +57,11 @@
#define STATS_UPDATE_PERIOD_MS 500 #define STATS_UPDATE_PERIOD_MS 500
#define RADIOSTATS_UPDATE_PERIOD_MS 500 #define RADIOSTATS_UPDATE_PERIOD_MS 500
#define MAX_LOST_CONTACT_TIME 10 #define MAX_LOST_CONTACT_TIME 10
#define PACKET_QUEUE_SIZE 5 #define PACKET_QUEUE_SIZE 10
#define MAX_PORT_DELAY 200 #define MAX_PORT_DELAY 200
#define EV_PACKET_RECEIVED 0x10 #define EV_PACKET_RECEIVED 0x20
#define EV_SEND_ACK 0x20 #define EV_SEND_ACK 0x40
#define EV_SEND_NACK 0x40 #define EV_SEND_NACK 0x80
// **************** // ****************
// Private types // Private types
@ -137,7 +137,6 @@ static void SendPipXStatus(void);
*/ */
static void StatusHandler(PHPacketHandle p); static void StatusHandler(PHPacketHandle p);
static void PPMHandler(uint16_t *channels); static void PPMHandler(uint16_t *channels);
static void updateSettings();
static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length); static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms); static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port); static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port);
@ -235,8 +234,6 @@ static int32_t RadioComBridgeInitialize(void)
UAVObjConnectQueue(UAVObjGetByName("PipXStatus"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByName("PipXStatus"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByName("GCSReceiver"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByName("GCSReceiver"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
updateSettings();
return 0; return 0;
} }
MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart) MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
@ -387,6 +384,10 @@ static void radioReceiveTask(void *parameters)
// Get a RX packet from the packet handler if required. // Get a RX packet from the packet handler if required.
if (p == NULL) if (p == NULL)
p = PHGetRXPacket(pios_packet_handler); p = PHGetRXPacket(pios_packet_handler);
if(p == NULL) {
vTaskDelay(MAX_PORT_DELAY / portTICK_RATE_MS);
continue;
}
// Receive data from the radio port // Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(data->radio_port, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY); rx_bytes = PIOS_COM_ReceiveBuffer(data->radio_port, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY);
@ -401,6 +402,7 @@ static void radioReceiveTask(void *parameters)
xQueueSend(data->objEventQueue, &ev, portMAX_DELAY); xQueueSend(data->objEventQueue, &ev, portMAX_DELAY);
} else } else
PHReceivePacket(pios_packet_handler, p, false); PHReceivePacket(pios_packet_handler, p, false);
p = NULL;
} }
} }
@ -720,12 +722,6 @@ static void PPMHandler(uint16_t *channels)
rcvr.Channel[i] = channels[i]; rcvr.Channel[i] = channels[i];
// Set the GCSReceiverData object. // Set the GCSReceiverData object.
{
UAVObjMetadata metadata;
UAVObjGetMetadata(GCSReceiverHandle(), &metadata);
metadata.access = ACCESS_READWRITE;
UAVObjSetMetadata(GCSReceiverHandle(), &metadata);
}
GCSReceiverSet(&rcvr); GCSReceiverSet(&rcvr);
} }
@ -768,40 +764,3 @@ static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port)
{ {
h->com_port = com_port; h->com_port = com_port;
} }
static void updateSettings()
{
if (data->com_port) {
#ifdef NEVER
// Retrieve settings
uint8_t speed;
HwSettingsRadioComBridgeSpeedGet(&speed);
// Set port speed
switch (speed) {
case HWSETTINGS_RADIOCOMBRIDGESPEED_2400:
PIOS_COM_ChangeBaud(data->com_port, 2400);
break;
case HWSETTINGS_RADIOCOMBRIDGESPEED_4800:
PIOS_COM_ChangeBaud(data->com_port, 4800);
break;
case HWSETTINGS_RADIOCOMBRIDGESPEED_9600:
PIOS_COM_ChangeBaud(data->com_port, 9600);
break;
case HWSETTINGS_RADIOCOMBRIDGESPEED_19200:
PIOS_COM_ChangeBaud(data->com_port, 19200);
break;
case HWSETTINGS_RADIOCOMBRIDGESPEED_38400:
PIOS_COM_ChangeBaud(data->com_port, 38400);
break;
case HWSETTINGS_RADIOCOMBRIDGESPEED_57600:
PIOS_COM_ChangeBaud(data->com_port, 57600);
break;
case HWSETTINGS_RADIOCOMBRIDGESPEED_115200:
PIOS_COM_ChangeBaud(data->com_port, 115200);
break;
}
#endif
}
}

55
flight/Modules/Stabilization/stabilization.c
View File

@ -38,7 +38,7 @@
#include "ratedesired.h" #include "ratedesired.h"
#include "stabilizationdesired.h" #include "stabilizationdesired.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attituderaw.h" #include "gyros.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "manualcontrol.h" // Just to get a macro #include "manualcontrol.h" // Just to get a macro
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
@ -124,7 +124,7 @@ int32_t StabilizationInitialize()
// Listen for updates. // Listen for updates.
// AttitudeActualConnectQueue(queue); // AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue); GyrosConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb); StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle()); SettingsUpdatedCb(StabilizationSettingsHandle());
@ -140,22 +140,20 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
*/ */
static void stabilizationTask(void* parameters) static void stabilizationTask(void* parameters)
{ {
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev; UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired; ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
RateDesiredData rateDesired; RateDesiredData rateDesired;
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw; GyrosData gyrosData;
FlightStatusData flightStatus; FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL); SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount();
ZeroPids(); ZeroPids();
while(1) { while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
@ -167,16 +165,13 @@ static void stabilizationTask(void* parameters)
continue; 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); FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw); GyrosGet(&gyrosData);
#if defined(DIAGNOSTICS) #if defined(DIAGNOSTICS)
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
@ -216,13 +211,13 @@ static void stabilizationTask(void* parameters)
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
stabDesired.Pitch - attitudeActual.Pitch, stabDesired.Pitch - attitudeActual.Pitch,
stabDesired.Yaw - attitudeActual.Yaw}; 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 #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 *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll;
@ -373,24 +368,24 @@ float ApplyPid(pid_type * pid, const float err)
pid->lastErr = err; pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision // 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) static void ZeroPids(void)
{ {
for(int8_t ct = 0; ct < PID_MAX; ct++) { 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++) 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) 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; return val;
} }
@ -466,7 +461,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
if(settings.GyroTau < 0.0001) if(settings.GyroTau < 0.0001)
gyro_alpha = 0; // not trusting this to resolve to 0 gyro_alpha = 0; // not trusting this to resolve to 0
else else
gyro_alpha = exp(-fakeDt / settings.GyroTau); gyro_alpha = expf(-fakeDt / settings.GyroTau);
} }

14
flight/Modules/System/systemmod.c
View File

@ -384,7 +384,7 @@ static void updateStats()
if (now > lastTickCount) { if (now > lastTickCount) {
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
stats.CPULoad = 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 } //else: TickCount has wrapped, do not calc now
lastTickCount = now; lastTickCount = now;
idleCounterClear = 1; idleCounterClear = 1;
@ -457,11 +457,21 @@ static void updateSystemAlarms()
EventGetStats(&evStats); EventGetStats(&evStats);
UAVObjClearStats(); UAVObjClearStats();
EventClearStats(); 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); AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
} else { } else {
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM); 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);
}
} }
/** /**

2
flight/Modules/Telemetry/telemetry.c
View File

@ -531,8 +531,8 @@ static void updateTelemetryStats()
*/ */
static void updateSettings() static void updateSettings()
{ {
if (telemetryPort) { if (telemetryPort) {
// Retrieve settings // Retrieve settings
uint8_t speed; uint8_t speed;
HwSettingsTelemetrySpeedGet(&speed); HwSettingsTelemetrySpeedGet(&speed);

604
flight/OpenPilot/Makefile
View File

@ -1,604 +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 := 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 to compile for debugging
DEBUG ?= YES
# Include objects that are just nice information to show
DIAGNOSTICS ?= YES
DIAG_TASKS ?= 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
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
# 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
# List of modules to include
OPTMODULES = CameraStab GPS
MODULES = Actuator ManualControl Altitude AHRSComms Stabilization Guidance FirmwareIAP
PYMODULES = FlightPlan
#MODULES = Telemetry Example
#MODULES = Telemetry MK/MKSerial
#MODULES = Telemetry
#MODULES += Osd/OsdEtStd
MODULES += Telemetry
# 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
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
AHRSBOOTLOADER = ../Bootloaders/AHRS/
AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
PYMITE = $(FLIGHTLIB)/PyMite
PYMITELIB = $(PYMITE)/lib
PYMITEPLAT = $(PYMITE)/platform/openpilot
PYMITETOOLS = $(PYMITE)/tools
PYMITEVM = $(PYMITE)/vm
PYMITEINC = $(PYMITEVM)
PYMITEINC += $(PYMITEPLAT)
PYMITEINC += $(OUTDIR)
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
HWDEFSINC = ../board_hw_defs/$(BOARD_NAME)
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
ifndef TESTAPP
## PyMite files and modules
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}}
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/openpilot.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.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)
endif
## 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_adc.c
SRC += $(PIOSSTM32F10X)/pios_servo.c
SRC += $(PIOSSTM32F10X)/pios_i2c.c
SRC += $(PIOSSTM32F10X)/pios_spi.c
SRC += $(PIOSSTM32F10X)/pios_ppm.c
SRC += $(PIOSSTM32F10X)/pios_pwm.c
SRC += $(PIOSSTM32F10X)/pios_dsm.c
SRC += $(PIOSSTM32F10X)/pios_sbus.c
SRC += $(PIOSSTM32F10X)/pios_tim.c
SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.c
SRC += $(PIOSSTM32F10X)/pios_exti.c
SRC += $(PIOSSTM32F10X)/pios_rtc.c
SRC += $(PIOSSTM32F10X)/pios_wdg.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_cdc.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_cdc.c
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_crc.c
SRC += $(PIOSCOMMON)/pios_sdcard.c
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_bmp085.c
SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/taskmonitor.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)/stm32f10x_iwdg.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
## RTOS
SRC += $(RTOSSRCDIR)/list.c
SRC += $(RTOSSRCDIR)/queue.c
SRC += $(RTOSSRCDIR)/tasks.c
## RTOS Portable
SRC += $(RTOSSRCDIR)/portable/GCC/ARM_CM3/port.c
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_2.c
## Dosfs file system
SRC += $(DOSFSDIR)/dosfs.c
SRC += $(DOSFSDIR)/dfs_sdcard.c
## AHRS boot loader comms
SRC += $(AHRSBOOTLOADER)/ahrs_spi_program_master.c
SRC += $(AHRSBOOTLOADER)/ahrs_spi_program.c
## Mass Storage Device
#SRC += $(MSDDIR)/msd.c
#SRC += $(MSDDIR)/msd_bot.c
#SRC += $(MSDDIR)/msd_desc.c
#SRC += $(MSDDIR)/msd_memory.c
#SRC += $(MSDDIR)/msd_scsi.c
#SRC += $(MSDDIR)/msd_scsi_data.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 += $(UAVOBJSYNTHDIR)
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 += $(AHRSBOOTLOADERINC)
EXTRAINCDIRS += $(PYMITEINC)
EXTRAINCDIRS += $(HWDEFSINC)
EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES} ${PYMODULES}, $(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.
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
# Declare all non-optional modules as built-in to force inclusion
CDEFS += ${foreach MOD, ${MODULES}, -DMODULE_$(MOD)_BUILTIN }
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
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
ifeq ($(DIAGNOSTICS),YES)
CFLAGS += -DDIAGNOSTICS
endif
ifeq ($(DIAG_TASKS),YES)
CFLAGS += -DDIAG_TASKS
endif
CFLAGS += -g$(DEBUGF)
CFLAGS += -O$(OPT)
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) -mthumb -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
PYTHON = python
# 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
# Generate intermediate code
# 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
# 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) $(wildcard $(OUTDIR)/*.c)
$(V1) $(REMOVE) $(wildcard $(OUTDIR)/*.h)
$(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

665
flight/OpenPilot/Makefile.posix
View File

@ -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

637
flight/OpenPilot/Makefile.win32
View File

@ -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

210
flight/OpenPilot/System/alarms.c
View File

@ -1,210 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @brief OpenPilot System libraries are available to all OP modules.
* @{
* @file alarms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Library for setting and clearing system alarms
* @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 "alarms.h"
// Private constants
// Private types
// Private variables
static xSemaphoreHandle lock;
// Private functions
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
/**
* Initialize the alarms library
*/
int32_t AlarmsInitialize(void)
{
SystemAlarmsInitialize();
lock = xSemaphoreCreateRecursiveMutex();
return 0;
}
/**
* Set an alarm
* @param alarm The system alarm to be modified
* @param severity The alarm severity
* @return 0 if success, -1 if an error
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
{
return -1;
}
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if ( alarms.Alarm[alarm] != severity )
{
alarms.Alarm[alarm] = severity;
SystemAlarmsSet(&alarms);
}
// Release lock
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* Get an alarm
* @param alarm The system alarm to be read
* @return Alarm severity
*/
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
{
SystemAlarmsData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
{
return 0;
}
// Read alarm
SystemAlarmsGet(&alarms);
return alarms.Alarm[alarm];
}
/**
* Set an alarm to it's default value
* @param alarm The system alarm to be modified
* @return 0 if success, -1 if an error
*/
int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm)
{
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT);
}
/**
* Default all alarms
*/
void AlarmsDefaultAll()
{
uint32_t n;
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
AlarmsDefault(n);
}
}
/**
* Clear an alarm
* @param alarm The system alarm to be modified
* @return 0 if success, -1 if an error
*/
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm)
{
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK);
}
/**
* Clear all alarms
*/
void AlarmsClearAll()
{
uint32_t n;
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
AlarmsClear(n);
}
}
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasWarnings()
{
return hasSeverity(SYSTEMALARMS_ALARM_WARNING);
}
/**
* Check if there are any alarms with error or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasErrors()
{
return hasSeverity(SYSTEMALARMS_ALARM_ERROR);
};
/**
* Check if there are any alarms with critical or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasCritical()
{
return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL);
};
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
uint32_t n;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsGet(&alarms);
// Go through alarms and check if any are of the given severity or higher
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
if ( alarms.Alarm[n] >= severity)
{
xSemaphoreGiveRecursive(lock);
return 1;
}
}
// If this point is reached then no alarms found
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* @}
* @}
*/

99
flight/OpenPilot/System/inc/FreeRTOSConfig.h
View File

@ -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 */

50
flight/OpenPilot/System/inc/alarms.h
View File

@ -1,50 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
* @file alarms.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the alarm library
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ALARMS_H
#define ALARMS_H
#include "systemalarms.h"
#define SYSTEMALARMS_ALARM_DEFAULT SYSTEMALARMS_ALARM_UNINITIALISED
int32_t AlarmsInitialize(void);
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity);
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm);
int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm);
void AlarmsDefaultAll();
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm);
void AlarmsClearAll();
int32_t AlarmsHasWarnings();
int32_t AlarmsHasErrors();
int32_t AlarmsHasCritical();
#endif // ALARMS_H
/**
* @}
* @}
*/

39
flight/OpenPilot/System/inc/op_config.h
View File

@ -1,39 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file op_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief OpenPilot configuration header.
* Compile time config for OpenPilot Application
* @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 OP_CONFIG_H
#define OP_CONFIG_H
#endif /* OP_CONFIG_H */
/**
* @}
* @}
*/

53
flight/OpenPilot/System/inc/openpilot.h
View File

@ -1,53 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @file openpilot.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main OpenPilot 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 OPENPILOT_H
#define OPENPILOT_H
/* PIOS Includes */
#include <pios.h>
/* OpenPilot Libraries */
#include "op_config.h"
#include "utlist.h"
#include "uavobjectmanager.h"
#include "eventdispatcher.h"
#include "alarms.h"
#include "taskmonitor.h"
#include "uavtalk.h"
/* Global Functions */
void OpenPilotInit(void);
#endif /* OPENPILOT_H */
/**
* @}
* @}
*/

91
flight/OpenPilot/System/inc/pios_board_posix.h
View File

@ -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 */

106
flight/OpenPilot/System/inc/pios_config.h
View File

@ -1,106 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @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
/* 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_IAP
#define PIOS_INCLUDE_TIM
#define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_DSM
//#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_PWM
#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_USB
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_USB_CDC
#define PIOS_INCLUDE_BMP085
//#define PIOS_INCLUDE_HCSR04
#define PIOS_INCLUDE_OPAHRS
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_I2C_ESC
#define PIOS_INCLUDE_BL_HELPER
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
/* Enable a priority queue in telemetry */
#define PIOS_TELEM_PRIORITY_QUEUE
/* 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
/* GPS options */
#define PIOS_GPS_SETS_HOMELOCATION
/* PIOS Initcall infrastructure */
#define PIOS_INCLUDE_INITCALL
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/

78
flight/OpenPilot/System/inc/pios_config_posix.h
View File

@ -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 */

45
flight/OpenPilot/System/inc/pios_usb_board_data.h
View File

@ -1,45 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB definitions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 4
#include "pios_usb_defs.h" /* struct usb_* */
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPENPILOT_MAIN
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPENPILOT_MAIN, USB_OP_BOARD_MODE_FW)
#endif /* PIOS_USB_BOARD_DATA_H */

338
flight/OpenPilot/System/openpilot.c
View File

@ -1,338 +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 returns.*/
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
/**
* @}
* @}
*/

440
flight/OpenPilot/System/pios_board.c
View File

@ -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);
}
/**
* @}
*/

197
flight/OpenPilot/System/pios_board_posix.c
View File

@ -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();
}
/**
* @}
*/

100
flight/OpenPilot/Tests/test_BMP085.c
View File

@ -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 */
}

37
flight/OpenPilot/Tests/test_common.c
View File

@ -1,37 +0,0 @@
/**
******************************************************************************
*
* @file test_common.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up ans 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"
/**
* Called by the RTOS when a stack overflow is detected.
*/
void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed portCHAR *pcTaskName )
{
PIOS_DEBUG_Panic("STACK OVERFLOW");
}

89
flight/OpenPilot/Tests/test_cpuload.c
View File

@ -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;
}
}

282
flight/OpenPilot/Tests/test_i2c_PCF8570.c
View File

@ -1,282 +0,0 @@
/**
******************************************************************************
*
* @file openpilot.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up ans 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
*/
/*
* I2C Test: communicate with external PCF8570 ram chip
* For this test to function, PCF8570 chips need to be attached to the I2C port
*
* Blinking Blue LED: No errors detected, test continues
* Blinking Red LED: Error detected, test stopped
*
*/
/* OpenPilot Includes */
#include "openpilot.h"
//#define USE_DEBUG_PINS
#define DEVICE_1_ADDRESS 0x50
#define DEVICE_2_ADDRESS 0x51
#ifdef USE_DEBUG_PINS
#define DEBUG_PIN_TASK1_WAIT 0
#define DEBUG_PIN_TASK1_LOCKED 1
#define DEBUG_PIN_TASK2_WAIT 2
#define DEBUG_PIN_TASK2_LOCKED 3
#define DEBUG_PIN_TRANSFER 4
#define DEBUG_PIN_IDLE 6
#define DEBUG_PIN_ERROR 7
#define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
#define DebugPinLow(x) PIOS_DEBUG_PinLow(x)
#else
#define DebugPinHigh(x)
#define DebugPinLow(x)
#endif
#define MAX_LOCK_WAIT 2 // Time in ms that a thread can normally block I2C
/* Task Priorities */
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
/* Global Variables */
/* Local Variables */
/* Function Prototypes */
static void Task1(void *pvParameters);
static void Task2(void *pvParameters);
/**
* OpenPilot Main function
*/
int main()
{
// Init
PIOS_SYS_Init();
PIOS_DEBUG_Init();
PIOS_DELAY_Init();
PIOS_COM_Init();
PIOS_USB_Init(0);
PIOS_I2C_Init();
// Both leds off
PIOS_LED_Off(LED1);
PIOS_LED_Off(LED2);
// Create task
xTaskCreate(Task1, (signed portCHAR *)"Task1", 1024 , NULL, 1, NULL);
xTaskCreate(Task2, (signed portCHAR *)"Task2", 1024 , NULL, 2, NULL);
// Start the FreeRTOS scheduler
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* If we do get here, it will most likely be because we ran out of heap space. */
return 0;
}
static void OnError(void)
{
PIOS_LED_Off(LED1);
while(1)
{
DebugPinHigh(DEBUG_PIN_ERROR);
PIOS_LED_Toggle(LED2);
vTaskDelay(50 / portTICK_RATE_MS);
DebugPinLow(DEBUG_PIN_ERROR);
}
}
//
// This is a low priority task that will continuously access one I2C device
// Frequently it will release the I2C device so that others can also use I2C
//
static void Task1(void *pvParameters)
{
int i = 0;
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
{
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20\xB0\xB1\xB2", 4) != 0)
OnError();
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
for(;;)
{
i++;
if (i==100)
{
PIOS_LED_Toggle(LED1);
i = 0;
}
DebugPinHigh(DEBUG_PIN_TASK1_WAIT);
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
{
uint8_t buf[20];
DebugPinLow(DEBUG_PIN_TASK1_WAIT);
DebugPinHigh(DEBUG_PIN_TASK1_LOCKED);
// Write A0 A1 A2 at address 0x10
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10\xA0\xA1\xA2", 4) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
// Read 3 bytes at address 0x20 and check
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20", 1) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
if (memcmp(buf, "\xB0\xB1\xB2",3) != 0)
OnError();
// Read 3 bytes at address 0x10 and check
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
if (memcmp(buf, "\xA0\xA1\xA2",3) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TASK1_LOCKED);
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
}
}
// This is a high priority task that will periodically perform some actions on the second I2C device
// Most of the time it will have to wait for the other task task to release I2C
static void Task2(void *pvParameters)
{
portTickType xLastExecutionTime;
xLastExecutionTime = xTaskGetTickCount();
uint32_t count = 0;
for(;;)
{
uint8_t buf[20];
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
{
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
// Write value of count to address 0x10
buf[0] = 0x10; // The address
memcpy(&buf[1], &count, 4); // The data to write
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_2_ADDRESS<<1, buf, 5) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
vTaskDelay(2 / portTICK_RATE_MS);
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
if (PIOS_I2C_LockDevice(1 / portTICK_RATE_MS))
{
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
// Read at address 0x10 and check
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_2_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_2_ADDRESS<<1, buf, 4) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (memcmp(buf, &count, 4) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
vTaskDelay(5 / portTICK_RATE_MS);
count++;
}
}
/**
* Idle hook function
*/
void vApplicationIdleHook(void)
{
/* Called when the scheduler has no tasks to run */
DebugPinHigh(DEBUG_PIN_IDLE);
DebugPinLow(DEBUG_PIN_IDLE);
}

174
flight/OpenPilot/Tests/test_uavobjects.c
View File

@ -1,174 +0,0 @@
/**
******************************************************************************
*
* @file test_uavobjects.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Tests for the UAVObject libraries
* @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 "exampleobject1.h"
#include "uavobjectsinit.h"
// Local functions
static void testTask(void *pvParameters);
static void eventCallback(UAVObjEvent* ev);
static void eventCallbackPeriodic(UAVObjEvent* ev);
// Variables
int testDelay = 0;
int main()
{
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* SPI Init */
PIOS_SPI_Init();
/* Enables the SDCard */
PIOS_SDCARD_Init();
PIOS_SDCARD_MountFS(0);
// Turn on both LEDs
PIOS_LED_On(LED1);
PIOS_LED_On(LED2);
// Create test task
xTaskCreate(testTask, (signed portCHAR *)"ObjTest", 1000 , NULL, 1, NULL);
// Start the FreeRTOS scheduler
vTaskStartScheduler();
return 0;
}
static void testTask(void *pvParameters)
{
ExampleObject1Data data;
// Initialize object and manager
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
// Set data
ExampleObject1Get(&data);
data.Field1 = 1;
data.Field2 = 2;
ExampleObject1Set(&data);
// Get data
memset(&data, 0, sizeof(data));
ExampleObject1Get(&data);
// Benchmark time is takes to get and set data (both operations)
int tickStart = xTaskGetTickCount();
for (int n = 0; n < 10000; ++n)
{
ExampleObject1Set(&data);
ExampleObject1Get(&data);
}
int delay = xTaskGetTickCount() - tickStart;
// Create a new instance and get/set its data
uint16_t instId = ExampleObject1CreateInstance();
ExampleObject1InstSet(instId, &data);
memset(&data, 0, sizeof(data));
ExampleObject1InstGet(instId, &data);
// Test pack/unpack
uint8_t buffer[EXAMPLEOBJECT1_NUMBYTES];
memset(buffer, 0, EXAMPLEOBJECT1_NUMBYTES);
UAVObjPack(ExampleObject1Handle(), 0, buffer);
memset(&data, 0, sizeof(data));
ExampleObject1Set(&data);
UAVObjUnpack(ExampleObject1Handle(), 0, buffer);
ExampleObject1Get(&data);
// Test object saving/loading to SD card
UAVObjSave(ExampleObject1Handle(), 0);
memset(&data, 0, sizeof(data));
ExampleObject1Set(&data);
UAVObjLoad(ExampleObject1Handle(), 0);
ExampleObject1Get(&data);
// Retrieve object handle by ID
UAVObjHandle handle = UAVObjGetByID(EXAMPLEOBJECT1_OBJID);
const char* name = UAVObjGetName(handle);
// Get/Set the metadata
UAVObjMetadata mdata;
ExampleObject1GetMetadata(&mdata);
mdata.gcsTelemetryAcked = 0;
ExampleObject1SetMetadata(&mdata);
memset(&mdata, 0, sizeof(mdata));
ExampleObject1GetMetadata(&mdata);
// Test callbacks
ExampleObject1ConnectCallback(eventCallback);
ExampleObject1Set(&data);
// Test queue events
xQueueHandle queue;
queue = xQueueCreate(10, sizeof(UAVObjEvent));
ExampleObject1ConnectQueue(queue);
// Test periodic events
UAVObjEvent ev;
ev.event = 0;
ev.instId = 0;
ev.obj = ExampleObject1Handle();
EventPeriodicCallbackCreate(&ev, eventCallbackPeriodic, 500);
EventPeriodicQueueCreate(&ev, queue, 250);
// Done testing
while (1)
{
if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
{
name = UAVObjGetName(ev.obj);
PIOS_LED_Toggle(LED2);
}
}
}
static void eventCallback(UAVObjEvent* ev)
{
const char* name = UAVObjGetName(ev->obj);
}
static void eventCallbackPeriodic(UAVObjEvent* ev)
{
static int lastUpdate;
testDelay = xTaskGetTickCount() - lastUpdate;
lastUpdate = xTaskGetTickCount();
const char* name = UAVObjGetName(ev->obj);
PIOS_LED_Toggle(LED1);
//ExampleObject1Updated();
}
void vApplicationIdleHook(void)
{
/* Called when the scheduler has no tasks to run */
}

Some files were not shown because too many files have changed in this diff Show More