rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Merge branch 'next'

Browse Source
This commit is contained in:
James Cotton 2012-07-11 13:51:20 -05:00
commit 0fe18f2078
1332 changed files with 219412 additions and 95653 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

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

3
.gitmodules vendored Normal file
View File

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

51
HISTORY.txt
View File

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

323
Makefile
View File

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

BIN
artwork/Font/OpenPilot.ttf
View File

Binary file not shown.

48
artwork/Misc/multirotor-shapes.svg
View File

File diff suppressed because one or more lines are too long
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 438 KiB

After

Width:  |  Height:  |  Size: 436 KiB

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(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
// Dump raw buffer
result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
adc_oversampling *
PIOS_ADC_NUM_PINS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
#endif
#if defined(PIP_DUMP_RAW)
#define MAX_OVERSAMPLING PIOS_ADC_MAX_OVERSAMPLING
void print_ahrs_raw()
{
int16_t accel_x[MAX_OVERSAMPLING], accel_y[MAX_OVERSAMPLING], accel_z[MAX_OVERSAMPLING];
int16_t gyro_x[MAX_OVERSAMPLING], gyro_y[MAX_OVERSAMPLING], gyro_z[MAX_OVERSAMPLING];
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
int16_t mag[3];
int16_t mag_x, mag_y, mag_z;
#endif
static int previous_conversion = 0;
uint8_t framing[3] = {0xD2, 0x73, 0x00};
// wait for new raw samples
while (previous_conversion == total_conversion_blocks);
if ((previous_conversion + 1) != total_conversion_blocks)
PIOS_LED_On(LED1); // we are not keeping up
previous_conversion = total_conversion_blocks;
// fetch the buffer address for the new samples
int16_t *valid_data_buffer = PIOS_ADC_GetRawBuffer();
// fetch number of raw samples in the buffer (per channel)
int over_sampling = PIOS_ADC_GetOverSampling();
framing[2] = over_sampling;
// copy the raw samples into their own buffers
for (uint16_t i = 0, j = 0; i < over_sampling; i++, j += PIOS_ADC_NUM_CHANNELS)
{
accel_x[i] = valid_data_buffer[ACCEL_RAW_X_IDX + j];
accel_y[i] = valid_data_buffer[ACCEL_RAW_Y_IDX + j];
accel_z[i] = valid_data_buffer[ACCEL_RAW_Z_IDX + j];
gyro_x[i] = valid_data_buffer[GYRO_RAW_X_IDX + j];
gyro_y[i] = valid_data_buffer[GYRO_RAW_Y_IDX + j];
gyro_z[i] = valid_data_buffer[GYRO_RAW_Z_IDX + j];
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if (PIOS_HMC5843_NewDataAvailable())
{
PIOS_HMC5843_ReadMag(mag);
mag_x = mag[MAG_RAW_X_IDX];
mag_y = mag[MAG_RAW_Y_IDX];
mag_z = mag[MAG_RAW_Z_IDX];
}
#endif
// send the raw samples
int result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, framing, sizeof(framing));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_x, over_sampling * sizeof(accel_x[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_y, over_sampling * sizeof(accel_y[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_z, over_sampling * sizeof(accel_z[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_x, over_sampling * sizeof(gyro_x[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_y, over_sampling * sizeof(gyro_y[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0]));
if (result != 0)
PIOS_LED_On(LED1); // all data not sent
else
PIOS_LED_Off(LED1);
}
#endif
extern void PIOS_Board_Init(void);
/**
* @brief AHRS Main function
*/
int main()
{
gps_data.quality = -1;
uint32_t up_time_real = 0;
uint32_t up_time = 0;
uint32_t last_up_time = 0;
static int8_t last_ahrs_algorithm;
uint32_t last_counter_idle_start = 0;
uint32_t last_counter_idle_end = 0;
uint32_t idle_counts = 0;
uint32_t running_counts = 0;
uint32_t counter_val = 0;
ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE;
reset_values();
PIOS_Board_Init();
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
#endif
while(!AhrsLinkReady()) {
AhrsPoll();
}
/* we didn't connect the callbacks before because we have to wait
for all data to be up to date before doing anything*/
AHRSCalibrationConnectCallback(calibration_callback);
GPSPositionConnectCallback(gps_callback);
BaroAltitudeConnectCallback(altitude_callback);
AHRSSettingsConnectCallback(settings_callback);
HomeLocationConnectCallback(homelocation_callback);
FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
calibration_callback(AHRSCalibrationHandle()); //force an update
#if defined(DUMP_RAW) || defined(PIP_DUMP_RAW)
while (1) {
AhrsPoll();
print_ahrs_raw();
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while(1) {
AhrsPoll();
AhrsStatusData status;
AhrsStatusGet(&status);
status.CPULoad = ((float)running_counts /
(float)(idle_counts + running_counts)) * 100;
status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
status.RunningTimePerCyle = running_counts / (timer_rate() / 10000);
status.DroppedUpdates = ekf_too_slow;
up_time = timer_count();
if(up_time >= last_up_time) // normal condition
up_time_real += ((up_time - last_up_time) * 1000) / timer_rate();
else
up_time_real += ((0xFFFF - last_up_time + up_time) * 1000) / timer_rate();
last_up_time = up_time;
status.RunningTime = up_time_real;
AhrsStatusSet(&status);
// Alive signal
if (((total_conversion_blocks % 100) & 0xFFFE) == 0)
PIOS_LED_Toggle(LED1);
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
// This function blocks till data avilable
get_accel_gyro_data();
// Get any mag data available
process_mag_data();
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) ||
ACCEL_OOB(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
GYRO_OOB(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z)) {
// If any values are NaN or huge don't update
//TODO: add field to ahrs status to track number of these events
continue;
}
print_ekf_binary();
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
if (ahrs_algorithm != last_ahrs_algorithm)
ins_init_algorithm();
last_ahrs_algorithm = ahrs_algorithm;
switch(ahrs_algorithm) {
case AHRSSETTINGS_ALGORITHM_SIMPLE:
simple_update();
break;
case AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
ins_outdoor_update();
break;
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR:
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
ins_indoor_update();
break;
}
}
return 0;
}
/**
* @brief Get the accel and gyro data from whichever source when available
*
* This function will act as the HAL for the new INS sensors
*/
bool get_accel_gyro_data()
{
float accel[6];
float gyro[6];
uint16_t spin_count = 1;
while(fifoBuf_getUsed(&adc_fifo_buffer) < (sizeof(accel) + sizeof(gyro))) {
if(spin_count++ == 0)
AhrsPoll();
}
fifoBuf_getData(&adc_fifo_buffer, &accel[0], sizeof(float) * 3);
fifoBuf_getData(&adc_fifo_buffer, &gyro[0], sizeof(float) * 3);
fifoBuf_getData(&adc_fifo_buffer, &accel[3], sizeof(float) * 3);
fifoBuf_getData(&adc_fifo_buffer, &gyro[3], sizeof(float) * 3);
accel_data.filtered.x = (accel[0] + accel[3]) / 2;
accel_data.filtered.y = (accel[1] + accel[4]) / 2;
accel_data.filtered.z = (accel[2] + accel[5]) / 2;
gyro_data.filtered.x = (gyro[0] + gyro[3]) / 2;
gyro_data.filtered.y = (gyro[1] + gyro[4]) / 2;
gyro_data.filtered.z = (gyro[2] + gyro[5]) / 2;
return true;
}
/**
* @brief Perform calibration of a 3-axis field sensor using an affine transformation
* matrix.
*
* Computes result = scale * arg.
*
* @param result[out] The three-axis resultant field.
* @param scale[in] The 4x4 affine transformation matrix. The fourth row is implicitly
* [0 0 0 1]
* @param arg[in] The 3-axis input field. The 'w' component is implicitly 1.
*/
void calibration(float result[3], float scale[3][4], float arg[3])
{
for (int row = 0; row < 3; ++row) {
result[row] = 0.0f;
int col;
for (col = 0; col < 3; ++col) {
result[row] += scale[row][col] * arg[col];
}
// fourth column: arg has an implicit w value of 1.0f.
result[row] += scale[row][col];
}
}
/**
* @brief Scale an affine transformation matrix by a rotation, defined by a
* rotation vector. scale <- rotation * scale
*
* @param scale[in,out] The affine transformation matrix to be rotated
* @param rotation[in] The rotation vector defining the rotation
*/
void affine_rotate(float scale[3][4], float rotation[3])
{
// Rotate the scale factor matrix in-place
float rmatrix[3][3];
Rv2Rot(rotation, rmatrix);
float ret[3][4];
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
ret[row][col] = 0.0f;
for (int i = 0; i < 3; ++i) {
ret[row][col] += rmatrix[row][i] * scale[i][col];
}
}
}
// copy output to argument
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
scale[row][col] = ret[row][col];
}
}
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void adc_callback(float * downsampled_data)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
#if 0
// Get the Y data. Third byte in. Convert to m/s
float accel_filtered[3];
accel_filtered[1] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_filtered[1] += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_filtered[1] /= (float) fir_coeffs[adc_oversampling];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_filtered[0] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_filtered[0] += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_filtered[0] /= (float) fir_coeffs[adc_oversampling];
// Get the Z data. Third byte in. Convert to m/s
accel_filtered[2] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_filtered[2] += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_filtered[2] /= (float) fir_coeffs[adc_oversampling];
float accel_scaled[3];
calibration(accel_scaled, accel_data.calibration.scale, accel_filtered);
accel_data.filtered.x = accel_scaled[0];
accel_data.filtered.y = accel_scaled[1];
accel_data.filtered.z = accel_scaled[2];
#else
float accel[3], gyro[3];
float raw_accel[3] = {
downsampled_data[ACCEL_RAW_X_IDX],
downsampled_data[ACCEL_RAW_Y_IDX],
-downsampled_data[ACCEL_RAW_Z_IDX]
};
// Accel data is (y,x,z) in first third and fifth byte. Convert to m/s^2
calibration(accel, accel_data.calibration.scale, raw_accel);
// Gyro data is (x,y,z) in second, fifth and seventh byte. Convert to rad/s
gyro[0] = ( ( downsampled_data[GYRO_RAW_X_IDX] + gyro_data.calibration.tempcompfactor[0] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
gyro[1] = ( ( downsampled_data[GYRO_RAW_Y_IDX] + gyro_data.calibration.tempcompfactor[1] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
gyro[2] = ( ( downsampled_data[GYRO_RAW_Z_IDX] + gyro_data.calibration.tempcompfactor[2] * downsampled_data[GYRO_TEMP_RAW_Z_IDX] ) * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
#endif
#if 0
static float gravity_tracking[3] = {0,0,0};
const float tau = 0.9999;
gravity_tracking[0] = tau * gravity_tracking[0] + (1-tau) * accel[0];
gravity_tracking[1] = tau * gravity_tracking[1] + (1-tau) * accel[1];
gravity_tracking[2] = tau * gravity_tracking[2] + (1-tau) * (accel[2] + 9.81);
if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
accel[0] -= gravity_tracking[0];
accel[1] -= gravity_tracking[1];
accel[2] -= gravity_tracking[2];
}
#endif
if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
} else {
ekf_too_slow++;
}
AttitudeRawData raw;
raw.gyrotemp[0] = downsampled_data[GYRO_TEMP_RAW_XY_IDX];
raw.gyrotemp[1] = downsampled_data[GYRO_TEMP_RAW_Z_IDX];
raw.gyros[0] = gyro[0] * RAD_TO_DEG;
raw.gyros[1] = gyro[1] * RAD_TO_DEG;
raw.gyros[2] = gyro[2] * RAD_TO_DEG;
raw.accels[0] = accel[0];
raw.accels[1] = accel[1];
raw.accels[2] = accel[2];
raw.magnetometers[0] = mag_data.scaled.axis[0];
raw.magnetometers[1] = mag_data.scaled.axis[1];
raw.magnetometers[2] = mag_data.scaled.axis[2];
if (settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE)
{
raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
raw.accels[0] -= Nav.accel_bias[0];
raw.accels[1] -= Nav.accel_bias[1];
raw.accels[2] -= Nav.accel_bias[2];
}
AttitudeRawSet(&raw);
total_conversion_blocks++;
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/**
* @brief Get the mag data from the I2C sensor and load into structure
* @return none
*
* This function also considers if the home location is set and has a valid
* magnetic field before updating the mag data to prevent data being used that
* cannot be interpreted. In addition the mag data is not used for the first
* five seconds to allow the filter to start to converge
*/
void process_mag_data()
{
// Get magnetic readings
// For now don't use mags until the magnetic field is set AND until 5 seconds
// after initialization otherwise it seems to have problems
// TODO: Follow up this initialization issue
HomeLocationData home;
HomeLocationGet(&home);
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
// Only use if magnetic length reasonable
float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
mag_data.updated = (home.Set == HOMELOCATION_SET_TRUE) &&
((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) &&
((mag_data.raw.axis[MAG_RAW_X_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Y_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Z_IDX] != 0)) &&
((Blen < mag_len * (1 + INSGPS_MAGTOL)) && (Blen > mag_len * (1 - INSGPS_MAGTOL)));
}
}
#else
void process_mag_data() { }
#endif
/**
* @brief Assumes board is not moving computes biases and variances of sensors
* @returns None
*
* All data is stored in global structures. This function should be called from OP when
* aircraft is in stable state and then the data stored to SD card.
*
* After this function the bias for each sensor will be the mean value. This doesn't make
* sense for the z accel so make sure 6 point calibration is also run and those values set
* after these read.
*/
#define NBIAS 100
#define NVAR 500
void calibrate_sensors()
{
int i,j;
float accel_bias[3] = {0, 0, 0};
float gyro_bias[3] = {0, 0, 0};
float mag_bias[3] = {0, 0, 0};
for (i = 0, j = 0; i < NBIAS; i++) {
get_accel_gyro_data();
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
accel_bias[0] += accel_data.filtered.x / NBIAS;
accel_bias[1] += accel_data.filtered.y / NBIAS;
accel_bias[2] += accel_data.filtered.z / NBIAS;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_bias[0] += mag_data.scaled.axis[0];
mag_bias[1] += mag_data.scaled.axis[1];
mag_bias[2] += mag_data.scaled.axis[2];
}
#endif
}
mag_bias[0] /= j;
mag_bias[1] /= j;
mag_bias[2] /= j;
gyro_data.calibration.variance[0] = 0;
gyro_data.calibration.variance[1] = 0;
gyro_data.calibration.variance[2] = 0;
mag_data.calibration.variance[0] = 0;
mag_data.calibration.variance[1] = 0;
mag_data.calibration.variance[2] = 0;
accel_data.calibration.variance[0] = 0;
accel_data.calibration.variance[1] = 0;
accel_data.calibration.variance[2] = 0;
for (i = 0, j = 0; j < NVAR; j++) {
get_accel_gyro_data();
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
}
#endif
}
mag_data.calibration.variance[0] /= j;
mag_data.calibration.variance[1] /= j;
mag_data.calibration.variance[2] /= j;
gyro_data.calibration.bias[0] -= gyro_bias[0];
gyro_data.calibration.bias[1] -= gyro_bias[1];
gyro_data.calibration.bias[2] -= gyro_bias[2];
}
/**
* @brief Populate fields with initial values
*/
void reset_values()
{
accel_data.calibration.scale[0][1] = 0;
accel_data.calibration.scale[1][0] = 0;
accel_data.calibration.scale[0][2] = 0;
accel_data.calibration.scale[2][0] = 0;
accel_data.calibration.scale[1][2] = 0;
accel_data.calibration.scale[2][1] = 0;
accel_data.calibration.scale[0][0] = 0.0359;
accel_data.calibration.scale[1][1] = 0.0359;
accel_data.calibration.scale[2][2] = 0.0359;
accel_data.calibration.scale[0][3] = -73.5;
accel_data.calibration.scale[1][3] = -73.5;
accel_data.calibration.scale[2][3] = -73.5;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
mag_data.calibration.bias[0] = 0;
mag_data.calibration.bias[1] = 0;
mag_data.calibration.bias[2] = 0;
mag_data.calibration.variance[0] = 50;
mag_data.calibration.variance[1] = 50;
mag_data.calibration.variance[2] = 50;
}
void send_attitude(void)
{
AttitudeActualData attitude;
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
attitude.q1 = attitude_data.quaternion.q1;
attitude.q2 = attitude_data.quaternion.q2;
attitude.q3 = attitude_data.quaternion.q3;
attitude.q4 = attitude_data.quaternion.q4;
float rpy[3];
Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
attitude.Roll = rpy[0] + settings.RollBias;
attitude.Pitch = rpy[1] + settings.PitchBias;
attitude.Yaw = rpy[2] + settings.YawBias;
if(attitude.Yaw > 360)
attitude.Yaw -= 360;
AttitudeActualSet(&attitude);
}
void send_velocity(void)
{
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
// convert into cm
velocityActual.North = Nav.Vel[0] * 100;
velocityActual.East = Nav.Vel[1] * 100;
velocityActual.Down = Nav.Vel[2] * 100;
VelocityActualSet(&velocityActual);
}
void send_position(void)
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
// convert into cm
positionActual.North = Nav.Pos[0] * 100;
positionActual.East = Nav.Pos[1] * 100;
positionActual.Down = Nav.Pos[2] * 100;
PositionActualSet(&positionActual);
}
void send_calibration(void)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
for(int ct=0; ct<3; ct++)
{
cal.accel_var[ct] = accel_data.calibration.variance[ct];
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
cal.mag_var[ct] = mag_data.calibration.variance[ct];
}
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}
/**
* @brief AHRS calibration callback
*
* Called when the OP board sets the calibration
*/
void calibration_callback(AhrsObjHandle obj)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
accel_data.calibration.scale[0][1] = cal.accel_ortho[0];
accel_data.calibration.scale[1][0] = cal.accel_ortho[0];
accel_data.calibration.scale[0][2] = cal.accel_ortho[1];
accel_data.calibration.scale[2][0] = cal.accel_ortho[1];
accel_data.calibration.scale[1][2] = cal.accel_ortho[2];
accel_data.calibration.scale[2][1] = cal.accel_ortho[2];
#if 0
// TODO: Enable after v1.0 feature freeze.
float rotation[3] = { cal.accel_rotation[0],
cal.accel_rotation[1],
cal.accel_rotation[2],
};
affine_rotate(accel_data.calibration.scale, rotation);
#endif
for(int ct=0; ct<3; ct++)
{
accel_data.calibration.scale[ct][ct] = cal.accel_scale[ct];
accel_data.calibration.scale[ct][3] = cal.accel_bias[ct];
accel_data.calibration.variance[ct] = cal.accel_var[ct];
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
#if 1
gyro_data.calibration.tempcompfactor[ct] = cal.gyro_tempcompfactor[ct];
#endif
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct];
}
// Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to
// one in code
float mag_var[3] = {mag_data.calibration.variance[0] / INSGPS_MAGLEN / INSGPS_MAGLEN,
mag_data.calibration.variance[1] / INSGPS_MAGLEN / INSGPS_MAGLEN,
mag_data.calibration.variance[2] / INSGPS_MAGLEN / INSGPS_MAGLEN};
INSSetMagVar(mag_var);
INSSetAccelVar(accel_data.calibration.variance);
INSSetGyroVar(gyro_data.calibration.variance);
}
else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE) {
calibrate_sensors();
send_calibration();
}
INSSetPosVelVar(cal.pos_var, cal.vel_var);
}
void gps_callback(AhrsObjHandle obj)
{
GPSPositionData pos;
GPSPositionGet(&pos);
HomeLocationData home;
HomeLocationGet(&home);
// convert from cm back to meters
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
// put in local NED frame
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1; /* currently unused */
gps_data.updated = true;
// if poor don't use this update
if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
(pos.Satellites < INSGPS_GPS_MINSAT) ||
(pos.PDOP >= INSGPS_GPS_MINPDOP) ||
(home.Set == FALSE) ||
((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
{
gps_data.quality = 0;
gps_data.updated = false;
}
}
void altitude_callback(AhrsObjHandle obj)
{
BaroAltitudeData alt;
BaroAltitudeGet(&alt);
altitude_data.altitude = alt.Altitude;
altitude_data.updated = true;
}
void settings_callback(AhrsObjHandle obj)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
ahrs_algorithm = settings.Algorithm;
if(settings.Downsampling != adc_oversampling) {
adc_oversampling = settings.Downsampling;
PIOS_ADC_Config(adc_oversampling);
}
}
void homelocation_callback(AhrsObjHandle obj)
{
HomeLocationData data;
HomeLocationGet(&data);
mag_len = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2));
float Be[3] = {data.Be[0] / mag_len, data.Be[1] / mag_len, data.Be[2] / mag_len};
INSSetMagNorth(Be);
}
void firmwareiapobj_callback(AhrsObjHandle obj)
{
const struct pios_board_info * bdinfo = &pios_board_info_blob;
FirmwareIAPObjData firmwareIAPObj;
FirmwareIAPObjGet(&firmwareIAPObj);
if(firmwareIAPObj.ArmReset==0)
reset_count=0;
if(firmwareIAPObj.ArmReset==1)
{
if((firmwareIAPObj.BoardType==bdinfo->board_type) || (firmwareIAPObj.BoardType==0xFF))
{
++reset_count;
if(reset_count>2)
{
PIOS_IAP_SetRequest1();
PIOS_IAP_SetRequest2();
PIOS_SYS_Reset();
}
}
}
else if(firmwareIAPObj.BoardType==bdinfo->board_type && firmwareIAPObj.crc!=PIOS_BL_HELPER_CRC_Memory_Calc())
{
PIOS_BL_HELPER_FLASH_Read_Description(firmwareIAPObj.Description,bdinfo->desc_size);
firmwareIAPObj.crc=PIOS_BL_HELPER_CRC_Memory_Calc();
firmwareIAPObj.BoardRevision=bdinfo->board_rev;
FirmwareIAPObjSet(&firmwareIAPObj);
}
}
/**
* @}
*/

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

