mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into pt/MapEnhancements
Conflicts: ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
This commit is contained in:
PT_Dreamer
commit
e71ab57ffa
3
.gitmodules
vendored
Normal file
3
.gitmodules
vendored
Normal file
@ -0,0 +1,3 @@
|
||||
[submodule "overo"]
|
||||
path = overo
|
||||
url = git@github.com:peabody124/op_overo.git
|
||||
31
HISTORY.txt
31
HISTORY.txt
@ -1,5 +1,36 @@
|
||||
Short summary of changes. For a complete list see the git log.
|
||||
|
||||
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.
|
||||
|
||||
246
Makefile
246
Makefile
@ -72,6 +72,7 @@ help:
|
||||
@echo " arm_sdk_install - Install the Code Sourcery ARM gcc toolchain"
|
||||
@echo " openocd_install - Install the OpenOCD JTAG daemon"
|
||||
@echo " stm32flash_install - Install the stm32flash tool for unbricking boards"
|
||||
@echo " dfuutil_install - Install the dfu-util tool for unbricking F4-based boards"
|
||||
@echo
|
||||
@echo " [Big Hammer]"
|
||||
@echo " all - Generate UAVObjects, build openpilot firmware and gcs"
|
||||
@ -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"
|
||||
@ -172,7 +170,6 @@ qt_sdk_install : | $(DL_DIR) $(TOOLS_DIR)
|
||||
qt_sdk_install: qt_sdk_clean
|
||||
# download the source only if it's newer than what we already have
|
||||
$(V1) wget -N --content-disposition -P "$(DL_DIR)" "$(QT_SDK_URL)"
|
||||
|
||||
# tell the user exactly which path they should select in the GUI
|
||||
$(V1) echo "*** NOTE NOTE NOTE ***"
|
||||
$(V1) echo "*"
|
||||
@ -209,36 +206,173 @@ arm_sdk_clean:
|
||||
$(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR)
|
||||
|
||||
# Set up openocd tools
|
||||
OPENOCD_DIR := $(TOOLS_DIR)/openocd
|
||||
OPENOCD_DIR := $(TOOLS_DIR)/openocd
|
||||
OPENOCD_WIN_DIR := $(TOOLS_DIR)/openocd_win
|
||||
OPENOCD_BUILD_DIR := $(DL_DIR)/openocd-build
|
||||
|
||||
.PHONY: openocd_install
|
||||
openocd_install: | $(DL_DIR) $(TOOLS_DIR)
|
||||
openocd_install: OPENOCD_URL := http://sourceforge.net/projects/openocd/files/openocd/0.5.0/openocd-0.5.0.tar.bz2/download
|
||||
openocd_install: OPENOCD_FILE := openocd-0.5.0.tar.bz2
|
||||
# order-only prereq on directory existance:
|
||||
openocd_install: | $(DL_DIR) $(TOOLS_DIR)
|
||||
openocd_install: openocd_clean
|
||||
# download the source only if it's newer than what we already have
|
||||
$(V1) wget -N -P "$(DL_DIR)" --trust-server-name "$(OPENOCD_URL)"
|
||||
|
||||
# extract the source
|
||||
$(V1) [ ! -d "$(DL_DIR)/openocd-build" ] || $(RM) -r "$(DL_DIR)/openocd-build"
|
||||
$(V1) 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
|
||||
@ -260,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
|
||||
@ -429,32 +595,60 @@ bu_$(1)_clean:
|
||||
$(V1) $(RM) -fr $(BUILD_DIR)/bu_$(1)
|
||||
endef
|
||||
|
||||
# $(1) = Canonical board name all in lower case (e.g. coptercontrol)
|
||||
define EF_TEMPLATE
|
||||
.PHONY: ef_$(1)
|
||||
ef_$(1): ef_$(1)_bin
|
||||
|
||||
ef_$(1)_%: bl_$(1)_bin fw_$(1)_bin
|
||||
$(V1) mkdir -p $(BUILD_DIR)/ef_$(1)/dep
|
||||
$(V1) cd $(ROOT_DIR)/flight/EntireFlash && \
|
||||
$$(MAKE) -r --no-print-directory \
|
||||
BOARD_NAME=$(1) \
|
||||
TCHAIN_PREFIX="$(ARM_SDK_PREFIX)" \
|
||||
DFU_CMD="$(DFUUTIL_DIR)/bin/dfu-util" \
|
||||
$$*
|
||||
|
||||
.PHONY: ef_$(1)_clean
|
||||
ef_$(1)_clean:
|
||||
$(V0) @echo " CLEAN $$@"
|
||||
$(V1) $(RM) -fr $(BUILD_DIR)/ef_$(1)
|
||||
endef
|
||||
|
||||
# $(1) = Canonical board name all in lower case (e.g. coptercontrol)
|
||||
define BOARD_PHONY_TEMPLATE
|
||||
.PHONY: all_$(1)
|
||||
all_$(1): $$(filter fw_$(1), $$(FW_TARGETS))
|
||||
all_$(1): $$(filter bl_$(1), $$(BL_TARGETS))
|
||||
all_$(1): $$(filter bu_$(1), $$(BU_TARGETS))
|
||||
all_$(1): $$(filter ef_$(1), $$(EF_TARGETS))
|
||||
|
||||
.PHONY: all_$(1)_clean
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter fw_$(1), $$(FW_TARGETS)))
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter bl_$(1), $$(BL_TARGETS)))
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS)))
|
||||
endef
|
||||
|
||||
ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins
|
||||
ALL_BOARDS := coptercontrol pipxtreme simposix
|
||||
|
||||
# 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
|
||||
|
||||
# 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.
|
||||
@ -465,6 +659,7 @@ BU_BOARDS := $(filter-out ins, $(BU_BOARDS))
|
||||
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))
|
||||
@ -478,9 +673,13 @@ all_bl_clean: $(addsuffix _clean, $(BL_TARGETS))
|
||||
all_bu: $(addsuffix _opfw, $(BU_TARGETS))
|
||||
all_bu_clean: $(addsuffix _clean, $(BU_TARGETS))
|
||||
|
||||
.PHONY: all_ef all_ef_clean
|
||||
all_ef: $(EF_TARGETS))
|
||||
all_ef_clean: $(addsuffix _clean, $(EF_TARGETS))
|
||||
|
||||
.PHONY: all_flight all_flight_clean
|
||||
all_flight: all_fw all_bl all_bu
|
||||
all_flight_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))))
|
||||
@ -494,13 +693,8 @@ $(foreach board, $(ALL_BOARDS), $(eval $(call FW_TEMPLATE,$(board),$($(board)_fr
|
||||
# Expand the bootloader rules
|
||||
$(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_friendly))))
|
||||
|
||||
.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))))
|
||||
|
||||
.PHONY: sim_win32
|
||||
sim_win32: sim_win32_exe
|
||||
|
||||
1265
flight/AHRS/ahrs.c
1265
flight/AHRS/ahrs.c
@ -1,1265 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_Main
|
||||
* @brief Main function which does the hardware dependent stuff
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief INSGPS Test Program
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "ahrs.h"
|
||||
#include <pios_board_info.h>
|
||||
#include "pios.h"
|
||||
#include "ahrs_timer.h"
|
||||
#include "ahrs_spi_comm.h"
|
||||
#include "insgps.h"
|
||||
#include "CoordinateConversions.h"
|
||||
#include <stdbool.h>
|
||||
#include "fifo_buffer.h"
|
||||
|
||||
#define DEG_TO_RAD (M_PI / 180.0)
|
||||
#define RAD_TO_DEG (180.0 / M_PI)
|
||||
|
||||
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
|
||||
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
|
||||
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */
|
||||
#define INSGPS_MAGLEN 1000
|
||||
#define INSGPS_MAGTOL 0.5 /* error in magnetic vector length to use */
|
||||
|
||||
#define GYRO_OOB(x) ((x > (1000 * DEG_TO_RAD)) || (x < (-1000 * DEG_TO_RAD)))
|
||||
#define ACCEL_OOB(x) (((x > 12*9.81) || (x < -12*9.81)))
|
||||
#define ISNAN(x) (x != x)
|
||||
// down-sampled data index
|
||||
#define ACCEL_RAW_X_IDX 2
|
||||
#define ACCEL_RAW_Y_IDX 0
|
||||
#define ACCEL_RAW_Z_IDX 4
|
||||
#define GYRO_RAW_X_IDX 1
|
||||
#define GYRO_RAW_Y_IDX 3
|
||||
#define GYRO_RAW_Z_IDX 5
|
||||
#define GYRO_TEMP_RAW_XY_IDX 6
|
||||
#define GYRO_TEMP_RAW_Z_IDX 7
|
||||
#define MAG_RAW_X_IDX 1
|
||||
#define MAG_RAW_Y_IDX 0
|
||||
#define MAG_RAW_Z_IDX 2
|
||||
|
||||
// For debugging the raw sensors
|
||||
//#define DUMP_RAW
|
||||
//#define DUMP_EKF
|
||||
//#define PIP_DUMP_RAW
|
||||
|
||||
volatile int8_t ahrs_algorithm;
|
||||
|
||||
/* INS functions */
|
||||
void ins_outdoor_update();
|
||||
void ins_indoor_update();
|
||||
void simple_update();
|
||||
|
||||
/* Data accessors */
|
||||
void adc_callback(float *);
|
||||
bool get_accel_gyro_data();
|
||||
void process_mag_data();
|
||||
void reset_values();
|
||||
void calibrate_sensors(void);
|
||||
|
||||
/* Communication functions */
|
||||
void send_calibration(void);
|
||||
void send_attitude(void);
|
||||
void send_velocity(void);
|
||||
void send_position(void);
|
||||
void homelocation_callback(AhrsObjHandle obj);
|
||||
void altitude_callback(AhrsObjHandle obj);
|
||||
void calibration_callback(AhrsObjHandle obj);
|
||||
void gps_callback(AhrsObjHandle obj);
|
||||
void settings_callback(AhrsObjHandle obj);
|
||||
void affine_rotate(float scale[3][4], float rotation[3]);
|
||||
void calibration(float result[3], float scale[3][4], float arg[3]);
|
||||
|
||||
/* Bootloader related functions and var*/
|
||||
void firmwareiapobj_callback(AhrsObjHandle obj);
|
||||
volatile uint8_t reset_count=0;
|
||||
|
||||
/**
|
||||
* @addtogroup AHRS_Global_Data AHRS Global Data
|
||||
* @{
|
||||
* Public data. Used by both EKF and the sender
|
||||
*/
|
||||
|
||||
//! Contains the data from the mag sensor chip
|
||||
struct mag_sensor mag_data;
|
||||
|
||||
//! Contains the data from the accelerometer
|
||||
struct accel_sensor accel_data;
|
||||
|
||||
//! Contains the data from the gyro
|
||||
struct gyro_sensor gyro_data;
|
||||
|
||||
//! Conains the current estimate of the attitude
|
||||
struct attitude_solution attitude_data;
|
||||
|
||||
//! Contains data from the altitude sensor
|
||||
struct altitude_sensor altitude_data;
|
||||
|
||||
//! Contains data from the GPS (via the SPI link)
|
||||
struct gps_sensor gps_data;
|
||||
|
||||
//! The oversampling rate, ekf is 2k / this
|
||||
uint8_t adc_oversampling = 15;
|
||||
|
||||
//! Offset correction of barometric alt, to match gps data
|
||||
static float baro_offset = 0;
|
||||
|
||||
static float mag_len = 0;
|
||||
|
||||
typedef enum { AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING } states;
|
||||
volatile int32_t ekf_too_slow;
|
||||
volatile int32_t total_conversion_blocks;
|
||||
|
||||
//! Buffer to allow ADC to run a bit faster than EKF
|
||||
t_fifo_buffer adc_fifo_buffer;
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* INS functions */
|
||||
|
||||
/**
|
||||
* @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values.
|
||||
*/
|
||||
void ins_outdoor_update()
|
||||
{
|
||||
float gyro[3], accel[3], vel[3];
|
||||
static uint32_t last_gps_time = 0;
|
||||
uint16_t sensors;
|
||||
|
||||
// format data for INS algo
|
||||
gyro[0] = gyro_data.filtered.x;
|
||||
gyro[1] = gyro_data.filtered.y;
|
||||
gyro[2] = gyro_data.filtered.z;
|
||||
accel[0] = accel_data.filtered.x,
|
||||
accel[1] = accel_data.filtered.y,
|
||||
accel[2] = accel_data.filtered.z,
|
||||
|
||||
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
|
||||
attitude_data.quaternion.q1 = Nav.q[0];
|
||||
attitude_data.quaternion.q2 = Nav.q[1];
|
||||
attitude_data.quaternion.q3 = Nav.q[2];
|
||||
attitude_data.quaternion.q4 = Nav.q[3];
|
||||
send_attitude(); // get message out quickly
|
||||
send_velocity();
|
||||
send_position();
|
||||
INSCovariancePrediction(1 / (float)EKF_RATE);
|
||||
|
||||
sensors = 0;
|
||||
|
||||
/*
|
||||
* Detect if greater than certain time since last gps update and if so
|
||||
* reset EKF to that position since probably drifted too far for safe
|
||||
* update
|
||||
*/
|
||||
uint32_t this_gps_time = timer_count();
|
||||
float gps_delay;
|
||||
|
||||
if (this_gps_time < last_gps_time)
|
||||
gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
|
||||
else
|
||||
gps_delay = (this_gps_time - last_gps_time) / timer_rate();
|
||||
last_gps_time = this_gps_time;
|
||||
|
||||
if (gps_data.updated)
|
||||
{
|
||||
vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
|
||||
vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
|
||||
vel[2] = 0;
|
||||
|
||||
if(gps_delay > INSGPS_GPS_TIMEOUT)
|
||||
INSPosVelReset(gps_data.NED,vel); // position stale, reset
|
||||
else {
|
||||
sensors |= HORIZ_SENSORS | POS_SENSORS;
|
||||
}
|
||||
|
||||
/*
|
||||
* When using gps need to make sure that barometer is brought into NED frame
|
||||
* we should try and see if the altitude from the home location is good enough
|
||||
* to use for the offset but for now starting with this conservative filter
|
||||
*/
|
||||
if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
|
||||
baro_offset = gps_data.NED[2] + altitude_data.altitude;
|
||||
} else {
|
||||
/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
|
||||
baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
|
||||
}
|
||||
gps_data.updated = false;
|
||||
} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
|
||||
vel[0] = 0;
|
||||
vel[1] = 0;
|
||||
vel[2] = 0;
|
||||
sensors |= VERT_SENSORS | HORIZ_SENSORS;
|
||||
}
|
||||
|
||||
if(mag_data.updated) {
|
||||
sensors |= MAG_SENSORS;
|
||||
mag_data.updated = false;
|
||||
}
|
||||
|
||||
if(altitude_data.updated) {
|
||||
sensors |= BARO_SENSOR;
|
||||
altitude_data.updated = false;
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
|
||||
* although probably should occur within INS itself
|
||||
*/
|
||||
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Update the EKF when in indoor mode
|
||||
*/
|
||||
void ins_indoor_update()
|
||||
{
|
||||
float gyro[3], accel[3], vel[3];
|
||||
static uint32_t last_indoor_time = 0;
|
||||
uint16_t sensors = 0;
|
||||
|
||||
// format data for INS algo
|
||||
gyro[0] = gyro_data.filtered.x;
|
||||
gyro[1] = gyro_data.filtered.y;
|
||||
gyro[2] = gyro_data.filtered.z;
|
||||
accel[0] = accel_data.filtered.x,
|
||||
accel[1] = accel_data.filtered.y,
|
||||
accel[2] = accel_data.filtered.z,
|
||||
|
||||
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
|
||||
attitude_data.quaternion.q1 = Nav.q[0];
|
||||
attitude_data.quaternion.q2 = Nav.q[1];
|
||||
attitude_data.quaternion.q3 = Nav.q[2];
|
||||
attitude_data.quaternion.q4 = Nav.q[3];
|
||||
send_attitude(); // get message out quickly
|
||||
send_velocity();
|
||||
send_position();
|
||||
INSCovariancePrediction(1 / (float)EKF_RATE);
|
||||
|
||||
/* Indoors, update with zero position and velocity and high covariance */
|
||||
vel[0] = 0;
|
||||
vel[1] = 0;
|
||||
vel[2] = 0;
|
||||
|
||||
uint32_t this_indoor_time = timer_count();
|
||||
float indoor_delay;
|
||||
|
||||
/*
|
||||
* Detect if greater than certain time since last gps update and if so
|
||||
* reset EKF to that position since probably drifted too far for safe
|
||||
* update
|
||||
*/
|
||||
if (this_indoor_time < last_indoor_time)
|
||||
indoor_delay = ((0xFFFF - last_indoor_time) - this_indoor_time) / timer_rate();
|
||||
else
|
||||
indoor_delay = (this_indoor_time - last_indoor_time) / timer_rate();
|
||||
last_indoor_time = this_indoor_time;
|
||||
|
||||
if(indoor_delay > INSGPS_GPS_TIMEOUT)
|
||||
INSPosVelReset(vel,vel);
|
||||
else
|
||||
sensors = HORIZ_SENSORS | VERT_SENSORS;
|
||||
|
||||
if(mag_data.updated && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
|
||||
sensors |= MAG_SENSORS;
|
||||
mag_data.updated = false;
|
||||
}
|
||||
|
||||
if(altitude_data.updated) {
|
||||
sensors |= BARO_SENSOR;
|
||||
altitude_data.updated = false;
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
|
||||
* although probably should occur within INS itself
|
||||
*/
|
||||
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize the EKF assuming stationary
|
||||
*/
|
||||
void ins_init_algorithm()
|
||||
{
|
||||
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
|
||||
float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
|
||||
bool using_mags, using_gps;
|
||||
|
||||
INSGPSInit();
|
||||
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
accels[0]=accel_data.filtered.x;
|
||||
accels[1]=accel_data.filtered.y;
|
||||
accels[2]=accel_data.filtered.z;
|
||||
|
||||
using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
|
||||
using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */
|
||||
|
||||
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0);
|
||||
|
||||
/* Block till a data update */
|
||||
get_accel_gyro_data();
|
||||
|
||||
/* Ensure we get mag data in a timely manner */
|
||||
uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec
|
||||
while(using_mags && !mag_data.updated && fail_count--) {
|
||||
get_accel_gyro_data();
|
||||
AhrsPoll();
|
||||
}
|
||||
using_mags &= mag_data.updated;
|
||||
|
||||
if (using_mags) {
|
||||
/* Spin waiting for mag data */
|
||||
while(!mag_data.updated) {
|
||||
get_accel_gyro_data();
|
||||
AhrsPoll();
|
||||
}
|
||||
mag_data.updated = false;
|
||||
|
||||
RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
|
||||
R2Quaternion(Rbe,q);
|
||||
if (using_gps)
|
||||
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
|
||||
else
|
||||
INSSetState(zeros, zeros, q, zeros, zeros);
|
||||
} else {
|
||||
// assume yaw = 0
|
||||
mag = VectorMagnitude(accels);
|
||||
rpy[1] = asinf(-accels[0]/mag);
|
||||
rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
|
||||
rpy[2] = 0;
|
||||
RPY2Quaternion(rpy,q);
|
||||
if (using_gps)
|
||||
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
|
||||
else
|
||||
INSSetState(zeros, zeros, q, zeros, zeros);
|
||||
}
|
||||
|
||||
INSResetP(Pdiag);
|
||||
|
||||
// TODO: include initial estimate of gyro bias?
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Simple update using just mag and accel. Yaw biased and big attitude changes.
|
||||
*/
|
||||
void simple_update() {
|
||||
float q[4];
|
||||
float rpy[3];
|
||||
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
|
||||
/* Very simple computation of the heading and attitude from accel. */
|
||||
rpy[2] = atan2((mag_data.raw.axis[MAG_RAW_Y_IDX]), (-1 * mag_data.raw.axis[MAG_RAW_X_IDX])) * RAD_TO_DEG;
|
||||
rpy[1] = atan2(accel_data.filtered.x, accel_data.filtered.z) * RAD_TO_DEG;
|
||||
rpy[0] = atan2(accel_data.filtered.y, accel_data.filtered.z) * RAD_TO_DEG;
|
||||
|
||||
RPY2Quaternion(rpy, q);
|
||||
attitude_data.quaternion.q1 = q[0];
|
||||
attitude_data.quaternion.q2 = q[1];
|
||||
attitude_data.quaternion.q3 = q[2];
|
||||
attitude_data.quaternion.q4 = q[3];
|
||||
send_attitude();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Output all the important inputs and states of the ekf through serial port
|
||||
*/
|
||||
#ifdef DUMP_EKF
|
||||
|
||||
extern float **P, *X; // covariance matrix and state vector
|
||||
|
||||
void print_ekf_binary()
|
||||
{
|
||||
uint16_t states = ins_get_num_states();
|
||||
uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
|
||||
// Dump raw buffer
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number (17:20)
|
||||
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); // accel data (21:32)
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); // gyro data (33:44)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * states); // X (86:149)
|
||||
for(uint8_t i = 0; i < states; i++)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &((*P)[i + i * states]), 4); // diag(P) (150:213)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (214:217)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4); // baro_offset (218:221)
|
||||
}
|
||||
#else
|
||||
void print_ekf_binary() {}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Debugging function to output all the ADC samples
|
||||
*/
|
||||
#if defined(DUMP_RAW)
|
||||
void print_ahrs_raw()
|
||||
{
|
||||
int result;
|
||||
static int previous_conversion = 0;
|
||||
int16_t * valid_data_buffer;
|
||||
|
||||
uint8_t framing[16] =
|
||||
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
|
||||
15 };
|
||||
|
||||
get_accel_gyro_data();
|
||||
|
||||
valid_data_buffer = PIOS_ADC_GetRawBuffer();
|
||||
|
||||
if (total_conversion_blocks != previous_conversion + 1)
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT); // not keeping up
|
||||
else
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
previous_conversion = total_conversion_blocks;
|
||||
|
||||
|
||||
// Dump raw buffer
|
||||
result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, &framing[0], 16); // framing header
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
|
||||
result +=
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX,
|
||||
(uint8_t *) & valid_data_buffer[0],
|
||||
adc_oversampling *
|
||||
PIOS_ADC_NUM_PINS *
|
||||
sizeof(valid_data_buffer[0]));
|
||||
if (result == 0)
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
else {
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(PIP_DUMP_RAW)
|
||||
|
||||
#define MAX_OVERSAMPLING PIOS_ADC_MAX_OVERSAMPLING
|
||||
|
||||
void print_ahrs_raw()
|
||||
{
|
||||
int16_t accel_x[MAX_OVERSAMPLING], accel_y[MAX_OVERSAMPLING], accel_z[MAX_OVERSAMPLING];
|
||||
int16_t gyro_x[MAX_OVERSAMPLING], gyro_y[MAX_OVERSAMPLING], gyro_z[MAX_OVERSAMPLING];
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
int16_t mag[3];
|
||||
int16_t mag_x, mag_y, mag_z;
|
||||
#endif
|
||||
|
||||
static int previous_conversion = 0;
|
||||
|
||||
uint8_t framing[3] = {0xD2, 0x73, 0x00};
|
||||
|
||||
// wait for new raw samples
|
||||
while (previous_conversion == total_conversion_blocks);
|
||||
if ((previous_conversion + 1) != total_conversion_blocks)
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT); // we are not keeping up
|
||||
previous_conversion = total_conversion_blocks;
|
||||
|
||||
// fetch the buffer address for the new samples
|
||||
int16_t *valid_data_buffer = PIOS_ADC_GetRawBuffer();
|
||||
|
||||
// fetch number of raw samples in the buffer (per channel)
|
||||
int over_sampling = PIOS_ADC_GetOverSampling();
|
||||
|
||||
framing[2] = over_sampling;
|
||||
|
||||
// copy the raw samples into their own buffers
|
||||
for (uint16_t i = 0, j = 0; i < over_sampling; i++, j += PIOS_ADC_NUM_CHANNELS)
|
||||
{
|
||||
accel_x[i] = valid_data_buffer[ACCEL_RAW_X_IDX + j];
|
||||
accel_y[i] = valid_data_buffer[ACCEL_RAW_Y_IDX + j];
|
||||
accel_z[i] = valid_data_buffer[ACCEL_RAW_Z_IDX + j];
|
||||
|
||||
gyro_x[i] = valid_data_buffer[GYRO_RAW_X_IDX + j];
|
||||
gyro_y[i] = valid_data_buffer[GYRO_RAW_Y_IDX + j];
|
||||
gyro_z[i] = valid_data_buffer[GYRO_RAW_Z_IDX + j];
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if (PIOS_HMC5843_NewDataAvailable())
|
||||
{
|
||||
PIOS_HMC5843_ReadMag(mag);
|
||||
|
||||
mag_x = mag[MAG_RAW_X_IDX];
|
||||
mag_y = mag[MAG_RAW_Y_IDX];
|
||||
mag_z = mag[MAG_RAW_Z_IDX];
|
||||
}
|
||||
#endif
|
||||
|
||||
// send the raw samples
|
||||
int result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, framing, sizeof(framing));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_x, over_sampling * sizeof(accel_x[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_y, over_sampling * sizeof(accel_y[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_z, over_sampling * sizeof(accel_z[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_x, over_sampling * sizeof(gyro_x[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_y, over_sampling * sizeof(gyro_y[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0]));
|
||||
|
||||
if (result != 0)
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT); // all data not sent
|
||||
else
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
}
|
||||
#endif
|
||||
|
||||
extern void PIOS_Board_Init(void);
|
||||
|
||||
/**
|
||||
* @brief AHRS Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
gps_data.quality = -1;
|
||||
uint32_t up_time_real = 0;
|
||||
uint32_t up_time = 0;
|
||||
uint32_t last_up_time = 0;
|
||||
static int8_t last_ahrs_algorithm;
|
||||
uint32_t last_counter_idle_start = 0;
|
||||
uint32_t last_counter_idle_end = 0;
|
||||
uint32_t idle_counts = 0;
|
||||
uint32_t running_counts = 0;
|
||||
uint32_t counter_val = 0;
|
||||
ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE;
|
||||
|
||||
reset_values();
|
||||
|
||||
PIOS_Board_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
// Get 3 ID bytes
|
||||
strcpy((char *)mag_data.id, "ZZZ");
|
||||
PIOS_HMC5843_ReadID(mag_data.id);
|
||||
#endif
|
||||
|
||||
while(!AhrsLinkReady()) {
|
||||
AhrsPoll();
|
||||
}
|
||||
/* we didn't connect the callbacks before because we have to wait
|
||||
for all data to be up to date before doing anything*/
|
||||
|
||||
AHRSCalibrationConnectCallback(calibration_callback);
|
||||
GPSPositionConnectCallback(gps_callback);
|
||||
BaroAltitudeConnectCallback(altitude_callback);
|
||||
AHRSSettingsConnectCallback(settings_callback);
|
||||
HomeLocationConnectCallback(homelocation_callback);
|
||||
FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
|
||||
|
||||
calibration_callback(AHRSCalibrationHandle()); //force an update
|
||||
|
||||
#if defined(DUMP_RAW) || defined(PIP_DUMP_RAW)
|
||||
while (1) {
|
||||
AhrsPoll();
|
||||
print_ahrs_raw();
|
||||
}
|
||||
#endif
|
||||
|
||||
timer_start();
|
||||
|
||||
/******************* Main EKF loop ****************************/
|
||||
while(1) {
|
||||
|
||||
|
||||
AhrsPoll();
|
||||
AhrsStatusData status;
|
||||
AhrsStatusGet(&status);
|
||||
status.CPULoad = ((float)running_counts /
|
||||
(float)(idle_counts + running_counts)) * 100;
|
||||
status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
|
||||
status.RunningTimePerCyle = running_counts / (timer_rate() / 10000);
|
||||
status.DroppedUpdates = ekf_too_slow;
|
||||
up_time = timer_count();
|
||||
if(up_time >= last_up_time) // normal condition
|
||||
up_time_real += ((up_time - last_up_time) * 1000) / timer_rate();
|
||||
else
|
||||
up_time_real += ((0xFFFF - last_up_time + up_time) * 1000) / timer_rate();
|
||||
last_up_time = up_time;
|
||||
status.RunningTime = up_time_real;
|
||||
AhrsStatusSet(&status);
|
||||
|
||||
// Alive signal
|
||||
if (((total_conversion_blocks % 100) & 0xFFFE) == 0)
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
|
||||
// Delay for valid data
|
||||
|
||||
counter_val = timer_count();
|
||||
running_counts = counter_val - last_counter_idle_end;
|
||||
last_counter_idle_start = counter_val;
|
||||
|
||||
// This function blocks till data avilable
|
||||
get_accel_gyro_data();
|
||||
|
||||
// Get any mag data available
|
||||
process_mag_data();
|
||||
|
||||
counter_val = timer_count();
|
||||
idle_counts = counter_val - last_counter_idle_start;
|
||||
last_counter_idle_end = counter_val;
|
||||
|
||||
|
||||
if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
|
||||
ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) ||
|
||||
ACCEL_OOB(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
|
||||
GYRO_OOB(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z)) {
|
||||
// If any values are NaN or huge don't update
|
||||
//TODO: add field to ahrs status to track number of these events
|
||||
continue;
|
||||
}
|
||||
|
||||
print_ekf_binary();
|
||||
|
||||
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
|
||||
if (ahrs_algorithm != last_ahrs_algorithm)
|
||||
ins_init_algorithm();
|
||||
last_ahrs_algorithm = ahrs_algorithm;
|
||||
|
||||
switch(ahrs_algorithm) {
|
||||
case AHRSSETTINGS_ALGORITHM_SIMPLE:
|
||||
simple_update();
|
||||
break;
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
|
||||
ins_outdoor_update();
|
||||
break;
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR:
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
|
||||
ins_indoor_update();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the accel and gyro data from whichever source when available
|
||||
*
|
||||
* This function will act as the HAL for the new INS sensors
|
||||
*/
|
||||
bool get_accel_gyro_data()
|
||||
{
|
||||
float accel[6];
|
||||
float gyro[6];
|
||||
uint16_t spin_count = 1;
|
||||
while(fifoBuf_getUsed(&adc_fifo_buffer) < (sizeof(accel) + sizeof(gyro))) {
|
||||
if(spin_count++ == 0)
|
||||
AhrsPoll();
|
||||
}
|
||||
|
||||
fifoBuf_getData(&adc_fifo_buffer, &accel[0], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &gyro[0], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &accel[3], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &gyro[3], sizeof(float) * 3);
|
||||
|
||||
accel_data.filtered.x = (accel[0] + accel[3]) / 2;
|
||||
accel_data.filtered.y = (accel[1] + accel[4]) / 2;
|
||||
accel_data.filtered.z = (accel[2] + accel[5]) / 2;
|
||||
gyro_data.filtered.x = (gyro[0] + gyro[3]) / 2;
|
||||
gyro_data.filtered.y = (gyro[1] + gyro[4]) / 2;
|
||||
gyro_data.filtered.z = (gyro[2] + gyro[5]) / 2;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Perform calibration of a 3-axis field sensor using an affine transformation
|
||||
* matrix.
|
||||
*
|
||||
* Computes result = scale * arg.
|
||||
*
|
||||
* @param result[out] The three-axis resultant field.
|
||||
* @param scale[in] The 4x4 affine transformation matrix. The fourth row is implicitly
|
||||
* [0 0 0 1]
|
||||
* @param arg[in] The 3-axis input field. The 'w' component is implicitly 1.
|
||||
*/
|
||||
void calibration(float result[3], float scale[3][4], float arg[3])
|
||||
{
|
||||
for (int row = 0; row < 3; ++row) {
|
||||
result[row] = 0.0f;
|
||||
int col;
|
||||
for (col = 0; col < 3; ++col) {
|
||||
result[row] += scale[row][col] * arg[col];
|
||||
}
|
||||
// fourth column: arg has an implicit w value of 1.0f.
|
||||
result[row] += scale[row][col];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Scale an affine transformation matrix by a rotation, defined by a
|
||||
* rotation vector. scale <- rotation * scale
|
||||
*
|
||||
* @param scale[in,out] The affine transformation matrix to be rotated
|
||||
* @param rotation[in] The rotation vector defining the rotation
|
||||
*/
|
||||
void affine_rotate(float scale[3][4], float rotation[3])
|
||||
{
|
||||
// Rotate the scale factor matrix in-place
|
||||
float rmatrix[3][3];
|
||||
Rv2Rot(rotation, rmatrix);
|
||||
|
||||
float ret[3][4];
|
||||
for (int row = 0; row < 3; ++row) {
|
||||
for (int col = 0; col < 4; ++col) {
|
||||
ret[row][col] = 0.0f;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
ret[row][col] += rmatrix[row][i] * scale[i][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
// copy output to argument
|
||||
for (int row = 0; row < 3; ++row) {
|
||||
for (int col = 0; col < 4; ++col) {
|
||||
scale[row][col] = ret[row][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Downsample the analog data
|
||||
* @return none
|
||||
*
|
||||
* Tried to make as much of the filtering fixed point when possible. Need to account
|
||||
* for offset for each sample before the multiplication if filter not a boxcar. Could
|
||||
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
|
||||
* data structures @ref accel_data and @ref gyro_data.
|
||||
*
|
||||
* The accel_data values are converted into a coordinate system where X is forwards along
|
||||
* the fuselage, Y is along right the wing, and Z is down.
|
||||
*/
|
||||
void adc_callback(float * downsampled_data)
|
||||
{
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
#if 0
|
||||
// Get the Y data. Third byte in. Convert to m/s
|
||||
float accel_filtered[3];
|
||||
accel_filtered[1] = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_filtered[1] += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_filtered[1] /= (float) fir_coeffs[adc_oversampling];
|
||||
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
|
||||
accel_filtered[0] = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_filtered[0] += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_filtered[0] /= (float) fir_coeffs[adc_oversampling];
|
||||
|
||||
// Get the Z data. Third byte in. Convert to m/s
|
||||
accel_filtered[2] = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_filtered[2] += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_filtered[2] /= (float) fir_coeffs[adc_oversampling];
|
||||
|
||||
float accel_scaled[3];
|
||||
calibration(accel_scaled, accel_data.calibration.scale, accel_filtered);
|
||||
accel_data.filtered.x = accel_scaled[0];
|
||||
accel_data.filtered.y = accel_scaled[1];
|
||||
accel_data.filtered.z = accel_scaled[2];
|
||||
#else
|
||||
float accel[3], gyro[3];
|
||||
|
||||
float raw_accel[3] = {
|
||||
downsampled_data[ACCEL_RAW_X_IDX],
|
||||
downsampled_data[ACCEL_RAW_Y_IDX],
|
||||
-downsampled_data[ACCEL_RAW_Z_IDX]
|
||||
};
|
||||
|
||||
// Accel data is (y,x,z) in first third and fifth byte. Convert to m/s^2
|
||||
calibration(accel, accel_data.calibration.scale, raw_accel);
|
||||
|
||||
// Gyro data is (x,y,z) in second, fifth and seventh byte. Convert to rad/s
|
||||
gyro[0] = ( ( downsampled_data[GYRO_RAW_X_IDX] + gyro_data.calibration.tempcompfactor[0] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
|
||||
gyro[1] = ( ( downsampled_data[GYRO_RAW_Y_IDX] + gyro_data.calibration.tempcompfactor[1] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
|
||||
gyro[2] = ( ( downsampled_data[GYRO_RAW_Z_IDX] + gyro_data.calibration.tempcompfactor[2] * downsampled_data[GYRO_TEMP_RAW_Z_IDX] ) * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
|
||||
|
||||
#endif
|
||||
#if 0
|
||||
static float gravity_tracking[3] = {0,0,0};
|
||||
const float tau = 0.9999;
|
||||
gravity_tracking[0] = tau * gravity_tracking[0] + (1-tau) * accel[0];
|
||||
gravity_tracking[1] = tau * gravity_tracking[1] + (1-tau) * accel[1];
|
||||
gravity_tracking[2] = tau * gravity_tracking[2] + (1-tau) * (accel[2] + 9.81);
|
||||
if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
|
||||
accel[0] -= gravity_tracking[0];
|
||||
accel[1] -= gravity_tracking[1];
|
||||
accel[2] -= gravity_tracking[2];
|
||||
}
|
||||
#endif
|
||||
if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
|
||||
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
|
||||
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
|
||||
} else {
|
||||
ekf_too_slow++;
|
||||
}
|
||||
|
||||
AttitudeRawData raw;
|
||||
|
||||
raw.gyrotemp[0] = downsampled_data[GYRO_TEMP_RAW_XY_IDX];
|
||||
raw.gyrotemp[1] = downsampled_data[GYRO_TEMP_RAW_Z_IDX];
|
||||
|
||||
raw.gyros[0] = gyro[0] * RAD_TO_DEG;
|
||||
raw.gyros[1] = gyro[1] * RAD_TO_DEG;
|
||||
raw.gyros[2] = gyro[2] * RAD_TO_DEG;
|
||||
|
||||
raw.accels[0] = accel[0];
|
||||
raw.accels[1] = accel[1];
|
||||
raw.accels[2] = accel[2];
|
||||
|
||||
raw.magnetometers[0] = mag_data.scaled.axis[0];
|
||||
raw.magnetometers[1] = mag_data.scaled.axis[1];
|
||||
raw.magnetometers[2] = mag_data.scaled.axis[2];
|
||||
|
||||
if (settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE)
|
||||
{
|
||||
raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
|
||||
raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
|
||||
raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
|
||||
|
||||
raw.accels[0] -= Nav.accel_bias[0];
|
||||
raw.accels[1] -= Nav.accel_bias[1];
|
||||
raw.accels[2] -= Nav.accel_bias[2];
|
||||
}
|
||||
|
||||
AttitudeRawSet(&raw);
|
||||
|
||||
total_conversion_blocks++;
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
/**
|
||||
* @brief Get the mag data from the I2C sensor and load into structure
|
||||
* @return none
|
||||
*
|
||||
* This function also considers if the home location is set and has a valid
|
||||
* magnetic field before updating the mag data to prevent data being used that
|
||||
* cannot be interpreted. In addition the mag data is not used for the first
|
||||
* five seconds to allow the filter to start to converge
|
||||
*/
|
||||
void process_mag_data()
|
||||
{
|
||||
// Get magnetic readings
|
||||
// For now don't use mags until the magnetic field is set AND until 5 seconds
|
||||
// after initialization otherwise it seems to have problems
|
||||
// TODO: Follow up this initialization issue
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
if (PIOS_HMC5843_NewDataAvailable()) {
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
|
||||
// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
|
||||
// Only use if magnetic length reasonable
|
||||
float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
|
||||
|
||||
mag_data.updated = (home.Set == HOMELOCATION_SET_TRUE) &&
|
||||
((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) &&
|
||||
((mag_data.raw.axis[MAG_RAW_X_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Y_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Z_IDX] != 0)) &&
|
||||
((Blen < mag_len * (1 + INSGPS_MAGTOL)) && (Blen > mag_len * (1 - INSGPS_MAGTOL)));
|
||||
}
|
||||
}
|
||||
#else
|
||||
void process_mag_data() { }
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Assumes board is not moving computes biases and variances of sensors
|
||||
* @returns None
|
||||
*
|
||||
* All data is stored in global structures. This function should be called from OP when
|
||||
* aircraft is in stable state and then the data stored to SD card.
|
||||
*
|
||||
* After this function the bias for each sensor will be the mean value. This doesn't make
|
||||
* sense for the z accel so make sure 6 point calibration is also run and those values set
|
||||
* after these read.
|
||||
*/
|
||||
#define NBIAS 100
|
||||
#define NVAR 500
|
||||
void calibrate_sensors()
|
||||
{
|
||||
int i,j;
|
||||
float accel_bias[3] = {0, 0, 0};
|
||||
float gyro_bias[3] = {0, 0, 0};
|
||||
float mag_bias[3] = {0, 0, 0};
|
||||
|
||||
|
||||
for (i = 0, j = 0; i < NBIAS; i++) {
|
||||
|
||||
get_accel_gyro_data();
|
||||
|
||||
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
|
||||
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
|
||||
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
|
||||
accel_bias[0] += accel_data.filtered.x / NBIAS;
|
||||
accel_bias[1] += accel_data.filtered.y / NBIAS;
|
||||
accel_bias[2] += accel_data.filtered.z / NBIAS;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
mag_bias[0] += mag_data.scaled.axis[0];
|
||||
mag_bias[1] += mag_data.scaled.axis[1];
|
||||
mag_bias[2] += mag_data.scaled.axis[2];
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
mag_bias[0] /= j;
|
||||
mag_bias[1] /= j;
|
||||
mag_bias[2] /= j;
|
||||
|
||||
gyro_data.calibration.variance[0] = 0;
|
||||
gyro_data.calibration.variance[1] = 0;
|
||||
gyro_data.calibration.variance[2] = 0;
|
||||
mag_data.calibration.variance[0] = 0;
|
||||
mag_data.calibration.variance[1] = 0;
|
||||
mag_data.calibration.variance[2] = 0;
|
||||
accel_data.calibration.variance[0] = 0;
|
||||
accel_data.calibration.variance[1] = 0;
|
||||
accel_data.calibration.variance[2] = 0;
|
||||
|
||||
for (i = 0, j = 0; j < NVAR; j++) {
|
||||
get_accel_gyro_data();
|
||||
|
||||
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
|
||||
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
|
||||
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
|
||||
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
|
||||
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
|
||||
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
|
||||
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
|
||||
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
mag_data.calibration.variance[0] /= j;
|
||||
mag_data.calibration.variance[1] /= j;
|
||||
mag_data.calibration.variance[2] /= j;
|
||||
|
||||
gyro_data.calibration.bias[0] -= gyro_bias[0];
|
||||
gyro_data.calibration.bias[1] -= gyro_bias[1];
|
||||
gyro_data.calibration.bias[2] -= gyro_bias[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Populate fields with initial values
|
||||
*/
|
||||
void reset_values()
|
||||
{
|
||||
accel_data.calibration.scale[0][1] = 0;
|
||||
accel_data.calibration.scale[1][0] = 0;
|
||||
accel_data.calibration.scale[0][2] = 0;
|
||||
accel_data.calibration.scale[2][0] = 0;
|
||||
accel_data.calibration.scale[1][2] = 0;
|
||||
accel_data.calibration.scale[2][1] = 0;
|
||||
|
||||
accel_data.calibration.scale[0][0] = 0.0359;
|
||||
accel_data.calibration.scale[1][1] = 0.0359;
|
||||
accel_data.calibration.scale[2][2] = 0.0359;
|
||||
accel_data.calibration.scale[0][3] = -73.5;
|
||||
accel_data.calibration.scale[1][3] = -73.5;
|
||||
accel_data.calibration.scale[2][3] = -73.5;
|
||||
|
||||
accel_data.calibration.variance[0] = 1e-4;
|
||||
accel_data.calibration.variance[1] = 1e-4;
|
||||
accel_data.calibration.variance[2] = 1e-4;
|
||||
|
||||
gyro_data.calibration.scale[0] = -0.014;
|
||||
gyro_data.calibration.scale[1] = 0.014;
|
||||
gyro_data.calibration.scale[2] = -0.014;
|
||||
gyro_data.calibration.bias[0] = -24;
|
||||
gyro_data.calibration.bias[1] = -24;
|
||||
gyro_data.calibration.bias[2] = -24;
|
||||
gyro_data.calibration.variance[0] = 1;
|
||||
gyro_data.calibration.variance[1] = 1;
|
||||
gyro_data.calibration.variance[2] = 1;
|
||||
mag_data.calibration.scale[0] = 1;
|
||||
mag_data.calibration.scale[1] = 1;
|
||||
mag_data.calibration.scale[2] = 1;
|
||||
mag_data.calibration.bias[0] = 0;
|
||||
mag_data.calibration.bias[1] = 0;
|
||||
mag_data.calibration.bias[2] = 0;
|
||||
mag_data.calibration.variance[0] = 50;
|
||||
mag_data.calibration.variance[1] = 50;
|
||||
mag_data.calibration.variance[2] = 50;
|
||||
}
|
||||
|
||||
|
||||
void send_attitude(void)
|
||||
{
|
||||
AttitudeActualData attitude;
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
attitude.q1 = attitude_data.quaternion.q1;
|
||||
attitude.q2 = attitude_data.quaternion.q2;
|
||||
attitude.q3 = attitude_data.quaternion.q3;
|
||||
attitude.q4 = attitude_data.quaternion.q4;
|
||||
float rpy[3];
|
||||
Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
|
||||
attitude.Roll = rpy[0] + settings.RollBias;
|
||||
attitude.Pitch = rpy[1] + settings.PitchBias;
|
||||
attitude.Yaw = rpy[2] + settings.YawBias;
|
||||
if(attitude.Yaw > 360)
|
||||
attitude.Yaw -= 360;
|
||||
AttitudeActualSet(&attitude);
|
||||
}
|
||||
|
||||
void send_velocity(void)
|
||||
{
|
||||
VelocityActualData velocityActual;
|
||||
VelocityActualGet(&velocityActual);
|
||||
|
||||
// convert into cm
|
||||
velocityActual.North = Nav.Vel[0] * 100;
|
||||
velocityActual.East = Nav.Vel[1] * 100;
|
||||
velocityActual.Down = Nav.Vel[2] * 100;
|
||||
|
||||
VelocityActualSet(&velocityActual);
|
||||
}
|
||||
|
||||
void send_position(void)
|
||||
{
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
|
||||
// convert into cm
|
||||
positionActual.North = Nav.Pos[0] * 100;
|
||||
positionActual.East = Nav.Pos[1] * 100;
|
||||
positionActual.Down = Nav.Pos[2] * 100;
|
||||
|
||||
PositionActualSet(&positionActual);
|
||||
}
|
||||
|
||||
void send_calibration(void)
|
||||
{
|
||||
AHRSCalibrationData cal;
|
||||
AHRSCalibrationGet(&cal);
|
||||
for(int ct=0; ct<3; ct++)
|
||||
{
|
||||
cal.accel_var[ct] = accel_data.calibration.variance[ct];
|
||||
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
|
||||
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
|
||||
cal.mag_var[ct] = mag_data.calibration.variance[ct];
|
||||
}
|
||||
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
|
||||
AHRSCalibrationSet(&cal);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief AHRS calibration callback
|
||||
*
|
||||
* Called when the OP board sets the calibration
|
||||
*/
|
||||
void calibration_callback(AhrsObjHandle obj)
|
||||
{
|
||||
AHRSCalibrationData cal;
|
||||
AHRSCalibrationGet(&cal);
|
||||
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
|
||||
|
||||
accel_data.calibration.scale[0][1] = cal.accel_ortho[0];
|
||||
accel_data.calibration.scale[1][0] = cal.accel_ortho[0];
|
||||
|
||||
accel_data.calibration.scale[0][2] = cal.accel_ortho[1];
|
||||
accel_data.calibration.scale[2][0] = cal.accel_ortho[1];
|
||||
|
||||
accel_data.calibration.scale[1][2] = cal.accel_ortho[2];
|
||||
accel_data.calibration.scale[2][1] = cal.accel_ortho[2];
|
||||
|
||||
#if 0
|
||||
// TODO: Enable after v1.0 feature freeze.
|
||||
float rotation[3] = { cal.accel_rotation[0],
|
||||
cal.accel_rotation[1],
|
||||
cal.accel_rotation[2],
|
||||
};
|
||||
|
||||
affine_rotate(accel_data.calibration.scale, rotation);
|
||||
#endif
|
||||
|
||||
for(int ct=0; ct<3; ct++)
|
||||
{
|
||||
accel_data.calibration.scale[ct][ct] = cal.accel_scale[ct];
|
||||
accel_data.calibration.scale[ct][3] = cal.accel_bias[ct];
|
||||
accel_data.calibration.variance[ct] = cal.accel_var[ct];
|
||||
|
||||
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
|
||||
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
|
||||
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
|
||||
#if 1
|
||||
gyro_data.calibration.tempcompfactor[ct] = cal.gyro_tempcompfactor[ct];
|
||||
#endif
|
||||
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
|
||||
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
|
||||
mag_data.calibration.variance[ct] = cal.mag_var[ct];
|
||||
}
|
||||
// Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to
|
||||
// one in code
|
||||
float mag_var[3] = {mag_data.calibration.variance[0] / INSGPS_MAGLEN / INSGPS_MAGLEN,
|
||||
mag_data.calibration.variance[1] / INSGPS_MAGLEN / INSGPS_MAGLEN,
|
||||
mag_data.calibration.variance[2] / INSGPS_MAGLEN / INSGPS_MAGLEN};
|
||||
INSSetMagVar(mag_var);
|
||||
INSSetAccelVar(accel_data.calibration.variance);
|
||||
INSSetGyroVar(gyro_data.calibration.variance);
|
||||
}
|
||||
else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE) {
|
||||
calibrate_sensors();
|
||||
send_calibration();
|
||||
}
|
||||
|
||||
INSSetPosVelVar(cal.pos_var, cal.vel_var);
|
||||
|
||||
}
|
||||
|
||||
void gps_callback(AhrsObjHandle obj)
|
||||
{
|
||||
GPSPositionData pos;
|
||||
GPSPositionGet(&pos);
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
// convert from cm back to meters
|
||||
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
|
||||
// put in local NED frame
|
||||
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
|
||||
|
||||
gps_data.heading = pos.Heading;
|
||||
gps_data.groundspeed = pos.Groundspeed;
|
||||
gps_data.quality = 1; /* currently unused */
|
||||
gps_data.updated = true;
|
||||
|
||||
// if poor don't use this update
|
||||
if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
|
||||
(pos.Satellites < INSGPS_GPS_MINSAT) ||
|
||||
(pos.PDOP >= INSGPS_GPS_MINPDOP) ||
|
||||
(home.Set == FALSE) ||
|
||||
((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
|
||||
{
|
||||
gps_data.quality = 0;
|
||||
gps_data.updated = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void altitude_callback(AhrsObjHandle obj)
|
||||
{
|
||||
BaroAltitudeData alt;
|
||||
BaroAltitudeGet(&alt);
|
||||
altitude_data.altitude = alt.Altitude;
|
||||
altitude_data.updated = true;
|
||||
}
|
||||
|
||||
void settings_callback(AhrsObjHandle obj)
|
||||
{
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
ahrs_algorithm = settings.Algorithm;
|
||||
|
||||
if(settings.Downsampling != adc_oversampling) {
|
||||
adc_oversampling = settings.Downsampling;
|
||||
PIOS_ADC_Config(adc_oversampling);
|
||||
}
|
||||
}
|
||||
|
||||
void homelocation_callback(AhrsObjHandle obj)
|
||||
{
|
||||
HomeLocationData data;
|
||||
HomeLocationGet(&data);
|
||||
|
||||
mag_len = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2));
|
||||
float Be[3] = {data.Be[0] / mag_len, data.Be[1] / mag_len, data.Be[2] / mag_len};
|
||||
|
||||
INSSetMagNorth(Be);
|
||||
}
|
||||
|
||||
void firmwareiapobj_callback(AhrsObjHandle obj)
|
||||
{
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
FirmwareIAPObjData firmwareIAPObj;
|
||||
FirmwareIAPObjGet(&firmwareIAPObj);
|
||||
if(firmwareIAPObj.ArmReset==0)
|
||||
reset_count=0;
|
||||
if(firmwareIAPObj.ArmReset==1)
|
||||
{
|
||||
|
||||
if((firmwareIAPObj.BoardType==bdinfo->board_type) || (firmwareIAPObj.BoardType==0xFF))
|
||||
{
|
||||
|
||||
++reset_count;
|
||||
if(reset_count>2)
|
||||
{
|
||||
PIOS_IAP_SetRequest1();
|
||||
PIOS_IAP_SetRequest2();
|
||||
PIOS_SYS_Reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(firmwareIAPObj.BoardType==bdinfo->board_type && firmwareIAPObj.crc!=PIOS_BL_HELPER_CRC_Memory_Calc())
|
||||
{
|
||||
PIOS_BL_HELPER_FLASH_Read_Description(firmwareIAPObj.Description,bdinfo->desc_size);
|
||||
firmwareIAPObj.crc=PIOS_BL_HELPER_CRC_Memory_Calc();
|
||||
firmwareIAPObj.BoardRevision=bdinfo->board_rev;
|
||||
FirmwareIAPObjSet(&firmwareIAPObj);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
@ -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 */
|
||||
@ -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 */
|
||||
@ -1,118 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_board.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board specific static initializers for hardware for the AHRS board.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* Pull in the board-specific static HW definitions.
|
||||
* Including .c files is a bit ugly but this allows all of
|
||||
* the HW definitions to be const and static to limit their
|
||||
* scope.
|
||||
*
|
||||
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
|
||||
*/
|
||||
#include "board_hw_defs.c"
|
||||
|
||||
#include <pios.h>
|
||||
|
||||
#define PIOS_COM_AUX_TX_BUF_LEN 192
|
||||
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
|
||||
|
||||
uint32_t pios_com_aux_id;
|
||||
uint8_t adc_fifo_buf[sizeof(float) * 6 * 4] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
void PIOS_Board_Init(void) {
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
#if defined(PIOS_LED_HEARTBEAT)
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
#endif /* PIOS_LED_HEARTBEAT */
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Communication system */
|
||||
#if !defined(PIOS_ENABLE_DEBUG_PINS)
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
{
|
||||
uint32_t pios_usart_aux_id;
|
||||
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
|
||||
NULL, 0,
|
||||
pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
#endif
|
||||
|
||||
/* IAP System Setup */
|
||||
PIOS_IAP_Init();
|
||||
|
||||
/* ADC system */
|
||||
PIOS_ADC_Init();
|
||||
extern uint8_t adc_oversampling;
|
||||
PIOS_ADC_Config(adc_oversampling);
|
||||
extern void adc_callback(float *);
|
||||
PIOS_ADC_SetCallback(adc_callback);
|
||||
|
||||
/* ADC buffer */
|
||||
extern t_fifo_buffer adc_fifo_buffer;
|
||||
fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf));
|
||||
|
||||
/* Setup the Accelerometer FS (Full-Scale) GPIO */
|
||||
PIOS_GPIO_Enable(0);
|
||||
SET_ACCEL_6G;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
/* Magnetic sensor system */
|
||||
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
PIOS_HMC5843_Init();
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
#include "ahrs_spi_comm.h"
|
||||
AhrsInitComms();
|
||||
|
||||
/* Set up the SPI interface to the OP board */
|
||||
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
AhrsConnect(pios_spi_op_id);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -1,34 +0,0 @@
|
||||
#include "ahrs_bl.h"
|
||||
#include "ahrs_spi_program.h"
|
||||
|
||||
uint8_t buf[256];
|
||||
|
||||
bool StartProgramming(void) {
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Started programming\r\n");
|
||||
return (true);
|
||||
}
|
||||
|
||||
bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size) {
|
||||
if (size > SPI_MAX_PROGRAM_DATA_SIZE) {
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "oversize: %d\r\n", size);
|
||||
return (false);
|
||||
}
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Wrote %d bytes to %d\r\n",
|
||||
size, offset);
|
||||
memcpy(buf, buffer, size);
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
return (true);
|
||||
}
|
||||
|
||||
bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size) {
|
||||
if (size > SPI_MAX_PROGRAM_DATA_SIZE) {
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "oversize: %d\r\n", size);
|
||||
return (false);
|
||||
}
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Read %d bytes from %d\r\n",
|
||||
size, offset);
|
||||
memcpy(buffer, buf, size);
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
return (true);
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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 */
|
||||
@ -1,268 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS BOOTLOADER
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_BOOTLOADER_Main
|
||||
* @brief Main function which does the hardware dependent stuff
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file main.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "ahrs_bl.h"
|
||||
#include <pios_board_info.h>
|
||||
#include "pios_opahrs_proto.h"
|
||||
#include "bl_fsm.h" /* lfsm_state */
|
||||
#include "stm32f10x_flash.h"
|
||||
|
||||
extern void PIOS_Board_Init(void);
|
||||
|
||||
#define NSS_HOLD_STATE ((GPIOB->IDR & GPIO_Pin_12) ? 0 : 1)
|
||||
enum bootloader_status boot_status;
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
typedef void
|
||||
(*pFunction)(void);
|
||||
pFunction Jump_To_Application;
|
||||
uint32_t JumpAddress;
|
||||
/* Function Prototypes */
|
||||
void
|
||||
process_spi_request(void);
|
||||
void
|
||||
jump_to_app();
|
||||
uint8_t jumpFW = FALSE;
|
||||
uint32_t Fw_crc;
|
||||
/**
|
||||
* @brief Bootloader Main function
|
||||
*/
|
||||
int main() {
|
||||
|
||||
uint8_t GO_dfu = false;
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
/* Enable Prefetch Buffer */
|
||||
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
|
||||
|
||||
/* Flash 2 wait state */
|
||||
FLASH_SetLatency(FLASH_Latency_2);
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
for (uint32_t t = 0; t < 10000000; ++t) {
|
||||
if (NSS_HOLD_STATE == 1)
|
||||
GO_dfu = TRUE;
|
||||
else {
|
||||
GO_dfu = FALSE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
PIOS_IAP_Init();
|
||||
GO_dfu = GO_dfu | PIOS_IAP_CheckRequest();// OR with app boot request
|
||||
if (GO_dfu == FALSE) {
|
||||
jump_to_app();
|
||||
}
|
||||
if (PIOS_IAP_CheckRequest()) {
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
PIOS_IAP_ClearRequest();
|
||||
}
|
||||
PIOS_Board_Init();
|
||||
boot_status = idle;
|
||||
Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
while (1) {
|
||||
process_spi_request();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct opahrs_msg_v0 link_tx_v0;
|
||||
static struct opahrs_msg_v0 link_rx_v0;
|
||||
static struct opahrs_msg_v0 user_tx_v0;
|
||||
static struct opahrs_msg_v0 user_rx_v0;
|
||||
void process_spi_request(void) {
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
bool msg_to_process = FALSE;
|
||||
|
||||
PIOS_IRQ_Disable();
|
||||
/* Figure out if we're in an interesting stable state */
|
||||
switch (lfsm_get_state()) {
|
||||
case LFSM_STATE_USER_BUSY:
|
||||
msg_to_process = TRUE;
|
||||
break;
|
||||
case LFSM_STATE_INACTIVE:
|
||||
/* Queue up a receive buffer */
|
||||
lfsm_user_set_rx_v0(&user_rx_v0);
|
||||
lfsm_user_done();
|
||||
break;
|
||||
case LFSM_STATE_STOPPED:
|
||||
/* Get things going */
|
||||
lfsm_set_link_proto_v0(&link_tx_v0, &link_rx_v0);
|
||||
break;
|
||||
default:
|
||||
/* Not a stable state */
|
||||
break;
|
||||
}
|
||||
PIOS_IRQ_Enable();
|
||||
|
||||
if (!msg_to_process) {
|
||||
/* Nothing to do */
|
||||
//PIOS_COM_SendFormattedString(PIOS_COM_AUX, ".");
|
||||
return;
|
||||
}
|
||||
|
||||
if (user_rx_v0.tail.magic != OPAHRS_MSG_MAGIC_TAIL) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (user_rx_v0.payload.user.t) {
|
||||
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_VERIFY:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc();
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
boot_status = idle;
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_RESET:
|
||||
PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.reset.reset_delay_in_ms);
|
||||
PIOS_SYS_Reset();
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_VERSIONS:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS);
|
||||
user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION;
|
||||
user_tx_v0.payload.user.v.rsp.versions.hw_version = (BOARD_TYPE << 8)
|
||||
| BOARD_REVISION;
|
||||
user_tx_v0.payload.user.v.rsp.versions.fw_crc = Fw_crc;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_MEM_MAP:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP);
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.density = bdinfo->hw_type;
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE
|
||||
| (BOARD_WRITABLE << 1));
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory
|
||||
= bdinfo->fw_size;
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.size_of_description
|
||||
= bdinfo->desc_size;
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code
|
||||
= bdinfo->fw_base;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_SERIAL:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_SERIAL);
|
||||
PIOS_SYS_SerialNumberGet(
|
||||
(char *) &(user_tx_v0.payload.user.v.rsp.serial.serial_bcd));
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_STATUS:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_DATA:
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
if (!(user_rx_v0.payload.user.v.req.fwup_data.adress
|
||||
< bdinfo->fw_base)) {
|
||||
for (uint8_t x = 0; x
|
||||
< user_rx_v0.payload.user.v.req.fwup_data.size; ++x) {
|
||||
if (FLASH_ProgramWord(
|
||||
(user_rx_v0.payload.user.v.req.fwup_data.adress
|
||||
+ ((uint32_t)(x * 4))),
|
||||
user_rx_v0.payload.user.v.req.fwup_data.data[x])
|
||||
!= FLASH_COMPLETE) {
|
||||
boot_status = write_error;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
boot_status = outside_dev_capabilities;
|
||||
}
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWDN_DATA:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWDN_DATA);
|
||||
uint32_t adr = user_rx_v0.payload.user.v.req.fwdn_data.adress;
|
||||
for (uint8_t x = 0; x < 4; ++x) {
|
||||
user_tx_v0.payload.user.v.rsp.fw_dn.data[x]
|
||||
= *PIOS_BL_HELPER_FLASH_If_Read(adr + x);
|
||||
}
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_START:
|
||||
FLASH_Unlock();
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
if (PIOS_BL_HELPER_FLASH_Start() == TRUE) {
|
||||
boot_status = started;
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
} else {
|
||||
boot_status = start_failed;
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_BOOT:
|
||||
PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.boot.boot_delay_in_ms);
|
||||
FLASH_Lock();
|
||||
jump_to_app();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/* Finished processing the received message, requeue it */
|
||||
lfsm_user_set_rx_v0(&user_rx_v0);
|
||||
lfsm_user_done();
|
||||
return;
|
||||
}
|
||||
void jump_to_app() {
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
|
||||
FLASH_Lock();
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
|
||||
|
||||
JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
|
||||
Jump_To_Application = (pFunction) JumpAddress;
|
||||
/* Initialize user application's Stack Pointer */
|
||||
__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
|
||||
Jump_To_Application();
|
||||
} else {
|
||||
boot_status = jump_failed;
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -28,12 +28,13 @@
|
||||
/* Bootloader Includes */
|
||||
#include <pios.h>
|
||||
#include <stdbool.h>
|
||||
#include "pios_board_info.h"
|
||||
|
||||
#define MAX_WRI_RETRYS 3
|
||||
/* Prototype of PIOS_Board_Init() function */
|
||||
extern void PIOS_Board_Init(void);
|
||||
extern void FLASH_Download();
|
||||
void error(void);
|
||||
void error(int);
|
||||
|
||||
/* The ADDRESSES of the _binary_* symbols are the important
|
||||
* data. This is non-intuitive for _binary_size where you
|
||||
@ -50,12 +51,40 @@ int main() {
|
||||
|
||||
PIOS_SYS_Init();
|
||||
PIOS_Board_Init();
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
PIOS_DELAY_WaitmS(3000);
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
|
||||
/// Self overwrite check
|
||||
uint32_t base_address = SCB->VTOR;
|
||||
if ((0x08000000 + embedded_image_size) > base_address)
|
||||
error();
|
||||
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
|
||||
@ -79,7 +108,7 @@ int main() {
|
||||
}
|
||||
|
||||
if (fail == true)
|
||||
error();
|
||||
error(PIOS_LED_HEARTBEAT);
|
||||
|
||||
|
||||
///
|
||||
@ -87,6 +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(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])
|
||||
@ -94,9 +124,15 @@ int main() {
|
||||
}
|
||||
}
|
||||
if (result == false)
|
||||
error();
|
||||
error(PIOS_LED_HEARTBEAT);
|
||||
}
|
||||
///
|
||||
for (uint8_t x = 0; x < 3; ++x) {
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
}
|
||||
|
||||
/// Invalidate the bootloader updater so we won't run
|
||||
/// the update again on the next power cycle.
|
||||
@ -109,7 +145,11 @@ int main() {
|
||||
|
||||
}
|
||||
|
||||
void error(void) {
|
||||
void error(int led) {
|
||||
for (;;) {
|
||||
PIOS_LED_On(led);
|
||||
PIOS_DELAY_WaitmS(500);
|
||||
PIOS_LED_Off(led);
|
||||
PIOS_DELAY_WaitmS(500);
|
||||
}
|
||||
}
|
||||
|
||||
@ -35,6 +35,8 @@
|
||||
#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);
|
||||
|
||||
@ -42,13 +44,15 @@ void PIOS_Board_Init(void) {
|
||||
FLASH_SetLatency(FLASH_Latency_2);
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_GPIO_Init();
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* LEDs */
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
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();
|
||||
}
|
||||
|
||||
@ -104,6 +104,8 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_iap.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
|
||||
|
||||
# PIOS USB related files (seperated to make code maintenance more easy)
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb.c
|
||||
@ -113,12 +115,11 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
|
||||
SRC += $(OPSYSTEM)/pios_usb_board_data.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_util.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_board_info.c
|
||||
SRC += $(PIOSCOMMON)/pios_com_msg.c
|
||||
SRC += $(PIOSCOMMON)/pios_bl_helper.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
|
||||
## Libraries for flight calculations
|
||||
@ -414,7 +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
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
|
||||
#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
|
||||
|
||||
@ -70,14 +70,13 @@ uint8_t processRX();
|
||||
void jump_to_app();
|
||||
|
||||
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();
|
||||
|
||||
@ -31,7 +31,7 @@
|
||||
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
|
||||
*/
|
||||
#include "board_hw_defs.c"
|
||||
|
||||
#include <pios_board_info.h>
|
||||
#include <pios.h>
|
||||
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
@ -59,8 +59,12 @@ void PIOS_Board_Init(void) {
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
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)
|
||||
@ -71,8 +75,15 @@ void PIOS_Board_Init(void) {
|
||||
PIOS_USB_DESC_HID_ONLY_Init();
|
||||
|
||||
uint32_t pios_usb_id;
|
||||
if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
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;
|
||||
@ -90,3 +101,7 @@ void PIOS_Board_Init(void) {
|
||||
|
||||
board_init_complete = true;
|
||||
}
|
||||
|
||||
void PIOS_ADC_DMA_Handler()
|
||||
{
|
||||
}
|
||||
|
||||
@ -31,6 +31,7 @@
|
||||
#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),
|
||||
@ -50,40 +51,15 @@ static const uint8_t usb_product_id[28] = {
|
||||
'l', 0,
|
||||
};
|
||||
|
||||
static uint8_t usb_serial_number[52] = {
|
||||
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,
|
||||
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),
|
||||
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
|
||||
};
|
||||
|
||||
static const uint8_t usb_vendor_id[28] = {
|
||||
@ -107,11 +83,13 @@ static const uint8_t usb_vendor_id[28] = {
|
||||
int32_t PIOS_USB_BOARD_DATA_Init(void)
|
||||
{
|
||||
/* Load device serial number into serial number string */
|
||||
uint8_t sn[25];
|
||||
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
|
||||
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];
|
||||
}
|
||||
|
||||
/* 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));
|
||||
|
||||
@ -1,491 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot project build PiOS and the AP.
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
TOP := $(realpath $(WHEREAMI)/../../../)
|
||||
include $(TOP)/make/firmware-defs.mk
|
||||
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET := bl_$(BOARD_NAME)
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR := $(TOP)/build/$(TARGET)
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES to compile for debugging
|
||||
DEBUG ?= NO
|
||||
|
||||
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
|
||||
ENABLE_DEBUG_PINS ?= NO
|
||||
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= NO
|
||||
|
||||
# Remove command is different for Code Sourcery on Windows
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
REMOVE_CMD = cs-rm
|
||||
else
|
||||
REMOVE_CMD = rm
|
||||
endif
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# Paths
|
||||
OPSYSTEM = .
|
||||
OPSYSTEMINC = $(OPSYSTEM)/inc
|
||||
OPUAVTALK = ./UAVTalk
|
||||
OPUAVTALKINC = $(OPUAVTALK)/inc
|
||||
OPUAVOBJ = ./UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPTESTS = ./Tests
|
||||
OPMODULEDIR = ./Modules
|
||||
FLIGHTLIB = ../../Libraries
|
||||
FLIGHTLIBINC = ../../Libraries/inc
|
||||
PIOS = ../../PiOS
|
||||
PIOSINC = $(PIOS)/inc
|
||||
PIOSSTM32F10X = $(PIOS)/STM32F10x
|
||||
PIOSCOMMON = $(PIOS)/Common
|
||||
PIOSBOARDS = $(PIOS)/Boards
|
||||
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
|
||||
STMLIBDIR = $(APPLIBDIR)
|
||||
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
|
||||
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
|
||||
STMSPDSRCDIR = $(STMSPDDIR)/src
|
||||
STMSPDINCDIR = $(STMSPDDIR)/inc
|
||||
STMUSBSRCDIR = $(STMUSBDIR)/src
|
||||
STMUSBINCDIR = $(STMUSBDIR)/inc
|
||||
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
|
||||
DOSFSDIR = $(APPLIBDIR)/dosfs
|
||||
MSDDIR = $(APPLIBDIR)/msd
|
||||
RTOSDIR = $(APPLIBDIR)/FreeRTOS
|
||||
RTOSSRCDIR = $(RTOSDIR)/Source
|
||||
RTOSINCDIR = $(RTOSSRCDIR)/include
|
||||
DOXYGENDIR = ../Doc/Doxygen
|
||||
HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME)
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
## OPENPILOT_BL CORE:
|
||||
SRC += $(OPSYSTEM)/main.c
|
||||
SRC += $(OPSYSTEM)/pios_board.c
|
||||
SRC += $(OPSYSTEM)/op_dfu.c
|
||||
SRC += $(FLIGHTLIB)/stopwatch.c
|
||||
SRC += $(OPSYSTEM)/ssp.c
|
||||
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
SRC += $(PIOSSTM32F10X)/pios_sys.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_led.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_delay.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spi.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
|
||||
# PIOS USB related files (seperated to make code maintenance more easy)
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usbhook.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
|
||||
SRC += $(OPSYSTEM)/pios_usb_board_data.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_board_info.c
|
||||
SRC += $(PIOSCOMMON)/pios_com_msg.c
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_opahrs_v0.c
|
||||
SRC += $(PIOSCOMMON)/pios_bl_helper.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
|
||||
## Libraries for flight calculations
|
||||
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
|
||||
## CMSIS for STM32
|
||||
SRC += $(CMSISDIR)/core_cm3.c
|
||||
SRC += $(CMSISDIR)/system_stm32f10x.c
|
||||
|
||||
## Used parts of the STM-Library
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
|
||||
SRC += $(STMSPDSRCDIR)/misc.c
|
||||
|
||||
## STM32 USB Library
|
||||
SRC += $(STMUSBSRCDIR)/usb_core.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_init.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_int.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_mem.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_regs.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_sil.c
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
# will not be considered source files but generated files (assembler
|
||||
# output from the compiler), and will be deleted upon "make clean"!
|
||||
# Even though the DOS/Win* filesystem matches both .s and .S the same,
|
||||
# it will preserve the spelling of the filenames, and gcc itself does
|
||||
# care about how the name is spelled on its command-line.
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
|
||||
|
||||
# List Assembler source files here which must be assembled in ARM-Mode..
|
||||
ASRCARM =
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS = $(OPSYSTEM)
|
||||
EXTRAINCDIRS += $(OPSYSTEMINC)
|
||||
EXTRAINCDIRS += $(OPUAVTALK)
|
||||
EXTRAINCDIRS += $(OPUAVTALKINC)
|
||||
EXTRAINCDIRS += $(OPUAVOBJ)
|
||||
EXTRAINCDIRS += $(OPUAVOBJINC)
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PIOSSTM32F10X)
|
||||
EXTRAINCDIRS += $(PIOSCOMMON)
|
||||
EXTRAINCDIRS += $(PIOSBOARDS)
|
||||
EXTRAINCDIRS += $(STMSPDINCDIR)
|
||||
EXTRAINCDIRS += $(STMUSBINCDIR)
|
||||
EXTRAINCDIRS += $(CMSISDIR)
|
||||
EXTRAINCDIRS += $(DOSFSDIR)
|
||||
EXTRAINCDIRS += $(MSDDIR)
|
||||
EXTRAINCDIRS += $(RTOSINCDIR)
|
||||
EXTRAINCDIRS += $(APPLIBDIR)
|
||||
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
|
||||
EXTRAINCDIRS += $(HWDEFSINC)
|
||||
|
||||
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
OPT = 0
|
||||
else
|
||||
OPT = s
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
CDEFS += -DUSE_$(BOARD)
|
||||
ifeq ($(ENABLE_DEBUG_PINS), YES)
|
||||
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
|
||||
endif
|
||||
ifeq ($(ENABLE_AUX_UART), YES)
|
||||
CDEFS += -DPIOS_ENABLE_AUX_UART
|
||||
endif
|
||||
|
||||
# Provide (only) the bootloader with board-specific defines
|
||||
BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
|
||||
BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
|
||||
BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
|
||||
BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
|
||||
BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
|
||||
BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
|
||||
BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
|
||||
|
||||
# Place project-specific -D and/or -U options for
|
||||
# Assembler with preprocessor here.
|
||||
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
|
||||
ADEFS = -D__ASSEMBLY__
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS += -DDEBUG
|
||||
endif
|
||||
|
||||
CFLAGS += -g$(DEBUGF)
|
||||
CFLAGS += -O$(OPT)
|
||||
ifeq ($(DEBUG),NO)
|
||||
CFLAGS += -ffunction-sections
|
||||
endif
|
||||
|
||||
CFLAGS += -mcpu=$(MCU)
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(BLONLY_CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
|
||||
ifeq ($(DEBUG),NO)
|
||||
LDFLAGS += -Wl,-static
|
||||
endif
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_BL_sections.ld
|
||||
|
||||
# Define programs and commands.
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
# Default target.
|
||||
all: build
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
${OUTDIR}/InitMods.c: Makefile
|
||||
@echo $(MSG_MODINIT) $(call toprel, $@)
|
||||
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
|
||||
|
||||
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
bino: $(OUTDIR)/$(TARGET).bin.o
|
||||
|
||||
# Display sizes of sections.
|
||||
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Install: install binary file with prefix/suffix into install directory
|
||||
install: $(OUTDIR)/$(TARGET).bin
|
||||
ifneq ($(INSTALL_DIR),)
|
||||
@echo $(MSG_INSTALLING) $(call toprel, $<)
|
||||
$(V1) mkdir -p $(INSTALL_DIR)
|
||||
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).bin
|
||||
else
|
||||
$(error INSTALL_DIR must be specified for $@)
|
||||
endif
|
||||
|
||||
# Target: clean project.
|
||||
clean: clean_list
|
||||
|
||||
clean_list :
|
||||
@echo $(MSG_CLEANING)
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
|
||||
$(V1) $(REMOVE) $(ALLOBJ)
|
||||
$(V1) $(REMOVE) $(LSTFILES)
|
||||
$(V1) $(REMOVE) $(DEPFILES)
|
||||
$(V1) $(REMOVE) $(SRC:.c=.s)
|
||||
$(V1) $(REMOVE) $(SRCARM:.c=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
|
||||
else
|
||||
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir -p $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all build clean clean_list install
|
||||
@ -1,58 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotBL OpenPilot BootLoader
|
||||
* @{
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief PiOS configuration header.
|
||||
* Central compile time config for the project.
|
||||
* In particular, pios_config.h is where you define which PiOS libraries
|
||||
* and features are included in the firmware.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef PIOS_CONFIG_H
|
||||
#define PIOS_CONFIG_H
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_SPI
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_USART
|
||||
#define PIOS_INCLUDE_USB
|
||||
#define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_OPAHRS
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_COM_MSG
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_IAP
|
||||
//#define DEBUG_SSP
|
||||
|
||||
/* Defaults for Logging */
|
||||
#define LOG_FILENAME "PIOS.LOG"
|
||||
#define STARTUP_LOG_ENABLED 1
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -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
|
||||
@ -1,284 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotBL OpenPilot BootLoader
|
||||
* @brief These files contain the code to the OpenPilot MB Bootloader.
|
||||
*
|
||||
* @{
|
||||
* @file main.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief This is the file with the main function of the OpenPilot BootLoader
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
/* Bootloader Includes */
|
||||
#include <pios.h>
|
||||
#include <pios_board_info.h>
|
||||
#include "pios_opahrs.h"
|
||||
#include "stopwatch.h"
|
||||
#include "op_dfu.h"
|
||||
#include "usb_lib.h"
|
||||
#include "pios_iap.h"
|
||||
#include "ssp.h"
|
||||
#include "fifo_buffer.h"
|
||||
#include "pios_com_msg.h"
|
||||
/* Prototype of PIOS_Board_Init() function */
|
||||
extern void PIOS_Board_Init(void);
|
||||
extern void FLASH_Download();
|
||||
#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
typedef void (*pFunction)(void);
|
||||
/* Private define ------------------------------------------------------------*/
|
||||
/* Private macro -------------------------------------------------------------*/
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
pFunction Jump_To_Application;
|
||||
uint32_t JumpAddress;
|
||||
|
||||
/// LEDs PWM
|
||||
uint32_t period1 = 50; // *100 uS -> 5 mS
|
||||
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
|
||||
uint32_t period2 = 50; // *100 uS -> 5 mS
|
||||
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
|
||||
|
||||
|
||||
////////////////////////////////////////
|
||||
uint8_t tempcount = 0;
|
||||
|
||||
/// SSP SECTION
|
||||
/// SSP TIME SOURCE
|
||||
#define SSP_TIMER TIM7
|
||||
uint32_t ssp_time = 0;
|
||||
#define MAX_PACKET_DATA_LEN 255
|
||||
#define MAX_PACKET_BUF_SIZE (1+1+MAX_PACKET_DATA_LEN+2)
|
||||
#define UART_BUFFER_SIZE 1024
|
||||
uint8_t rx_buffer[UART_BUFFER_SIZE] __attribute__ ((aligned(4)));
|
||||
// align to 32-bit to try and provide speed improvement;
|
||||
// master buffers...
|
||||
uint8_t SSP_TxBuf[MAX_PACKET_BUF_SIZE];
|
||||
uint8_t SSP_RxBuf[MAX_PACKET_BUF_SIZE];
|
||||
void SSP_CallBack(uint8_t *buf, uint16_t len);
|
||||
int16_t SSP_SerialRead(void);
|
||||
void SSP_SerialWrite( uint8_t);
|
||||
uint32_t SSP_GetTime(void);
|
||||
PortConfig_t SSP_PortConfig = { .rxBuf = SSP_RxBuf,
|
||||
.rxBufSize = MAX_PACKET_DATA_LEN, .txBuf = SSP_TxBuf,
|
||||
.txBufSize = MAX_PACKET_DATA_LEN, .max_retry = 10, .timeoutLen = 1000,
|
||||
.pfCallBack = SSP_CallBack, .pfSerialRead = SSP_SerialRead,
|
||||
.pfSerialWrite = SSP_SerialWrite, .pfGetTime = SSP_GetTime, };
|
||||
Port_t ssp_port;
|
||||
t_fifo_buffer ssp_buffer;
|
||||
|
||||
/* Extern variables ----------------------------------------------------------*/
|
||||
DFUStates DeviceState;
|
||||
DFUPort ProgPort;
|
||||
int16_t status = 0;
|
||||
uint8_t JumpToApp = FALSE;
|
||||
uint8_t GO_dfu = FALSE;
|
||||
uint8_t USB_connected = FALSE;
|
||||
uint8_t User_DFU_request = FALSE;
|
||||
static uint8_t mReceive_Buffer[63];
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
|
||||
uint8_t processRX();
|
||||
void jump_to_app();
|
||||
uint32_t sspTimeSource();
|
||||
|
||||
#define LED_PWM_TIMER TIM6
|
||||
int main() {
|
||||
PIOS_SYS_Init();
|
||||
if (BSL_HOLD_STATE == 0)
|
||||
USB_connected = TRUE;
|
||||
|
||||
PIOS_IAP_Init();
|
||||
|
||||
if (PIOS_IAP_CheckRequest() == TRUE) {
|
||||
PIOS_Board_Init();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
User_DFU_request = TRUE;
|
||||
PIOS_IAP_ClearRequest();
|
||||
}
|
||||
|
||||
GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE);
|
||||
|
||||
if (GO_dfu == TRUE) {
|
||||
if (USB_connected)
|
||||
ProgPort = Usb;
|
||||
else
|
||||
ProgPort = Serial;
|
||||
PIOS_Board_Init();
|
||||
if (User_DFU_request == TRUE)
|
||||
DeviceState = DFUidle;
|
||||
else
|
||||
DeviceState = BLidle;
|
||||
STOPWATCH_Init(100, LED_PWM_TIMER);
|
||||
if (ProgPort == Serial) {
|
||||
fifoBuf_init(&ssp_buffer, rx_buffer, UART_BUFFER_SIZE);
|
||||
STOPWATCH_Init(100, SSP_TIMER);//nao devia ser 1000?
|
||||
STOPWATCH_Reset(SSP_TIMER);
|
||||
ssp_Init(&ssp_port, &SSP_PortConfig);
|
||||
}
|
||||
PIOS_OPAHRS_ForceSlaveSelected(true);
|
||||
} else
|
||||
JumpToApp = TRUE;
|
||||
|
||||
STOPWATCH_Reset(LED_PWM_TIMER);
|
||||
while (TRUE) {
|
||||
if (ProgPort == Serial) {
|
||||
ssp_ReceiveProcess(&ssp_port);
|
||||
status = ssp_SendProcess(&ssp_port);
|
||||
while ((status != SSP_TX_IDLE) && (status != SSP_TX_ACKED)) {
|
||||
ssp_ReceiveProcess(&ssp_port);
|
||||
status = ssp_SendProcess(&ssp_port);
|
||||
}
|
||||
}
|
||||
if (JumpToApp == TRUE)
|
||||
jump_to_app();
|
||||
//pwm_period = 50; // *100 uS -> 5 mS
|
||||
//pwm_sweep_steps =100; // * 5 mS -> 500 mS
|
||||
|
||||
switch (DeviceState) {
|
||||
case Last_operation_Success:
|
||||
case uploadingStarting:
|
||||
case DFUidle:
|
||||
period1 = 50;
|
||||
sweep_steps1 = 100;
|
||||
PIOS_LED_Off(PIOS_LED_ALARM);
|
||||
period2 = 0;
|
||||
break;
|
||||
case uploading:
|
||||
period1 = 50;
|
||||
sweep_steps1 = 100;
|
||||
period2 = 25;
|
||||
sweep_steps2 = 50;
|
||||
break;
|
||||
case downloading:
|
||||
period1 = 25;
|
||||
sweep_steps1 = 50;
|
||||
PIOS_LED_Off(PIOS_LED_ALARM);
|
||||
period2 = 0;
|
||||
break;
|
||||
case BLidle:
|
||||
period1 = 0;
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
period2 = 0;
|
||||
break;
|
||||
default://error
|
||||
period1 = 50;
|
||||
sweep_steps1 = 100;
|
||||
period2 = 50;
|
||||
sweep_steps2 = 100;
|
||||
}
|
||||
|
||||
if (period1 != 0) {
|
||||
if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER)))
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
else
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
} else
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
|
||||
if (period2 != 0) {
|
||||
if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER)))
|
||||
PIOS_LED_On(PIOS_LED_ALARM);
|
||||
else
|
||||
PIOS_LED_Off(PIOS_LED_ALARM);
|
||||
} else
|
||||
PIOS_LED_Off(PIOS_LED_ALARM);
|
||||
|
||||
if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100)
|
||||
STOPWATCH_Reset(LED_PWM_TIMER);
|
||||
if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState
|
||||
== BLidle))
|
||||
JumpToApp = TRUE;
|
||||
|
||||
processRX();
|
||||
DataDownload(start);
|
||||
}
|
||||
}
|
||||
|
||||
void jump_to_app() {
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
|
||||
FLASH_Lock();
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
|
||||
_SetCNTR(0); // clear interrupt mask
|
||||
_SetISTR(0); // clear all requests
|
||||
JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4);
|
||||
Jump_To_Application = (pFunction) JumpAddress;
|
||||
/* Initialize user application's Stack Pointer */
|
||||
__set_MSP(*(__IO uint32_t*) bdinfo->fw_base);
|
||||
Jump_To_Application();
|
||||
} else {
|
||||
DeviceState = failed_jump;
|
||||
return;
|
||||
}
|
||||
}
|
||||
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
|
||||
uint32_t pwm_duty = ((count / pwm_period) % pwm_sweep_steps)
|
||||
/ (pwm_sweep_steps / pwm_period);
|
||||
if ((count % (2 * pwm_period * pwm_sweep_steps)) > pwm_period
|
||||
* pwm_sweep_steps)
|
||||
pwm_duty = pwm_period - pwm_duty; // negative direction each 50*100 ticks
|
||||
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
|
||||
}
|
||||
|
||||
uint8_t processRX() {
|
||||
if (ProgPort == Usb) {
|
||||
if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
|
||||
processComand(mReceive_Buffer);
|
||||
}
|
||||
} else if (ProgPort == Serial) {
|
||||
|
||||
if (fifoBuf_getUsed(&ssp_buffer) >= 63) {
|
||||
for (int32_t x = 0; x < 63; ++x) {
|
||||
mReceive_Buffer[x] = fifoBuf_getByte(&ssp_buffer);
|
||||
}
|
||||
processComand(mReceive_Buffer);
|
||||
}
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
uint32_t sspTimeSource() {
|
||||
if (STOPWATCH_ValueGet(SSP_TIMER) > 5000) {
|
||||
++ssp_time;
|
||||
STOPWATCH_Reset(SSP_TIMER);
|
||||
}
|
||||
return ssp_time;
|
||||
}
|
||||
void SSP_CallBack(uint8_t *buf, uint16_t len) {
|
||||
fifoBuf_putData(&ssp_buffer, buf, len);
|
||||
}
|
||||
int16_t SSP_SerialRead(void) {
|
||||
uint8_t byte;
|
||||
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF, &byte, 1, 0) == 1) {
|
||||
return byte;
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
void SSP_SerialWrite(uint8_t value) {
|
||||
PIOS_COM_SendChar(PIOS_COM_TELEM_RF, value);
|
||||
}
|
||||
uint32_t SSP_GetTime(void) {
|
||||
return sspTimeSource();
|
||||
}
|
||||
@ -1,128 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotBL OpenPilot BootLoader
|
||||
* @{
|
||||
*
|
||||
* @file pios_board.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board specific static initialisers for hardware for the OpenPilot board.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* Pull in the board-specific static HW definitions.
|
||||
* Including .c files is a bit ugly but this allows all of
|
||||
* the HW definitions to be const and static to limit their
|
||||
* scope.
|
||||
*
|
||||
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
|
||||
*/
|
||||
#include "board_hw_defs.c"
|
||||
|
||||
#include <pios.h>
|
||||
|
||||
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
|
||||
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
|
||||
|
||||
static uint8_t pios_com_telem_rf_rx_buffer[PIOS_COM_TELEM_RF_RX_BUF_LEN];
|
||||
static uint8_t pios_com_telem_rf_tx_buffer[PIOS_COM_TELEM_RF_TX_BUF_LEN];
|
||||
|
||||
uint32_t pios_com_telem_rf_id;
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
|
||||
#include "pios_opahrs.h"
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
static bool board_init_complete = false;
|
||||
void PIOS_Board_Init(void) {
|
||||
if (board_init_complete) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Enable Prefetch Buffer */
|
||||
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
|
||||
|
||||
/* Flash 2 wait state */
|
||||
FLASH_SetLatency(FLASH_Latency_2);
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Initialize the PiOS library */
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
uint32_t pios_usart_telem_rf_id;
|
||||
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver,
|
||||
pios_usart_telem_rf_id,
|
||||
pios_com_telem_rf_rx_buffer, sizeof(pios_com_telem_rf_rx_buffer),
|
||||
pios_com_telem_rf_tx_buffer, sizeof(pios_com_telem_rf_tx_buffer))) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB)
|
||||
/* Initialize board specific USB data */
|
||||
PIOS_USB_BOARD_DATA_Init();
|
||||
|
||||
/* Activate the HID-only USB configuration */
|
||||
PIOS_USB_DESC_HID_ONLY_Init();
|
||||
|
||||
uint32_t pios_usb_id;
|
||||
if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
|
||||
uint32_t pios_usb_hid_id;
|
||||
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
|
||||
|
||||
/* Set up the SPI interface to the AHRS */
|
||||
if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
/* Bind the AHRS comms layer to the AHRS SPI link */
|
||||
PIOS_OPAHRS_Attach(pios_spi_ahrs_id);
|
||||
|
||||
board_init_complete = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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,6 +104,8 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_iap.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
|
||||
|
||||
# PIOS USB related files (seperated to make code maintenance more easy)
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb.c
|
||||
@ -112,12 +115,11 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
|
||||
SRC += $(OPSYSTEM)/pios_usb_board_data.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
|
||||
SRC += $(PIOSCOMMON)/pios_usb_util.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_board_info.c
|
||||
SRC += $(PIOSCOMMON)/pios_com_msg.c
|
||||
SRC += $(PIOSCOMMON)/pios_bl_helper.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
|
||||
## Libraries for flight calculations
|
||||
@ -198,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
|
||||
@ -416,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
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
|
||||
#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
|
||||
|
||||
@ -70,14 +70,13 @@ uint8_t processRX();
|
||||
void jump_to_app();
|
||||
|
||||
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();
|
||||
|
||||
@ -23,97 +23,9 @@
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "board_hw_defs.c"
|
||||
#include <pios.h>
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
|
||||
#include <pios_led_priv.h>
|
||||
static const struct pios_led pios_leds[] = {
|
||||
[PIOS_LED_USB] = {
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_3,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
[PIOS_LED_LINK] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_5,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
[PIOS_LED_RX] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
[PIOS_LED_TX] = {
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_led_cfg pios_led_cfg = {
|
||||
.leds = pios_leds,
|
||||
.num_leds = NELEMENTS(pios_leds),
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM_MSG)
|
||||
|
||||
#include <pios_com_msg_priv.h>
|
||||
|
||||
#endif /* PIOS_INCLUDE_COM_MSG */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB)
|
||||
#include "pios_usb_priv.h"
|
||||
|
||||
static const struct pios_usb_cfg pios_usb_main_cfg = {
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#include "pios_usb_board_data_priv.h"
|
||||
#include "pios_usb_desc_hid_only_priv.h"
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_HID)
|
||||
#include <pios_usb_hid_priv.h>
|
||||
|
||||
const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
|
||||
.data_if = 0,
|
||||
.data_rx_ep = 1,
|
||||
.data_tx_ep = 1,
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB_HID */
|
||||
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
|
||||
/**
|
||||
@ -139,6 +51,10 @@ void PIOS_Board_Init(void) {
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB)
|
||||
/* Initialize board specific USB data */
|
||||
PIOS_USB_BOARD_DATA_Init();
|
||||
|
||||
@ -31,6 +31,7 @@
|
||||
#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),
|
||||
@ -46,40 +47,15 @@ static const uint8_t usb_product_id[20] = {
|
||||
'e', 0,
|
||||
};
|
||||
|
||||
static uint8_t usb_serial_number[52] = {
|
||||
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,
|
||||
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),
|
||||
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
|
||||
};
|
||||
|
||||
static const uint8_t usb_vendor_id[28] = {
|
||||
@ -103,11 +79,13 @@ static const uint8_t usb_vendor_id[28] = {
|
||||
int32_t PIOS_USB_BOARD_DATA_Init(void)
|
||||
{
|
||||
/* Load device serial number into serial number string */
|
||||
uint8_t sn[25];
|
||||
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
|
||||
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];
|
||||
}
|
||||
|
||||
/* 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));
|
||||
|
||||
@ -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,17 @@ CPPSRC =
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
# will not be considered source files but generated files (assembler
|
||||
# output from the compiler), and will be deleted upon "make clean"!
|
||||
# Even though the DOS/Win* filesystem matches both .s and .S the same,
|
||||
# it will preserve the spelling of the filenames, and gcc itself does
|
||||
# care about how the name is spelled on its command-line.
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
|
||||
|
||||
# List Assembler source files here which must be assembled in ARM-Mode..
|
||||
ASRCARM =
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PIOSSTM32F10X)
|
||||
EXTRAINCDIRS += $(PIOSSTM34FXX)
|
||||
EXTRAINCDIRS += $(PIOSCOMMON)
|
||||
EXTRAINCDIRS += $(PIOSBOARDS)
|
||||
EXTRAINCDIRS += $(STMSPDINCDIR)
|
||||
EXTRAINCDIRS += $(CMSISDIR)
|
||||
EXTRAINCDIRS += $(AHRS_BLINC)
|
||||
EXTRAINCDIRS += $(REVO_BLINC)
|
||||
EXTRAINCDIRS += $(HWDEFSINC)
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
@ -172,7 +124,7 @@ EXTRA_LIBDIRS =
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32FXX)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
@ -196,7 +148,9 @@ DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS = -DSTM32F4XX
|
||||
CDEFS += -DSYSCLK_FREQ=$(SYSCLK_FREQ)
|
||||
CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
CDEFS += -DUSE_$(BOARD)
|
||||
|
||||
@ -238,6 +192,11 @@ ifeq ($(DEBUG),YES)
|
||||
CFLAGS =
|
||||
endif
|
||||
|
||||
# This is not the best place for these. Really should abstract out
|
||||
# to the board file or something
|
||||
CFLAGS += -DSTM32F4XX
|
||||
CFLAGS += -DMEM_SIZE=1024000000
|
||||
|
||||
CFLAGS += -g$(DEBUGF)
|
||||
CFLAGS += -O$(OPT)
|
||||
ifeq ($(DEBUG),NO)
|
||||
@ -255,7 +214,7 @@ CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
#CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
@ -288,9 +247,8 @@ LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
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
|
||||
@ -354,8 +312,10 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
|
||||
|
||||
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
@ -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
|
||||
@ -33,9 +33,11 @@
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_SPI
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_IAP
|
||||
#define PIOS_INCLUDE_USB
|
||||
#define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_COM_MSG
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_IAP
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
||||
@ -37,8 +37,8 @@
|
||||
|
||||
#include "pios_usb_defs.h" /* struct usb_* */
|
||||
|
||||
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPENPILOT_MAIN
|
||||
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPENPILOT_MAIN, USB_OP_BOARD_MODE_BL)
|
||||
#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
|
||||
208
flight/Bootloaders/Revolution/main.c
Normal file
208
flight/Bootloaders/Revolution/main.c
Normal 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;
|
||||
}
|
||||
|
||||
@ -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,12 +28,11 @@
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "pios.h"
|
||||
#include <stdbool.h>
|
||||
#include "op_dfu.h"
|
||||
#include "pios_bl_helper.h"
|
||||
#include "pios_com_msg.h"
|
||||
#include <pios_board_info.h>
|
||||
#include "pios_opahrs.h"
|
||||
#include "ssp.h"
|
||||
//programmable devices
|
||||
Device devicesTable[10];
|
||||
uint8_t numberOfDevices = 0;
|
||||
@ -72,8 +71,6 @@ DFUTransfer downType = 0;
|
||||
/* Extern variables ----------------------------------------------------------*/
|
||||
extern DFUStates DeviceState;
|
||||
extern uint8_t JumpToApp;
|
||||
extern Port_t ssp_port;
|
||||
extern DFUPort ProgPort;
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
/* Private functions ---------------------------------------------------------*/
|
||||
void sendData(uint8_t * buf, uint16_t size);
|
||||
@ -139,14 +136,14 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
Command = Command & 0b00011111;
|
||||
|
||||
if (EchoReqFlag == 1) {
|
||||
memcpy(echoBuffer, Buffer, 64);
|
||||
memcpy(echoBuffer, xReceive_Buffer, 64);
|
||||
}
|
||||
switch (Command) {
|
||||
case EnterDFU:
|
||||
if (((DeviceState == BLidle) && (Data0 < numberOfDevices))
|
||||
|| (DeviceState == DFUidle)) {
|
||||
if (Data0 > 0)
|
||||
OPDfuIni(TRUE);
|
||||
OPDfuIni(true);
|
||||
DeviceState = DFUidle;
|
||||
currentProgrammingDestination = devicesTable[Data0].programmingType;
|
||||
currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01;
|
||||
@ -159,7 +156,7 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
result = PIOS_BL_HELPER_FLASH_Ini();
|
||||
break;
|
||||
case Remote_flash_via_spi:
|
||||
result = TRUE;
|
||||
result = true;
|
||||
break;
|
||||
default:
|
||||
DeviceState = Last_operation_failed;
|
||||
@ -184,35 +181,18 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
SizeOfLastPacket = Data1;
|
||||
|
||||
if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1)
|
||||
* 14 * 4 + SizeOfLastPacket * 4) == TRUE) {
|
||||
* 14 * 4 + SizeOfLastPacket * 4) == true) {
|
||||
DeviceState = outsideDevCapabilities;
|
||||
Aditionals = (uint32_t) Command;
|
||||
} else {
|
||||
uint8_t result = 1;
|
||||
struct opahrs_msg_v0 rsp;
|
||||
if (TransferType == FW) {
|
||||
switch (currentProgrammingDestination) {
|
||||
case Self_flash:
|
||||
result = PIOS_BL_HELPER_FLASH_Start();
|
||||
break;
|
||||
case Remote_flash_via_spi:
|
||||
PIOS_OPAHRS_bl_FwupStart(&rsp);
|
||||
result = FALSE;
|
||||
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;
|
||||
@ -236,8 +216,6 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
{
|
||||
numberOfWords = SizeOfLastPacket;
|
||||
}
|
||||
struct opahrs_msg_v0 rsp;
|
||||
struct opahrs_msg_v0 req;
|
||||
uint8_t result = 0;
|
||||
switch (currentProgrammingDestination) {
|
||||
case Self_flash:
|
||||
@ -259,31 +237,7 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
}
|
||||
break;
|
||||
case Remote_flash_via_spi:
|
||||
for (uint8_t x = 0; x < numberOfWords; ++x) {
|
||||
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;
|
||||
@ -306,7 +260,7 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
}
|
||||
break;
|
||||
case Req_Capabilities:
|
||||
OPDfuIni(TRUE);
|
||||
OPDfuIni(true);
|
||||
Buffer[0] = 0x01;
|
||||
Buffer[1] = Rep_Capabilities;
|
||||
if (Data0 == 0) {
|
||||
@ -342,28 +296,12 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
sendData(Buffer + 1, 63);
|
||||
break;
|
||||
case JumpFW:
|
||||
if (numberOfDevices > 1) {
|
||||
struct opahrs_msg_v0 rsp;
|
||||
PIOS_OPAHRS_bl_boot(0);
|
||||
if (PIOS_OPAHRS_bl_FwupStatus(&rsp) == OPAHRS_RESULT_OK) {
|
||||
DeviceState = failed_jump;
|
||||
break;
|
||||
} else {
|
||||
if (Data == 0x5AFE) {
|
||||
/* Force board into safe mode */
|
||||
PIOS_IAP_WriteBootCount(0xFFFF);
|
||||
}
|
||||
FLASH_Lock();
|
||||
JumpToApp = 1;
|
||||
}
|
||||
} else {
|
||||
if (Data == 0x5AFE) {
|
||||
/* Force board into safe mode */
|
||||
PIOS_IAP_WriteBootCount(0xFFFF);
|
||||
}
|
||||
FLASH_Lock();
|
||||
JumpToApp = 1;
|
||||
if (Data == 0x5AFE) {
|
||||
/* Force board into safe mode */
|
||||
PIOS_IAP_WriteBootCount(0xFFFF);
|
||||
}
|
||||
FLASH_Lock();
|
||||
JumpToApp = 1;
|
||||
break;
|
||||
case Reset:
|
||||
PIOS_SYS_Reset();
|
||||
@ -401,8 +339,8 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
downType = Data0;
|
||||
downPacketTotal = Count;
|
||||
downSizeOfLastPacket = Data1;
|
||||
if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14
|
||||
+ downSizeOfLastPacket) == 1) {
|
||||
if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 * 4
|
||||
+ downSizeOfLastPacket * 4) == 1) {
|
||||
DeviceState = outsideDevCapabilities;
|
||||
Aditionals = (uint32_t) Command;
|
||||
|
||||
@ -445,8 +383,8 @@ void processComand(uint8_t *xReceive_Buffer) {
|
||||
|
||||
}
|
||||
if (EchoReqFlag == 1) {
|
||||
echoBuffer[1] = echoBuffer[1] | EchoAnsFlag;
|
||||
sendData(echoBuffer + 1, 63);
|
||||
echoBuffer[0] = echoBuffer[0] | (1 << 6);
|
||||
sendData(echoBuffer, 63);
|
||||
}
|
||||
return;
|
||||
}
|
||||
@ -466,41 +404,8 @@ void OPDfuIni(uint8_t discover) {
|
||||
numberOfDevices = 1;
|
||||
devicesTable[0] = dev;
|
||||
if (discover) {
|
||||
uint8_t found_spi_device = FALSE;
|
||||
|
||||
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) {
|
||||
@ -524,26 +429,16 @@ uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) {
|
||||
return (size > currentDevice.sizeOfDescription) ? 1 : 0;
|
||||
break;
|
||||
default:
|
||||
return TRUE;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t CalcFirmCRC() {
|
||||
struct opahrs_msg_v0 rsp;
|
||||
switch (currentProgrammingDestination) {
|
||||
case Self_flash:
|
||||
return PIOS_BL_HELPER_CRC_Memory_Calc();
|
||||
break;
|
||||
case Remote_flash_via_spi:
|
||||
PIOS_OPAHRS_bl_FwupVerify(&rsp);
|
||||
for (int i = 0; i < 5; ++i) {
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
PIOS_OPAHRS_bl_resync();
|
||||
if (PIOS_OPAHRS_bl_GetVersions(&rsp) == OPAHRS_RESULT_OK) {
|
||||
return rsp.payload.user.v.rsp.versions.fw_crc;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
break;
|
||||
default:
|
||||
@ -553,34 +448,21 @@ uint32_t CalcFirmCRC() {
|
||||
|
||||
}
|
||||
void sendData(uint8_t * buf, uint16_t size) {
|
||||
if (ProgPort == Usb) {
|
||||
PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size);
|
||||
} 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;
|
||||
}
|
||||
}
|
||||
@ -32,9 +32,10 @@
|
||||
*/
|
||||
#include "board_hw_defs.c"
|
||||
|
||||
#include <pios_board_info.h>
|
||||
#include <pios.h>
|
||||
|
||||
#include "bl_fsm.h" /* lfsm_* */
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
|
||||
static bool board_init_complete = false;
|
||||
void PIOS_Board_Init() {
|
||||
@ -42,16 +43,36 @@ void PIOS_Board_Init() {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
/* Set up the SPI interface to the OP board */
|
||||
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
#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);
|
||||
}
|
||||
lfsm_attach(pios_spi_op_id);
|
||||
lfsm_init();
|
||||
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;
|
||||
}
|
||||
@ -32,18 +32,19 @@
|
||||
#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */
|
||||
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
|
||||
|
||||
static const uint8_t usb_product_id[20] = {
|
||||
static const uint8_t usb_product_id[22] = {
|
||||
sizeof(usb_product_id),
|
||||
USB_DESC_TYPE_STRING,
|
||||
'O', 0,
|
||||
'p', 0,
|
||||
'R', 0,
|
||||
'e', 0,
|
||||
'n', 0,
|
||||
'P', 0,
|
||||
'i', 0,
|
||||
'l', 0,
|
||||
'v', 0,
|
||||
'o', 0,
|
||||
'l', 0,
|
||||
'u', 0,
|
||||
't', 0,
|
||||
'i', 0,
|
||||
'o', 0,
|
||||
'n', 0,
|
||||
};
|
||||
|
||||
static uint8_t usb_serial_number[52] = {
|
||||
@ -39,6 +39,7 @@ DEBUG ?= NO
|
||||
|
||||
# Include objects that are just nice information to show
|
||||
DIAGNOSTICS ?= NO
|
||||
DIAG_TASKS ?= NO
|
||||
|
||||
# Set to YES to build a FW version that will erase all flash memory
|
||||
ERASE_FLASH ?= NO
|
||||
@ -49,7 +50,7 @@ ENABLE_DEBUG_PINS ?= NO
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= YES
|
||||
CODE_SOURCERY ?= NO
|
||||
|
||||
# Remove command is different for Code Sourcery on Windows
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
@ -185,7 +186,8 @@ SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
|
||||
SRC += $(OPUAVSYNTHDIR)/attituderaw.c
|
||||
SRC += $(OPUAVSYNTHDIR)/accels.c
|
||||
SRC += $(OPUAVSYNTHDIR)/gyros.c
|
||||
SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
|
||||
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
|
||||
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
|
||||
@ -227,8 +229,9 @@ SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_exti.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_rtc.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_wdg.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_iap.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_tim.c
|
||||
|
||||
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
|
||||
|
||||
# PIOS USB related files (separated to make code maintenance more easy)
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb.c
|
||||
@ -240,17 +243,17 @@ 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_bmp085.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
|
||||
@ -603,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
|
||||
|
||||
@ -71,7 +71,7 @@ int main()
|
||||
PIOS_Board_Init();
|
||||
|
||||
#ifdef ERASE_FLASH
|
||||
PIOS_Flash_W25X_EraseChip();
|
||||
PIOS_Flash_Jedec_EraseChip();
|
||||
#if defined(PIOS_LED_HEARTBEAT)
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
#endif /* PIOS_LED_HEARTBEAT */
|
||||
@ -84,7 +84,20 @@ 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. */
|
||||
|
||||
@ -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
|
||||
|
||||
@ -36,10 +36,10 @@
|
||||
/* 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
|
||||
@ -77,8 +77,8 @@
|
||||
|
||||
#define PIOS_INCLUDE_ADXL345
|
||||
#define PIOS_INCLUDE_FLASH
|
||||
|
||||
#define PIOS_INCLUDE_BMP085
|
||||
#define PIOS_INCLUDE_MPU6000
|
||||
#define PIOS_MPU6000_ACCEL
|
||||
|
||||
/* A really shitty setting saving implementation */
|
||||
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
|
||||
|
||||
@ -41,5 +41,6 @@
|
||||
|
||||
#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 */
|
||||
|
||||
@ -43,6 +43,7 @@
|
||||
#include <manualcontrolsettings.h>
|
||||
#include <gcsreceiver.h>
|
||||
|
||||
|
||||
/* One slot per selectable receiver group.
|
||||
* eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
|
||||
* NOTE: No slot in this map for NONE.
|
||||
@ -66,25 +67,134 @@ 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();
|
||||
@ -95,10 +205,6 @@ void PIOS_Board_Init(void) {
|
||||
PIOS_RTC_Init(&pios_rtc_main_cfg);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
HwSettingsInitialize();
|
||||
|
||||
#ifndef ERASE_FLASH
|
||||
@ -134,35 +240,36 @@ void PIOS_Board_Init(void) {
|
||||
/* Initialize board specific USB data */
|
||||
PIOS_USB_BOARD_DATA_Init();
|
||||
|
||||
|
||||
/* Flags to determine if various USB interfaces are advertised */
|
||||
bool usb_hid_present = false;
|
||||
bool usb_cdc_present = false;
|
||||
|
||||
uint8_t hwsettings_usb_devicetype;
|
||||
HwSettingsUSB_DeviceTypeGet(&hwsettings_usb_devicetype);
|
||||
|
||||
switch (hwsettings_usb_devicetype) {
|
||||
case HWSETTINGS_USB_DEVICETYPE_HIDONLY:
|
||||
if (PIOS_USB_DESC_HID_ONLY_Init()) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
usb_hid_present = true;
|
||||
break;
|
||||
case HWSETTINGS_USB_DEVICETYPE_HIDVCP:
|
||||
if (PIOS_USB_DESC_HID_CDC_Init()) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
usb_hid_present = true;
|
||||
usb_cdc_present = true;
|
||||
break;
|
||||
case HWSETTINGS_USB_DEVICETYPE_VCPONLY:
|
||||
break;
|
||||
default:
|
||||
#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;
|
||||
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
|
||||
|
||||
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)
|
||||
|
||||
@ -575,8 +682,35 @@ void PIOS_Board_Init(void) {
|
||||
#else
|
||||
PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
|
||||
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */
|
||||
|
||||
PIOS_ADC_Init();
|
||||
|
||||
switch(bdinfo->board_rev) {
|
||||
case 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();
|
||||
|
||||
/* Make sure we have at least one telemetry link configured or else fail initialization */
|
||||
|
||||
@ -31,6 +31,7 @@
|
||||
#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),
|
||||
@ -50,40 +51,15 @@ static const uint8_t usb_product_id[28] = {
|
||||
'l', 0,
|
||||
};
|
||||
|
||||
static uint8_t usb_serial_number[52] = {
|
||||
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,
|
||||
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),
|
||||
.bLangID = htousbs(USB_LANGID_ENGLISH_US),
|
||||
};
|
||||
|
||||
static const uint8_t usb_vendor_id[28] = {
|
||||
@ -107,11 +83,13 @@ static const uint8_t usb_vendor_id[28] = {
|
||||
int32_t PIOS_USB_BOARD_DATA_Init(void)
|
||||
{
|
||||
/* Load device serial number into serial number string */
|
||||
uint8_t sn[25];
|
||||
uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
|
||||
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];
|
||||
}
|
||||
|
||||
/* 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));
|
||||
|
||||
@ -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
54
flight/EntireFlash/Makefile
Normal 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 " PADDING $@"
|
||||
$(V1) dd status=noxfer if=/dev/zero count=$(FW_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@
|
||||
|
||||
BL_BIN = $(TOP)/build/bl_$(BOARD_NAME)/bl_$(BOARD_NAME).bin
|
||||
FW_BIN = $(TOP)/build/fw_$(BOARD_NAME)/fw_$(BOARD_NAME).bin
|
||||
$(OUTDIR)/$(TARGET).bin: $(BL_BIN) $(FW_BIN) $(OUTDIR)/$(TARGET).pad
|
||||
$(V0) @echo " FLASH IMG $@"
|
||||
$(V1) cat $(BL_BIN) $(OUTDIR)/$(TARGET).pad $(FW_BIN) > $@
|
||||
|
||||
.PHONY: dfu
|
||||
dfu: $(OUTDIR)/$(TARGET).bin
|
||||
$(V0) @echo " DFU RESCUE $<"
|
||||
$(V1) ( \
|
||||
sudo $(DFU_CMD) -l && \
|
||||
sudo $(DFU_CMD) -d 0483:df11 -c 1 -i 0 -a 0 -D $< -s $(BL_BANK_BASE) ; \
|
||||
)
|
||||
|
||||
@ -1,431 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot INS
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot INS project
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
TOP := $(realpath $(WHEREAMI)/../../)
|
||||
include $(TOP)/make/firmware-defs.mk
|
||||
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET := fw_$(BOARD_NAME)
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR := $(TOP)/build/$(TARGET)
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES for debugging
|
||||
DEBUG ?= YES
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= YES
|
||||
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
REMOVE_CMD = cs-rm
|
||||
else
|
||||
REMOVE_CMD = rm
|
||||
endif
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# Paths
|
||||
INS = ./
|
||||
INSINC = $(INS)/inc
|
||||
PIOS = ../PiOS
|
||||
PIOSINC = $(PIOS)/inc
|
||||
FLIGHTLIB = ../Libraries
|
||||
FLIGHTLIBINC = ../Libraries/inc
|
||||
PIOSSTM32F10X = $(PIOS)/STM32F10x
|
||||
PIOSCOMMON = $(PIOS)/Common
|
||||
PIOSBOARDS = $(PIOS)/Boards
|
||||
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
|
||||
STMLIBDIR = $(APPLIBDIR)
|
||||
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
|
||||
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
|
||||
STMSPDSRCDIR = $(STMSPDDIR)/src
|
||||
STMSPDINCDIR = $(STMSPDDIR)/inc
|
||||
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
|
||||
OPDIR = ../OpenPilot
|
||||
OPUAVOBJ = ../UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPSYSINC = $(OPDIR)/System/inc
|
||||
BOOT = ../Bootloaders/INS
|
||||
BOOTINC = $(BOOT)/inc
|
||||
HWDEFSINC = ../board_hw_defs/$(BOARD_NAME)
|
||||
|
||||
OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
## INS:
|
||||
SRC = ins.c
|
||||
SRC += pios_board.c
|
||||
#SRC += ins_timer.c
|
||||
#SRC += insgps13state.c
|
||||
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
#SRC += $(FLIGHTLIB)/ins_spi_comm.c
|
||||
#SRC += $(FLIGHTLIB)/ins_comm_objects.c
|
||||
#SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
#SRC += $(BOOT)/ins_spi_program_slave.c
|
||||
#SRC += $(BOOT)/ins_slave_test.c
|
||||
#SRC += $(BOOT)/ins_spi_program.c
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
SRC += $(PIOSSTM32F10X)/pios_sys.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_led.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_delay.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_i2c.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spi.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_exti.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
SRC += $(PIOSCOMMON)/pios_bma180.c
|
||||
SRC += $(PIOSCOMMON)/pios_hmc5883.c
|
||||
SRC += $(PIOSCOMMON)/pios_bmp085.c
|
||||
SRC += $(PIOSCOMMON)/pios_imu3000.c
|
||||
|
||||
## CMSIS for STM32
|
||||
SRC += $(CMSISDIR)/core_cm3.c
|
||||
SRC += $(CMSISDIR)/system_stm32f10x.c
|
||||
|
||||
## Used parts of the STM-Library
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
|
||||
SRC += $(STMSPDSRCDIR)/misc.c
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
# will not be considered source files but generated files (assembler
|
||||
# output from the compiler), and will be deleted upon "make clean"!
|
||||
# Even though the DOS/Win* filesystem matches both .s and .S the same,
|
||||
# it will preserve the spelling of the filenames, and gcc itself does
|
||||
# care about how the name is spelled on its command-line.
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
|
||||
|
||||
# List Assembler source files here which must be assembled in ARM-Mode..
|
||||
ASRCARM =
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PIOSSTM32F10X)
|
||||
EXTRAINCDIRS += $(PIOSCOMMON)
|
||||
EXTRAINCDIRS += $(PIOSBOARDS)
|
||||
EXTRAINCDIRS += $(STMSPDINCDIR)
|
||||
EXTRAINCDIRS += $(CMSISDIR)
|
||||
EXTRAINCDIRS += $(INSINC)
|
||||
EXTRAINCDIRS += $(OPUAVSYNTHDIR)
|
||||
EXTRAINCDIRS += $(BOOTINC)
|
||||
EXTRAINCDIRS += $(HWDEFSINC)
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS += -Os
|
||||
CFLAGS += -DGENERAL_COV
|
||||
else
|
||||
CFLAGS += -Os
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
CDEFS += -DUSE_$(BOARD)
|
||||
CDEFS += -DIN_INS
|
||||
|
||||
# Place project-specific -D and/or -U options for
|
||||
# Assembler with preprocessor here.
|
||||
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
|
||||
ADEFS = -D__ASSEMBLY__
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS += -g$(DEBUGF) #-DDEBUG
|
||||
else
|
||||
CFLAGS += -g$(DEBUGF)
|
||||
endif
|
||||
|
||||
|
||||
CFLAGS += -ffast-math
|
||||
|
||||
CFLAGS += -mcpu=$(MCU)
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
#CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
|
||||
|
||||
# Define programs and commands.
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
# Default target.
|
||||
all: gccversion build
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
|
||||
|
||||
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino opfw
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
bino: $(OUTDIR)/$(TARGET).bin.o
|
||||
opfw: $(OUTDIR)/$(TARGET).opfw
|
||||
|
||||
# Display sizes of sections.
|
||||
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Install: install binary file with prefix/suffix into install directory
|
||||
install: $(OUTDIR)/$(TARGET).opfw
|
||||
ifneq ($(INSTALL_DIR),)
|
||||
@echo $(MSG_INSTALLING) $(call toprel, $<)
|
||||
$(V1) mkdir -p $(INSTALL_DIR)
|
||||
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
|
||||
else
|
||||
$(error INSTALL_DIR must be specified for $@)
|
||||
endif
|
||||
|
||||
# Target: clean project.
|
||||
clean: clean_list
|
||||
|
||||
clean_list :
|
||||
@echo $(MSG_CLEANING)
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(V1) $(REMOVE) $(ALLOBJ)
|
||||
$(V1) $(REMOVE) $(LSTFILES)
|
||||
$(V1) $(REMOVE) $(DEPFILES)
|
||||
$(V1) $(REMOVE) $(SRC:.c=.s)
|
||||
$(V1) $(REMOVE) $(SRCARM:.c=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
|
||||
else
|
||||
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all build clean clean_list install
|
||||
@ -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 */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -1,62 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup INS INS
|
||||
|
||||
* @brief The INS configuration
|
||||
*
|
||||
* @{
|
||||
* @addtogroup INS_Main
|
||||
* @brief INS configuration
|
||||
* @{
|
||||
*
|
||||
* @file pios_config.h
|
||||
* @author David "Buzz" Carlson (buzz@chebuzz.com)
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief PiOS configuration header.
|
||||
* - Central compile time config for the project.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef PIOS_CONFIG_H
|
||||
#define PIOS_CONFIG_H
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_SPI
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_USART
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_GPS
|
||||
#define PIOS_INCLUDE_HMC5883
|
||||
#define PIOS_INCLUDE_BMP085
|
||||
#define PIOS_INCLUDE_IMU3000
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_EXTI
|
||||
#define PIOS_INCLUDE_IAP
|
||||
|
||||
#define PIOS_INCLUDE_BMA180
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
218
flight/INS/ins.c
218
flight/INS/ins.c
@ -1,218 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup INS INS
|
||||
|
||||
* @brief The INS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup INS_Main
|
||||
* @brief Main function which does the hardware dependent stuff
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ins.c
|
||||
* @author David "Buzz" Carlson (buzz@chebuzz.com)
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief INSGPS Test Program
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "ins.h"
|
||||
#include "pios.h"
|
||||
#include <stdbool.h>
|
||||
#include "fifo_buffer.h"
|
||||
|
||||
void reset_values();
|
||||
|
||||
/**
|
||||
* @addtogroup INS_Global_Data INS Global Data
|
||||
* @{
|
||||
* Public data. Used by both EKF and the sender
|
||||
*/
|
||||
|
||||
//! Contains the data from the mag sensor chip
|
||||
struct mag_sensor mag_data;
|
||||
|
||||
//! Contains the data from the accelerometer
|
||||
struct accel_sensor accel_data;
|
||||
|
||||
//! Contains the data from the gyro
|
||||
struct gyro_sensor gyro_data;
|
||||
|
||||
//! Conains the current estimate of the attitude
|
||||
struct attitude_solution attitude_data;
|
||||
|
||||
//! Contains data from the altitude sensor
|
||||
struct altitude_sensor altitude_data;
|
||||
|
||||
//! Contains data from the GPS (via the SPI link)
|
||||
struct gps_sensor gps_data;
|
||||
|
||||
//! Offset correction of barometric alt, to match gps data
|
||||
//static float baro_offset = 0;
|
||||
|
||||
//static float mag_len = 0;
|
||||
|
||||
typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states;
|
||||
volatile int32_t ekf_too_slow;
|
||||
volatile int32_t total_conversion_blocks;
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* INS functions */
|
||||
void blink(int led, int times)
|
||||
{
|
||||
for(int i=0; i<times; i++)
|
||||
{
|
||||
PIOS_LED_Toggle(led);
|
||||
PIOS_DELAY_WaitmS(250);
|
||||
PIOS_LED_Toggle(led);
|
||||
PIOS_DELAY_WaitmS(250);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void test_accel()
|
||||
{
|
||||
if(PIOS_BMA180_Test())
|
||||
blink(PIOS_LED_HEARTBEAT, 1);
|
||||
else
|
||||
blink(PIOS_LED_ALARM, 1);
|
||||
}
|
||||
|
||||
#if defined (PIOS_INCLUDE_HMC5883)
|
||||
void test_mag()
|
||||
{
|
||||
if(PIOS_HMC5883_Test())
|
||||
blink(PIOS_LED_HEARTBEAT, 2);
|
||||
else
|
||||
blink(PIOS_LED_ALARM, 2);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_BMP085)
|
||||
void test_pressure()
|
||||
{
|
||||
if(PIOS_BMP085_Test())
|
||||
blink(PIOS_LED_HEARTBEAT, 3);
|
||||
else
|
||||
blink(PIOS_LED_ALARM, 3);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_IMU3000)
|
||||
void test_imu()
|
||||
{
|
||||
if(PIOS_IMU3000_Test())
|
||||
blink(PIOS_LED_HEARTBEAT, 4);
|
||||
else
|
||||
blink(PIOS_LED_ALARM, 4);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
extern void PIOS_Board_Init(void);
|
||||
struct pios_bma180_data bma180_data;
|
||||
|
||||
/**
|
||||
* @brief INS Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
reset_values();
|
||||
|
||||
PIOS_Board_Init();
|
||||
|
||||
while (1)
|
||||
{
|
||||
test_accel();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
|
||||
#if defined (PIOS_INCLUDE_HMC5883)
|
||||
test_mag();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_BMP085)
|
||||
test_pressure();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_IMU3000)
|
||||
test_imu();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
PIOS_DELAY_WaitmS(3000);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Populate fields with initial values
|
||||
*/
|
||||
void reset_values()
|
||||
{
|
||||
accel_data.calibration.scale[0][1] = 0;
|
||||
accel_data.calibration.scale[1][0] = 0;
|
||||
accel_data.calibration.scale[0][2] = 0;
|
||||
accel_data.calibration.scale[2][0] = 0;
|
||||
accel_data.calibration.scale[1][2] = 0;
|
||||
accel_data.calibration.scale[2][1] = 0;
|
||||
|
||||
accel_data.calibration.scale[0][0] = 0.0359;
|
||||
accel_data.calibration.scale[1][1] = 0.0359;
|
||||
accel_data.calibration.scale[2][2] = 0.0359;
|
||||
accel_data.calibration.scale[0][3] = -73.5;
|
||||
accel_data.calibration.scale[1][3] = -73.5;
|
||||
accel_data.calibration.scale[2][3] = -73.5;
|
||||
|
||||
accel_data.calibration.variance[0] = 1e-4;
|
||||
accel_data.calibration.variance[1] = 1e-4;
|
||||
accel_data.calibration.variance[2] = 1e-4;
|
||||
|
||||
gyro_data.calibration.scale[0] = -0.014;
|
||||
gyro_data.calibration.scale[1] = 0.014;
|
||||
gyro_data.calibration.scale[2] = -0.014;
|
||||
gyro_data.calibration.bias[0] = -24;
|
||||
gyro_data.calibration.bias[1] = -24;
|
||||
gyro_data.calibration.bias[2] = -24;
|
||||
gyro_data.calibration.variance[0] = 1;
|
||||
gyro_data.calibration.variance[1] = 1;
|
||||
gyro_data.calibration.variance[2] = 1;
|
||||
mag_data.calibration.scale[0] = 1;
|
||||
mag_data.calibration.scale[1] = 1;
|
||||
mag_data.calibration.scale[2] = 1;
|
||||
mag_data.calibration.bias[0] = 0;
|
||||
mag_data.calibration.bias[1] = 0;
|
||||
mag_data.calibration.bias[2] = 0;
|
||||
mag_data.calibration.variance[0] = 50;
|
||||
mag_data.calibration.variance[1] = 50;
|
||||
mag_data.calibration.variance[2] = 50;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
@ -1,130 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_HMC5883 HMC5883 Functions
|
||||
* @brief Deals with the hardware interface to the magnetometers
|
||||
* @{
|
||||
*
|
||||
* @file pios_board.c
|
||||
* @author David "Buzz" Carlson (buzz@chebuzz.com)
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief Defines board specific static initializers for hardware for the INS board.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* Pull in the board-specific static HW definitions.
|
||||
* Including .c files is a bit ugly but this allows all of
|
||||
* the HW definitions to be const and static to limit their
|
||||
* scope.
|
||||
*
|
||||
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
|
||||
*/
|
||||
#include "board_hw_defs.c"
|
||||
|
||||
#include <pios.h>
|
||||
|
||||
#if 0
|
||||
#define PIOS_COM_AUX_TX_BUF_LEN 192
|
||||
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
|
||||
#endif
|
||||
|
||||
#define PIOS_COM_GPS_RX_BUF_LEN 96
|
||||
static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN];
|
||||
|
||||
uint32_t pios_com_aux_id;
|
||||
uint32_t pios_com_gps_id;
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
void PIOS_Board_Init(void) {
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* IAP System Setup */
|
||||
PIOS_IAP_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
#if defined(PIOS_INCLUDE_GPS)
|
||||
uint32_t pios_usart_gps_id;
|
||||
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
|
||||
pios_com_gps_rx_buffer, sizeof(pios_com_gps_rx_buffer),
|
||||
NULL, 0)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_GPS */
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
|
||||
#if defined (PIOS_INCLUDE_I2C)
|
||||
if (PIOS_I2C_Init(&pios_i2c_pres_mag_adapter_id, &pios_i2c_pres_mag_adapter_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#if defined (PIOS_INCLUDE_BMP085)
|
||||
PIOS_BMP085_Init();
|
||||
#endif /* PIOS_INCLUDE_BMP085 */
|
||||
#if defined (PIOS_INCLUDE_HMC5883)
|
||||
PIOS_HMC5883_Init();
|
||||
#endif /* PIOS_INCLUDE_HMC5883 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_IMU3000)
|
||||
if (PIOS_I2C_Init(&pios_i2c_gyro_adapter_id, &pios_i2c_gyro_adapter_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
PIOS_IMU3000_Init();
|
||||
#endif /* PIOS_INCLUDE_IMU3000 */
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
/* Set up the SPI interface to the accelerometer*/
|
||||
if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
PIOS_BMA180_Attach(pios_spi_accel_id);
|
||||
|
||||
// #include "ahrs_spi_comm.h"
|
||||
// InsInitComms();
|
||||
//
|
||||
// /* Set up the SPI interface to the OP board */
|
||||
// if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
|
||||
// PIOS_DEBUG_Assert(0);
|
||||
// }
|
||||
//
|
||||
// InsConnect(pios_spi_op_id);
|
||||
#endif /* PIOS_INCLUDE_SPI */
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
@ -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]));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
// *****************************************************************************
|
||||
|
||||
@ -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 ****
|
||||
|
||||
@ -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 */
|
||||
@ -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.
|
||||
*/
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
121
flight/Libraries/inc/packet_handler.h
Normal file
121
flight/Libraries/inc/packet_handler.h
Normal 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__
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -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
0
flight/AHRS/insgps16state.c → flight/Libraries/insgps16state.c
Normal file → Executable file
328
flight/Libraries/insgps_helper.c
Normal file
328
flight/Libraries/insgps_helper.c
Normal 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
507
flight/Libraries/packet_handler.c
Normal 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
10
flight/Libraries/rscode/LICENSE
Normal 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
50
flight/Libraries/rscode/Makefile
Normal 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
27
flight/Libraries/rscode/README
Normal 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
324
flight/Libraries/rscode/berlekamp.c
Normal 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
18
flight/Libraries/rscode/config.doc
Normal 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
66
flight/Libraries/rscode/crcgen.c
Normal 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
98
flight/Libraries/rscode/ecc.h
Normal 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
128
flight/Libraries/rscode/example.c
Normal 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);
|
||||
}
|
||||
|
||||
164
flight/Libraries/rscode/galois.c
Normal file
164
flight/Libraries/rscode/galois.c
Normal file
@ -0,0 +1,164 @@
|
||||
/*****************************
|
||||
* 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
|
||||
*
|
||||
*
|
||||
* Multiplication and Arithmetic on Galois Field GF(256)
|
||||
*
|
||||
* From Mee, Daniel, "Magnetic Recording, Volume III", Ch. 5 by Patel.
|
||||
*
|
||||
*
|
||||
******************************/
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include "ecc.h"
|
||||
|
||||
/* This is one of 14 irreducible polynomials
|
||||
* of degree 8 and cycle length 255. (Ch 5, pp. 275, Magnetic Recording)
|
||||
* The high order 1 bit is implicit */
|
||||
/* x^8 + x^4 + x^3 + x^2 + 1 */
|
||||
#define PPOLY 0x1D
|
||||
|
||||
|
||||
const int gexp[512] = {
|
||||
1, 2, 4, 8, 16, 32, 64, 128, 29, 58, 116, 232, 205, 135, 19, 38,
|
||||
76, 152, 45, 90, 180, 117, 234, 201, 143, 3, 6, 12, 24, 48, 96, 192,
|
||||
157, 39, 78, 156, 37, 74, 148, 53, 106, 212, 181, 119, 238, 193, 159, 35,
|
||||
70, 140, 5, 10, 20, 40, 80, 160, 93, 186, 105, 210, 185, 111, 222, 161,
|
||||
95, 190, 97, 194, 153, 47, 94, 188, 101, 202, 137, 15, 30, 60, 120, 240,
|
||||
253, 231, 211, 187, 107, 214, 177, 127, 254, 225, 223, 163, 91, 182, 113, 226,
|
||||
217, 175, 67, 134, 17, 34, 68, 136, 13, 26, 52, 104, 208, 189, 103, 206,
|
||||
129, 31, 62, 124, 248, 237, 199, 147, 59, 118, 236, 197, 151, 51, 102, 204,
|
||||
133, 23, 46, 92, 184, 109, 218, 169, 79, 158, 33, 66, 132, 21, 42, 84,
|
||||
168, 77, 154, 41, 82, 164, 85, 170, 73, 146, 57, 114, 228, 213, 183, 115,
|
||||
230, 209, 191, 99, 198, 145, 63, 126, 252, 229, 215, 179, 123, 246, 241, 255,
|
||||
227, 219, 171, 75, 150, 49, 98, 196, 149, 55, 110, 220, 165, 87, 174, 65,
|
||||
130, 25, 50, 100, 200, 141, 7, 14, 28, 56, 112, 224, 221, 167, 83, 166,
|
||||
81, 162, 89, 178, 121, 242, 249, 239, 195, 155, 43, 86, 172, 69, 138, 9,
|
||||
18, 36, 72, 144, 61, 122, 244, 245, 247, 243, 251, 235, 203, 139, 11, 22,
|
||||
44, 88, 176, 125, 250, 233, 207, 131, 27, 54, 108, 216, 173, 71, 142, 1,
|
||||
2, 4, 8, 16, 32, 64, 128, 29, 58, 116, 232, 205, 135, 19, 38, 76,
|
||||
152, 45, 90, 180, 117, 234, 201, 143, 3, 6, 12, 24, 48, 96, 192, 157,
|
||||
39, 78, 156, 37, 74, 148, 53, 106, 212, 181, 119, 238, 193, 159, 35, 70,
|
||||
140, 5, 10, 20, 40, 80, 160, 93, 186, 105, 210, 185, 111, 222, 161, 95,
|
||||
190, 97, 194, 153, 47, 94, 188, 101, 202, 137, 15, 30, 60, 120, 240, 253,
|
||||
231, 211, 187, 107, 214, 177, 127, 254, 225, 223, 163, 91, 182, 113, 226, 217,
|
||||
175, 67, 134, 17, 34, 68, 136, 13, 26, 52, 104, 208, 189, 103, 206, 129,
|
||||
31, 62, 124, 248, 237, 199, 147, 59, 118, 236, 197, 151, 51, 102, 204, 133,
|
||||
23, 46, 92, 184, 109, 218, 169, 79, 158, 33, 66, 132, 21, 42, 84, 168,
|
||||
77, 154, 41, 82, 164, 85, 170, 73, 146, 57, 114, 228, 213, 183, 115, 230,
|
||||
209, 191, 99, 198, 145, 63, 126, 252, 229, 215, 179, 123, 246, 241, 255, 227,
|
||||
219, 171, 75, 150, 49, 98, 196, 149, 55, 110, 220, 165, 87, 174, 65, 130,
|
||||
25, 50, 100, 200, 141, 7, 14, 28, 56, 112, 224, 221, 167, 83, 166, 81,
|
||||
162, 89, 178, 121, 242, 249, 239, 195, 155, 43, 86, 172, 69, 138, 9, 18,
|
||||
36, 72, 144, 61, 122, 244, 245, 247, 243, 251, 235, 203, 139, 11, 22, 44,
|
||||
88, 176, 125, 250, 233, 207, 131, 27, 54, 108, 216, 173, 71, 142, 1, 0,
|
||||
};
|
||||
const int glog[256] = {
|
||||
0, 0, 1, 25, 2, 50, 26, 198, 3, 223, 51, 238, 27, 104, 199, 75,
|
||||
4, 100, 224, 14, 52, 141, 239, 129, 28, 193, 105, 248, 200, 8, 76, 113,
|
||||
5, 138, 101, 47, 225, 36, 15, 33, 53, 147, 142, 218, 240, 18, 130, 69,
|
||||
29, 181, 194, 125, 106, 39, 249, 185, 201, 154, 9, 120, 77, 228, 114, 166,
|
||||
6, 191, 139, 98, 102, 221, 48, 253, 226, 152, 37, 179, 16, 145, 34, 136,
|
||||
54, 208, 148, 206, 143, 150, 219, 189, 241, 210, 19, 92, 131, 56, 70, 64,
|
||||
30, 66, 182, 163, 195, 72, 126, 110, 107, 58, 40, 84, 250, 133, 186, 61,
|
||||
202, 94, 155, 159, 10, 21, 121, 43, 78, 212, 229, 172, 115, 243, 167, 87,
|
||||
7, 112, 192, 247, 140, 128, 99, 13, 103, 74, 222, 237, 49, 197, 254, 24,
|
||||
227, 165, 153, 119, 38, 184, 180, 124, 17, 68, 146, 217, 35, 32, 137, 46,
|
||||
55, 63, 209, 91, 149, 188, 207, 205, 144, 135, 151, 178, 220, 252, 190, 97,
|
||||
242, 86, 211, 171, 20, 42, 93, 158, 132, 60, 57, 83, 71, 109, 65, 162,
|
||||
31, 45, 67, 216, 183, 123, 164, 118, 196, 23, 73, 236, 127, 12, 111, 246,
|
||||
108, 161, 59, 82, 41, 157, 85, 170, 251, 96, 134, 177, 187, 204, 62, 90,
|
||||
203, 89, 95, 176, 156, 169, 160, 81, 11, 245, 22, 235, 122, 117, 44, 215,
|
||||
79, 174, 213, 233, 230, 231, 173, 232, 116, 214, 244, 234, 168, 80, 88, 175,
|
||||
};
|
||||
|
||||
|
||||
//static void init_exp_table (void);
|
||||
|
||||
|
||||
void
|
||||
init_galois_tables (void)
|
||||
{
|
||||
/* initialize the table of powers of alpha */
|
||||
//init_exp_table();
|
||||
}
|
||||
|
||||
|
||||
#ifdef NEVER
|
||||
static void
|
||||
init_exp_table (void)
|
||||
{
|
||||
int i, z;
|
||||
int pinit,p1,p2,p3,p4,p5,p6,p7,p8;
|
||||
|
||||
pinit = p2 = p3 = p4 = p5 = p6 = p7 = p8 = 0;
|
||||
p1 = 1;
|
||||
|
||||
gexp[0] = 1;
|
||||
gexp[255] = gexp[0];
|
||||
glog[0] = 0; /* shouldn't log[0] be an error? */
|
||||
|
||||
for (i = 1; i < 256; i++) {
|
||||
pinit = p8;
|
||||
p8 = p7;
|
||||
p7 = p6;
|
||||
p6 = p5;
|
||||
p5 = p4 ^ pinit;
|
||||
p4 = p3 ^ pinit;
|
||||
p3 = p2 ^ pinit;
|
||||
p2 = p1;
|
||||
p1 = pinit;
|
||||
gexp[i] = p1 + p2*2 + p3*4 + p4*8 + p5*16 + p6*32 + p7*64 + p8*128;
|
||||
gexp[i+255] = gexp[i];
|
||||
}
|
||||
|
||||
for (i = 1; i < 256; i++) {
|
||||
for (z = 0; z < 256; z++) {
|
||||
if (gexp[z] == i) {
|
||||
glog[i] = z;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* multiplication using logarithms */
|
||||
int gmult(int a, int b)
|
||||
{
|
||||
int i,j;
|
||||
if (a==0 || b == 0) return (0);
|
||||
i = glog[a];
|
||||
j = glog[b];
|
||||
return (gexp[i+j]);
|
||||
}
|
||||
|
||||
|
||||
int ginv (int elt)
|
||||
{
|
||||
return (gexp[255-glog[elt]]);
|
||||
}
|
||||
|
||||
674
flight/Libraries/rscode/gpl.txt
Normal file
674
flight/Libraries/rscode/gpl.txt
Normal file
@ -0,0 +1,674 @@
|
||||
GNU GENERAL PUBLIC LICENSE
|
||||
Version 3, 29 June 2007
|
||||
|
||||
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
|
||||
Everyone is permitted to copy and distribute verbatim copies
|
||||
of this license document, but changing it is not allowed.
|
||||
|
||||
Preamble
|
||||
|
||||
The GNU General Public License is a free, copyleft license for
|
||||
software and other kinds of works.
|
||||
|
||||
The licenses for most software and other practical works are designed
|
||||
to take away your freedom to share and change the works. By contrast,
|
||||
the GNU General Public License is intended to guarantee your freedom to
|
||||
share and change all versions of a program--to make sure it remains free
|
||||
software for all its users. We, the Free Software Foundation, use the
|
||||
GNU General Public License for most of our software; it applies also to
|
||||
any other work released this way by its authors. You can apply it to
|
||||
your programs, too.
|
||||
|
||||
When we speak of free software, we are referring to freedom, not
|
||||
price. Our General Public Licenses are designed to make sure that you
|
||||
have the freedom to distribute copies of free software (and charge for
|
||||
them if you wish), that you receive source code or can get it if you
|
||||
want it, that you can change the software or use pieces of it in new
|
||||
free programs, and that you know you can do these things.
|
||||
|
||||
To protect your rights, we need to prevent others from denying you
|
||||
these rights or asking you to surrender the rights. Therefore, you have
|
||||
certain responsibilities if you distribute copies of the software, or if
|
||||
you modify it: responsibilities to respect the freedom of others.
|
||||
|
||||
For example, if you distribute copies of such a program, whether
|
||||
gratis or for a fee, you must pass on to the recipients the same
|
||||
freedoms that you received. You must make sure that they, too, receive
|
||||
or can get the source code. And you must show them these terms so they
|
||||
know their rights.
|
||||
|
||||
Developers that use the GNU GPL protect your rights with two steps:
|
||||
(1) assert copyright on the software, and (2) offer you this License
|
||||
giving you legal permission to copy, distribute and/or modify it.
|
||||
|
||||
For the developers' and authors' protection, the GPL clearly explains
|
||||
that there is no warranty for this free software. For both users' and
|
||||
authors' sake, the GPL requires that modified versions be marked as
|
||||
changed, so that their problems will not be attributed erroneously to
|
||||
authors of previous versions.
|
||||
|
||||
Some devices are designed to deny users access to install or run
|
||||
modified versions of the software inside them, although the manufacturer
|
||||
can do so. This is fundamentally incompatible with the aim of
|
||||
protecting users' freedom to change the software. The systematic
|
||||
pattern of such abuse occurs in the area of products for individuals to
|
||||
use, which is precisely where it is most unacceptable. Therefore, we
|
||||
have designed this version of the GPL to prohibit the practice for those
|
||||
products. If such problems arise substantially in other domains, we
|
||||
stand ready to extend this provision to those domains in future versions
|
||||
of the GPL, as needed to protect the freedom of users.
|
||||
|
||||
Finally, every program is threatened constantly by software patents.
|
||||
States should not allow patents to restrict development and use of
|
||||
software on general-purpose computers, but in those that do, we wish to
|
||||
avoid the special danger that patents applied to a free program could
|
||||
make it effectively proprietary. To prevent this, the GPL assures that
|
||||
patents cannot be used to render the program non-free.
|
||||
|
||||
The precise terms and conditions for copying, distribution and
|
||||
modification follow.
|
||||
|
||||
TERMS AND CONDITIONS
|
||||
|
||||
0. Definitions.
|
||||
|
||||
"This License" refers to version 3 of the GNU General Public License.
|
||||
|
||||
"Copyright" also means copyright-like laws that apply to other kinds of
|
||||
works, such as semiconductor masks.
|
||||
|
||||
"The Program" refers to any copyrightable work licensed under this
|
||||
License. Each licensee is addressed as "you". "Licensees" and
|
||||
"recipients" may be individuals or organizations.
|
||||
|
||||
To "modify" a work means to copy from or adapt all or part of the work
|
||||
in a fashion requiring copyright permission, other than the making of an
|
||||
exact copy. The resulting work is called a "modified version" of the
|
||||
earlier work or a work "based on" the earlier work.
|
||||
|
||||
A "covered work" means either the unmodified Program or a work based
|
||||
on the Program.
|
||||
|
||||
To "propagate" a work means to do anything with it that, without
|
||||
permission, would make you directly or secondarily liable for
|
||||
infringement under applicable copyright law, except executing it on a
|
||||
computer or modifying a private copy. Propagation includes copying,
|
||||
distribution (with or without modification), making available to the
|
||||
public, and in some countries other activities as well.
|
||||
|
||||
To "convey" a work means any kind of propagation that enables other
|
||||
parties to make or receive copies. Mere interaction with a user through
|
||||
a computer network, with no transfer of a copy, is not conveying.
|
||||
|
||||
An interactive user interface displays "Appropriate Legal Notices"
|
||||
to the extent that it includes a convenient and prominently visible
|
||||
feature that (1) displays an appropriate copyright notice, and (2)
|
||||
tells the user that there is no warranty for the work (except to the
|
||||
extent that warranties are provided), that licensees may convey the
|
||||
work under this License, and how to view a copy of this License. If
|
||||
the interface presents a list of user commands or options, such as a
|
||||
menu, a prominent item in the list meets this criterion.
|
||||
|
||||
1. Source Code.
|
||||
|
||||
The "source code" for a work means the preferred form of the work
|
||||
for making modifications to it. "Object code" means any non-source
|
||||
form of a work.
|
||||
|
||||
A "Standard Interface" means an interface that either is an official
|
||||
standard defined by a recognized standards body, or, in the case of
|
||||
interfaces specified for a particular programming language, one that
|
||||
is widely used among developers working in that language.
|
||||
|
||||
The "System Libraries" of an executable work include anything, other
|
||||
than the work as a whole, that (a) is included in the normal form of
|
||||
packaging a Major Component, but which is not part of that Major
|
||||
Component, and (b) serves only to enable use of the work with that
|
||||
Major Component, or to implement a Standard Interface for which an
|
||||
implementation is available to the public in source code form. A
|
||||
"Major Component", in this context, means a major essential component
|
||||
(kernel, window system, and so on) of the specific operating system
|
||||
(if any) on which the executable work runs, or a compiler used to
|
||||
produce the work, or an object code interpreter used to run it.
|
||||
|
||||
The "Corresponding Source" for a work in object code form means all
|
||||
the source code needed to generate, install, and (for an executable
|
||||
work) run the object code and to modify the work, including scripts to
|
||||
control those activities. However, it does not include the work's
|
||||
System Libraries, or general-purpose tools or generally available free
|
||||
programs which are used unmodified in performing those activities but
|
||||
which are not part of the work. For example, Corresponding Source
|
||||
includes interface definition files associated with source files for
|
||||
the work, and the source code for shared libraries and dynamically
|
||||
linked subprograms that the work is specifically designed to require,
|
||||
such as by intimate data communication or control flow between those
|
||||
subprograms and other parts of the work.
|
||||
|
||||
The Corresponding Source need not include anything that users
|
||||
can regenerate automatically from other parts of the Corresponding
|
||||
Source.
|
||||
|
||||
The Corresponding Source for a work in source code form is that
|
||||
same work.
|
||||
|
||||
2. Basic Permissions.
|
||||
|
||||
All rights granted under this License are granted for the term of
|
||||
copyright on the Program, and are irrevocable provided the stated
|
||||
conditions are met. This License explicitly affirms your unlimited
|
||||
permission to run the unmodified Program. The output from running a
|
||||
covered work is covered by this License only if the output, given its
|
||||
content, constitutes a covered work. This License acknowledges your
|
||||
rights of fair use or other equivalent, as provided by copyright law.
|
||||
|
||||
You may make, run and propagate covered works that you do not
|
||||
convey, without conditions so long as your license otherwise remains
|
||||
in force. You may convey covered works to others for the sole purpose
|
||||
of having them make modifications exclusively for you, or provide you
|
||||
with facilities for running those works, provided that you comply with
|
||||
the terms of this License in conveying all material for which you do
|
||||
not control copyright. Those thus making or running the covered works
|
||||
for you must do so exclusively on your behalf, under your direction
|
||||
and control, on terms that prohibit them from making any copies of
|
||||
your copyrighted material outside their relationship with you.
|
||||
|
||||
Conveying under any other circumstances is permitted solely under
|
||||
the conditions stated below. Sublicensing is not allowed; section 10
|
||||
makes it unnecessary.
|
||||
|
||||
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
|
||||
|
||||
No covered work shall be deemed part of an effective technological
|
||||
measure under any applicable law fulfilling obligations under article
|
||||
11 of the WIPO copyright treaty adopted on 20 December 1996, or
|
||||
similar laws prohibiting or restricting circumvention of such
|
||||
measures.
|
||||
|
||||
When you convey a covered work, you waive any legal power to forbid
|
||||
circumvention of technological measures to the extent such circumvention
|
||||
is effected by exercising rights under this License with respect to
|
||||
the covered work, and you disclaim any intention to limit operation or
|
||||
modification of the work as a means of enforcing, against the work's
|
||||
users, your or third parties' legal rights to forbid circumvention of
|
||||
technological measures.
|
||||
|
||||
4. Conveying Verbatim Copies.
|
||||
|
||||
You may convey verbatim copies of the Program's source code as you
|
||||
receive it, in any medium, provided that you conspicuously and
|
||||
appropriately publish on each copy an appropriate copyright notice;
|
||||
keep intact all notices stating that this License and any
|
||||
non-permissive terms added in accord with section 7 apply to the code;
|
||||
keep intact all notices of the absence of any warranty; and give all
|
||||
recipients a copy of this License along with the Program.
|
||||
|
||||
You may charge any price or no price for each copy that you convey,
|
||||
and you may offer support or warranty protection for a fee.
|
||||
|
||||
5. Conveying Modified Source Versions.
|
||||
|
||||
You may convey a work based on the Program, or the modifications to
|
||||
produce it from the Program, in the form of source code under the
|
||||
terms of section 4, provided that you also meet all of these conditions:
|
||||
|
||||
a) The work must carry prominent notices stating that you modified
|
||||
it, and giving a relevant date.
|
||||
|
||||
b) The work must carry prominent notices stating that it is
|
||||
released under this License and any conditions added under section
|
||||
7. This requirement modifies the requirement in section 4 to
|
||||
"keep intact all notices".
|
||||
|
||||
c) You must license the entire work, as a whole, under this
|
||||
License to anyone who comes into possession of a copy. This
|
||||
License will therefore apply, along with any applicable section 7
|
||||
additional terms, to the whole of the work, and all its parts,
|
||||
regardless of how they are packaged. This License gives no
|
||||
permission to license the work in any other way, but it does not
|
||||
invalidate such permission if you have separately received it.
|
||||
|
||||
d) If the work has interactive user interfaces, each must display
|
||||
Appropriate Legal Notices; however, if the Program has interactive
|
||||
interfaces that do not display Appropriate Legal Notices, your
|
||||
work need not make them do so.
|
||||
|
||||
A compilation of a covered work with other separate and independent
|
||||
works, which are not by their nature extensions of the covered work,
|
||||
and which are not combined with it such as to form a larger program,
|
||||
in or on a volume of a storage or distribution medium, is called an
|
||||
"aggregate" if the compilation and its resulting copyright are not
|
||||
used to limit the access or legal rights of the compilation's users
|
||||
beyond what the individual works permit. Inclusion of a covered work
|
||||
in an aggregate does not cause this License to apply to the other
|
||||
parts of the aggregate.
|
||||
|
||||
6. Conveying Non-Source Forms.
|
||||
|
||||
You may convey a covered work in object code form under the terms
|
||||
of sections 4 and 5, provided that you also convey the
|
||||
machine-readable Corresponding Source under the terms of this License,
|
||||
in one of these ways:
|
||||
|
||||
a) Convey the object code in, or embodied in, a physical product
|
||||
(including a physical distribution medium), accompanied by the
|
||||
Corresponding Source fixed on a durable physical medium
|
||||
customarily used for software interchange.
|
||||
|
||||
b) Convey the object code in, or embodied in, a physical product
|
||||
(including a physical distribution medium), accompanied by a
|
||||
written offer, valid for at least three years and valid for as
|
||||
long as you offer spare parts or customer support for that product
|
||||
model, to give anyone who possesses the object code either (1) a
|
||||
copy of the Corresponding Source for all the software in the
|
||||
product that is covered by this License, on a durable physical
|
||||
medium customarily used for software interchange, for a price no
|
||||
more than your reasonable cost of physically performing this
|
||||
conveying of source, or (2) access to copy the
|
||||
Corresponding Source from a network server at no charge.
|
||||
|
||||
c) Convey individual copies of the object code with a copy of the
|
||||
written offer to provide the Corresponding Source. This
|
||||
alternative is allowed only occasionally and noncommercially, and
|
||||
only if you received the object code with such an offer, in accord
|
||||
with subsection 6b.
|
||||
|
||||
d) Convey the object code by offering access from a designated
|
||||
place (gratis or for a charge), and offer equivalent access to the
|
||||
Corresponding Source in the same way through the same place at no
|
||||
further charge. You need not require recipients to copy the
|
||||
Corresponding Source along with the object code. If the place to
|
||||
copy the object code is a network server, the Corresponding Source
|
||||
may be on a different server (operated by you or a third party)
|
||||
that supports equivalent copying facilities, provided you maintain
|
||||
clear directions next to the object code saying where to find the
|
||||
Corresponding Source. Regardless of what server hosts the
|
||||
Corresponding Source, you remain obligated to ensure that it is
|
||||
available for as long as needed to satisfy these requirements.
|
||||
|
||||
e) Convey the object code using peer-to-peer transmission, provided
|
||||
you inform other peers where the object code and Corresponding
|
||||
Source of the work are being offered to the general public at no
|
||||
charge under subsection 6d.
|
||||
|
||||
A separable portion of the object code, whose source code is excluded
|
||||
from the Corresponding Source as a System Library, need not be
|
||||
included in conveying the object code work.
|
||||
|
||||
A "User Product" is either (1) a "consumer product", which means any
|
||||
tangible personal property which is normally used for personal, family,
|
||||
or household purposes, or (2) anything designed or sold for incorporation
|
||||
into a dwelling. In determining whether a product is a consumer product,
|
||||
doubtful cases shall be resolved in favor of coverage. For a particular
|
||||
product received by a particular user, "normally used" refers to a
|
||||
typical or common use of that class of product, regardless of the status
|
||||
of the particular user or of the way in which the particular user
|
||||
actually uses, or expects or is expected to use, the product. A product
|
||||
is a consumer product regardless of whether the product has substantial
|
||||
commercial, industrial or non-consumer uses, unless such uses represent
|
||||
the only significant mode of use of the product.
|
||||
|
||||
"Installation Information" for a User Product means any methods,
|
||||
procedures, authorization keys, or other information required to install
|
||||
and execute modified versions of a covered work in that User Product from
|
||||
a modified version of its Corresponding Source. The information must
|
||||
suffice to ensure that the continued functioning of the modified object
|
||||
code is in no case prevented or interfered with solely because
|
||||
modification has been made.
|
||||
|
||||
If you convey an object code work under this section in, or with, or
|
||||
specifically for use in, a User Product, and the conveying occurs as
|
||||
part of a transaction in which the right of possession and use of the
|
||||
User Product is transferred to the recipient in perpetuity or for a
|
||||
fixed term (regardless of how the transaction is characterized), the
|
||||
Corresponding Source conveyed under this section must be accompanied
|
||||
by the Installation Information. But this requirement does not apply
|
||||
if neither you nor any third party retains the ability to install
|
||||
modified object code on the User Product (for example, the work has
|
||||
been installed in ROM).
|
||||
|
||||
The requirement to provide Installation Information does not include a
|
||||
requirement to continue to provide support service, warranty, or updates
|
||||
for a work that has been modified or installed by the recipient, or for
|
||||
the User Product in which it has been modified or installed. Access to a
|
||||
network may be denied when the modification itself materially and
|
||||
adversely affects the operation of the network or violates the rules and
|
||||
protocols for communication across the network.
|
||||
|
||||
Corresponding Source conveyed, and Installation Information provided,
|
||||
in accord with this section must be in a format that is publicly
|
||||
documented (and with an implementation available to the public in
|
||||
source code form), and must require no special password or key for
|
||||
unpacking, reading or copying.
|
||||
|
||||
7. Additional Terms.
|
||||
|
||||
"Additional permissions" are terms that supplement the terms of this
|
||||
License by making exceptions from one or more of its conditions.
|
||||
Additional permissions that are applicable to the entire Program shall
|
||||
be treated as though they were included in this License, to the extent
|
||||
that they are valid under applicable law. If additional permissions
|
||||
apply only to part of the Program, that part may be used separately
|
||||
under those permissions, but the entire Program remains governed by
|
||||
this License without regard to the additional permissions.
|
||||
|
||||
When you convey a copy of a covered work, you may at your option
|
||||
remove any additional permissions from that copy, or from any part of
|
||||
it. (Additional permissions may be written to require their own
|
||||
removal in certain cases when you modify the work.) You may place
|
||||
additional permissions on material, added by you to a covered work,
|
||||
for which you have or can give appropriate copyright permission.
|
||||
|
||||
Notwithstanding any other provision of this License, for material you
|
||||
add to a covered work, you may (if authorized by the copyright holders of
|
||||
that material) supplement the terms of this License with terms:
|
||||
|
||||
a) Disclaiming warranty or limiting liability differently from the
|
||||
terms of sections 15 and 16 of this License; or
|
||||
|
||||
b) Requiring preservation of specified reasonable legal notices or
|
||||
author attributions in that material or in the Appropriate Legal
|
||||
Notices displayed by works containing it; or
|
||||
|
||||
c) Prohibiting misrepresentation of the origin of that material, or
|
||||
requiring that modified versions of such material be marked in
|
||||
reasonable ways as different from the original version; or
|
||||
|
||||
d) Limiting the use for publicity purposes of names of licensors or
|
||||
authors of the material; or
|
||||
|
||||
e) Declining to grant rights under trademark law for use of some
|
||||
trade names, trademarks, or service marks; or
|
||||
|
||||
f) Requiring indemnification of licensors and authors of that
|
||||
material by anyone who conveys the material (or modified versions of
|
||||
it) with contractual assumptions of liability to the recipient, for
|
||||
any liability that these contractual assumptions directly impose on
|
||||
those licensors and authors.
|
||||
|
||||
All other non-permissive additional terms are considered "further
|
||||
restrictions" within the meaning of section 10. If the Program as you
|
||||
received it, or any part of it, contains a notice stating that it is
|
||||
governed by this License along with a term that is a further
|
||||
restriction, you may remove that term. If a license document contains
|
||||
a further restriction but permits relicensing or conveying under this
|
||||
License, you may add to a covered work material governed by the terms
|
||||
of that license document, provided that the further restriction does
|
||||
not survive such relicensing or conveying.
|
||||
|
||||
If you add terms to a covered work in accord with this section, you
|
||||
must place, in the relevant source files, a statement of the
|
||||
additional terms that apply to those files, or a notice indicating
|
||||
where to find the applicable terms.
|
||||
|
||||
Additional terms, permissive or non-permissive, may be stated in the
|
||||
form of a separately written license, or stated as exceptions;
|
||||
the above requirements apply either way.
|
||||
|
||||
8. Termination.
|
||||
|
||||
You may not propagate or modify a covered work except as expressly
|
||||
provided under this License. Any attempt otherwise to propagate or
|
||||
modify it is void, and will automatically terminate your rights under
|
||||
this License (including any patent licenses granted under the third
|
||||
paragraph of section 11).
|
||||
|
||||
However, if you cease all violation of this License, then your
|
||||
license from a particular copyright holder is reinstated (a)
|
||||
provisionally, unless and until the copyright holder explicitly and
|
||||
finally terminates your license, and (b) permanently, if the copyright
|
||||
holder fails to notify you of the violation by some reasonable means
|
||||
prior to 60 days after the cessation.
|
||||
|
||||
Moreover, your license from a particular copyright holder is
|
||||
reinstated permanently if the copyright holder notifies you of the
|
||||
violation by some reasonable means, this is the first time you have
|
||||
received notice of violation of this License (for any work) from that
|
||||
copyright holder, and you cure the violation prior to 30 days after
|
||||
your receipt of the notice.
|
||||
|
||||
Termination of your rights under this section does not terminate the
|
||||
licenses of parties who have received copies or rights from you under
|
||||
this License. If your rights have been terminated and not permanently
|
||||
reinstated, you do not qualify to receive new licenses for the same
|
||||
material under section 10.
|
||||
|
||||
9. Acceptance Not Required for Having Copies.
|
||||
|
||||
You are not required to accept this License in order to receive or
|
||||
run a copy of the Program. Ancillary propagation of a covered work
|
||||
occurring solely as a consequence of using peer-to-peer transmission
|
||||
to receive a copy likewise does not require acceptance. However,
|
||||
nothing other than this License grants you permission to propagate or
|
||||
modify any covered work. These actions infringe copyright if you do
|
||||
not accept this License. Therefore, by modifying or propagating a
|
||||
covered work, you indicate your acceptance of this License to do so.
|
||||
|
||||
10. Automatic Licensing of Downstream Recipients.
|
||||
|
||||
Each time you convey a covered work, the recipient automatically
|
||||
receives a license from the original licensors, to run, modify and
|
||||
propagate that work, subject to this License. You are not responsible
|
||||
for enforcing compliance by third parties with this License.
|
||||
|
||||
An "entity transaction" is a transaction transferring control of an
|
||||
organization, or substantially all assets of one, or subdividing an
|
||||
organization, or merging organizations. If propagation of a covered
|
||||
work results from an entity transaction, each party to that
|
||||
transaction who receives a copy of the work also receives whatever
|
||||
licenses to the work the party's predecessor in interest had or could
|
||||
give under the previous paragraph, plus a right to possession of the
|
||||
Corresponding Source of the work from the predecessor in interest, if
|
||||
the predecessor has it or can get it with reasonable efforts.
|
||||
|
||||
You may not impose any further restrictions on the exercise of the
|
||||
rights granted or affirmed under this License. For example, you may
|
||||
not impose a license fee, royalty, or other charge for exercise of
|
||||
rights granted under this License, and you may not initiate litigation
|
||||
(including a cross-claim or counterclaim in a lawsuit) alleging that
|
||||
any patent claim is infringed by making, using, selling, offering for
|
||||
sale, or importing the Program or any portion of it.
|
||||
|
||||
11. Patents.
|
||||
|
||||
A "contributor" is a copyright holder who authorizes use under this
|
||||
License of the Program or a work on which the Program is based. The
|
||||
work thus licensed is called the contributor's "contributor version".
|
||||
|
||||
A contributor's "essential patent claims" are all patent claims
|
||||
owned or controlled by the contributor, whether already acquired or
|
||||
hereafter acquired, that would be infringed by some manner, permitted
|
||||
by this License, of making, using, or selling its contributor version,
|
||||
but do not include claims that would be infringed only as a
|
||||
consequence of further modification of the contributor version. For
|
||||
purposes of this definition, "control" includes the right to grant
|
||||
patent sublicenses in a manner consistent with the requirements of
|
||||
this License.
|
||||
|
||||
Each contributor grants you a non-exclusive, worldwide, royalty-free
|
||||
patent license under the contributor's essential patent claims, to
|
||||
make, use, sell, offer for sale, import and otherwise run, modify and
|
||||
propagate the contents of its contributor version.
|
||||
|
||||
In the following three paragraphs, a "patent license" is any express
|
||||
agreement or commitment, however denominated, not to enforce a patent
|
||||
(such as an express permission to practice a patent or covenant not to
|
||||
sue for patent infringement). To "grant" such a patent license to a
|
||||
party means to make such an agreement or commitment not to enforce a
|
||||
patent against the party.
|
||||
|
||||
If you convey a covered work, knowingly relying on a patent license,
|
||||
and the Corresponding Source of the work is not available for anyone
|
||||
to copy, free of charge and under the terms of this License, through a
|
||||
publicly available network server or other readily accessible means,
|
||||
then you must either (1) cause the Corresponding Source to be so
|
||||
available, or (2) arrange to deprive yourself of the benefit of the
|
||||
patent license for this particular work, or (3) arrange, in a manner
|
||||
consistent with the requirements of this License, to extend the patent
|
||||
license to downstream recipients. "Knowingly relying" means you have
|
||||
actual knowledge that, but for the patent license, your conveying the
|
||||
covered work in a country, or your recipient's use of the covered work
|
||||
in a country, would infringe one or more identifiable patents in that
|
||||
country that you have reason to believe are valid.
|
||||
|
||||
If, pursuant to or in connection with a single transaction or
|
||||
arrangement, you convey, or propagate by procuring conveyance of, a
|
||||
covered work, and grant a patent license to some of the parties
|
||||
receiving the covered work authorizing them to use, propagate, modify
|
||||
or convey a specific copy of the covered work, then the patent license
|
||||
you grant is automatically extended to all recipients of the covered
|
||||
work and works based on it.
|
||||
|
||||
A patent license is "discriminatory" if it does not include within
|
||||
the scope of its coverage, prohibits the exercise of, or is
|
||||
conditioned on the non-exercise of one or more of the rights that are
|
||||
specifically granted under this License. You may not convey a covered
|
||||
work if you are a party to an arrangement with a third party that is
|
||||
in the business of distributing software, under which you make payment
|
||||
to the third party based on the extent of your activity of conveying
|
||||
the work, and under which the third party grants, to any of the
|
||||
parties who would receive the covered work from you, a discriminatory
|
||||
patent license (a) in connection with copies of the covered work
|
||||
conveyed by you (or copies made from those copies), or (b) primarily
|
||||
for and in connection with specific products or compilations that
|
||||
contain the covered work, unless you entered into that arrangement,
|
||||
or that patent license was granted, prior to 28 March 2007.
|
||||
|
||||
Nothing in this License shall be construed as excluding or limiting
|
||||
any implied license or other defenses to infringement that may
|
||||
otherwise be available to you under applicable patent law.
|
||||
|
||||
12. No Surrender of Others' Freedom.
|
||||
|
||||
If conditions are imposed on you (whether by court order, agreement or
|
||||
otherwise) that contradict the conditions of this License, they do not
|
||||
excuse you from the conditions of this License. If you cannot convey a
|
||||
covered work so as to satisfy simultaneously your obligations under this
|
||||
License and any other pertinent obligations, then as a consequence you may
|
||||
not convey it at all. For example, if you agree to terms that obligate you
|
||||
to collect a royalty for further conveying from those to whom you convey
|
||||
the Program, the only way you could satisfy both those terms and this
|
||||
License would be to refrain entirely from conveying the Program.
|
||||
|
||||
13. Use with the GNU Affero General Public License.
|
||||
|
||||
Notwithstanding any other provision of this License, you have
|
||||
permission to link or combine any covered work with a work licensed
|
||||
under version 3 of the GNU Affero General Public License into a single
|
||||
combined work, and to convey the resulting work. The terms of this
|
||||
License will continue to apply to the part which is the covered work,
|
||||
but the special requirements of the GNU Affero General Public License,
|
||||
section 13, concerning interaction through a network will apply to the
|
||||
combination as such.
|
||||
|
||||
14. Revised Versions of this License.
|
||||
|
||||
The Free Software Foundation may publish revised and/or new versions of
|
||||
the GNU General Public License from time to time. Such new versions will
|
||||
be similar in spirit to the present version, but may differ in detail to
|
||||
address new problems or concerns.
|
||||
|
||||
Each version is given a distinguishing version number. If the
|
||||
Program specifies that a certain numbered version of the GNU General
|
||||
Public License "or any later version" applies to it, you have the
|
||||
option of following the terms and conditions either of that numbered
|
||||
version or of any later version published by the Free Software
|
||||
Foundation. If the Program does not specify a version number of the
|
||||
GNU General Public License, you may choose any version ever published
|
||||
by the Free Software Foundation.
|
||||
|
||||
If the Program specifies that a proxy can decide which future
|
||||
versions of the GNU General Public License can be used, that proxy's
|
||||
public statement of acceptance of a version permanently authorizes you
|
||||
to choose that version for the Program.
|
||||
|
||||
Later license versions may give you additional or different
|
||||
permissions. However, no additional obligations are imposed on any
|
||||
author or copyright holder as a result of your choosing to follow a
|
||||
later version.
|
||||
|
||||
15. Disclaimer of Warranty.
|
||||
|
||||
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
|
||||
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
|
||||
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
|
||||
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
|
||||
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
|
||||
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
|
||||
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
|
||||
|
||||
16. Limitation of Liability.
|
||||
|
||||
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
|
||||
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
|
||||
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
|
||||
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
|
||||
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
|
||||
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
|
||||
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
|
||||
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGES.
|
||||
|
||||
17. Interpretation of Sections 15 and 16.
|
||||
|
||||
If the disclaimer of warranty and limitation of liability provided
|
||||
above cannot be given local legal effect according to their terms,
|
||||
reviewing courts shall apply local law that most closely approximates
|
||||
an absolute waiver of all civil liability in connection with the
|
||||
Program, unless a warranty or assumption of liability accompanies a
|
||||
copy of the Program in return for a fee.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
How to Apply These Terms to Your New Programs
|
||||
|
||||
If you develop a new program, and you want it to be of the greatest
|
||||
possible use to the public, the best way to achieve this is to make it
|
||||
free software which everyone can redistribute and change under these terms.
|
||||
|
||||
To do so, attach the following notices to the program. It is safest
|
||||
to attach them to the start of each source file to most effectively
|
||||
state the exclusion of warranty; and each file should have at least
|
||||
the "copyright" line and a pointer to where the full notice is found.
|
||||
|
||||
<one line to give the program's name and a brief idea of what it does.>
|
||||
Copyright (C) <year> <name of author>
|
||||
|
||||
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, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
Also add information on how to contact you by electronic and paper mail.
|
||||
|
||||
If the program does terminal interaction, make it output a short
|
||||
notice like this when it starts in an interactive mode:
|
||||
|
||||
<program> Copyright (C) <year> <name of author>
|
||||
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
|
||||
This is free software, and you are welcome to redistribute it
|
||||
under certain conditions; type `show c' for details.
|
||||
|
||||
The hypothetical commands `show w' and `show c' should show the appropriate
|
||||
parts of the General Public License. Of course, your program's commands
|
||||
might be different; for a GUI interface, you would use an "about box".
|
||||
|
||||
You should also get your employer (if you work as a programmer) or school,
|
||||
if any, to sign a "copyright disclaimer" for the program, if necessary.
|
||||
For more information on this, and how to apply and follow the GNU GPL, see
|
||||
<http://www.gnu.org/licenses/>.
|
||||
|
||||
The GNU General Public License does not permit incorporating your program
|
||||
into proprietary programs. If your program is a subroutine library, you
|
||||
may consider it more useful to permit linking proprietary applications with
|
||||
the library. If this is what you want to do, use the GNU Lesser General
|
||||
Public License instead of this License. But first, please read
|
||||
<http://www.gnu.org/philosophy/why-not-lgpl.html>.
|
||||
207
flight/Libraries/rscode/rs.c
Normal file
207
flight/Libraries/rscode/rs.c
Normal file
@ -0,0 +1,207 @@
|
||||
/*
|
||||
* Reed Solomon Encoder/Decoder
|
||||
*
|
||||
* 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
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
#include "ecc.h"
|
||||
|
||||
/* Encoder parity bytes */
|
||||
int pBytes[MAXDEG];
|
||||
|
||||
/* Decoder syndrome bytes */
|
||||
int synBytes[MAXDEG];
|
||||
|
||||
/* generator polynomial */
|
||||
int genPoly[MAXDEG*2];
|
||||
|
||||
int DEBUG = FALSE;
|
||||
|
||||
static void
|
||||
compute_genpoly (int nbytes, int genpoly[]);
|
||||
|
||||
/* Initialize lookup tables, polynomials, etc. */
|
||||
void
|
||||
initialize_ecc ()
|
||||
{
|
||||
/* Initialize the galois field arithmetic tables */
|
||||
init_galois_tables();
|
||||
|
||||
/* Compute the encoder generator polynomial */
|
||||
compute_genpoly(RS_ECC_NPARITY, genPoly);
|
||||
}
|
||||
|
||||
void
|
||||
zero_fill_from (unsigned char buf[], int from, int to)
|
||||
{
|
||||
int i;
|
||||
for (i = from; i < to; i++) buf[i] = 0;
|
||||
}
|
||||
|
||||
/* debugging routines */
|
||||
void
|
||||
print_parity (void)
|
||||
{
|
||||
#ifdef NEVER
|
||||
int i;
|
||||
printf("Parity Bytes: ");
|
||||
for (i = 0; i < RS_ECC_NPARITY; i++)
|
||||
printf("[%d]:%x, ",i,pBytes[i]);
|
||||
printf("\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
print_syndrome (void)
|
||||
{
|
||||
#ifdef NEVER
|
||||
int i;
|
||||
printf("Syndrome Bytes: ");
|
||||
for (i = 0; i < RS_ECC_NPARITY; i++)
|
||||
printf("[%d]:%x, ",i,synBytes[i]);
|
||||
printf("\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
/* Append the parity bytes onto the end of the message */
|
||||
void
|
||||
build_codeword (unsigned char msg[], int nbytes, unsigned char dst[])
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < nbytes; i++) dst[i] = msg[i];
|
||||
|
||||
for (i = 0; i < RS_ECC_NPARITY; i++) {
|
||||
dst[i+nbytes] = pBytes[RS_ECC_NPARITY-1-i];
|
||||
}
|
||||
}
|
||||
|
||||
/**********************************************************
|
||||
* Reed Solomon Decoder
|
||||
*
|
||||
* Computes the syndrome of a codeword. Puts the results
|
||||
* into the synBytes[] array.
|
||||
*/
|
||||
|
||||
void
|
||||
decode_data(unsigned char data[], int nbytes)
|
||||
{
|
||||
int i, j, sum;
|
||||
for (j = 0; j < RS_ECC_NPARITY; j++) {
|
||||
sum = 0;
|
||||
for (i = 0; i < nbytes; i++) {
|
||||
sum = data[i] ^ gmult(gexp[j+1], sum);
|
||||
}
|
||||
synBytes[j] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* Check if the syndrome is zero */
|
||||
int
|
||||
check_syndrome (void)
|
||||
{
|
||||
int i, nz = 0;
|
||||
for (i =0 ; i < RS_ECC_NPARITY; i++) {
|
||||
if (synBytes[i] != 0) {
|
||||
nz = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return nz;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
debug_check_syndrome (void)
|
||||
{
|
||||
#ifdef NEVER
|
||||
int i;
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
printf(" inv log S[%d]/S[%d] = %d\n", i, i+1,
|
||||
glog[gmult(synBytes[i], ginv(synBytes[i+1]))]);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/* Create a generator polynomial for an n byte RS code.
|
||||
* The coefficients are returned in the genPoly arg.
|
||||
* Make sure that the genPoly array which is passed in is
|
||||
* at least n+1 bytes long.
|
||||
*/
|
||||
|
||||
static void
|
||||
compute_genpoly (int nbytes, int genpoly[])
|
||||
{
|
||||
int i, tp[256], tp1[256];
|
||||
|
||||
/* multiply (x + a^n) for n = 1 to nbytes */
|
||||
|
||||
zero_poly(tp1);
|
||||
tp1[0] = 1;
|
||||
|
||||
for (i = 1; i <= nbytes; i++) {
|
||||
zero_poly(tp);
|
||||
tp[0] = gexp[i]; /* set up x+a^n */
|
||||
tp[1] = 1;
|
||||
|
||||
mult_polys(genpoly, tp, tp1);
|
||||
copy_poly(tp1, genpoly);
|
||||
}
|
||||
}
|
||||
|
||||
/* Simulate a LFSR with generator polynomial for n byte RS code.
|
||||
* Pass in a pointer to the data array, and amount of data.
|
||||
*
|
||||
* The parity bytes are deposited into pBytes[], and the whole message
|
||||
* and parity are copied to dest to make a codeword.
|
||||
*
|
||||
*/
|
||||
|
||||
void
|
||||
encode_data (unsigned char msg[], int nbytes, unsigned char dst[])
|
||||
{
|
||||
int i, LFSR[RS_ECC_NPARITY+1],dbyte, j;
|
||||
|
||||
for(i=0; i < RS_ECC_NPARITY+1; i++) LFSR[i]=0;
|
||||
|
||||
for (i = 0; i < nbytes; i++) {
|
||||
dbyte = msg[i] ^ LFSR[RS_ECC_NPARITY-1];
|
||||
for (j = RS_ECC_NPARITY-1; j > 0; j--) {
|
||||
LFSR[j] = LFSR[j-1] ^ gmult(genPoly[j], dbyte);
|
||||
}
|
||||
LFSR[0] = gmult(genPoly[0], dbyte);
|
||||
}
|
||||
|
||||
for (i = 0; i < RS_ECC_NPARITY; i++)
|
||||
pBytes[i] = LFSR[i];
|
||||
|
||||
build_codeword(msg, nbytes, dst);
|
||||
}
|
||||
|
||||
86
flight/Libraries/rscode/rs.doc
Normal file
86
flight/Libraries/rscode/rs.doc
Normal file
@ -0,0 +1,86 @@
|
||||
|
||||
|
||||
Introduction to Reed Solomon Codes:
|
||||
|
||||
Henry Minsky, Universal Access Inc.
|
||||
hqm@alum.mit.edu
|
||||
|
||||
[For details see Cain, Clark, "Error-Correction Coding For Digital
|
||||
Communications", pp. 205.] The Reed-Solomon Code is an algebraic code
|
||||
belonging to the class of BCH (Bose-Chaudry-Hocquehen) multiple burst
|
||||
correcting cyclic codes. The Reed Solomon code operates on bytes of
|
||||
fixed length.
|
||||
|
||||
Given m parity bytes, a Reed-Solomon code can correct up to m byte
|
||||
errors in known positions (erasures), or detect and correct up to m/2
|
||||
byte errors in unknown positions.
|
||||
|
||||
This is an implementation of a Reed-Solomon code with 8 bit bytes, and
|
||||
a configurable number of parity bytes. The maximum sequence length
|
||||
(codeword) that can be generated is 255 bytes, including parity bytes.
|
||||
In practice, shorter sequences are used.
|
||||
|
||||
ENCODING: The basic principle of encoding is to find the remainder of
|
||||
the message divided by a generator polynomial G(x). The encoder works
|
||||
by simulating a Linear Feedback Shift Register with degree equal to
|
||||
G(x), and feedback taps with the coefficents of the generating
|
||||
polynomial of the code.
|
||||
|
||||
The rs.c file contains an algorithm to generate the encoder polynomial
|
||||
for any number of bytes of parity, configurable as the NPAR constant
|
||||
in the file ecc.h.
|
||||
|
||||
For this RS code, G(x) = (x-a^1)(x-a^2)(x-a^3)(x-a^4)...(x-a^NPAR)
|
||||
where 'a' is a primitive element of the Galois Field GF(256) (== 2).
|
||||
|
||||
DECODING
|
||||
|
||||
The decoder generates four syndrome bytes, which will be all zero if
|
||||
the message has no errors. If there are errors, the location and value
|
||||
of the errors can be determined in a number of ways.
|
||||
|
||||
Computing the syndromes is easily done as a sum of products, see pp.
|
||||
179 [Rhee 89].
|
||||
|
||||
Fundamentally, the syndome bytes form four simultaneous equations
|
||||
which can be solved to find the error locations. Once error locations
|
||||
are known, the syndrome bytes can be used to find the value of the
|
||||
errors, and they can thus be corrected.
|
||||
|
||||
A simplified solution for locating and correcting single errors is
|
||||
given in Cain and Clark, Ch. 5.
|
||||
|
||||
The more general error-location algorithm is the Berlekamp-Massey
|
||||
algorithm, which will locate up to four errors, by iteratively solving
|
||||
for the error-locator polynomial. The Modified Berlekamp Massey
|
||||
algorithm takes as initial conditions any known suspicious bytes
|
||||
(erasure flags) which you may have (such as might be flagged by
|
||||
a laser demodulator, or deduced from a failure in a cross-interleaved
|
||||
block code row or column).
|
||||
|
||||
Once the location of errors is known, error correction is done using
|
||||
the error-evaluator polynomial.
|
||||
|
||||
APPLICATION IDEAS
|
||||
|
||||
As an example application, this library could be used to implement the
|
||||
Compact Disc standard of 24 data bytes and 4 parity bytes. A RS code
|
||||
with 24 data bytes and 4 parity bytes is referred to as a (28,24) RS
|
||||
code. A (n, k) RS code is said to have efficiency k/n. This first
|
||||
(28,24) coding is called the C2 or level 2 encoding, because in a
|
||||
doubly encoded scheme, the codewords are decoded at the second
|
||||
decoding step.
|
||||
|
||||
In following the approach used by Compact Disc digital audio, the 28
|
||||
byte C2 codewords are four way interleaved and then the interleaved
|
||||
data is encoded again with a (32,28) RS code. The is the C1 encoding
|
||||
stage. This produces what is known as a "product code", and has
|
||||
excellent error correction capability due to the imposition of
|
||||
two-dimensional structure on the parity checks. The interleave helps
|
||||
to insure against the case that a multibyte burst error wipes out more
|
||||
than two bytes in each codeword. The cross-correction capability of
|
||||
the product code can provide backup if in fact there are more than 2
|
||||
uncorrectable errors in a block.
|
||||
|
||||
|
||||
|
||||
@ -52,11 +52,10 @@
|
||||
|
||||
#include "ahrs_comms.h"
|
||||
#include "ahrs_spi_comm.h"
|
||||
#include "ahrsstatus.h"
|
||||
#include "ahrscalibration.h"
|
||||
#include "insstatus.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE-128
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
|
||||
// Private types
|
||||
@ -87,9 +86,10 @@ int32_t AHRSCommsStart(void)
|
||||
*/
|
||||
int32_t AHRSCommsInitialize(void)
|
||||
{
|
||||
AhrsStatusInitialize();
|
||||
AHRSCalibrationInitialize();
|
||||
InsStatusInitialize();
|
||||
InsSettingsInitialize();
|
||||
AttitudeRawInitialize();
|
||||
AttitudeActualInitialize();
|
||||
VelocityActualInitialize();
|
||||
PositionActualInitialize();
|
||||
|
||||
@ -109,19 +109,17 @@ static void ahrscommsTask(void *parameters)
|
||||
// Main task loop
|
||||
while (1) {
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS);
|
||||
AhrsCommStatus stat;
|
||||
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
AhrsSendObjects();
|
||||
AhrsCommStatus stat = AhrsGetStatus();
|
||||
AhrsGetStatus(&stat);
|
||||
if (stat.linkOk) {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
AhrsStatusData sData;
|
||||
AhrsStatusGet(&sData);
|
||||
InsStatusData sData;
|
||||
InsStatusGet(&sData);
|
||||
|
||||
sData.LinkRunning = stat.linkOk;
|
||||
sData.AhrsKickstarts = stat.remote.kickStarts;
|
||||
@ -132,9 +130,9 @@ static void ahrscommsTask(void *parameters)
|
||||
sData.OpRetries = stat.local.retries;
|
||||
sData.OpInvalidPackets = stat.local.invalidPacket;
|
||||
|
||||
AhrsStatusSet(&sData);
|
||||
InsStatusSet(&sData);
|
||||
/* Wait for the next update interval */
|
||||
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS);
|
||||
vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -216,7 +216,7 @@ static void actuatorTask(void* parameters)
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
||||
|
||||
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
|
||||
bool positiveThrottle = desired.Throttle >= 0.00;
|
||||
bool positiveThrottle = desired.Throttle >= 0.00f;
|
||||
bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
|
||||
|
||||
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
|
||||
@ -325,26 +325,29 @@ static void actuatorTask(void* parameters)
|
||||
else
|
||||
status[ct] = -1;
|
||||
}
|
||||
|
||||
command.Channel[ct] = scaleChannel(status[ct],
|
||||
ChannelMax[ct],
|
||||
ChannelMin[ct],
|
||||
ChannelNeutral[ct]);
|
||||
}
|
||||
#if defined(DIAGNOSTICS)
|
||||
MixerStatusSet(&mixerStatus);
|
||||
#endif
|
||||
|
||||
|
||||
for(int i = 0; i < MAX_MIX_ACTUATORS; i++)
|
||||
command.Channel[i] = scaleChannel(status[i],
|
||||
ChannelMax[i],
|
||||
ChannelMin[i],
|
||||
ChannelNeutral[i]);
|
||||
|
||||
// Store update time
|
||||
command.UpdateTime = 1000*dT;
|
||||
if(1000*dT > command.MaxUpdateTime)
|
||||
command.MaxUpdateTime = 1000*dT;
|
||||
|
||||
command.UpdateTime = 1000.0f*dT;
|
||||
if(1000.0f*dT > command.MaxUpdateTime)
|
||||
command.MaxUpdateTime = 1000.0f*dT;
|
||||
|
||||
// Update output object
|
||||
ActuatorCommandSet(&command);
|
||||
// Update in case read only (eg. during servo configuration)
|
||||
ActuatorCommandGet(&command);
|
||||
|
||||
#if defined(DIAGNOSTICS)
|
||||
MixerStatusSet(&mixerStatus);
|
||||
#endif
|
||||
|
||||
|
||||
// Update servo outputs
|
||||
bool success = true;
|
||||
|
||||
@ -367,22 +370,21 @@ static void actuatorTask(void* parameters)
|
||||
/**
|
||||
*Process mixing for one actuator
|
||||
*/
|
||||
|
||||
float ProcessMixer(const int index, const float curve1, const float curve2,
|
||||
MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period)
|
||||
{
|
||||
Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects
|
||||
Mixer_t * mixer = &mixers[index];
|
||||
float result = ((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
|
||||
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
|
||||
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
|
||||
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
|
||||
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
|
||||
float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
|
||||
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
|
||||
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
|
||||
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
|
||||
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
|
||||
if(mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR)
|
||||
{
|
||||
if(result < 0) //idle throttle
|
||||
if(result < 0.0f) //idle throttle
|
||||
{
|
||||
result = 0;
|
||||
result = 0.0f;
|
||||
}
|
||||
|
||||
//feed forward
|
||||
@ -392,7 +394,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
|
||||
result += accumulator;
|
||||
if(period !=0)
|
||||
{
|
||||
if(accumulator > 0)
|
||||
if(accumulator > 0.0f)
|
||||
{
|
||||
float filter = mixerSettings->AccelTime / period;
|
||||
if(filter <1)
|
||||
@ -422,6 +424,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
|
||||
}
|
||||
lastFilteredResult[index] = result;
|
||||
}
|
||||
|
||||
return(result);
|
||||
}
|
||||
|
||||
@ -432,7 +435,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
|
||||
*/
|
||||
static float MixerCurve(const float throttle, const float* curve, uint8_t elements)
|
||||
{
|
||||
float scale = throttle * (elements - 1);
|
||||
float scale = throttle * (float) (elements - 1);
|
||||
int idx1 = scale;
|
||||
scale -= (float)idx1; //remainder
|
||||
if(curve[0] < -1)
|
||||
@ -453,7 +456,7 @@ static float MixerCurve(const float throttle, const float* curve, uint8_t elemen
|
||||
idx1 = elements -1;
|
||||
}
|
||||
}
|
||||
return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale));
|
||||
return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
|
||||
}
|
||||
|
||||
|
||||
@ -464,7 +467,7 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
|
||||
{
|
||||
int16_t valueScaled;
|
||||
// Scale
|
||||
if ( value >= 0.0)
|
||||
if ( value >= 0.0f)
|
||||
{
|
||||
valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral;
|
||||
}
|
||||
@ -520,6 +523,8 @@ static void setFailsafe()
|
||||
{
|
||||
Channel[n] = 0;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// Set alarm
|
||||
@ -532,7 +537,7 @@ static void setFailsafe()
|
||||
}
|
||||
|
||||
// Update output object's parts that we changed
|
||||
ActuatorCommandChannelGet(Channel);
|
||||
ActuatorCommandChannelSet(Channel);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -120,7 +120,7 @@ static void altitudeTask(void *parameters)
|
||||
{
|
||||
BaroAltitudeData data;
|
||||
portTickType lastSysTime;
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
SonarAltitudeData sonardata;
|
||||
int32_t value=0,timeout=5;
|
||||
@ -129,7 +129,7 @@ static void altitudeTask(void *parameters)
|
||||
PIOS_HCSR04_Trigger();
|
||||
#endif
|
||||
PIOS_BMP085_Init();
|
||||
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1)
|
||||
@ -145,7 +145,7 @@ static void altitudeTask(void *parameters)
|
||||
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
|
||||
sonardata.Altitude = height_out; // m/us
|
||||
}
|
||||
|
||||
|
||||
// Update the AltitudeActual UAVObject
|
||||
SonarAltitudeSet(&sonardata);
|
||||
timeout=5;
|
||||
@ -167,7 +167,7 @@ static void altitudeTask(void *parameters)
|
||||
#endif
|
||||
PIOS_BMP085_ReadADC();
|
||||
alt_ds_temp += PIOS_BMP085_GetTemperature();
|
||||
|
||||
|
||||
// Update the pressure data
|
||||
PIOS_BMP085_StartADC(PressureConv);
|
||||
#ifdef PIOS_BMP085_HAS_GPIOS
|
||||
@ -177,7 +177,7 @@ static void altitudeTask(void *parameters)
|
||||
#endif
|
||||
PIOS_BMP085_ReadADC();
|
||||
alt_ds_pres += PIOS_BMP085_GetPressure();
|
||||
|
||||
|
||||
if (++alt_ds_count >= alt_ds_size)
|
||||
{
|
||||
alt_ds_count = 0;
|
||||
@ -203,6 +203,6 @@ static void altitudeTask(void *parameters)
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
158
flight/Modules/Altitude/revolution/altitude.c
Normal file
158
flight/Modules/Altitude/revolution/altitude.c
Normal file
@ -0,0 +1,158 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup AltitudeModule Altitude Module
|
||||
* @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object"
|
||||
* @{
|
||||
*
|
||||
* @file altitude.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Altitude module, handles temperature and pressure readings from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Output object: BaroAltitude
|
||||
*
|
||||
* This module will periodically update the value of the BaroAltitude object.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "altitude.h"
|
||||
#include "baroaltitude.h" // object that will be updated by the module
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
#include "sonaraltitude.h" // object that will be updated by the module
|
||||
#endif
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 500
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void altitudeTask(void *parameters);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeStart()
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
SonarAltitudeInitialze();
|
||||
#endif
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeInitialize()
|
||||
{
|
||||
BaroAltitudeInitialize();
|
||||
|
||||
return 0;
|
||||
}
|
||||
MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void altitudeTask(void *parameters)
|
||||
{
|
||||
BaroAltitudeData data;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
SonarAltitudeData sonardata;
|
||||
int32_t value=0,timeout=5;
|
||||
float coeff=0.25,height_out=0,height_in=0;
|
||||
PIOS_HCSR04_Init();
|
||||
PIOS_HCSR04_Trigger();
|
||||
#endif
|
||||
|
||||
// TODO: Check the pressure sensor and set a warning if it fails test
|
||||
|
||||
// Main task loop
|
||||
while (1)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
// Compute the current altitude (all pressures in kPa)
|
||||
if(PIOS_HCSR04_Completed())
|
||||
{
|
||||
value = PIOS_HCSR04_Get();
|
||||
if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
|
||||
{
|
||||
height_in = value*0.00034;
|
||||
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
|
||||
sonardata.Altitude = height_out; // m/us
|
||||
}
|
||||
|
||||
// Update the AltitudeActual UAVObject
|
||||
SonarAltitudeSet(&sonardata);
|
||||
timeout=5;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
if(timeout--)
|
||||
{
|
||||
//retrigger
|
||||
timeout=5;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
#endif
|
||||
float temp, press;
|
||||
|
||||
// Update the temperature data
|
||||
PIOS_MS5611_StartADC(TemperatureConv);
|
||||
vTaskDelay(PIOS_MS5611_GetDelay());
|
||||
PIOS_MS5611_ReadADC();
|
||||
|
||||
// Update the pressure data
|
||||
PIOS_MS5611_StartADC(PressureConv);
|
||||
vTaskDelay(PIOS_MS5611_GetDelay());
|
||||
PIOS_MS5611_ReadADC();
|
||||
|
||||
|
||||
temp = PIOS_MS5611_GetTemperature();
|
||||
press = PIOS_MS5611_GetPressure();
|
||||
|
||||
data.Temperature = temp;
|
||||
data.Pressure = press;
|
||||
data.Altitude = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f)));
|
||||
|
||||
// Update the AltitudeActual UAVObject
|
||||
BaroAltitudeSet(&data);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
41
flight/Modules/Altitude/revolution/inc/altitude.h
Normal file
41
flight/Modules/Altitude/revolution/inc/altitude.h
Normal file
@ -0,0 +1,41 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup AltitudeModule Altitude Module
|
||||
* @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
|
||||
* @{
|
||||
*
|
||||
* @file altitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Altitude module, reads temperature and pressure from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* 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 ALTITUDE_H
|
||||
#define ALTITUDE_H
|
||||
|
||||
int32_t AltitudeInitialize();
|
||||
|
||||
#endif // ALTITUDE_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
372
flight/Modules/AltitudeHold/altitudehold.c
Normal file
372
flight/Modules/AltitudeHold/altitudehold.c
Normal file
@ -0,0 +1,372 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file guidance.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
|
||||
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
|
||||
* of @ref ManualControlCommand is Auto.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input object: ActiveWaypoint
|
||||
* Input object: PositionActual
|
||||
* Input object: ManualControlCommand
|
||||
* Output object: AttitudeDesired
|
||||
*
|
||||
* This module will periodically update the value of the AttitudeDesired object.
|
||||
*
|
||||
* The module executes in its own thread in this example.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include <math.h>
|
||||
#include "CoordinateConversions.h"
|
||||
#include "altholdsmoothed.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "altitudeholdsettings.h"
|
||||
#include "altitudeholddesired.h" // object that will be updated by the module
|
||||
#include "baroaltitude.h"
|
||||
#include "positionactual.h"
|
||||
#include "flightstatus.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "accels.h"
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 2
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle altitudeHoldTaskHandle;
|
||||
static xQueueHandle queue;
|
||||
static AltitudeHoldSettingsData altitudeHoldSettings;
|
||||
|
||||
// Private functions
|
||||
static void altitudeHoldTask(void *parameters);
|
||||
static void SettingsUpdatedCb(UAVObjEvent * ev);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeHoldStart()
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeHoldInitialize()
|
||||
{
|
||||
AltitudeHoldSettingsInitialize();
|
||||
AltitudeHoldDesiredInitialize();
|
||||
AltHoldSmoothedInitialize();
|
||||
|
||||
// Create object queue
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
|
||||
return 0;
|
||||
}
|
||||
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart)
|
||||
|
||||
float tau;
|
||||
float throttleIntegral;
|
||||
float velocity;
|
||||
float decay;
|
||||
float velocity_decay;
|
||||
bool running = false;
|
||||
float error;
|
||||
float switchThrottle;
|
||||
float smoothed_altitude;
|
||||
float starting_altitude;
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void altitudeHoldTask(void *parameters)
|
||||
{
|
||||
AltitudeHoldDesiredData altitudeHoldDesired;
|
||||
StabilizationDesiredData stabilizationDesired;
|
||||
|
||||
portTickType thisTime, lastUpdateTime;
|
||||
UAVObjEvent ev;
|
||||
|
||||
// Force update of the settings
|
||||
SettingsUpdatedCb(&ev);
|
||||
|
||||
// Listen for updates.
|
||||
AltitudeHoldDesiredConnectQueue(queue);
|
||||
BaroAltitudeConnectQueue(queue);
|
||||
FlightStatusConnectQueue(queue);
|
||||
AccelsConnectQueue(queue);
|
||||
|
||||
BaroAltitudeAltitudeGet(&smoothed_altitude);
|
||||
running = false;
|
||||
enum init_state {WAITING_BARO, WAITIING_INIT, INITED} init = WAITING_BARO;
|
||||
|
||||
// Main task loop
|
||||
bool baro_updated = false;
|
||||
while (1) {
|
||||
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
|
||||
if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE )
|
||||
{
|
||||
if(!running)
|
||||
throttleIntegral = 0;
|
||||
|
||||
// Todo: Add alarm if it should be running
|
||||
continue;
|
||||
} else if (ev.obj == BaroAltitudeHandle()) {
|
||||
baro_updated = true;
|
||||
|
||||
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
|
||||
} else if (ev.obj == FlightStatusHandle()) {
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
if(flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) {
|
||||
// Copy the current throttle as a starting point for integral
|
||||
StabilizationDesiredThrottleGet(&throttleIntegral);
|
||||
switchThrottle = throttleIntegral;
|
||||
error = 0;
|
||||
velocity = 0;
|
||||
running = true;
|
||||
|
||||
AltHoldSmoothedData altHold;
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
starting_altitude = altHold.Altitude;
|
||||
} else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
|
||||
running = false;
|
||||
} else if (ev.obj == AccelsHandle()) {
|
||||
static uint32_t timeval;
|
||||
|
||||
static float z[4] = {0, 0, 0, 0};
|
||||
float z_new[4];
|
||||
float P[4][4], K[4][2], x[2];
|
||||
float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7};
|
||||
static float V[4][4] = {{10.0f, 0, 0, 0}, {0, 100.0f, 0, 0}, {0, 0, 100.0f, 0}, {0, 0, 0, 1000.0f}};
|
||||
|
||||
float dT;
|
||||
static float S[2] = {1.0f,10.0f};
|
||||
|
||||
thisTime = xTaskGetTickCount();
|
||||
|
||||
/* Somehow this always assigns to zero. Compiler bug? Race condition? */
|
||||
S[0] = altitudeHoldSettings.PressureNoise;
|
||||
S[1] = altitudeHoldSettings.AccelNoise;
|
||||
G[2] = altitudeHoldSettings.AccelDrift;
|
||||
|
||||
AccelsData accels;
|
||||
AccelsGet(&accels);
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
BaroAltitudeData baro;
|
||||
BaroAltitudeGet(&baro);
|
||||
|
||||
if (init == WAITIING_INIT) {
|
||||
z[0] = baro.Altitude;
|
||||
z[1] = 0;
|
||||
z[2] = accels.z;
|
||||
z[3] = 0;
|
||||
init = INITED;
|
||||
} else if (init == WAITING_BARO)
|
||||
continue;
|
||||
|
||||
x[0] = baro.Altitude;
|
||||
//rotate avg accels into earth frame and store it
|
||||
if(1) {
|
||||
float q[4], Rbe[3][3];
|
||||
q[0]=attitudeActual.q1;
|
||||
q[1]=attitudeActual.q2;
|
||||
q[2]=attitudeActual.q3;
|
||||
q[3]=attitudeActual.q4;
|
||||
Quaternion2R(q, Rbe);
|
||||
x[1] = -(Rbe[0][2]*accels.x+ Rbe[1][2]*accels.y + Rbe[2][2]*accels.z + 9.81f);
|
||||
} else {
|
||||
x[1] = -accels.z + 9.81f;
|
||||
}
|
||||
|
||||
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
P[0][0] = dT*(V[0][1]+dT*V[1][1])+V[0][0]+G[0]+dT*V[1][0];
|
||||
P[0][1] = dT*(V[0][2]+dT*V[1][2])+V[0][1]+dT*V[1][1];
|
||||
P[0][2] = V[0][2]+dT*V[1][2];
|
||||
P[0][3] = V[0][3]+dT*V[1][3];
|
||||
P[1][0] = dT*(V[1][1]+dT*V[2][1])+V[1][0]+dT*V[2][0];
|
||||
P[1][1] = dT*(V[1][2]+dT*V[2][2])+V[1][1]+G[1]+dT*V[2][1];
|
||||
P[1][2] = V[1][2]+dT*V[2][2];
|
||||
P[1][3] = V[1][3]+dT*V[2][3];
|
||||
P[2][0] = V[2][0]+dT*V[2][1];
|
||||
P[2][1] = V[2][1]+dT*V[2][2];
|
||||
P[2][2] = V[2][2]+G[2];
|
||||
P[2][3] = V[2][3];
|
||||
P[3][0] = V[3][0]+dT*V[3][1];
|
||||
P[3][1] = V[3][1]+dT*V[3][2];
|
||||
P[3][2] = V[3][2];
|
||||
P[3][3] = V[3][3]+G[3];
|
||||
|
||||
if (baro_updated) {
|
||||
K[0][0] = -(V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])+1.0f;
|
||||
K[0][1] = ((V[0][2]+V[0][3])*S[0]+dT*(V[1][2]+V[1][3])*S[0])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
|
||||
K[1][0] = (V[1][0]*G[2]+V[1][0]*G[3]+V[1][0]*S[1]+V[1][0]*V[2][2]-V[2][0]*V[1][2]+V[1][0]*V[2][3]+V[1][0]*V[3][2]-V[2][0]*V[1][3]-V[1][2]*V[3][0]+V[1][0]*V[3][3]-V[3][0]*V[1][3]+(dT*dT)*V[2][1]*V[3][2]-(dT*dT)*V[2][2]*V[3][1]+(dT*dT)*V[2][1]*V[3][3]-(dT*dT)*V[3][1]*V[2][3]+dT*V[1][1]*G[2]+dT*V[2][0]*G[2]+dT*V[1][1]*G[3]+dT*V[2][0]*G[3]+dT*V[1][1]*S[1]+dT*V[2][0]*S[1]+(dT*dT)*V[2][1]*G[2]+(dT*dT)*V[2][1]*G[3]+(dT*dT)*V[2][1]*S[1]+dT*V[1][1]*V[2][2]-dT*V[1][2]*V[2][1]+dT*V[1][1]*V[2][3]+dT*V[1][1]*V[3][2]+dT*V[2][0]*V[3][2]-dT*V[1][2]*V[3][1]-dT*V[2][1]*V[1][3]-dT*V[3][0]*V[2][2]+dT*V[1][1]*V[3][3]+dT*V[2][0]*V[3][3]-dT*V[3][0]*V[2][3]-dT*V[1][3]*V[3][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
|
||||
K[1][1] = (V[1][2]*G[0]+V[1][3]*G[0]+V[1][2]*S[0]+V[1][3]*S[0]+V[0][0]*V[1][2]-V[1][0]*V[0][2]+V[0][0]*V[1][3]-V[1][0]*V[0][3]+(dT*dT)*V[0][1]*V[2][2]+(dT*dT)*V[1][0]*V[2][2]-(dT*dT)*V[0][2]*V[2][1]-(dT*dT)*V[2][0]*V[1][2]+(dT*dT)*V[0][1]*V[2][3]+(dT*dT)*V[1][0]*V[2][3]-(dT*dT)*V[2][0]*V[1][3]-(dT*dT)*V[0][3]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][2]-(dT*dT*dT)*V[1][2]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][3]-(dT*dT*dT)*V[2][1]*V[1][3]+dT*V[2][2]*G[0]+dT*V[2][3]*G[0]+dT*V[2][2]*S[0]+dT*V[2][3]*S[0]+dT*V[0][0]*V[2][2]+dT*V[0][1]*V[1][2]-dT*V[0][2]*V[1][1]-dT*V[0][2]*V[2][0]+dT*V[0][0]*V[2][3]+dT*V[0][1]*V[1][3]-dT*V[1][1]*V[0][3]-dT*V[2][0]*V[0][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
|
||||
K[2][0] = (V[2][0]*G[3]-V[3][0]*G[2]+V[2][0]*S[1]+V[2][0]*V[3][2]-V[3][0]*V[2][2]+V[2][0]*V[3][3]-V[3][0]*V[2][3]+dT*V[2][1]*G[3]-dT*V[3][1]*G[2]+dT*V[2][1]*S[1]+dT*V[2][1]*V[3][2]-dT*V[2][2]*V[3][1]+dT*V[2][1]*V[3][3]-dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
|
||||
K[2][1] = (V[0][0]*G[2]+V[2][2]*G[0]+V[2][3]*G[0]+V[2][2]*S[0]+V[2][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]-V[2][0]*V[0][3]+G[0]*G[2]+G[2]*S[0]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]-(dT*dT)*V[2][1]*V[1][3]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+(dT*dT)*V[1][1]*G[2]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[1][0]*V[2][3]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
|
||||
K[3][0] = (-V[2][0]*G[3]+V[3][0]*G[2]+V[3][0]*S[1]-V[2][0]*V[3][2]+V[3][0]*V[2][2]-V[2][0]*V[3][3]+V[3][0]*V[2][3]-dT*V[2][1]*G[3]+dT*V[3][1]*G[2]+dT*V[3][1]*S[1]-dT*V[2][1]*V[3][2]+dT*V[2][2]*V[3][1]-dT*V[2][1]*V[3][3]+dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
|
||||
K[3][1] = (V[0][0]*G[3]+V[3][2]*G[0]+V[3][3]*G[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[3][2]-V[0][2]*V[3][0]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[3]+G[3]*S[0]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+(dT*dT)*V[1][1]*G[3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
|
||||
|
||||
z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]-K[0][1]*(-x[1]+z[2]+z[3])+z[0];
|
||||
z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]-K[1][1]*(-x[1]+z[2]+z[3])+z[1];
|
||||
z_new[2] = -K[2][0]*(dT*z[1]-x[0]+z[0])-K[2][1]*(-x[1]+z[2]+z[3])+z[2];
|
||||
z_new[3] = -K[3][0]*(dT*z[1]-x[0]+z[0])-K[3][1]*(-x[1]+z[2]+z[3])+z[3];
|
||||
|
||||
memcpy(z, z_new, sizeof(z_new));
|
||||
|
||||
V[0][0] = -K[0][1]*P[2][0]-K[0][1]*P[3][0]-P[0][0]*(K[0][0]-1.0f);
|
||||
V[0][1] = -K[0][1]*P[2][1]-K[0][1]*P[3][2]-P[0][1]*(K[0][0]-1.0f);
|
||||
V[0][2] = -K[0][1]*P[2][2]-K[0][1]*P[3][2]-P[0][2]*(K[0][0]-1.0f);
|
||||
V[0][3] = -K[0][1]*P[2][3]-K[0][1]*P[3][3]-P[0][3]*(K[0][0]-1.0f);
|
||||
V[1][0] = P[1][0]-K[1][0]*P[0][0]-K[1][1]*P[2][0]-K[1][1]*P[3][0];
|
||||
V[1][1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1]-K[1][1]*P[3][2];
|
||||
V[1][2] = P[1][2]-K[1][0]*P[0][2]-K[1][1]*P[2][2]-K[1][1]*P[3][2];
|
||||
V[1][3] = P[1][3]-K[1][0]*P[0][3]-K[1][1]*P[2][3]-K[1][1]*P[3][3];
|
||||
V[2][0] = -K[2][0]*P[0][0]-K[2][1]*P[3][0]-P[2][0]*(K[2][1]-1.0f);
|
||||
V[2][1] = -K[2][0]*P[0][1]-K[2][1]*P[3][2]-P[2][1]*(K[2][1]-1.0f);
|
||||
V[2][2] = -K[2][0]*P[0][2]-K[2][1]*P[3][2]-P[2][2]*(K[2][1]-1.0f);
|
||||
V[2][3] = -K[2][0]*P[0][3]-K[2][1]*P[3][3]-P[2][3]*(K[2][1]-1.0f);
|
||||
V[3][0] = -K[3][0]*P[0][0]-K[3][1]*P[2][0]-P[3][0]*(K[3][1]-1.0f);
|
||||
V[3][1] = -K[3][0]*P[0][1]-K[3][1]*P[2][1]-P[3][2]*(K[3][1]-1.0f);
|
||||
V[3][2] = -K[3][0]*P[0][2]-K[3][1]*P[2][2]-P[3][2]*(K[3][1]-1.0f);
|
||||
V[3][3] = -K[3][0]*P[0][3]-K[3][1]*P[2][3]-P[3][3]*(K[3][1]-1.0f);
|
||||
|
||||
|
||||
baro_updated = false;
|
||||
} else {
|
||||
K[0][0] = (V[0][2]+V[0][3]+dT*V[1][2]+dT*V[1][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
|
||||
K[1][0] = (V[1][2]+V[1][3]+dT*V[2][2]+dT*V[2][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
|
||||
K[2][0] = (V[2][2]+V[2][3]+G[2])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
|
||||
K[3][0] = (V[3][2]+V[3][3]+G[3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
|
||||
|
||||
|
||||
z_new[0] = dT*z[1]-K[0][0]*(-x[1]+z[2]+z[3])+z[0];
|
||||
z_new[1] = dT*z[2]-K[1][0]*(-x[1]+z[2]+z[3])+z[1];
|
||||
z_new[2] = -K[2][0]*(-x[1]+z[2]+z[3])+z[2];
|
||||
z_new[3] = -K[3][0]*(-x[1]+z[2]+z[3])+z[3];
|
||||
|
||||
memcpy(z, z_new, sizeof(z_new));
|
||||
|
||||
V[0][0] = P[0][0]-K[0][0]*P[2][0]-K[0][0]*P[3][0];
|
||||
V[0][1] = P[0][1]-K[0][0]*P[2][1]-K[0][0]*P[3][2];
|
||||
V[0][2] = P[0][2]-K[0][0]*P[2][2]-K[0][0]*P[3][2];
|
||||
V[0][3] = P[0][3]-K[0][0]*P[2][3]-K[0][0]*P[3][3];
|
||||
V[1][0] = P[1][0]-K[1][0]*P[2][0]-K[1][0]*P[3][0];
|
||||
V[1][1] = P[1][1]-K[1][0]*P[2][1]-K[1][0]*P[3][2];
|
||||
V[1][2] = P[1][2]-K[1][0]*P[2][2]-K[1][0]*P[3][2];
|
||||
V[1][3] = P[1][3]-K[1][0]*P[2][3]-K[1][0]*P[3][3];
|
||||
V[2][0] = -K[2][0]*P[3][0]-P[2][0]*(K[2][0]-1.0f);
|
||||
V[2][1] = -K[2][0]*P[3][2]-P[2][1]*(K[2][0]-1.0f);
|
||||
V[2][2] = -K[2][0]*P[3][2]-P[2][2]*(K[2][0]-1.0f);
|
||||
V[2][3] = -K[2][0]*P[3][3]-P[2][3]*(K[2][0]-1.0f);
|
||||
V[3][0] = -K[3][0]*P[2][0]-P[3][0]*(K[3][0]-1.0f);
|
||||
V[3][1] = -K[3][0]*P[2][1]-P[3][2]*(K[3][0]-1.0f);
|
||||
V[3][2] = -K[3][0]*P[2][2]-P[3][2]*(K[3][0]-1.0f);
|
||||
V[3][3] = -K[3][0]*P[2][3]-P[3][3]*(K[3][0]-1.0f);
|
||||
}
|
||||
|
||||
AltHoldSmoothedData altHold;
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
altHold.Altitude = z[0];
|
||||
altHold.Velocity = z[1];
|
||||
altHold.Accel = z[2];
|
||||
AltHoldSmoothedSet(&altHold);
|
||||
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
|
||||
// Verify that we are in altitude hold mode
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
|
||||
running = false;
|
||||
}
|
||||
|
||||
if (!running)
|
||||
continue;
|
||||
|
||||
// Compute the altitude error
|
||||
error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
|
||||
|
||||
// Compute integral off altitude error
|
||||
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
|
||||
|
||||
// Only update stabilizationDesired less frequently
|
||||
if((thisTime - lastUpdateTime) < 20)
|
||||
continue;
|
||||
|
||||
lastUpdateTime = thisTime;
|
||||
|
||||
// Instead of explicit limit on integral you output limit feedback
|
||||
StabilizationDesiredGet(&stabilizationDesired);
|
||||
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
|
||||
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
|
||||
if(stabilizationDesired.Throttle > 1) {
|
||||
throttleIntegral -= (stabilizationDesired.Throttle - 1);
|
||||
stabilizationDesired.Throttle = 1;
|
||||
}
|
||||
else if (stabilizationDesired.Throttle < 0) {
|
||||
throttleIntegral -= stabilizationDesired.Throttle;
|
||||
stabilizationDesired.Throttle = 0;
|
||||
}
|
||||
|
||||
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
|
||||
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
|
||||
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
|
||||
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
|
||||
StabilizationDesiredSet(&stabilizationDesired);
|
||||
|
||||
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
|
||||
AltitudeHoldDesiredGet(&altitudeHoldDesired);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
AltitudeHoldSettingsGet(&altitudeHoldSettings);
|
||||
}
|
||||
@ -1,9 +1,10 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file watchdog.h
|
||||
* @file examplemodperiodic.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief RF Module hardware layer
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
@ -22,11 +23,9 @@
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef EXAMPLEMODPERIODIC_H
|
||||
#define EXAMPLEMODPERIODIC_H
|
||||
|
||||
#ifndef _WATCHDOG_H_
|
||||
#define _WATCHDOG_H_
|
||||
|
||||
uint16_t watchdog_Init(uint16_t delayMs);
|
||||
void watchdog_Clear(void);
|
||||
|
||||
#endif
|
||||
int32_t ExampleModPeriodicInitialize();
|
||||
int32_t GuidanceInitialize(void);
|
||||
#endif // EXAMPLEMODPERIODIC_H
|
||||
@ -50,19 +50,21 @@
|
||||
|
||||
#include "pios.h"
|
||||
#include "attitude.h"
|
||||
#include "attituderaw.h"
|
||||
#include "gyros.h"
|
||||
#include "accels.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudesettings.h"
|
||||
#include "flightstatus.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "CoordinateConversions.h"
|
||||
#include "pios_flash_w25x.h"
|
||||
|
||||
#include <pios_board_info.h>
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 540
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
|
||||
#define UPDATE_RATE 2.0f
|
||||
#define SENSOR_PERIOD 4
|
||||
#define UPDATE_RATE 25.0f
|
||||
#define GYRO_NEUTRAL 1665
|
||||
|
||||
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
|
||||
@ -77,8 +79,9 @@ static void AttitudeTask(void *parameters);
|
||||
static float gyro_correct_int[3] = {0,0,0};
|
||||
static xQueueHandle gyro_queue;
|
||||
|
||||
static int8_t updateSensors(AttitudeRawData *);
|
||||
static void updateAttitude(AttitudeRawData *);
|
||||
static int32_t updateSensors(AccelsData *, GyrosData *);
|
||||
static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData);
|
||||
static void updateAttitude(AccelsData *, GyrosData *);
|
||||
static void settingsUpdatedCb(UAVObjEvent * objEv);
|
||||
|
||||
static float accelKi = 0;
|
||||
@ -124,8 +127,9 @@ int32_t AttitudeStart(void)
|
||||
int32_t AttitudeInitialize(void)
|
||||
{
|
||||
AttitudeActualInitialize();
|
||||
AttitudeRawInitialize();
|
||||
AttitudeSettingsInitialize();
|
||||
AccelsInitialize();
|
||||
GyrosInitialize();
|
||||
|
||||
// Initialize quaternion
|
||||
AttitudeActualData attitude;
|
||||
@ -151,14 +155,6 @@ int32_t AttitudeInitialize(void)
|
||||
|
||||
trim_requested = false;
|
||||
|
||||
// Create queue for passing gyro data, allow 2 back samples in case
|
||||
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
|
||||
if(gyro_queue == NULL)
|
||||
return -1;
|
||||
|
||||
|
||||
PIOS_ADC_SetQueue(gyro_queue);
|
||||
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
return 0;
|
||||
@ -169,23 +165,42 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
|
||||
int32_t accel_test;
|
||||
int32_t gyro_test;
|
||||
static void AttitudeTask(void *parameters)
|
||||
{
|
||||
uint8_t init = 0;
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
|
||||
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
|
||||
|
||||
// Keep flash CS pin high while talking accel
|
||||
PIOS_FLASH_DISABLE;
|
||||
PIOS_ADXL345_Init();
|
||||
|
||||
// Set critical error and wait until the accel is producing data
|
||||
while(PIOS_ADXL345_FifoElements() == 0) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
|
||||
}
|
||||
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
bool cc3d = bdinfo->board_rev == 0x02;
|
||||
|
||||
if(cc3d) {
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
gyro_test = PIOS_MPU6000_Test();
|
||||
#endif
|
||||
} else {
|
||||
#if defined(PIOS_INCLUDE_ADXL345)
|
||||
accel_test = PIOS_ADXL345_Test();
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
// Create queue for passing gyro data, allow 2 back samples in case
|
||||
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
|
||||
PIOS_Assert(gyro_queue != NULL);
|
||||
PIOS_ADC_SetQueue(gyro_queue);
|
||||
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
|
||||
#endif
|
||||
|
||||
}
|
||||
// Force settings update to make sure rotation loaded
|
||||
settingsUpdatedCb(AttitudeSettingsHandle());
|
||||
|
||||
@ -216,27 +231,37 @@ static void AttitudeTask(void *parameters)
|
||||
}
|
||||
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
|
||||
|
||||
AttitudeRawData attitudeRaw;
|
||||
AttitudeRawGet(&attitudeRaw);
|
||||
if(updateSensors(&attitudeRaw) != 0)
|
||||
|
||||
AccelsData accels;
|
||||
GyrosData gyros;
|
||||
int32_t retval = 0;
|
||||
|
||||
if (cc3d)
|
||||
retval = updateSensorsCC3D(&accels, &gyros);
|
||||
else
|
||||
retval = updateSensors(&accels, &gyros);
|
||||
|
||||
// Only update attitude when sensor data is good
|
||||
if (retval != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
else {
|
||||
// Only update attitude when sensor data is good
|
||||
updateAttitude(&attitudeRaw);
|
||||
AttitudeRawSet(&attitudeRaw);
|
||||
// Do not update attitude data in simulation mode
|
||||
if (!AttitudeActualReadOnly())
|
||||
updateAttitude(&accels, &gyros);
|
||||
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
float gyros_passed[3];
|
||||
|
||||
/**
|
||||
* Get an update from the sensors
|
||||
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
|
||||
* @return 0 if successfull, -1 if not
|
||||
*/
|
||||
static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
|
||||
{
|
||||
struct pios_adxl345_data accel_data;
|
||||
float gyro[4];
|
||||
@ -246,15 +271,19 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
// Do not read raw sensor data in simulation mode
|
||||
if (GyrosReadOnly() || AccelsReadOnly())
|
||||
return 0;
|
||||
|
||||
// No accel data available
|
||||
if(PIOS_ADXL345_FifoElements() == 0)
|
||||
return -1;
|
||||
|
||||
// First sample is temperature
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
|
||||
gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
|
||||
gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
|
||||
gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
|
||||
|
||||
int32_t x = 0;
|
||||
int32_t y = 0;
|
||||
@ -268,26 +297,25 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
y += -accel_data.y;
|
||||
z += -accel_data.z;
|
||||
} while ( (i < 32) && (samples_remaining > 0) );
|
||||
attitudeRaw->gyrotemp[0] = samples_remaining;
|
||||
attitudeRaw->gyrotemp[1] = i;
|
||||
|
||||
gyros->temperature = samples_remaining;
|
||||
|
||||
float accel[3] = {(float) x / i, (float) y / i, (float) z / i};
|
||||
|
||||
if(rotate) {
|
||||
// TODO: rotate sensors too so stabilization is well behaved
|
||||
float vec_out[3];
|
||||
rot_mult(R, accel, vec_out);
|
||||
attitudeRaw->accels[0] = vec_out[0];
|
||||
attitudeRaw->accels[1] = vec_out[1];
|
||||
attitudeRaw->accels[2] = vec_out[2];
|
||||
rot_mult(R, attitudeRaw->gyros, vec_out);
|
||||
attitudeRaw->gyros[0] = vec_out[0];
|
||||
attitudeRaw->gyros[1] = vec_out[1];
|
||||
attitudeRaw->gyros[2] = vec_out[2];
|
||||
accels->x = vec_out[0];
|
||||
accels->y = vec_out[1];
|
||||
accels->z = vec_out[2];
|
||||
rot_mult(R, &gyros->x, vec_out);
|
||||
gyros->x = vec_out[0];
|
||||
gyros->y = vec_out[1];
|
||||
gyros->z = vec_out[2];
|
||||
} else {
|
||||
attitudeRaw->accels[0] = accel[0];
|
||||
attitudeRaw->accels[1] = accel[1];
|
||||
attitudeRaw->accels[2] = accel[2];
|
||||
accels->x = accel[0];
|
||||
accels->y = accel[1];
|
||||
accels->z = accel[2];
|
||||
}
|
||||
|
||||
if (trim_requested) {
|
||||
@ -301,33 +329,100 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
|
||||
trim_samples++;
|
||||
// Store the digitally scaled version since that is what we use for bias
|
||||
trim_accels[0] += attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
|
||||
trim_accels[1] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Y];
|
||||
trim_accels[2] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Z];
|
||||
trim_accels[0] += accels->x;
|
||||
trim_accels[1] += accels->y;
|
||||
trim_accels[2] += accels->z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Scale accels and correct bias
|
||||
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * ACCEL_SCALE;
|
||||
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * ACCEL_SCALE;
|
||||
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * ACCEL_SCALE;
|
||||
accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE;
|
||||
accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE;
|
||||
accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE;
|
||||
|
||||
if(bias_correct_gyro) {
|
||||
// Applying integral component here so it can be seen on the gyros and correct bias
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
|
||||
gyros->x += gyro_correct_int[0];
|
||||
gyros->y += gyro_correct_int[1];
|
||||
gyros->z += gyro_correct_int[2];
|
||||
}
|
||||
|
||||
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
|
||||
// and make it average zero (weakly)
|
||||
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
|
||||
|
||||
gyro_correct_int[2] += - gyros->z * yawBiasRate;
|
||||
|
||||
GyrosSet(gyros);
|
||||
AccelsSet(accels);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
/**
|
||||
* Get an update from the sensors
|
||||
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
|
||||
* @return 0 if successfull, -1 if not
|
||||
*/
|
||||
struct pios_mpu6000_data mpu6000_data;
|
||||
static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
{
|
||||
float accels[3], gyros[3];
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
|
||||
xQueueHandle queue = PIOS_MPU6000_GetQueue();
|
||||
|
||||
if(xQueueReceive(queue, (void *) &mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY)
|
||||
return -1; // Error, no data
|
||||
|
||||
gyros[0] = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
|
||||
gyros[1] = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
|
||||
gyros[2] = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
|
||||
|
||||
accels[0] = -mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
|
||||
accels[1] = -mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
|
||||
accels[2] = -mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
|
||||
|
||||
gyrosData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
|
||||
accelsData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
|
||||
#endif
|
||||
|
||||
if(rotate) {
|
||||
// TODO: rotate sensors too so stabilization is well behaved
|
||||
float vec_out[3];
|
||||
rot_mult(R, accels, vec_out);
|
||||
accels[0] = vec_out[0];
|
||||
accels[1] = vec_out[1];
|
||||
accels[2] = vec_out[2];
|
||||
rot_mult(R, gyros, vec_out);
|
||||
gyros[0] = vec_out[0];
|
||||
gyros[1] = vec_out[1];
|
||||
gyros[2] = vec_out[2];
|
||||
}
|
||||
|
||||
accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
|
||||
accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
|
||||
accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
|
||||
AccelsSet(&accelsData);
|
||||
|
||||
gyrosData->x = gyros[0];
|
||||
gyrosData->y = gyros[1];
|
||||
gyrosData->z = gyros[2];
|
||||
|
||||
if(bias_correct_gyro) {
|
||||
// Applying integral component here so it can be seen on the gyros and correct bias
|
||||
gyrosData->x += gyro_correct_int[0];
|
||||
gyrosData->y += gyro_correct_int[1];
|
||||
gyrosData->z += gyro_correct_int[2];
|
||||
}
|
||||
|
||||
GyrosSet(gyrosData);
|
||||
AccelsSet(accelsData);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
{
|
||||
float dT;
|
||||
portTickType thisSysTime = xTaskGetTickCount();
|
||||
@ -337,48 +432,46 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
// Bad practice to assume structure order, but saves memory
|
||||
float gyro[3];
|
||||
gyro[0] = attitudeRaw->gyros[0];
|
||||
gyro[1] = attitudeRaw->gyros[1];
|
||||
gyro[2] = attitudeRaw->gyros[2];
|
||||
float * gyros = &gyrosData->x;
|
||||
float * accels = &accelsData->x;
|
||||
|
||||
{
|
||||
float * accels = attitudeRaw->accels;
|
||||
float grot[3];
|
||||
float accel_err[3];
|
||||
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
|
||||
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
|
||||
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
|
||||
CrossProduct((const float *) accels, (const float *) grot, accel_err);
|
||||
|
||||
// Account for accel magnitude
|
||||
float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
|
||||
accel_err[0] /= accel_mag;
|
||||
accel_err[1] /= accel_mag;
|
||||
accel_err[2] /= accel_mag;
|
||||
|
||||
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
||||
gyro_correct_int[0] += accel_err[0] * accelKi;
|
||||
gyro_correct_int[1] += accel_err[1] * accelKi;
|
||||
|
||||
//gyro_correct_int[2] += accel_err[2] * accelKi;
|
||||
|
||||
// Correct rates based on error, integral component dealt with in updateSensors
|
||||
gyro[0] += accel_err[0] * accelKp / dT;
|
||||
gyro[1] += accel_err[1] * accelKp / dT;
|
||||
gyro[2] += accel_err[2] * accelKp / dT;
|
||||
}
|
||||
float grot[3];
|
||||
float accel_err[3];
|
||||
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
|
||||
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
|
||||
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
|
||||
CrossProduct((const float *) accels, (const float *) grot, accel_err);
|
||||
|
||||
// Account for accel magnitude
|
||||
float accel_mag = sqrtf(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
|
||||
if(accel_mag < 1.0e-3f)
|
||||
return;
|
||||
|
||||
accel_err[0] /= accel_mag;
|
||||
accel_err[1] /= accel_mag;
|
||||
accel_err[2] /= accel_mag;
|
||||
|
||||
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
||||
gyro_correct_int[0] += accel_err[0] * accelKi;
|
||||
gyro_correct_int[1] += accel_err[1] * accelKi;
|
||||
|
||||
//gyro_correct_int[2] += accel_err[2] * accelKi;
|
||||
|
||||
// Correct rates based on error, integral component dealt with in updateSensors
|
||||
gyros[0] += accel_err[0] * accelKp / dT;
|
||||
gyros[1] += accel_err[1] * accelKp / dT;
|
||||
gyros[2] += accel_err[2] * accelKp / dT;
|
||||
|
||||
{ // scoping variables to save memory
|
||||
// Work out time derivative from INSAlgo writeup
|
||||
// Also accounts for the fact that gyros are in deg/s
|
||||
float qdot[4];
|
||||
qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
|
||||
// Take a time step
|
||||
q[0] = q[0] + qdot[0];
|
||||
@ -395,7 +488,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
}
|
||||
|
||||
// Renomalize
|
||||
float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
q[0] = q[0] / qmag;
|
||||
q[1] = q[1] / qmag;
|
||||
q[2] = q[2] / qmag;
|
||||
|
||||
629
flight/Modules/Attitude/revolution/attitude.c
Normal file
629
flight/Modules/Attitude/revolution/attitude.c
Normal file
@ -0,0 +1,629 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup Attitude Copter Control Attitude Estimation
|
||||
* @brief Acquires sensor data and computes attitude estimate
|
||||
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
|
||||
* @{
|
||||
*
|
||||
* @file attitude.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
******************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input objects: None, takes sensor data via pios
|
||||
* Output objects: @ref AttitudeRaw @ref AttitudeActual
|
||||
*
|
||||
* This module computes an attitude estimate from the sensor data
|
||||
*
|
||||
* The module executes in its own thread.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
#include "attitude.h"
|
||||
#include "magnetometer.h"
|
||||
#include "accels.h"
|
||||
#include "gyros.h"
|
||||
#include "gyrosbias.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudesettings.h"
|
||||
#include "positionactual.h"
|
||||
#include "velocityactual.h"
|
||||
#include "gpsposition.h"
|
||||
#include "baroaltitude.h"
|
||||
#include "flightstatus.h"
|
||||
#include "homelocation.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 5540
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
#define FAILSAFE_TIMEOUT_MS 10
|
||||
|
||||
#define F_PI 3.14159265358979323846f
|
||||
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle attitudeTaskHandle;
|
||||
|
||||
static xQueueHandle gyroQueue;
|
||||
static xQueueHandle accelQueue;
|
||||
static xQueueHandle magQueue;
|
||||
static xQueueHandle baroQueue;
|
||||
static xQueueHandle gpsQueue;
|
||||
const uint32_t SENSOR_QUEUE_SIZE = 10;
|
||||
|
||||
// Private functions
|
||||
static void AttitudeTask(void *parameters);
|
||||
|
||||
static int32_t updateAttitudeComplimentary(bool first_run);
|
||||
static int32_t updateAttitudeINSGPS(bool first_run);
|
||||
static void settingsUpdatedCb(UAVObjEvent * objEv);
|
||||
|
||||
static float accelKi = 0;
|
||||
static float accelKp = 0;
|
||||
static float yawBiasRate = 0;
|
||||
static float gyroGain = 0.42;
|
||||
static int16_t accelbias[3];
|
||||
static float R[3][3];
|
||||
static int8_t rotate = 0;
|
||||
static bool zero_during_arming = false;
|
||||
|
||||
|
||||
/**
|
||||
* API for sensor fusion algorithms:
|
||||
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
|
||||
* Stores all the queues the algorithm will pull data from
|
||||
* FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
|
||||
* Update() -- queries queues and updates the attitude estiamte
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* Initialise the module. Called before the start function
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AttitudeInitialize(void)
|
||||
{
|
||||
AttitudeActualInitialize();
|
||||
AttitudeSettingsInitialize();
|
||||
PositionActualInitialize();
|
||||
VelocityActualInitialize();
|
||||
|
||||
// Initialize this here while we aren't setting the homelocation in GPS
|
||||
HomeLocationInitialize();
|
||||
|
||||
// Initialize quaternion
|
||||
AttitudeActualData attitude;
|
||||
AttitudeActualGet(&attitude);
|
||||
attitude.q1 = 1;
|
||||
attitude.q2 = 0;
|
||||
attitude.q3 = 0;
|
||||
attitude.q4 = 0;
|
||||
AttitudeActualSet(&attitude);
|
||||
|
||||
// Cannot trust the values to init right above if BL runs
|
||||
GyrosBiasData gyrosBias;
|
||||
GyrosBiasGet(&gyrosBias);
|
||||
gyrosBias.x = 0;
|
||||
gyrosBias.y = 0;
|
||||
gyrosBias.z = 0;
|
||||
GyrosBiasSet(&gyrosBias);
|
||||
|
||||
for(uint8_t i = 0; i < 3; i++)
|
||||
for(uint8_t j = 0; j < 3; j++)
|
||||
R[i][j] = 0;
|
||||
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Start the task. Expects all objects to be initialized by this point.
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AttitudeStart(void)
|
||||
{
|
||||
// Create the queues for the sensors
|
||||
gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
||||
accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
||||
magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
||||
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
||||
gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
|
||||
|
||||
GyrosConnectQueue(gyroQueue);
|
||||
AccelsConnectQueue(accelQueue);
|
||||
MagnetometerConnectQueue(magQueue);
|
||||
BaroAltitudeConnectQueue(baroQueue);
|
||||
GPSPositionConnectQueue(gpsQueue);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void AttitudeTask(void *parameters)
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
|
||||
// Force settings update to make sure rotation loaded
|
||||
settingsUpdatedCb(AttitudeSettingsHandle());
|
||||
|
||||
bool first_run = true;
|
||||
|
||||
// Wait for all the sensors be to read
|
||||
vTaskDelay(100);
|
||||
|
||||
// Main task loop
|
||||
while (1) {
|
||||
|
||||
// This function blocks on data queue
|
||||
if(1)
|
||||
updateAttitudeComplimentary(first_run);
|
||||
else
|
||||
updateAttitudeINSGPS(first_run);
|
||||
|
||||
if (first_run)
|
||||
first_run = false;
|
||||
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
|
||||
}
|
||||
}
|
||||
|
||||
float accel_mag;
|
||||
float qmag;
|
||||
float attitudeDt;
|
||||
float mag_err[3];
|
||||
float magKi = 0.000001f;
|
||||
float magKp = 0.0001f;
|
||||
|
||||
static int32_t updateAttitudeComplimentary(bool first_run)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
GyrosData gyrosData;
|
||||
AccelsData accelsData;
|
||||
static int32_t timeval;
|
||||
float dT;
|
||||
static uint8_t init = 0;
|
||||
|
||||
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
|
||||
if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
|
||||
return -1;
|
||||
}
|
||||
if ( xQueueReceive(accelQueue, &ev, 0) != pdTRUE )
|
||||
{
|
||||
// When one of these is updated so should the other
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// During initialization and
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if(first_run)
|
||||
init = 0;
|
||||
|
||||
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
|
||||
// For first 7 seconds use accels to get gyro bias
|
||||
accelKp = 1;
|
||||
accelKi = 0.9;
|
||||
yawBiasRate = 0.23;
|
||||
} else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
|
||||
accelKp = 1;
|
||||
accelKi = 0.9;
|
||||
yawBiasRate = 0.23;
|
||||
init = 0;
|
||||
} else if (init == 0) {
|
||||
// Reload settings (all the rates)
|
||||
AttitudeSettingsAccelKiGet(&accelKi);
|
||||
AttitudeSettingsAccelKpGet(&accelKp);
|
||||
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
|
||||
init = 1;
|
||||
}
|
||||
|
||||
GyrosGet(&gyrosData);
|
||||
AccelsGet(&accelsData);
|
||||
|
||||
// Compute the dT using the cpu clock
|
||||
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
float q[4];
|
||||
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
|
||||
float grot[3];
|
||||
float accel_err[3];
|
||||
|
||||
// Get the current attitude estimate
|
||||
quat_copy(&attitudeActual.q1, q);
|
||||
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
|
||||
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
|
||||
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
|
||||
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
|
||||
|
||||
// Account for accel magnitude
|
||||
accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
|
||||
accel_mag = sqrtf(accel_mag);
|
||||
accel_err[0] /= accel_mag;
|
||||
accel_err[1] /= accel_mag;
|
||||
accel_err[2] /= accel_mag;
|
||||
|
||||
if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
|
||||
{
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
float brot[3];
|
||||
float Rbe[3][3];
|
||||
MagnetometerData mag;
|
||||
HomeLocationData home;
|
||||
|
||||
Quaternion2R(q, Rbe);
|
||||
MagnetometerGet(&mag);
|
||||
HomeLocationGet(&home);
|
||||
rot_mult(Rbe, home.Be, brot);
|
||||
|
||||
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
|
||||
mag.x /= mag_len;
|
||||
mag.y /= mag_len;
|
||||
mag.z /= mag_len;
|
||||
|
||||
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
|
||||
brot[0] /= bmag;
|
||||
brot[1] /= bmag;
|
||||
brot[2] /= bmag;
|
||||
|
||||
// Only compute if neither vector is null
|
||||
if (bmag < 1 || mag_len < 1)
|
||||
mag_err[0] = mag_err[1] = mag_err[2] = 0;
|
||||
else
|
||||
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
|
||||
} else {
|
||||
mag_err[0] = mag_err[1] = mag_err[2] = 0;
|
||||
}
|
||||
|
||||
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
||||
GyrosBiasData gyrosBias;
|
||||
GyrosBiasGet(&gyrosBias);
|
||||
gyrosBias.x += accel_err[0] * accelKi;
|
||||
gyrosBias.y += accel_err[1] * accelKi;
|
||||
gyrosBias.z += mag_err[2] * magKi;
|
||||
GyrosBiasSet(&gyrosBias);
|
||||
|
||||
// Correct rates based on error, integral component dealt with in updateSensors
|
||||
gyrosData.x += accel_err[0] * accelKp / dT;
|
||||
gyrosData.y += accel_err[1] * accelKp / dT;
|
||||
gyrosData.z += accel_err[2] * accelKp / dT + mag_err[2] * magKp / dT;
|
||||
|
||||
// Work out time derivative from INSAlgo writeup
|
||||
// Also accounts for the fact that gyros are in deg/s
|
||||
float qdot[4];
|
||||
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
|
||||
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
|
||||
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
|
||||
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
|
||||
|
||||
// Take a time step
|
||||
q[0] = q[0] + qdot[0];
|
||||
q[1] = q[1] + qdot[1];
|
||||
q[2] = q[2] + qdot[2];
|
||||
q[3] = q[3] + qdot[3];
|
||||
|
||||
if(q[0] < 0) {
|
||||
q[0] = -q[0];
|
||||
q[1] = -q[1];
|
||||
q[2] = -q[2];
|
||||
q[3] = -q[3];
|
||||
}
|
||||
|
||||
// Renomalize
|
||||
qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
q[0] = q[0] / qmag;
|
||||
q[1] = q[1] / qmag;
|
||||
q[2] = q[2] / qmag;
|
||||
q[3] = q[3] / qmag;
|
||||
|
||||
// If quaternion has become inappropriately short or is nan reinit.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 0;
|
||||
q[3] = 0;
|
||||
}
|
||||
|
||||
quat_copy(q, &attitudeActual.q1);
|
||||
|
||||
// Convert into eueler degrees (makes assumptions about RPY order)
|
||||
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
|
||||
|
||||
AttitudeActualSet(&attitudeActual);
|
||||
|
||||
// Flush these queues for avoid errors
|
||||
if ( xQueueReceive(baroQueue, &ev, 0) != pdTRUE )
|
||||
{
|
||||
}
|
||||
if ( xQueueReceive(gpsQueue, &ev, 0) != pdTRUE )
|
||||
{
|
||||
}
|
||||
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#include "insgps.h"
|
||||
int32_t ins_failed = 0;
|
||||
extern struct NavStruct Nav;
|
||||
static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
GyrosData gyrosData;
|
||||
AccelsData accelsData;
|
||||
MagnetometerData magData;
|
||||
BaroAltitudeData baroData;
|
||||
|
||||
static uint32_t ins_last_time = 0;
|
||||
|
||||
static bool inited;
|
||||
if (first_run)
|
||||
inited = false;
|
||||
|
||||
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
|
||||
if ( (xQueueReceive(gyroQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) ||
|
||||
(xQueueReceive(accelQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Get most recent data
|
||||
// TODO: Acquire all data in a queue
|
||||
GyrosGet(&gyrosData);
|
||||
AccelsGet(&accelsData);
|
||||
MagnetometerGet(&magData);
|
||||
BaroAltitudeGet(&baroData);
|
||||
|
||||
bool mag_updated;
|
||||
bool baro_updated;
|
||||
bool gps_updated;
|
||||
|
||||
if (inited) {
|
||||
mag_updated = 0;
|
||||
baro_updated = 0;
|
||||
}
|
||||
|
||||
mag_updated |= xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
|
||||
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
|
||||
gps_updated |= xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
|
||||
|
||||
if (!inited && (!mag_updated || !baro_updated || !gps_updated)) {
|
||||
// Don't initialize until all sensors are read
|
||||
return -1;
|
||||
} else if (!inited ) {
|
||||
inited = true;
|
||||
|
||||
float Rbe[3][3], q[4];
|
||||
float ge[3]={0.0f,0.0f,-9.81f};
|
||||
float zeros[3]={0.0f,0.0f,0.0f};
|
||||
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
|
||||
float vel[3], NED[3];
|
||||
|
||||
INSGPSInit();
|
||||
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
GPSPositionData gpsPosition;
|
||||
GPSPositionGet(&gpsPosition);
|
||||
|
||||
vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
|
||||
vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
|
||||
vel[2] = 0;
|
||||
|
||||
// convert from cm back to meters
|
||||
float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
|
||||
// put in local NED frame
|
||||
float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
|
||||
|
||||
RotFrom2Vectors(&accelsData.x, ge, &magData.x, home.Be, Rbe);
|
||||
R2Quaternion(Rbe,q);
|
||||
INSSetState(NED, vel, q, &gyrosData.x, zeros);
|
||||
INSSetGyroBias(&gyrosData.x);
|
||||
INSResetP(Pdiag);
|
||||
|
||||
ins_last_time = PIOS_DELAY_GetRaw();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Perform the update
|
||||
uint16_t sensors = 0;
|
||||
float dT;
|
||||
|
||||
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
|
||||
ins_last_time = PIOS_DELAY_GetRaw();
|
||||
|
||||
// This should only happen at start up or at mode switches
|
||||
if(dT > 0.01f)
|
||||
dT = 0.01f;
|
||||
else if(dT <= 0.001f)
|
||||
dT = 0.001f;
|
||||
|
||||
|
||||
GyrosBiasData gyrosBias;
|
||||
GyrosBiasGet(&gyrosBias);
|
||||
|
||||
float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f,
|
||||
(gyrosData.y + gyrosBias.y) * F_PI / 180.0f,
|
||||
(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
|
||||
|
||||
// Advance the state estimate
|
||||
INSStatePrediction(gyros, &accelsData.x, dT);
|
||||
|
||||
// Copy the attitude into the UAVO
|
||||
AttitudeActualData attitude;
|
||||
AttitudeActualGet(&attitude);
|
||||
attitude.q1 = Nav.q[0];
|
||||
attitude.q2 = Nav.q[1];
|
||||
attitude.q3 = Nav.q[2];
|
||||
attitude.q4 = Nav.q[3];
|
||||
Quaternion2RPY(&attitude.q1,&attitude.Roll);
|
||||
AttitudeActualSet(&attitude);
|
||||
|
||||
// Copy the gyro bias into the UAVO
|
||||
gyrosBias.x = Nav.gyro_bias[0];
|
||||
gyrosBias.y = Nav.gyro_bias[1];
|
||||
gyrosBias.z = Nav.gyro_bias[2];
|
||||
GyrosBiasSet(&gyrosBias);
|
||||
|
||||
// Advance the covariance estimate
|
||||
INSCovariancePrediction(dT);
|
||||
|
||||
if(mag_updated)
|
||||
sensors |= MAG_SENSORS;
|
||||
if(baro_updated)
|
||||
sensors |= BARO_SENSOR;
|
||||
|
||||
float NED[3] = {0,0,0};
|
||||
float vel[3] = {0,0,0};
|
||||
|
||||
if(gps_updated)
|
||||
{
|
||||
sensors = HORIZ_SENSORS | VERT_SENSORS;
|
||||
GPSPositionData gpsPosition;
|
||||
GPSPositionGet(&gpsPosition);
|
||||
|
||||
vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
|
||||
vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
|
||||
vel[2] = 0;
|
||||
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
// convert from cm back to meters
|
||||
float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
|
||||
// put in local NED frame
|
||||
float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
|
||||
* although probably should occur within INS itself
|
||||
*/
|
||||
INSCorrection(&magData.x, NED, vel, baroData.Altitude, sensors);
|
||||
|
||||
// Copy the position and velocity into the UAVO
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
positionActual.North = Nav.Pos[0];
|
||||
positionActual.East = Nav.Pos[1];
|
||||
positionActual.Down = Nav.Pos[2];
|
||||
PositionActualSet(&positionActual);
|
||||
|
||||
VelocityActualData velocityActual;
|
||||
VelocityActualGet(&velocityActual);
|
||||
velocityActual.North = Nav.Vel[0];
|
||||
velocityActual.East = Nav.Vel[1];
|
||||
velocityActual.Down = Nav.Vel[2];
|
||||
VelocityActualSet(&velocityActual);
|
||||
|
||||
|
||||
if(fabs(Nav.gyro_bias[0]) > 0.1f || fabs(Nav.gyro_bias[1]) > 0.1f || fabs(Nav.gyro_bias[2]) > 0.1f) {
|
||||
float zeros[3] = {0.0f,0.0f,0.0f};
|
||||
INSSetGyroBias(zeros);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void settingsUpdatedCb(UAVObjEvent * objEv)
|
||||
{
|
||||
AttitudeSettingsData attitudeSettings;
|
||||
AttitudeSettingsGet(&attitudeSettings);
|
||||
|
||||
|
||||
accelKp = attitudeSettings.AccelKp;
|
||||
accelKi = attitudeSettings.AccelKi;
|
||||
yawBiasRate = attitudeSettings.YawBiasRate;
|
||||
gyroGain = attitudeSettings.GyroGain;
|
||||
|
||||
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
|
||||
|
||||
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
|
||||
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
|
||||
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
|
||||
|
||||
GyrosBiasData gyrosBias;
|
||||
GyrosBiasGet(&gyrosBias);
|
||||
gyrosBias.x = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
|
||||
gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
|
||||
gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
|
||||
GyrosBiasSet(&gyrosBias);
|
||||
|
||||
// Indicates not to expend cycles on rotation
|
||||
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
|
||||
attitudeSettings.BoardRotation[2] == 0) {
|
||||
rotate = 0;
|
||||
|
||||
// Shouldn't be used but to be safe
|
||||
float rotationQuat[4] = {1,0,0,0};
|
||||
Quaternion2R(rotationQuat, R);
|
||||
} else {
|
||||
float rotationQuat[4];
|
||||
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
|
||||
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
|
||||
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
|
||||
RPY2Quaternion(rpy, rotationQuat);
|
||||
Quaternion2R(rotationQuat, R);
|
||||
rotate = 1;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -1,37 +1,37 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file test_common.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up ans runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
|
||||
/**
|
||||
* Called by the RTOS when a stack overflow is detected.
|
||||
*/
|
||||
void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed portCHAR *pcTaskName )
|
||||
{
|
||||
PIOS_DEBUG_Panic("STACK OVERFLOW");
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup Attitude Attitude Module
|
||||
* @{
|
||||
*
|
||||
* @file attitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief Acquires sensor data and fuses it into attitude estimate for CC
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ATTITUDE_H
|
||||
#define ATTITUDE_H
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
int32_t AttitudeInitialize(void);
|
||||
|
||||
#endif // ATTITUDE_H
|
||||
@ -66,8 +66,6 @@ static portTickType lastResetSysTime;
|
||||
// Private functions
|
||||
static void FirmwareIAPCallback(UAVObjEvent* ev);
|
||||
|
||||
FirmwareIAPObjData data;
|
||||
|
||||
static uint32_t get_time(void);
|
||||
|
||||
// Private types
|
||||
@ -96,6 +94,9 @@ int32_t FirmwareIAPInitialize()
|
||||
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
FirmwareIAPObjData data;
|
||||
FirmwareIAPObjGet(&data);
|
||||
|
||||
data.BoardType= bdinfo->board_type;
|
||||
PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
|
||||
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
|
||||
@ -125,6 +126,9 @@ static void FirmwareIAPCallback(UAVObjEvent* ev)
|
||||
|
||||
if(iap_state == IAP_STATE_RESETTING)
|
||||
return;
|
||||
|
||||
FirmwareIAPObjData data;
|
||||
FirmwareIAPObjGet(&data);
|
||||
|
||||
if ( ev->obj == FirmwareIAPObjHandle() ) {
|
||||
// Get the input object data
|
||||
@ -238,7 +242,10 @@ static void resetTask(UAVObjEvent * ev)
|
||||
#if defined (PIOS_LED_ALARM)
|
||||
PIOS_LED_Toggle(PIOS_LED_ALARM);
|
||||
#endif /* PIOS_LED_ALARM */
|
||||
|
||||
|
||||
FirmwareIAPObjData data;
|
||||
FirmwareIAPObjGet(&data);
|
||||
|
||||
if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) {
|
||||
lastResetSysTime = xTaskGetTickCount();
|
||||
data.BoardType=0xFF;
|
||||
|
||||
@ -144,7 +144,6 @@ int32_t GPSInitialize(void)
|
||||
#ifdef PIOS_GPS_SETS_HOMELOCATION
|
||||
HomeLocationInitialize();
|
||||
#endif
|
||||
HwSettingsInitialize();
|
||||
updateSettings();
|
||||
|
||||
gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
x
Reference in New Issue
Block a user