428
flight/AHRS/pios_board.c
View File

@ -1,428 +0,0 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the AHRS board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* OP Interface
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_op_irq_handler(void);
void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
static const struct pios_spi_cfg pios_spi_op_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
uint32_t pios_spi_op_id;
void PIOS_SPI_op_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_op_id);
}
#endif /* PIOS_INCLUDE_SPI */
/*
* ADC system
*/
#include "pios_adc_priv.h"
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one
static const struct pios_adc_cfg pios_adc_cfg = {
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel1,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR,
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
}
},
.half_flag = DMA1_IT_HT1,
.full_flag = DMA1_IT_TC1,
};
struct pios_adc_dev pios_adc_devs[] = {
{
.cfg = &pios_adc_cfg,
.callback_function = NULL,
},
};
uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
void PIOS_ADC_handler() {
PIOS_ADC_DMA_Handler();
}
#if defined(PIOS_INCLUDE_USART)
#include <pios_usart_priv.h>
/*
* AUX USART
*/
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 230400,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl =
USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>
#define PIOS_COM_AUX_TX_BUF_LEN 192
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>
/*
* I2C Adapters
*/
void PIOS_I2C_main_adapter_ev_irq_handler(void);
void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C1_EV_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 200000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_main_adapter_id;
void PIOS_I2C_main_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id);
}
void PIOS_I2C_main_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id);
}
#endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_ENABLE_DEBUG_PINS)
static const struct stm32_gpio pios_debug_pins[] = {
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
};
#endif /* PIOS_ENABLE_DEBUG_PINS */
extern const struct pios_com_driver pios_usart_com_driver;
uint32_t pios_com_aux_id;
uint8_t adc_fifo_buf[sizeof(float) * 6 * 4] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement
/**
* PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
PIOS_LED_On(LED1);
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
#if !defined(PIOS_ENABLE_DEBUG_PINS)
#if defined(PIOS_INCLUDE_COM)
{
uint32_t pios_usart_aux_id;
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
NULL, 0,
pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
PIOS_DEBUG_Assert(0);
}
}
#endif /* PIOS_INCLUDE_COM */
#endif
/* IAP System Setup */
PIOS_IAP_Init();
/* ADC system */
PIOS_ADC_Init();
extern uint8_t adc_oversampling;
PIOS_ADC_Config(adc_oversampling);
extern void adc_callback(float *);
PIOS_ADC_SetCallback(adc_callback);
/* ADC buffer */
extern t_fifo_buffer adc_fifo_buffer;
fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf));
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0);
SET_ACCEL_6G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
PIOS_HMC5843_Init();
#endif
#if defined(PIOS_INCLUDE_SPI)
#include "ahrs_spi_comm.h"
AhrsInitComms();
/* Set up the SPI interface to the OP board */
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
PIOS_DEBUG_Assert(0);
}
AhrsConnect(pios_spi_op_id);
#endif
}

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

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(LED1);
return (true);
}
bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size) {
if (size > SPI_MAX_PROGRAM_DATA_SIZE) {
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "oversize: %d\r\n", size);
return (false);
}
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Read %d bytes from %d\r\n",
size, offset);
memcpy(buffer, buf, size);
PIOS_LED_Toggle(LED1);
return (true);
}

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

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

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

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

@ -1,276 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS BOOTLOADER
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_BOOTLOADER_Main
* @brief Main function which does the hardware dependent stuff
* @{
*
*
* @file main.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "ahrs_bl.h"
#include <pios_board_info.h>
#include "pios_opahrs_proto.h"
#include "bl_fsm.h" /* lfsm_state */
#include "stm32f10x_flash.h"
extern void PIOS_Board_Init(void);
#define NSS_HOLD_STATE ((GPIOB->IDR & GPIO_Pin_12) ? 0 : 1)
enum bootloader_status boot_status;
/* Private typedef -----------------------------------------------------------*/
typedef void
(*pFunction)(void);
pFunction Jump_To_Application;
uint32_t JumpAddress;
/* Function Prototypes */
void
process_spi_request(void);
void
jump_to_app();
uint8_t jumpFW = FALSE;
uint32_t Fw_crc;
/**
* @brief Bootloader Main function
*/
int main() {
uint8_t GO_dfu = false;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Enable Prefetch Buffer */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
/* Flash 2 wait state */
FLASH_SetLatency(FLASH_Latency_2);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
/* Delay system */
PIOS_DELAY_Init();
for (uint32_t t = 0; t < 10000000; ++t) {
if (NSS_HOLD_STATE == 1)
GO_dfu = TRUE;
else {
GO_dfu = FALSE;
break;
}
}
//while(TRUE)
// {
// PIOS_LED_Toggle(LED1);
// PIOS_DELAY_WaitmS(1000);
// }
//GO_dfu = TRUE;
PIOS_IAP_Init();
GO_dfu = GO_dfu | PIOS_IAP_CheckRequest();// OR with app boot request
if (GO_dfu == FALSE) {
jump_to_app();
}
if (PIOS_IAP_CheckRequest()) {
PIOS_DELAY_WaitmS(1000);
PIOS_IAP_ClearRequest();
}
PIOS_Board_Init();
boot_status = idle;
Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
PIOS_LED_On(LED1);
while (1) {
process_spi_request();
}
return 0;
}
static struct opahrs_msg_v0 link_tx_v0;
static struct opahrs_msg_v0 link_rx_v0;
static struct opahrs_msg_v0 user_tx_v0;
static struct opahrs_msg_v0 user_rx_v0;
void process_spi_request(void) {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
bool msg_to_process = FALSE;
PIOS_IRQ_Disable();
/* Figure out if we're in an interesting stable state */
switch (lfsm_get_state()) {
case LFSM_STATE_USER_BUSY:
msg_to_process = TRUE;
break;
case LFSM_STATE_INACTIVE:
/* Queue up a receive buffer */
lfsm_user_set_rx_v0(&user_rx_v0);
lfsm_user_done();
break;
case LFSM_STATE_STOPPED:
/* Get things going */
lfsm_set_link_proto_v0(&link_tx_v0, &link_rx_v0);
break;
default:
/* Not a stable state */
break;
}
PIOS_IRQ_Enable();
if (!msg_to_process) {
/* Nothing to do */
//PIOS_COM_SendFormattedString(PIOS_COM_AUX, ".");
return;
}
if (user_rx_v0.tail.magic != OPAHRS_MSG_MAGIC_TAIL) {
return;
}
switch (user_rx_v0.payload.user.t) {
case OPAHRS_MSG_V0_REQ_FWUP_VERIFY:
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
lfsm_user_set_tx_v0(&user_tx_v0);
boot_status = idle;
PIOS_LED_Off(LED1);
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
break;
case OPAHRS_MSG_V0_REQ_RESET:
PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.reset.reset_delay_in_ms);
PIOS_SYS_Reset();
break;
case OPAHRS_MSG_V0_REQ_VERSIONS:
//PIOS_LED_On(LED1);
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS);
user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION;
user_tx_v0.payload.user.v.rsp.versions.hw_version = (BOARD_TYPE << 8)
| BOARD_REVISION;
user_tx_v0.payload.user.v.rsp.versions.fw_crc = Fw_crc;
lfsm_user_set_tx_v0(&user_tx_v0);
break;
case OPAHRS_MSG_V0_REQ_MEM_MAP:
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP);
user_tx_v0.payload.user.v.rsp.mem_map.density = bdinfo->hw_type;
user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE
| (BOARD_WRITABLE << 1));
user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory
= bdinfo->fw_size;
user_tx_v0.payload.user.v.rsp.mem_map.size_of_description
= bdinfo->desc_size;
user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code
= bdinfo->fw_base;
lfsm_user_set_tx_v0(&user_tx_v0);
break;
case OPAHRS_MSG_V0_REQ_SERIAL:
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_SERIAL);
PIOS_SYS_SerialNumberGet(
(char *) &(user_tx_v0.payload.user.v.rsp.serial.serial_bcd));
lfsm_user_set_tx_v0(&user_tx_v0);
break;
case OPAHRS_MSG_V0_REQ_FWUP_STATUS:
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
lfsm_user_set_tx_v0(&user_tx_v0);
break;
case OPAHRS_MSG_V0_REQ_FWUP_DATA:
PIOS_LED_On(LED1);
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
if (!(user_rx_v0.payload.user.v.req.fwup_data.adress
< bdinfo->fw_base)) {
for (uint8_t x = 0; x
< user_rx_v0.payload.user.v.req.fwup_data.size; ++x) {
if (FLASH_ProgramWord(
(user_rx_v0.payload.user.v.req.fwup_data.adress
+ ((uint32_t)(x * 4))),
user_rx_v0.payload.user.v.req.fwup_data.data[x])
!= FLASH_COMPLETE) {
boot_status = write_error;
break;
}
}
} else {
boot_status = outside_dev_capabilities;
}
PIOS_LED_Off(LED1);
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
lfsm_user_set_tx_v0(&user_tx_v0);
break;
case OPAHRS_MSG_V0_REQ_FWDN_DATA:
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWDN_DATA);
uint32_t adr = user_rx_v0.payload.user.v.req.fwdn_data.adress;
for (uint8_t x = 0; x < 4; ++x) {
user_tx_v0.payload.user.v.rsp.fw_dn.data[x]
= *PIOS_BL_HELPER_FLASH_If_Read(adr + x);
}
lfsm_user_set_tx_v0(&user_tx_v0);
break;
case OPAHRS_MSG_V0_REQ_FWUP_START:
FLASH_Unlock();
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
lfsm_user_set_tx_v0(&user_tx_v0);
PIOS_LED_On(LED1);
if (PIOS_BL_HELPER_FLASH_Start() == TRUE) {
boot_status = started;
PIOS_LED_Off(LED1);
} else {
boot_status = start_failed;
break;
}
break;
case OPAHRS_MSG_V0_REQ_BOOT:
PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.boot.boot_delay_in_ms);
FLASH_Lock();
jump_to_app();
break;
default:
break;
}
/* Finished processing the received message, requeue it */
lfsm_user_set_rx_v0(&user_rx_v0);
lfsm_user_done();
return;
}
void jump_to_app() {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
PIOS_LED_On(LED1);
if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
FLASH_Lock();
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
//_SetCNTR(0); // clear interrupt mask
//_SetISTR(0); // clear all requests
JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
Jump_To_Application = (pFunction) JumpAddress;
/* Initialize user application's Stack Pointer */
__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
Jump_To_Application();
} else {
boot_status = jump_failed;
return;
}
}

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

@ -1,149 +0,0 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the AHRS board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* OP Interface
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_op_irq_handler(void);
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler")));
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler")));
static const struct pios_spi_cfg
pios_spi_op_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
}, .ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
}, .sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
}, .miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
}, .mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
}, };
uint32_t pios_spi_op_id;
void PIOS_SPI_op_irq_handler(void) {
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_op_id);
}
#endif /* PIOS_INCLUDE_SPI */
#include "bl_fsm.h" /* lfsm_* */
static bool board_init_complete = false;
void PIOS_Board_Init() {
if (board_init_complete) {
return;
}
/* Set up the SPI interface to the OP board */
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
PIOS_DEBUG_Assert(0);
}
lfsm_attach(pios_spi_op_id);
lfsm_init();
board_init_complete = true;
}

2
flight/Bootloaders/BootloaderUpdater/Makefile
View File

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

42
flight/Bootloaders/BootloaderUpdater/main.c
View File

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

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

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

20
flight/Bootloaders/CopterControl/Makefile
View File

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

5
flight/Bootloaders/CopterControl/inc/pios_config.h
View File

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

85
flight/Bootloaders/CopterControl/inc/pios_usb.h
View File

@ -1,85 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB USB Functions
* @brief PIOS USB interface code
* @{
*
* @file pios_usb.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief USB functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_H
#define PIOS_USB_H
/* Local defines */
/* Following settings allow to customise the USB device descriptor */
#ifndef PIOS_USB_VENDOR_ID
#define PIOS_USB_VENDOR_ID 0x20A0
#endif
#ifndef PIOS_USB_VENDOR_STR
#define PIOS_USB_VENDOR_STR "openpilot.org"
#endif
#ifndef PIOS_USB_PRODUCT_STR
#define PIOS_USB_PRODUCT_STR "CopterControl"
#endif
#ifndef PIOS_USB_PRODUCT_ID
#define PIOS_USB_PRODUCT_ID 0x415B
#endif
#ifndef PIOS_USB_VERSION_ID
#define PIOS_USB_VERSION_ID 0x0401 /* CopterControl (04) Bootloader (01) */
#endif
/* Internal defines which are used by PIOS USB HID (don't touch) */
#define PIOS_USB_EP_NUM 2
/* Buffer table base address */
#define PIOS_USB_BTABLE_ADDRESS 0x000
/* EP0 rx/tx buffer base address */
#define PIOS_USB_ENDP0_RXADDR 0x040
#define PIOS_USB_ENDP0_TXADDR 0x080
/* EP1 Rx/Tx buffer base address for HID driver */
#define PIOS_USB_ENDP1_TXADDR 0x0C0
#define PIOS_USB_ENDP1_RXADDR 0x100
/* Global Variables */
extern void (*pEpInt_IN[7])(void);
extern void (*pEpInt_OUT[7])(void);
/* Public Functions */
extern int32_t PIOS_USB_Init(uint32_t mode);
extern int32_t PIOS_USB_IsInitialized(void);
extern int32_t PIOS_USB_CableConnected(void);
#endif /* PIOS_USB_H */
/**
* @}
* @}
*/

52
flight/Bootloaders/CopterControl/inc/pios_usb_board_data.h Normal file
View File

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

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

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

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

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

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

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

101
flight/Bootloaders/CopterControl/pios_usb_board_data.c Normal file
View File

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

486
flight/Bootloaders/OpenPilot/Makefile
View File

@ -1,486 +0,0 @@
#####
# Project: OpenPilot
#
#
# Makefile for OpenPilot project build PiOS and the AP.
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := bl_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES to compile for debugging
DEBUG ?= NO
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= NO
# Remove command is different for Code Sourcery on Windows
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
else
REMOVE_CMD = rm
endif
FLASH_TOOL = OPENOCD
# Paths
OPSYSTEM = .
OPSYSTEMINC = $(OPSYSTEM)/inc
OPUAVTALK = ./UAVTalk
OPUAVTALKINC = $(OPUAVTALK)/inc
OPUAVOBJ = ./UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPTESTS = ./Tests
OPMODULEDIR = ./Modules
FLIGHTLIB = ../../Libraries
FLIGHTLIBINC = ../../Libraries/inc
PIOS = ../../PiOS
PIOSINC = $(PIOS)/inc
PIOSSTM32F10X = $(PIOS)/STM32F10x
PIOSCOMMON = $(PIOS)/Common
PIOSBOARDS = $(PIOS)/Boards
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
STMLIBDIR = $(APPLIBDIR)
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
STMSPDSRCDIR = $(STMSPDDIR)/src
STMSPDINCDIR = $(STMSPDDIR)/inc
STMUSBSRCDIR = $(STMUSBDIR)/src
STMUSBINCDIR = $(STMUSBDIR)/inc
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
DOSFSDIR = $(APPLIBDIR)/dosfs
MSDDIR = $(APPLIBDIR)/msd
RTOSDIR = $(APPLIBDIR)/FreeRTOS
RTOSSRCDIR = $(RTOSDIR)/Source
RTOSINCDIR = $(RTOSSRCDIR)/include
DOXYGENDIR = ../Doc/Doxygen
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
## OPENPILOT_BL CORE:
SRC += $(OPSYSTEM)/main.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(OPSYSTEM)/op_dfu.c
SRC += $(FLIGHTLIB)/stopwatch.c
SRC += $(OPSYSTEM)/ssp.c
## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_sys.c
SRC += $(PIOSSTM32F10X)/pios_led.c
SRC += $(PIOSSTM32F10X)/pios_delay.c
SRC += $(PIOSSTM32F10X)/pios_usart.c
SRC += $(PIOSSTM32F10X)/pios_irq.c
SRC += $(PIOSSTM32F10X)/pios_spi.c
SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.c
# PIOS USB related files (seperated to make code maintenance more easy)
SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_desc.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_board_info.c
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_opahrs_v0.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c
SRC += $(CMSISDIR)/system_stm32f10x.c
## Used parts of the STM-Library
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
SRC += $(STMSPDSRCDIR)/misc.c
## STM32 USB Library
SRC += $(STMUSBSRCDIR)/usb_core.c
SRC += $(STMUSBSRCDIR)/usb_init.c
SRC += $(STMUSBSRCDIR)/usb_int.c
SRC += $(STMUSBSRCDIR)/usb_mem.c
SRC += $(STMUSBSRCDIR)/usb_regs.c
SRC += $(STMUSBSRCDIR)/usb_sil.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
# List Assembler source files here which must be assembled in ARM-Mode..
ASRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS = $(OPSYSTEM)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(OPUAVTALK)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSSTM32F10X)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)
EXTRAINCDIRS += $(STMSPDINCDIR)
EXTRAINCDIRS += $(STMUSBINCDIR)
EXTRAINCDIRS += $(CMSISDIR)
EXTRAINCDIRS += $(DOSFSDIR)
EXTRAINCDIRS += $(MSDDIR)
EXTRAINCDIRS += $(RTOSINCDIR)
EXTRAINCDIRS += $(APPLIBDIR)
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
OPT = 0
else
OPT = s
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
CDEFS = -DSTM32F10X_$(MODEL)
CDEFS += -DUSE_STDPERIPH_DRIVER
CDEFS += -DUSE_$(BOARD)
ifeq ($(ENABLE_DEBUG_PINS), YES)
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
endif
ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART
endif
# Provide (only) the bootloader with board-specific defines
BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS = -DDEBUG
endif
CFLAGS += -g$(DEBUGF)
CFLAGS += -O$(OPT)
ifeq ($(DEBUG),NO)
CFLAGS += -ffunction-sections
endif
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(BLONLY_CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
CFLAGS += -Werror
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
ifeq ($(DEBUG),NO)
LDFLAGS += -Wl,-static
endif
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
# Set linker-script name depending on selected submodel name
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_BL_sections.ld
# Define programs and commands.
REMOVE = $(REMOVE_CMD) -f
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
# Default target.
all: build
ifeq ($(LOADFORMAT),ihex)
build: elf hex sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
${OUTDIR}/InitMods.c: Makefile
@echo $(MSG_MODINIT) $(call toprel, $@)
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
# Compile: create assembler files from C source files. ARM only
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
# Add jtag targets (program and wipe)
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
.PHONY: elf lss sym hex bin bino
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
bino: $(OUTDIR)/$(TARGET).bin.o
# Display sizes of sections.
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Install: install binary file with prefix/suffix into install directory
install: $(OUTDIR)/$(TARGET).bin
ifneq ($(INSTALL_DIR),)
@echo $(MSG_INSTALLING) $(call toprel, $<)
$(V1) mkdir -p $(INSTALL_DIR)
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).bin
else
$(error INSTALL_DIR must be specified for $@)
endif
# Target: clean project.
clean: clean_list
clean_list :
@echo $(MSG_CLEANING)
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
$(V1) $(REMOVE) $(ALLOBJ)
$(V1) $(REMOVE) $(LSTFILES)
$(V1) $(REMOVE) $(DEPFILES)
$(V1) $(REMOVE) $(SRC:.c=.s)
$(V1) $(REMOVE) $(SRCARM:.c=.s)
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
else
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir -p $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all build clean clean_list install

85
flight/Bootloaders/OpenPilot/inc/pios_usb.h
View File

@ -1,85 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB USB Functions
* @brief PIOS USB interface code
* @{
*
* @file pios_usb.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief USB functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_H
#define PIOS_USB_H
/* Local defines */
/* Following settings allow to customise the USB device descriptor */
#ifndef PIOS_USB_VENDOR_ID
#define PIOS_USB_VENDOR_ID 0x20A0
#endif
#ifndef PIOS_USB_VENDOR_STR
#define PIOS_USB_VENDOR_STR "openpilot.org"
#endif
#ifndef PIOS_USB_PRODUCT_STR
#define PIOS_USB_PRODUCT_STR "OpenPilot"
#endif
#ifndef PIOS_USB_PRODUCT_ID
#define PIOS_USB_PRODUCT_ID 0x415A
#endif
#ifndef PIOS_USB_VERSION_ID
#define PIOS_USB_VERSION_ID 0x0101 /* OpenPilot (01) Bootloader (01) */
#endif
/* Internal defines which are used by PIOS USB HID (don't touch) */
#define PIOS_USB_EP_NUM 2
/* Buffer table base address */
#define PIOS_USB_BTABLE_ADDRESS 0x000
/* EP0 rx/tx buffer base address */
#define PIOS_USB_ENDP0_RXADDR 0x040
#define PIOS_USB_ENDP0_TXADDR 0x080
/* EP1 Rx/Tx buffer base address for HID driver */
#define PIOS_USB_ENDP1_TXADDR 0x0C0
#define PIOS_USB_ENDP1_RXADDR 0x100
/* Global Variables */
extern void (*pEpInt_IN[7])(void);
extern void (*pEpInt_OUT[7])(void);
/* Public Functions */
extern int32_t PIOS_USB_Init(uint32_t mode);
extern int32_t PIOS_USB_IsInitialized(void);
extern int32_t PIOS_USB_CableConnected(void);
#endif /* PIOS_USB_H */
/**
* @}
* @}
*/

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

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

@ -1,295 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotBL OpenPilot BootLoader
* @brief These files contain the code to the OpenPilot MB Bootloader.
*
* @{
* @file main.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This is the file with the main function of the OpenPilot BootLoader
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Bootloader Includes */
#include <pios.h>
#include <pios_board_info.h>
#include "pios_opahrs.h"
#include "stopwatch.h"
#include "op_dfu.h"
#include "usb_lib.h"
#include "pios_iap.h"
#include "ssp.h"
#include "fifo_buffer.h"
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void FLASH_Download();
#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
/* Private typedef -----------------------------------------------------------*/
typedef void (*pFunction)(void);
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
pFunction Jump_To_Application;
uint32_t JumpAddress;
/// LEDs PWM
uint32_t period1 = 50; // *100 uS -> 5 mS
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
uint32_t period2 = 50; // *100 uS -> 5 mS
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
////////////////////////////////////////
uint8_t tempcount = 0;
/// SSP SECTION
/// SSP TIME SOURCE
#define SSP_TIMER TIM7
uint32_t ssp_time = 0;
#define MAX_PACKET_DATA_LEN 255
#define MAX_PACKET_BUF_SIZE (1+1+MAX_PACKET_DATA_LEN+2)
#define UART_BUFFER_SIZE 1024
uint8_t rx_buffer[UART_BUFFER_SIZE] __attribute__ ((aligned(4)));
// align to 32-bit to try and provide speed improvement;
// master buffers...
uint8_t SSP_TxBuf[MAX_PACKET_BUF_SIZE];
uint8_t SSP_RxBuf[MAX_PACKET_BUF_SIZE];
void SSP_CallBack(uint8_t *buf, uint16_t len);
int16_t SSP_SerialRead(void);
void SSP_SerialWrite( uint8_t);
uint32_t SSP_GetTime(void);
PortConfig_t SSP_PortConfig = { .rxBuf = SSP_RxBuf,
.rxBufSize = MAX_PACKET_DATA_LEN, .txBuf = SSP_TxBuf,
.txBufSize = MAX_PACKET_DATA_LEN, .max_retry = 10, .timeoutLen = 1000,
.pfCallBack = SSP_CallBack, .pfSerialRead = SSP_SerialRead,
.pfSerialWrite = SSP_SerialWrite, .pfGetTime = SSP_GetTime, };
Port_t ssp_port;
t_fifo_buffer ssp_buffer;
/* Extern variables ----------------------------------------------------------*/
DFUStates DeviceState;
DFUPort ProgPort;
int16_t status = 0;
uint8_t JumpToApp = FALSE;
uint8_t GO_dfu = FALSE;
uint8_t USB_connected = FALSE;
uint8_t User_DFU_request = FALSE;
static uint8_t mReceive_Buffer[64];
/* Private function prototypes -----------------------------------------------*/
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
uint8_t processRX();
void jump_to_app();
uint32_t sspTimeSource();
#define BLUE LED1
#define RED LED2
#define LED_PWM_TIMER TIM6
int main() {
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
if (BSL_HOLD_STATE == 0)
USB_connected = TRUE;
PIOS_IAP_Init();
if (PIOS_IAP_CheckRequest() == TRUE) {
PIOS_Board_Init();
PIOS_DELAY_WaitmS(1000);
User_DFU_request = TRUE;
PIOS_IAP_ClearRequest();
}
GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE);
if (GO_dfu == TRUE) {
if (USB_connected)
ProgPort = Usb;
else
ProgPort = Serial;
PIOS_Board_Init();
if (User_DFU_request == TRUE)
DeviceState = DFUidle;
else
DeviceState = BLidle;
STOPWATCH_Init(100, LED_PWM_TIMER);
if (ProgPort == Serial) {
fifoBuf_init(&ssp_buffer, rx_buffer, UART_BUFFER_SIZE);
STOPWATCH_Init(100, SSP_TIMER);//nao devia ser 1000?
STOPWATCH_Reset(SSP_TIMER);
ssp_Init(&ssp_port, &SSP_PortConfig);
}
PIOS_OPAHRS_ForceSlaveSelected(true);
} else
JumpToApp = TRUE;
STOPWATCH_Reset(LED_PWM_TIMER);
while (TRUE) {
if (ProgPort == Serial) {
ssp_ReceiveProcess(&ssp_port);
status = ssp_SendProcess(&ssp_port);
while ((status != SSP_TX_IDLE) && (status != SSP_TX_ACKED)) {
ssp_ReceiveProcess(&ssp_port);
status = ssp_SendProcess(&ssp_port);
}
}
if (JumpToApp == TRUE)
jump_to_app();
//pwm_period = 50; // *100 uS -> 5 mS
//pwm_sweep_steps =100; // * 5 mS -> 500 mS
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 50;
sweep_steps1 = 100;
PIOS_LED_Off(RED);
period2 = 0;
break;
case uploading:
period1 = 50;
sweep_steps1 = 100;
period2 = 25;
sweep_steps2 = 50;
break;
case downloading:
period1 = 25;
sweep_steps1 = 50;
PIOS_LED_Off(RED);
period2 = 0;
break;
case BLidle:
period1 = 0;
PIOS_LED_On(BLUE);
period2 = 0;
break;
default://error
period1 = 50;
sweep_steps1 = 100;
period2 = 50;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER)))
PIOS_LED_On(BLUE);
else
PIOS_LED_Off(BLUE);
} else
PIOS_LED_On(BLUE);
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER)))
PIOS_LED_On(RED);
else
PIOS_LED_Off(RED);
} else
PIOS_LED_Off(RED);
if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100)
STOPWATCH_Reset(LED_PWM_TIMER);
if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState
== BLidle))
JumpToApp = TRUE;
processRX();
DataDownload(start);
}
}
void jump_to_app() {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
FLASH_Lock();
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
_SetCNTR(0); // clear interrupt mask
_SetISTR(0); // clear all requests
JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
Jump_To_Application = (pFunction) JumpAddress;
/* Initialize user application's Stack Pointer */
__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
Jump_To_Application();
} else {
DeviceState = failed_jump;
return;
}
}
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
uint32_t pwm_duty = ((count / pwm_period) % pwm_sweep_steps)
/ (pwm_sweep_steps / pwm_period);
if ((count % (2 * pwm_period * pwm_sweep_steps)) > pwm_period
* pwm_sweep_steps)
pwm_duty = pwm_period - pwm_duty; // negative direction each 50*100 ticks
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
}
uint8_t processRX() {
if (ProgPort == Usb) {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
processComand(mReceive_Buffer);
}
}
} else if (ProgPort == Serial) {
if (fifoBuf_getUsed(&ssp_buffer) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = fifoBuf_getByte(&ssp_buffer);
}
processComand(mReceive_Buffer);
}
}
return TRUE;
}
uint32_t sspTimeSource() {
if (STOPWATCH_ValueGet(SSP_TIMER) > 5000) {
++ssp_time;
STOPWATCH_Reset(SSP_TIMER);
}
return ssp_time;
}
void SSP_CallBack(uint8_t *buf, uint16_t len) {
fifoBuf_putData(&ssp_buffer, buf, len);
}
int16_t SSP_SerialRead(void) {
if (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_RF) > 0) {
uint8_t byte;
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF, &byte, 1, 0) == 1) {
return byte;
} else {
return -1;
}
} else
return -1;
}
void SSP_SerialWrite(uint8_t value) {
PIOS_COM_SendChar(PIOS_COM_TELEM_RF, value);
}
uint32_t SSP_GetTime(void) {
return sspTimeSource();
}

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

@ -1,286 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotBL OpenPilot BootLoader
* @{
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initialisers for hardware for the OpenPilot board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
//#include <openpilot.h>
//#include <uavobjectsinit.h>
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* AHRS Interface
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_ahrs_irq_handler(void);
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
const struct pios_spi_cfg
pios_spi_ahrs_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
}, .ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
}, .sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
}, .miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
}, .mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
}, };
uint32_t pios_spi_ahrs_id;
void PIOS_SPI_ahrs_irq_handler(void) {
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_ahrs_id);
}
#endif /* PIOS_INCLUDE_SPI */
#if defined(PIOS_INCLUDE_USART)
#include "pios_usart_priv.h"
/*
* Telemetry USART
*/
const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = USART2,
.init = {
#if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, .irq = {
.init = {
.NVIC_IRQChannel = USART2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
}, .rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_3,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
}, .tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
}, };
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include "pios_com_priv.h"
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN];
static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN];
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
static uint8_t pios_com_telem_rf_rx_buffer[PIOS_COM_TELEM_RF_RX_BUF_LEN];
static uint8_t pios_com_telem_rf_tx_buffer[PIOS_COM_TELEM_RF_TX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_USB_HID)
#include "pios_usb_hid_priv.h"
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#endif /* PIOS_INCLUDE_USB_HID */
extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
#include "pios_opahrs.h"
/**
* PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
static bool board_init_complete = false;
void PIOS_Board_Init(void) {
if (board_init_complete) {
return;
}
/* Enable Prefetch Buffer */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
/* Flash 2 wait state */
FLASH_SetLatency(FLASH_Latency_2);
/* Delay system */
PIOS_DELAY_Init();
/* Initialize the PiOS library */
#if defined(PIOS_INCLUDE_COM)
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver,
pios_usart_telem_rf_id,
pios_com_telem_rf_rx_buffer, sizeof(pios_com_telem_rf_rx_buffer),
pios_com_telem_rf_tx_buffer, sizeof(pios_com_telem_rf_tx_buffer))) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_USB_HID)
uint32_t pios_usb_hid_id;
PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
#if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer),
pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
#endif /* PIOS_INCLUDE_USB_HID */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
/* Set up the SPI interface to the AHRS */
if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
PIOS_DEBUG_Assert(0);
}
/* Bind the AHRS comms layer to the AHRS SPI link */
PIOS_OPAHRS_Attach(pios_spi_ahrs_id);
board_init_complete = true;
}
/**
* @}
*/

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

21
flight/Bootloaders/PipXtreme/Makefile
View File

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

6
flight/Bootloaders/PipXtreme/inc/pios_config.h
View File

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

85
flight/Bootloaders/PipXtreme/inc/pios_usb.h
View File

@ -1,85 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB USB Functions
* @brief PIOS USB interface code
* @{
*
* @file pios_usb.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief USB functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_H
#define PIOS_USB_H
/* Local defines */
/* Following settings allow to customise the USB device descriptor */
#ifndef PIOS_USB_VENDOR_ID
#define PIOS_USB_VENDOR_ID 0x20A0
#endif
#ifndef PIOS_USB_VENDOR_STR
#define PIOS_USB_VENDOR_STR "openpilot.org"
#endif
#ifndef PIOS_USB_PRODUCT_STR
#define PIOS_USB_PRODUCT_STR "PipXtreme"
#endif
#ifndef PIOS_USB_PRODUCT_ID
#define PIOS_USB_PRODUCT_ID 0x415C
#endif
#ifndef PIOS_USB_VERSION_ID
#define PIOS_USB_VERSION_ID 0x0301 /* PipXtreme (03) Bootloader (01) */
#endif
/* Internal defines which are used by PIOS USB HID (don't touch) */
#define PIOS_USB_EP_NUM 2
/* Buffer table base address */
#define PIOS_USB_BTABLE_ADDRESS 0x000
/* EP0 rx/tx buffer base address */
#define PIOS_USB_ENDP0_RXADDR 0x040
#define PIOS_USB_ENDP0_TXADDR 0x080
/* EP1 Rx/Tx buffer base address for HID driver */
#define PIOS_USB_ENDP1_TXADDR 0x0C0
#define PIOS_USB_ENDP1_RXADDR 0x100
/* Global Variables */
extern void (*pEpInt_IN[7])(void);
extern void (*pEpInt_OUT[7])(void);
/* Public Functions */
extern int32_t PIOS_USB_Init(uint32_t mode);
extern int32_t PIOS_USB_IsInitialized(void);
extern int32_t PIOS_USB_CableConnected(void);
#endif /* PIOS_USB_H */
/**
* @}
* @}
*/

52
flight/Bootloaders/PipXtreme/inc/pios_usb_board_data.h Normal file
View File

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

43
flight/Bootloaders/PipXtreme/main.c
View File

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

45
flight/Bootloaders/PipXtreme/op_dfu.c
View File

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

67
flight/Bootloaders/PipXtreme/pios_board.c
View File

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

97
flight/Bootloaders/PipXtreme/pios_usb_board_data.c Normal file
View File

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

115
flight/Bootloaders/AHRS/Makefile → flight/Bootloaders/Revolution/Makefile
View File

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

4
flight/Bootloaders/OpenPilot/inc/common.h → flight/Bootloaders/Revolution/inc/common.h
View File

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

0
flight/Bootloaders/OpenPilot/inc/op_dfu.h → flight/Bootloaders/Revolution/inc/op_dfu.h
View File

5
flight/Bootloaders/AHRS/inc/pios_config.h → flight/Bootloaders/Revolution/inc/pios_config.h
View File

@ -33,8 +33,11 @@
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_USB
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_COM_MSG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
#define PIOS_INCLUDE_GPIO
#endif /* PIOS_CONFIG_H */

51
flight/Bootloaders/Revolution/inc/pios_usb_board_data.h Normal file
View File

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

208
flight/Bootloaders/Revolution/main.c Normal file
View File

@ -0,0 +1,208 @@
/**
******************************************************************************
* @addtogroup RevolutionBL Revolution BootLoader
* @brief These files contain the code to the Revolution Bootloader.
*
* @{
* @file main.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This is the file with the main function of the Revolution BootLoader
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Bootloader Includes */
#include <pios.h>
#include <pios_board_info.h>
#include <stdbool.h>
#include "op_dfu.h"
#include "pios_iap.h"
#include "fifo_buffer.h"
#include "pios_com_msg.h"
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void FLASH_Download();
#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
/* Private typedef -----------------------------------------------------------*/
typedef void (*pFunction)(void);
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
pFunction Jump_To_Application;
uint32_t JumpAddress;
/// LEDs PWM
uint32_t period1 = 5000; // 5 mS
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
uint32_t period2 = 5000; // 5 mS
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
////////////////////////////////////////
uint8_t tempcount = 0;
/* Extern variables ----------------------------------------------------------*/
DFUStates DeviceState;
int16_t status = 0;
bool JumpToApp = false;
bool GO_dfu = false;
bool USB_connected = false;
bool User_DFU_request = false;
static uint8_t mReceive_Buffer[63];
/* Private function prototypes -----------------------------------------------*/
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
uint8_t processRX();
void jump_to_app();
int main() {
PIOS_SYS_Init();
PIOS_Board_Init();
PIOS_IAP_Init();
USB_connected = PIOS_USB_CheckAvailable(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);
User_DFU_request = true;
PIOS_IAP_ClearRequest();
}
GO_dfu = (USB_connected == true) || (User_DFU_request == true);
if (GO_dfu == true) {
PIOS_Board_Init();
if (User_DFU_request == true)
DeviceState = DFUidle;
else
DeviceState = BLidle;
} else
JumpToApp = true;
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (true) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == true)
jump_to_app();
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case uploading:
period1 = 5000;
sweep_steps1 = 100;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case BLidle:
period1 = 0;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
default://error
period1 = 5000;
sweep_steps1 = 100;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, stopwatch))
PIOS_LED_On(PIOS_LED_HEARTBEAT);
else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
} else
PIOS_LED_On(PIOS_LED_HEARTBEAT);
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, stopwatch))
PIOS_LED_On(PIOS_LED_HEARTBEAT);
else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
} else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
if (stopwatch > 50 * 1000 * 1000)
stopwatch = 0;
if ((stopwatch > 6 * 1000 * 1000) && (DeviceState
== BLidle))
JumpToApp = true;
processRX();
DataDownload(start);
}
}
void jump_to_app() {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
FLASH_Lock();
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
PIOS_USBHOOK_Deactivate();
JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
Jump_To_Application = (pFunction) JumpAddress;
/* Initialize user application's Stack Pointer */
__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
Jump_To_Application();
} else {
DeviceState = failed_jump;
return;
}
}
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
if (curr_sweep & 1) {
pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
}
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
}
uint8_t processRX() {
if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
processComand(mReceive_Buffer);
}
return true;
}

174
flight/Bootloaders/OpenPilot/op_dfu.c → flight/Bootloaders/Revolution/op_dfu.c
View File

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

78
flight/Bootloaders/Revolution/pios_board.c Normal file
View File

@ -0,0 +1,78 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the AHRS board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of
* the HW definitions to be const and static to limit their
* scope.
*
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
*/
#include "board_hw_defs.c"
#include <pios_board_info.h>
#include <pios.h>
uint32_t pios_com_telem_usb_id;
static bool board_init_complete = false;
void PIOS_Board_Init() {
if (board_init_complete) {
return;
}
/* Delay system */
PIOS_DELAY_Init();
#if defined(PIOS_INCLUDE_LED)
PIOS_LED_Init(&pios_led_cfg);
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();
/* Activate the HID-only USB configuration */
PIOS_USB_DESC_HID_ONLY_Init();
uint32_t pios_usb_id;
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
uint32_t pios_usb_hid_id;
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
PIOS_Assert(0);
}
if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
PIOS_USBHOOK_Activate();
#endif /* PIOS_INCLUDE_USB */
board_init_complete = true;
}

120
flight/Bootloaders/Revolution/pios_usb_board_data.c Normal file
View File

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

85
flight/CopterControl/Makefile
View File

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

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

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

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

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

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

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

85
flight/CopterControl/System/inc/pios_usb.h
View File

@ -1,85 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB USB Functions
* @brief PIOS USB interface code
* @{
*
* @file pios_usb.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief USB functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_H
#define PIOS_USB_H
/* Local defines */
/* Following settings allow to customise the USB device descriptor */
#ifndef PIOS_USB_VENDOR_ID
#define PIOS_USB_VENDOR_ID 0x20A0
#endif
#ifndef PIOS_USB_VENDOR_STR
#define PIOS_USB_VENDOR_STR "openpilot.org"
#endif
#ifndef PIOS_USB_PRODUCT_STR
#define PIOS_USB_PRODUCT_STR "CopterControl"
#endif
#ifndef PIOS_USB_PRODUCT_ID
#define PIOS_USB_PRODUCT_ID 0x415B
#endif
#ifndef PIOS_USB_VERSION_ID
#define PIOS_USB_VERSION_ID 0x0402 /* CopterControl board (04), Running state (02) */
#endif
/* Internal defines which are used by PIOS USB HID (don't touch) */
#define PIOS_USB_EP_NUM 2
/* Buffer table base address */
#define PIOS_USB_BTABLE_ADDRESS 0x000
/* EP0 rx/tx buffer base address */
#define PIOS_USB_ENDP0_RXADDR 0x040
#define PIOS_USB_ENDP0_TXADDR 0x080
/* EP1 Rx/Tx buffer base address for HID driver */
#define PIOS_USB_ENDP1_TXADDR 0x0C0
#define PIOS_USB_ENDP1_RXADDR 0x100
/* Global Variables */
extern void (*pEpInt_IN[7]) (void);
extern void (*pEpInt_OUT[7]) (void);
/* Public Functions */
extern int32_t PIOS_USB_Init(uint32_t mode);
extern int32_t PIOS_USB_IsInitialized(void);
extern int32_t PIOS_USB_CableConnected(void);
#endif /* PIOS_USB_H */
/**
* @}
* @}
*/

46
flight/CopterControl/System/inc/pios_usb_board_data.h Normal file
View File

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

56
flight/CopterControl/System/inc/pios_usb_hid_desc.h
View File

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

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

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

101
flight/CopterControl/System/pios_usb_board_data.c Normal file
View File

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

224
flight/CopterControl/System/pios_usb_hid_desc.c
View File

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

131
flight/CopterControl/System/taskmonitor.c
View File

@ -1,131 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
* @file taskmonitor.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Task monitoring library
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
//#include "taskmonitor.h"
// Private constants
// Private types
// Private variables
static xSemaphoreHandle lock;
static xTaskHandle handles[TASKINFO_RUNNING_NUMELEM];
static uint32_t lastMonitorTime;
// Private functions
/**
* Initialize library
*/
int32_t TaskMonitorInitialize(void)
{
lock = xSemaphoreCreateRecursiveMutex();
memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
lastMonitorTime = 0;
#if defined(DIAGNOSTICS)
lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
#endif
return 0;
}
/**
* Register a task handle with the library
*/
int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
{
if (task < TASKINFO_RUNNING_NUMELEM)
{
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
handles[task] = handle;
xSemaphoreGiveRecursive(lock);
return 0;
}
else
{
return -1;
}
}
/**
* Update the status of all tasks
*/
void TaskMonitorUpdateAll(void)
{
#if defined(DIAGNOSTICS)
TaskInfoData data;
int n;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
#if ( configGENERATE_RUN_TIME_STATS == 1 )
uint32_t currentTime;
uint32_t deltaTime;
/*
* Calculate the amount of elapsed run time between the last time we
* measured and now. Scale so that we can convert task run times
* directly to percentages.
*/
currentTime = portGET_RUN_TIME_COUNTER_VALUE();
deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */
lastMonitorTime = currentTime;
#endif
// Update all task information
for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)
{
if (handles[n] != 0)
{
data.Running[n] = TASKINFO_RUNNING_TRUE;
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
data.StackRemaining[n] = 10000;
#else
data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;
#if ( configGENERATE_RUN_TIME_STATS == 1 )
/* Generate run time stats */
data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;
#endif
#endif
}
else
{
data.Running[n] = TASKINFO_RUNNING_FALSE;
data.StackRemaining[n] = 0;
data.RunningTime[n] = 0;
}
}
// Update object
TaskInfoSet(&data);
// Done
xSemaphoreGiveRecursive(lock);
#endif
}

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

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

54
flight/EntireFlash/Makefile Normal file
View File

@ -0,0 +1,54 @@
#####
# Makefile for Entire Flash (EF) images
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2012.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := ef_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
.PHONY: bin
bin: $(OUTDIR)/$(TARGET).bin
FW_PAD = $(shell echo $$[$(FW_BANK_BASE)-$(BL_BANK_BASE)-$(BL_BANK_SIZE)])
$(OUTDIR)/$(TARGET).pad:
$(V0) @echo $(MSG_PADDING) $@
$(V1) dd status=noxfer if=/dev/zero count=$(FW_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@
BL_BIN = $(TOP)/build/bl_$(BOARD_NAME)/bl_$(BOARD_NAME).bin
FW_BIN = $(TOP)/build/fw_$(BOARD_NAME)/fw_$(BOARD_NAME).bin
$(OUTDIR)/$(TARGET).bin: $(BL_BIN) $(FW_BIN) $(OUTDIR)/$(TARGET).pad
$(V0) @echo $(MSG_FLASH_IMG) $@
$(V1) cat $(BL_BIN) $(OUTDIR)/$(TARGET).pad $(FW_BIN) > $@
.PHONY: dfu
dfu: $(OUTDIR)/$(TARGET).bin
$(V0) @echo " DFU RESCUE $<"
$(V1) ( \
sudo $(DFU_CMD) -l && \
sudo $(DFU_CMD) -d 0483:df11 -c 1 -i 0 -a 0 -D $< -s $(BL_BANK_BASE) ; \
)

429
flight/INS/Makefile
View File

@ -1,429 +0,0 @@
#####
# Project: OpenPilot INS
#
#
# Makefile for OpenPilot INS project
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := fw_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= YES
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
else
REMOVE_CMD = rm
endif
FLASH_TOOL = OPENOCD
# Paths
INS = ./
INSINC = $(INS)/inc
PIOS = ../PiOS
PIOSINC = $(PIOS)/inc
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = ../Libraries/inc
PIOSSTM32F10X = $(PIOS)/STM32F10x
PIOSCOMMON = $(PIOS)/Common
PIOSBOARDS = $(PIOS)/Boards
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
STMLIBDIR = $(APPLIBDIR)
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
STMSPDSRCDIR = $(STMSPDDIR)/src
STMSPDINCDIR = $(STMSPDDIR)/inc
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
OPDIR = ../OpenPilot
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPSYSINC = $(OPDIR)/System/inc
BOOT = ../Bootloaders/INS
BOOTINC = $(BOOT)/inc
OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
## INS:
SRC = ins.c
SRC += pios_board.c
#SRC += ins_timer.c
#SRC += insgps13state.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
#SRC += $(FLIGHTLIB)/ins_spi_comm.c
#SRC += $(FLIGHTLIB)/ins_comm_objects.c
#SRC += $(FLIGHTLIB)/CoordinateConversions.c
#SRC += $(BOOT)/ins_spi_program_slave.c
#SRC += $(BOOT)/ins_slave_test.c
#SRC += $(BOOT)/ins_spi_program.c
## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_sys.c
SRC += $(PIOSSTM32F10X)/pios_led.c
SRC += $(PIOSSTM32F10X)/pios_delay.c
SRC += $(PIOSSTM32F10X)/pios_usart.c
SRC += $(PIOSSTM32F10X)/pios_irq.c
SRC += $(PIOSSTM32F10X)/pios_i2c.c
SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.c
SRC += $(PIOSSTM32F10X)/pios_spi.c
SRC += $(PIOSSTM32F10X)/pios_exti.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bma180.c
SRC += $(PIOSCOMMON)/pios_hmc5883.c
SRC += $(PIOSCOMMON)/pios_bmp085.c
SRC += $(PIOSCOMMON)/pios_imu3000.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c
SRC += $(CMSISDIR)/system_stm32f10x.c
## Used parts of the STM-Library
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
SRC += $(STMSPDSRCDIR)/misc.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
# List Assembler source files here which must be assembled in ARM-Mode..
ASRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSSTM32F10X)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)
EXTRAINCDIRS += $(STMSPDINCDIR)
EXTRAINCDIRS += $(CMSISDIR)
EXTRAINCDIRS += $(INSINC)
EXTRAINCDIRS += $(OPUAVSYNTHDIR)
EXTRAINCDIRS += $(BOOTINC)
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
CFLAGS += -Os
CFLAGS += -DGENERAL_COV
else
CFLAGS += -Os
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
CDEFS = -DSTM32F10X_$(MODEL)
CDEFS += -DUSE_STDPERIPH_DRIVER
CDEFS += -DUSE_$(BOARD)
CDEFS += -DIN_INS
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS += -g$(DEBUGF) #-DDEBUG
else
CFLAGS += -g$(DEBUGF)
endif
CFLAGS += -ffast-math
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
#CFLAGS += -Werror
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
# Set linker-script name depending on selected submodel name
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
# Define programs and commands.
REMOVE = $(REMOVE_CMD) -f
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
# Default target.
all: gccversion build
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin lss sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin lss sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
# Compile: create assembler files from C source files. ARM only
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
# Add jtag targets (program and wipe)
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
.PHONY: elf lss sym hex bin bino opfw
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
bino: $(OUTDIR)/$(TARGET).bin.o
opfw: $(OUTDIR)/$(TARGET).opfw
# Display sizes of sections.
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Install: install binary file with prefix/suffix into install directory
install: $(OUTDIR)/$(TARGET).opfw
ifneq ($(INSTALL_DIR),)
@echo $(MSG_INSTALLING) $(call toprel, $<)
$(V1) mkdir -p $(INSTALL_DIR)
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
else
$(error INSTALL_DIR must be specified for $@)
endif
# Target: clean project.
clean: clean_list
clean_list :
@echo $(MSG_CLEANING)
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
$(V1) $(REMOVE) $(ALLOBJ)
$(V1) $(REMOVE) $(LSTFILES)
$(V1) $(REMOVE) $(DEPFILES)
$(V1) $(REMOVE) $(SRC:.c=.s)
$(V1) $(REMOVE) $(SRCARM:.c=.s)
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
else
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all build clean clean_list install

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

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

@ -1,61 +0,0 @@
/**
******************************************************************************
* @addtogroup INS INS
* @brief The INS configuration
*
* @{
* @addtogroup INS_Main
* @brief INS configuration
* @{
*
* @file pios_config.h
* @author David "Buzz" Carlson (buzz@chebuzz.com)
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief PiOS configuration header.
* - Central compile time config for the project.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_HMC5883
#define PIOS_INCLUDE_BMP085
#define PIOS_INCLUDE_IMU3000
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_BMA180
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/

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(LED1, 1);
else
blink(LED2, 1);
}
#if defined (PIOS_INCLUDE_HMC5883)
void test_mag()
{
if(PIOS_HMC5883_Test())
blink(LED1, 2);
else
blink(LED2, 2);
}
#endif
#if defined (PIOS_INCLUDE_BMP085)
void test_pressure()
{
if(PIOS_BMP085_Test())
blink(LED1, 3);
else
blink(LED2, 3);
}
#endif
#if defined (PIOS_INCLUDE_IMU3000)
void test_imu()
{
if(PIOS_IMU3000_Test())
blink(LED1, 4);
else
blink(LED2, 4);
}
#endif
extern void PIOS_Board_Init(void);
struct pios_bma180_data bma180_data;
/**
* @brief INS Main function
*/
int main()
{
reset_values();
PIOS_Board_Init();
while (1)
{
test_accel();
PIOS_DELAY_WaitmS(1000);
#if defined (PIOS_INCLUDE_HMC5883)
test_mag();
PIOS_DELAY_WaitmS(1000);
#endif
#if defined (PIOS_INCLUDE_BMP085)
test_pressure();
PIOS_DELAY_WaitmS(1000);
#endif
#if defined (PIOS_INCLUDE_IMU3000)
test_imu();
PIOS_DELAY_WaitmS(1000);
#endif
PIOS_DELAY_WaitmS(3000);
}
}
/**
* @brief Populate fields with initial values
*/
void reset_values()
{
accel_data.calibration.scale[0][1] = 0;
accel_data.calibration.scale[1][0] = 0;
accel_data.calibration.scale[0][2] = 0;
accel_data.calibration.scale[2][0] = 0;
accel_data.calibration.scale[1][2] = 0;
accel_data.calibration.scale[2][1] = 0;
accel_data.calibration.scale[0][0] = 0.0359;
accel_data.calibration.scale[1][1] = 0.0359;
accel_data.calibration.scale[2][2] = 0.0359;
accel_data.calibration.scale[0][3] = -73.5;
accel_data.calibration.scale[1][3] = -73.5;
accel_data.calibration.scale[2][3] = -73.5;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
mag_data.calibration.bias[0] = 0;
mag_data.calibration.bias[1] = 0;
mag_data.calibration.bias[2] = 0;
mag_data.calibration.variance[0] = 50;
mag_data.calibration.variance[1] = 50;
mag_data.calibration.variance[2] = 50;
}
/**
* @}
*/

586
flight/INS/pios_board.c
View File

@ -1,586 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5883 HMC5883 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
*
* @file pios_board.c
* @author David "Buzz" Carlson (buzz@chebuzz.com)
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Defines board specific static initializers for hardware for the INS board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* SPI2 Interface
* - Used for mainboard communications and magnetometer
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_op_mag_irq_handler(void);
void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler")));
void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler")));
static const struct pios_spi_cfg pios_spi_op_mag_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
uint32_t pios_spi_op_mag_id;
void PIOS_SPI_op_mag_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_op_mag_id);
}
/* SPI1 Interface
* - Used for BMA180 accelerometer
*/
void PIOS_SPI_accel_irq_handler(void);
void DMA1_Channel2_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler")));
void DMA1_Channel3_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler")));
static const struct pios_spi_cfg pios_spi_accel_cfg = {
.regs = SPI1,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_1Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
},
.use_crc = FALSE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel2,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel3,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.sclk = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.miso = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.mosi = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
static uint32_t pios_spi_accel_id;
void PIOS_SPI_accel_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_accel_id);
}
#endif /* PIOS_INCLUDE_SPI */
#if defined(PIOS_INCLUDE_GPS)
#include <pios_usart_priv.h>
/*
* GPS USART
*/
static const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl =
USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_INCLUDE_GPS */
#ifdef PIOS_COM_AUX
/*
* AUX USART
*/
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART4,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl =
USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_COM_AUX */
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>
#if 0
#define PIOS_COM_AUX_TX_BUF_LEN 192
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
#endif
#define PIOS_COM_GPS_RX_BUF_LEN 96
static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>
/*
* I2C Adapters
*/
void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void);
void PIOS_I2C_pres_mag_adapter_er_irq_handler(void);
void I2C1_EV_IRQHandler()
__attribute__ ((alias("PIOS_I2C_pres_mag_adapter_ev_irq_handler")));
void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
.regs = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 200000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_pres_mag_adapter_id;
void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_pres_mag_adapter_id);
}
void PIOS_I2C_pres_mag_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_pres_mag_adapter_id);
}
void PIOS_I2C_gyro_adapter_ev_irq_handler(void);
void PIOS_I2C_gyro_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
.regs = I2C2,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 400000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_gyro_adapter_id;
void PIOS_I2C_gyro_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_gyro_adapter_id);
}
void PIOS_I2C_gyro_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_gyro_adapter_id);
}
#endif /* PIOS_INCLUDE_I2C */
extern const struct pios_com_driver pios_usart_com_driver;
uint32_t pios_com_aux_id;
uint32_t pios_com_gps_id;
/**
* PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* IAP System Setup */
PIOS_IAP_Init();
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_GPS)
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
pios_com_gps_rx_buffer, sizeof(pios_com_gps_rx_buffer),
NULL, 0)) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_GPS */
#endif /* PIOS_INCLUDE_COM */
#if defined (PIOS_INCLUDE_I2C)
if (PIOS_I2C_Init(&pios_i2c_pres_mag_adapter_id, &pios_i2c_pres_mag_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
#if defined (PIOS_INCLUDE_BMP085)
PIOS_BMP085_Init();
#endif /* PIOS_INCLUDE_BMP085 */
#if defined (PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init();
#endif /* PIOS_INCLUDE_HMC5883 */
#if defined(PIOS_INCLUDE_IMU3000)
if (PIOS_I2C_Init(&pios_i2c_gyro_adapter_id, &pios_i2c_gyro_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
PIOS_IMU3000_Init();
#endif /* PIOS_INCLUDE_IMU3000 */
#endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_INCLUDE_SPI)
/* Set up the SPI interface to the accelerometer*/
if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) {
PIOS_DEBUG_Assert(0);
}
PIOS_BMA180_Attach(pios_spi_accel_id);
// #include "ahrs_spi_comm.h"
// InsInitComms();
//
// /* Set up the SPI interface to the OP board */
// if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
// PIOS_DEBUG_Assert(0);
// }
//
// InsConnect(pios_spi_op_id);
#endif /* PIOS_INCLUDE_SPI */
}
/**
* @}
* @}
*/

43
flight/Libraries/CoordinateConversions.c
View File

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

0
flight/PipXtreme/aes.c → flight/Libraries/aes.c
View File

35
flight/Libraries/ahrs_comm_objects.c
View File

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

16
flight/Libraries/ahrs_spi_comm.c
View File

@ -33,7 +33,7 @@
#include "pios_spi.h"
#include "pios_irq.h"
#include "ahrs_spi_program_slave.h"
#include "STM32103CB_AHRS.h"
//#include "STM32103CB_AHRS.h"
#endif
/*transmit and receive packet magic numbers.
@ -53,7 +53,7 @@ typedef enum { COMMS_NULL, COMMS_OBJECT } COMMSCOMMAND;
//The maximum number of objects that can be updated in one cycle.
//Currently the link is capable of sending 3 packets per cycle but 2 is enough
#define MAX_UPDATE_OBJECTS 1
#define MAX_UPDATE_OBJECTS 2
//Number of transmissions + 1 before we expect to see the data acknowledge
//This is controlled by the SPI hardware.
@ -298,13 +298,11 @@ int32_t AhrsConnectCallBack(AhrsObjHandle obj, AhrsEventCallback cb)
return (0);
}
AhrsCommStatus AhrsGetStatus()
void AhrsGetStatus(AhrsCommStatus * status)
{
AhrsCommStatus status;
status.remote = rxPacket.status;
status.local = txPacket.status;
status.linkOk = linkOk;
return (status);
status->remote = rxPacket.status;
status->local = txPacket.status;
status->linkOk = linkOk;
}
@ -439,7 +437,9 @@ void AhrsSendObjects(void)
void SendPacket(void)
{
#ifndef IN_AHRS
PIOS_SPI_RC_PinSet(opahrs_spi_id, 0);
#endif
//no point checking if this failed. There isn't much we could do about it if it did fail
PIOS_SPI_TransferBlock(opahrs_spi_id, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
}

532
flight/Libraries/fifo_buffer.c
View File

@ -1,266 +1,266 @@
/**
******************************************************************************
*
* @file fifo_buffer.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief GPIO input functions
* @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 <string.h>
#include "fifo_buffer.h"
// *****************************************************************************
// circular buffer functions
uint16_t fifoBuf_getSize(t_fifo_buffer *buf)
{ // return the usable size of the buffer
uint16_t buf_size = buf->buf_size;
if (buf_size > 0)
return (buf_size - 1);
else
return 0;
}
uint16_t fifoBuf_getUsed(t_fifo_buffer *buf)
{ // return the number of bytes available in the rx buffer
uint16_t rd = buf->rd;
uint16_t wr = buf->wr;
uint16_t buf_size = buf->buf_size;
uint16_t num_bytes = wr - rd;
if (wr < rd)
num_bytes = (buf_size - rd) + wr;
return num_bytes;
}
uint16_t fifoBuf_getFree(t_fifo_buffer *buf)
{ // return the free space size in the buffer
uint16_t buf_size = buf->buf_size;
uint16_t num_bytes = fifoBuf_getUsed(buf);
return ((buf_size - num_bytes) - 1);
}
void fifoBuf_clearData(t_fifo_buffer *buf)
{ // remove all data from the buffer
buf->rd = buf->wr;
}
void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len)
{ // remove a number of bytes from the buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return; // nothing to remove
rd += num_bytes;
if (rd >= buf_size)
rd -= buf_size;
buf->rd = rd;
}
int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf)
{ // get a data byte from the buffer without removing it
uint16_t rd = buf->rd;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes < 1)
return -1; // no byte retuened
return buf->buf_ptr[rd]; // return the byte
}
int16_t fifoBuf_getByte(t_fifo_buffer *buf)
{ // get a data byte from the buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes < 1)
return -1; // no byte returned
uint8_t b = buff[rd];
if (++rd >= buf_size)
rd = 0;
buf->rd = rd;
return b; // return the byte
}
uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
{ // get data from the buffer without removing it
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = 0;
while (num_bytes > 0)
{
uint16_t j = buf_size - rd;
if (j > num_bytes)
j = num_bytes;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
return i; // return number of bytes copied
}
uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
{ // get data from our rx buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = 0;
while (num_bytes > 0)
{
uint16_t j = buf_size - rd;
if (j > num_bytes)
j = num_bytes;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
buf->rd = rd;
return i; // return number of bytes copied
}
uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b)
{ // add a data byte to the buffer
uint16_t wr = buf->wr;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
uint16_t num_bytes = fifoBuf_getFree(buf);
if (num_bytes < 1)
return 0;
buff[wr] = b;
if (++wr >= buf_size)
wr = 0;
buf->wr = wr;
return 1; // return number of bytes copied
}
uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
{ // add data to the buffer
uint16_t wr = buf->wr;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
uint16_t num_bytes = fifoBuf_getFree(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = 0;
while (num_bytes > 0)
{
uint16_t j = buf_size - wr;
if (j > num_bytes)
j = num_bytes;
memcpy(buff + wr, p + i, j);
i += j;
num_bytes -= j;
wr += j;
if (wr >= buf_size)
wr = 0;
}
buf->wr = wr;
return i; // return number of bytes copied
}
void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size)
{
buf->buf_ptr = (uint8_t *)buffer;
buf->rd = 0;
buf->wr = 0;
buf->buf_size = buffer_size;
}
// *****************************************************************************
/**
******************************************************************************
*
* @file fifo_buffer.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief GPIO input functions
* @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 <string.h>
#include "fifo_buffer.h"
// *****************************************************************************
// circular buffer functions
uint16_t fifoBuf_getSize(t_fifo_buffer *buf)
{ // return the usable size of the buffer
uint16_t buf_size = buf->buf_size;
if (buf_size > 0)
return (buf_size - 1);
else
return 0;
}
uint16_t fifoBuf_getUsed(t_fifo_buffer *buf)
{ // return the number of bytes available in the rx buffer
uint16_t rd = buf->rd;
uint16_t wr = buf->wr;
uint16_t buf_size = buf->buf_size;
uint16_t num_bytes = wr - rd;
if (wr < rd)
num_bytes = (buf_size - rd) + wr;
return num_bytes;
}
uint16_t fifoBuf_getFree(t_fifo_buffer *buf)
{ // return the free space size in the buffer
uint16_t buf_size = buf->buf_size;
uint16_t num_bytes = fifoBuf_getUsed(buf);
return ((buf_size - num_bytes) - 1);
}
void fifoBuf_clearData(t_fifo_buffer *buf)
{ // remove all data from the buffer
buf->rd = buf->wr;
}
void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len)
{ // remove a number of bytes from the buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return; // nothing to remove
rd += num_bytes;
if (rd >= buf_size)
rd -= buf_size;
buf->rd = rd;
}
int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf)
{ // get a data byte from the buffer without removing it
uint16_t rd = buf->rd;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes < 1)
return -1; // no byte retuened
return buf->buf_ptr[rd]; // return the byte
}
int16_t fifoBuf_getByte(t_fifo_buffer *buf)
{ // get a data byte from the buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes < 1)
return -1; // no byte returned
uint8_t b = buff[rd];
if (++rd >= buf_size)
rd = 0;
buf->rd = rd;
return b; // return the byte
}
uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
{ // get data from the buffer without removing it
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = 0;
while (num_bytes > 0)
{
uint16_t j = buf_size - rd;
if (j > num_bytes)
j = num_bytes;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
return i; // return number of bytes copied
}
uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
{ // get data from our rx buffer
uint16_t rd = buf->rd;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = 0;
while (num_bytes > 0)
{
uint16_t j = buf_size - rd;
if (j > num_bytes)
j = num_bytes;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
buf->rd = rd;
return i; // return number of bytes copied
}
uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b)
{ // add a data byte to the buffer
uint16_t wr = buf->wr;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
uint16_t num_bytes = fifoBuf_getFree(buf);
if (num_bytes < 1)
return 0;
buff[wr] = b;
if (++wr >= buf_size)
wr = 0;
buf->wr = wr;
return 1; // return number of bytes copied
}
uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
{ // add data to the buffer
uint16_t wr = buf->wr;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buf_ptr;
uint16_t num_bytes = fifoBuf_getFree(buf);
if (num_bytes > len)
num_bytes = len;
if (num_bytes < 1)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = 0;
while (num_bytes > 0)
{
uint16_t j = buf_size - wr;
if (j > num_bytes)
j = num_bytes;
memcpy(buff + wr, p + i, j);
i += j;
num_bytes -= j;
wr += j;
if (wr >= buf_size)
wr = 0;
}
buf->wr = wr;
return i; // return number of bytes copied
}
void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size)
{
buf->buf_ptr = (uint8_t *)buffer;
buf->rd = 0;
buf->wr = 0;
buf->buf_size = buffer_size;
}
// *****************************************************************************

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

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

82
flight/PipXtreme/reserved/api_comms.h → flight/Libraries/inc/NMEA.h
View File

@ -1,40 +1,42 @@
/**
******************************************************************************
*
* @file api_comms.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief RF Module hardware layer
* @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 __API_COMMS_H__
#define __API_COMMS_H__
#include "stdint.h"
// *****************************************************************************
void api_1ms_tick(void);
void api_process(void);
void api_init(void);
// *****************************************************************************
#endif
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup GSPModule GPS Module
* @brief Process GPS information
* @{
*
* @file NMEA.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief GPS module, handles GPS and NMEA stream
* @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 NMEA_H
#define NMEA_H
#include <stdbool.h>
#include <stdint.h>
#define NMEA_MAX_PACKET_LENGTH 96
extern bool NMEA_update_position(char *nmea_sentence);
extern bool NMEA_checksum(char *nmea_sentence);
#endif /* NMEA_H */

0
flight/PipXtreme/inc/aes.h → flight/Libraries/inc/aes.h
View File

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

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

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

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

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

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

0
flight/AHRS/inc/insgps.h → flight/Libraries/inc/insgps.h
View File

121
flight/Libraries/inc/packet_handler.h Normal file
View File

@ -0,0 +1,121 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef __PACKET_HANDLER_H__
#define __PACKET_HANDLER_H__
// Public defines / macros
#define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p)
#define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t*)p)
// Public types
typedef enum {
PACKET_TYPE_NONE = 0,
PACKET_TYPE_CONNECT, // for requesting a connection
PACKET_TYPE_DISCONNECT, // to tell the other modem they cannot connect to us
PACKET_TYPE_READY, // tells the other modem we are ready to accept more data
PACKET_TYPE_NOTREADY, // tells the other modem we're not ready to accept more data - we can also send user data in this packet type
PACKET_TYPE_STATUS, // broadcasts status of this modem
PACKET_TYPE_DATARATE, // for changing the RF data rate
PACKET_TYPE_PING, // used to check link is still up
PACKET_TYPE_ADJUST_TX_PWR, // used to ask the other modem to adjust it's tx power
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_ACKED_DATA, // data packet that requies an ACK
PACKET_TYPE_PPM, // PPM relay values
PACKET_TYPE_ACK,
PACKET_TYPE_NACK
} PHPacketType;
typedef struct {
uint32_t destination_id;
uint32_t source_id;
uint8_t type;
uint8_t data_size;
uint8_t tx_seq;
uint8_t rx_seq;
} PHPacketHeader;
#define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY)
#define PH_PACKET_SIZE(p) (p->data + p->header.data_size - (uint8_t*)p + RS_ECC_NPARITY)
typedef struct {
PHPacketHeader header;
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
} PHPacket, *PHPacketHandle;
#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint16_t channels[PIOS_RCVR_MAX_CHANNELS];
uint8_t ecc[RS_ECC_NPARITY];
} PHPpmPacket, *PHPpmPacketHandle;
#define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint16_t retries;
uint16_t errors;
uint16_t uavtalk_errors;
uint16_t dropped;
uint16_t resets;
uint8_t ecc[RS_ECC_NPARITY];
} PHStatusPacket, *PHStatusPacketHandle;
typedef struct {
uint8_t winSize;
uint16_t maxConnections;
} PacketHandlerConfig;
typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
typedef void (*PHDataHandler)(uint8_t *data, uint8_t len, int8_t rssi, int8_t afc);
typedef void (*PHStatusHandler)(PHStatusPacketHandle s, int8_t rssi, int8_t afc);
typedef void (*PHPPMHandler)(uint16_t *channels);
typedef uint32_t PHInstHandle;
// Public functions
PHInstHandle PHInitialize(PacketHandlerConfig *cfg);
void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f);
void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f);
void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f);
void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
PHPacketHandle PHGetRXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
PHPacketHandle PHGetTXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len);
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error);
#endif // __PACKET_HANDLER_H__
/**
* @}
* @}
*/

0
flight/OpenPilot/System/inc/taskmonitor.h → flight/Libraries/inc/taskmonitor.h
View File

66
flight/AHRS/insgps13state.c → flight/Libraries/insgps13state.c
View File

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

0
flight/AHRS/insgps16state.c → flight/Libraries/insgps16state.c Normal file → Executable file
View File

328
flight/Libraries/insgps_helper.c Normal file
View File

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

507
flight/Libraries/packet_handler.c Normal file
View File

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

10
flight/Libraries/rscode/LICENSE Normal file
View File

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

50
flight/Libraries/rscode/Makefile Normal file
View File

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

27
flight/Libraries/rscode/README Normal file
View File

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

324
flight/Libraries/rscode/berlekamp.c Normal file
View File

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

18
flight/Libraries/rscode/config.doc Normal file
View File

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

66
flight/Libraries/rscode/crcgen.c Normal file
View File

@ -0,0 +1,66 @@
/*****************************
* Copyright Henry Minsky (hqm@alum.mit.edu) 1991-2009
*
* This software library is licensed under terms of the GNU GENERAL
* PUBLIC LICENSE
*
* RSCODE is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RSCODE is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Rscode. If not, see <http://www.gnu.org/licenses/>.
* Commercial licensing is available under a separate license, please
* contact author for details.
*
* Source code is available at http://rscode.sourceforge.net
*
* CRC-CCITT generator simulator for byte wide data.
*
*
* CRC-CCITT = x^16 + x^12 + x^5 + 1
*
*
******************************/
#include "ecc.h"
BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum);
/* Computes the CRC-CCITT checksum on array of byte data, length len
*/
BIT16 crc_ccitt(unsigned char *msg, int len)
{
int i;
BIT16 acc = 0;
for (i = 0; i < len; i++) {
acc = crchware((BIT16) msg[i], (BIT16) 0x1021, acc);
}
return(acc);
}
/* models crc hardware (minor variation on polynomial division algorithm) */
BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum)
{
static BIT16 i;
data <<= 8;
for (i = 8; i > 0; i--) {
if ((data ^ accum) & 0x8000)
accum = ((accum << 1) ^ genpoly) & 0xFFFF;
else
accum = (accum<<1) & 0xFFFF;
data = (data<<1) & 0xFFFF;
}
return (accum);
}

98
flight/Libraries/rscode/ecc.h Normal file
View File

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

128
flight/Libraries/rscode/example.c Normal file
View File

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

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