mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Merge branch 'next' into MikeL
Conflicts: flight/Bootloaders/Revolution/inc/pios_config.h flight/Modules/OveroSync/inc/overosync.h flight/Modules/Sensors/inc/sensors.h flight/PiOS/Boards/STM32F4xx_Revolution.h flight/PiOS/STM32F4xx/pios_iap.c flight/Revolution/System/inc/pios_config.h ground/openpilotgcs/src/plugins/config/config.pro ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp
This commit is contained in:
Mike LaBranche
commit
bf591ebe45
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
|
||||
17
HISTORY.txt
17
HISTORY.txt
@ -1,5 +1,22 @@
|
||||
Short summary of changes. For a complete list see the git log.
|
||||
|
||||
2012-05-04
|
||||
Support for CC3D. This involved changes to various things such as the sensors
|
||||
being split from AttitudeRaw to Accels,Gyros,Magnetometer. A single firmware
|
||||
image fw_coptercontrol will run on both CC and CC3D. When compiling the
|
||||
bootloader one must set the HW_REVISION to the appropriate value. 0x01 is for
|
||||
CC and 0x02 is for CC3D. If the wrong bootloader is installed the firmware
|
||||
will not run.
|
||||
|
||||
2012-05-02
|
||||
Reduction in the memory usage due to the UAVObject metadata. Now the update
|
||||
periods are using a smaller data type and the various flags relating to access
|
||||
controls and update modes are stored in a bitfield. The UAVObjectBrowser has
|
||||
not been updated to allow these modes to be easily changed.
|
||||
|
||||
2012-03-31
|
||||
Support for ground vehicle configuration has been added to the the GCS.
|
||||
|
||||
2012-02-14
|
||||
New QML based system to allow more flexible UI. Upgraded stabilization
|
||||
configuration.
|
||||
|
||||
236
Makefile
236
Makefile
@ -72,6 +72,7 @@ help:
|
||||
@echo " arm_sdk_install - Install the Code Sourcery ARM gcc toolchain"
|
||||
@echo " openocd_install - Install the OpenOCD JTAG daemon"
|
||||
@echo " stm32flash_install - Install the stm32flash tool for unbricking boards"
|
||||
@echo " dfuutil_install - Install the dfu-util tool for unbricking F4-based boards"
|
||||
@echo
|
||||
@echo " [Big Hammer]"
|
||||
@echo " all - Generate UAVObjects, build openpilot firmware and gcs"
|
||||
@ -172,7 +173,6 @@ qt_sdk_install : | $(DL_DIR) $(TOOLS_DIR)
|
||||
qt_sdk_install: qt_sdk_clean
|
||||
# download the source only if it's newer than what we already have
|
||||
$(V1) wget -N --content-disposition -P "$(DL_DIR)" "$(QT_SDK_URL)"
|
||||
|
||||
# tell the user exactly which path they should select in the GUI
|
||||
$(V1) echo "*** NOTE NOTE NOTE ***"
|
||||
$(V1) echo "*"
|
||||
@ -209,36 +209,173 @@ arm_sdk_clean:
|
||||
$(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR)
|
||||
|
||||
# Set up openocd tools
|
||||
OPENOCD_DIR := $(TOOLS_DIR)/openocd
|
||||
OPENOCD_DIR := $(TOOLS_DIR)/openocd
|
||||
OPENOCD_WIN_DIR := $(TOOLS_DIR)/openocd_win
|
||||
OPENOCD_BUILD_DIR := $(DL_DIR)/openocd-build
|
||||
|
||||
.PHONY: openocd_install
|
||||
openocd_install: | $(DL_DIR) $(TOOLS_DIR)
|
||||
openocd_install: OPENOCD_URL := http://sourceforge.net/projects/openocd/files/openocd/0.5.0/openocd-0.5.0.tar.bz2/download
|
||||
openocd_install: OPENOCD_FILE := openocd-0.5.0.tar.bz2
|
||||
# order-only prereq on directory existance:
|
||||
openocd_install: | $(DL_DIR) $(TOOLS_DIR)
|
||||
openocd_install: openocd_clean
|
||||
# download the source only if it's newer than what we already have
|
||||
$(V1) wget -N -P "$(DL_DIR)" --trust-server-name "$(OPENOCD_URL)"
|
||||
|
||||
# extract the source
|
||||
$(V1) [ ! -d "$(DL_DIR)/openocd-build" ] || $(RM) -r "$(DL_DIR)/openocd-build"
|
||||
$(V1) 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 ; \
|
||||
cd $(OPENOCD_BUILD_DIR)/openocd-0.5.0 ; \
|
||||
./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi --enable-buspirate; \
|
||||
$(MAKE) --silent ; \
|
||||
$(MAKE) --silent install ; \
|
||||
)
|
||||
|
||||
# delete the extracted source when we're done
|
||||
$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
|
||||
|
||||
.PHONY: ftd2xx_install
|
||||
|
||||
FTD2XX_DIR := $(DL_DIR)/ftd2xx
|
||||
|
||||
ftd2xx_install: | $(DL_DIR)
|
||||
ftd2xx_install: FTD2XX_URL := http://www.ftdichip.com/Drivers/CDM/Beta/CDM20817.zip
|
||||
ftd2xx_install: FTD2XX_FILE := CDM20817.zip
|
||||
ftd2xx_install: ftd2xx_clean
|
||||
# download the file only if it's newer than what we already have
|
||||
$(V0) @echo " DOWNLOAD $(FTD2XX_URL)"
|
||||
$(V1) wget -q -N -P "$(DL_DIR)" "$(FTD2XX_URL)"
|
||||
|
||||
# extract the source
|
||||
$(V0) @echo " EXTRACT $(FTD2XX_FILE) -> $(FTD2XX_DIR)"
|
||||
$(V1) mkdir -p "$(FTD2XX_DIR)"
|
||||
$(V1) unzip -q -d "$(FTD2XX_DIR)" "$(DL_DIR)/$(FTD2XX_FILE)"
|
||||
|
||||
.PHONY: ftd2xx_clean
|
||||
ftd2xx_clean:
|
||||
$(V0) @echo " CLEAN $(FTD2XX_DIR)"
|
||||
$(V1) [ ! -d "$(FTD2XX_DIR)" ] || $(RM) -r "$(FTD2XX_DIR)"
|
||||
|
||||
.PHONY: ftd2xx_install
|
||||
|
||||
LIBUSB_WIN_DIR := $(DL_DIR)/libusb-win32-bin-1.2.6.0
|
||||
|
||||
libusb_win_install: | $(DL_DIR)
|
||||
libusb_win_install: LIBUSB_WIN_URL := http://sourceforge.net/projects/libusb-win32/files/libusb-win32-releases/1.2.6.0/libusb-win32-bin-1.2.6.0.zip/download
|
||||
libusb_win_install: LIBUSB_WIN_FILE := libusb-win32-bin-1.2.6.0.zip
|
||||
libusb_win_install: libusb_win_clean
|
||||
# download the file only if it's newer than what we already have
|
||||
$(V0) @echo " DOWNLOAD $(LIBUSB_WIN_URL)"
|
||||
$(V1) wget -q -N -P "$(DL_DIR)" --trust-server-name "$(LIBUSB_WIN_URL)"
|
||||
|
||||
# extract the source
|
||||
$(V0) @echo " EXTRACT $(LIBUSB_WIN_FILE) -> $(LIBUSB_WIN_DIR)"
|
||||
$(V1) mkdir -p "$(LIBUSB_WIN_DIR)"
|
||||
$(V1) unzip -q -d "$(DL_DIR)" "$(DL_DIR)/$(LIBUSB_WIN_FILE)"
|
||||
|
||||
# fixup .h file needed by openocd build
|
||||
$(V0) @echo " FIXUP $(LIBUSB_WIN_DIR)"
|
||||
$(V1) ln -s "$(LIBUSB_WIN_DIR)/include/lusb0_usb.h" "$(LIBUSB_WIN_DIR)/include/usb.h"
|
||||
|
||||
.PHONY: libusb_win_clean
|
||||
libusb_win_clean:
|
||||
$(V0) @echo " CLEAN $(LIBUSB_WIN_DIR)"
|
||||
$(V1) [ ! -d "$(LIBUSB_WIN_DIR)" ] || $(RM) -r "$(LIBUSB_WIN_DIR)"
|
||||
|
||||
.PHONY: openocd_git_win_install
|
||||
|
||||
openocd_git_win_install: | $(DL_DIR) $(TOOLS_DIR)
|
||||
openocd_git_win_install: OPENOCD_URL := git://openocd.git.sourceforge.net/gitroot/openocd/openocd
|
||||
openocd_git_win_install: OPENOCD_REV := f1c0133321c8fcadadd10bba5537c0a634eb183b
|
||||
openocd_git_win_install: openocd_win_clean libusb_win_install ftd2xx_install
|
||||
# download the source
|
||||
$(V0) @echo " DOWNLOAD $(OPENOCD_URL) @ $(OPENOCD_REV)"
|
||||
$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
|
||||
$(V1) mkdir -p "$(OPENOCD_BUILD_DIR)"
|
||||
$(V1) git clone --no-checkout $(OPENOCD_URL) "$(DL_DIR)/openocd-build"
|
||||
$(V1) ( \
|
||||
cd $(OPENOCD_BUILD_DIR) ; \
|
||||
git checkout -q $(OPENOCD_REV) ; \
|
||||
)
|
||||
|
||||
# apply patches
|
||||
$(V0) @echo " PATCH $(OPENOCD_BUILD_DIR)"
|
||||
$(V1) ( \
|
||||
cd $(OPENOCD_BUILD_DIR) ; \
|
||||
git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0001-armv7m-remove-dummy-FP-regs-for-new-gdb.patch ; \
|
||||
git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0002-rtos-add-stm32_stlink-to-FreeRTOS-targets.patch ; \
|
||||
)
|
||||
|
||||
# build and install
|
||||
$(V0) @echo " BUILD $(OPENOCD_WIN_DIR)"
|
||||
$(V1) mkdir -p "$(OPENOCD_WIN_DIR)"
|
||||
$(V1) ( \
|
||||
cd $(OPENOCD_BUILD_DIR) ; \
|
||||
./bootstrap ; \
|
||||
./configure --enable-maintainer-mode --prefix="$(OPENOCD_WIN_DIR)" \
|
||||
--build=i686-pc-linux-gnu --host=i586-mingw32msvc \
|
||||
CPPFLAGS=-I$(LIBUSB_WIN_DIR)/include \
|
||||
LDFLAGS=-L$(LIBUSB_WIN_DIR)/lib/gcc \
|
||||
--enable-ft2232_ftd2xx --with-ftd2xx-win32-zipdir=$(FTD2XX_DIR) \
|
||||
--disable-werror \
|
||||
--enable-stlink ; \
|
||||
$(MAKE) ; \
|
||||
$(MAKE) install ; \
|
||||
)
|
||||
|
||||
# delete the extracted source when we're done
|
||||
$(V1) [ ! -d "$(DL_DIR)/openocd-build" ] || $(RM) -r "$(DL_DIR)/openocd-build"
|
||||
$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
|
||||
|
||||
.PHONY: openocd_win_clean
|
||||
openocd_win_clean:
|
||||
$(V0) @echo " CLEAN $(OPENOCD_WIN_DIR)"
|
||||
$(V1) [ ! -d "$(OPENOCD_WIN_DIR)" ] || $(RM) -r "$(OPENOCD_WIN_DIR)"
|
||||
|
||||
.PHONY: openocd_git_install
|
||||
|
||||
openocd_git_install: | $(DL_DIR) $(TOOLS_DIR)
|
||||
openocd_git_install: OPENOCD_URL := git://openocd.git.sourceforge.net/gitroot/openocd/openocd
|
||||
openocd_git_install: OPENOCD_REV := f1c0133321c8fcadadd10bba5537c0a634eb183b
|
||||
openocd_git_install: openocd_clean
|
||||
# download the source
|
||||
$(V0) @echo " DOWNLOAD $(OPENOCD_URL) @ $(OPENOCD_REV)"
|
||||
$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
|
||||
$(V1) mkdir -p "$(OPENOCD_BUILD_DIR)"
|
||||
$(V1) git clone --no-checkout $(OPENOCD_URL) "$(OPENOCD_BUILD_DIR)"
|
||||
$(V1) ( \
|
||||
cd $(OPENOCD_BUILD_DIR) ; \
|
||||
git checkout -q $(OPENOCD_REV) ; \
|
||||
)
|
||||
|
||||
# apply patches
|
||||
$(V0) @echo " PATCH $(OPENOCD_DIR)"
|
||||
$(V1) ( \
|
||||
cd $(OPENOCD_BUILD_DIR) ; \
|
||||
git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0001-armv7m-remove-dummy-FP-regs-for-new-gdb.patch ; \
|
||||
git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0002-rtos-add-stm32_stlink-to-FreeRTOS-targets.patch ; \
|
||||
)
|
||||
|
||||
# build and install
|
||||
$(V0) @echo " BUILD $(OPENOCD_DIR)"
|
||||
$(V1) mkdir -p "$(OPENOCD_DIR)"
|
||||
$(V1) ( \
|
||||
cd $(OPENOCD_BUILD_DIR) ; \
|
||||
./bootstrap ; \
|
||||
./configure --enable-maintainer-mode --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi --enable-buspirate --enable-stlink ; \
|
||||
$(MAKE) ; \
|
||||
$(MAKE) install ; \
|
||||
)
|
||||
|
||||
# delete the extracted source when we're done
|
||||
$(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)"
|
||||
|
||||
.PHONY: openocd_clean
|
||||
openocd_clean:
|
||||
$(V0) @echo " CLEAN $(OPENOCD_DIR)"
|
||||
$(V1) [ ! -d "$(OPENOCD_DIR)" ] || $(RM) -r "$(OPENOCD_DIR)"
|
||||
|
||||
STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash
|
||||
@ -260,6 +397,38 @@ stm32flash_clean:
|
||||
$(V0) @echo " CLEAN $(STM32FLASH_DIR)"
|
||||
$(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -r "$(STM32FLASH_DIR)"
|
||||
|
||||
DFUUTIL_DIR := $(TOOLS_DIR)/dfu-util
|
||||
|
||||
.PHONY: dfuutil_install
|
||||
dfuutil_install: DFUUTIL_URL := http://dfu-util.gnumonks.org/releases/dfu-util-0.5.tar.gz
|
||||
dfuutil_install: DFUUTIL_FILE := $(notdir $(DFUUTIL_URL))
|
||||
dfuutil_install: | $(DL_DIR) $(TOOLS_DIR)
|
||||
dfuutil_install: dfuutil_clean
|
||||
# download the source
|
||||
$(V0) @echo " DOWNLOAD $(DFUUTIL_URL)"
|
||||
$(V1) wget -N -P "$(DL_DIR)" "$(DFUUTIL_URL)"
|
||||
|
||||
# extract the source
|
||||
$(V0) @echo " EXTRACT $(DFUUTIL_FILE)"
|
||||
$(V1) [ ! -d "$(DL_DIR)/dfuutil-build" ] || $(RM) -r "$(DL_DIR)/dfuutil-build"
|
||||
$(V1) mkdir -p "$(DL_DIR)/dfuutil-build"
|
||||
$(V1) tar -C $(DL_DIR)/dfuutil-build -xf "$(DL_DIR)/$(DFUUTIL_FILE)"
|
||||
|
||||
# build
|
||||
$(V0) @echo " BUILD $(DFUUTIL_DIR)"
|
||||
$(V1) mkdir -p "$(DFUUTIL_DIR)"
|
||||
$(V1) ( \
|
||||
cd $(DL_DIR)/dfuutil-build/dfu-util-0.5 ; \
|
||||
./configure --prefix="$(DFUUTIL_DIR)" ; \
|
||||
$(MAKE) ; \
|
||||
$(MAKE) install ; \
|
||||
)
|
||||
|
||||
.PHONY: dfuutil_clean
|
||||
dfuutil_clean:
|
||||
$(V0) @echo " CLEAN $(DFUUTIL_DIR)"
|
||||
$(V1) [ ! -d "$(DFUUTIL_DIR)" ] || $(RM) -r "$(DFUUTIL_DIR)"
|
||||
|
||||
##############################
|
||||
#
|
||||
# Set up paths to tools
|
||||
@ -429,50 +598,64 @@ bu_$(1)_clean:
|
||||
$(V1) $(RM) -fr $(BUILD_DIR)/bu_$(1)
|
||||
endef
|
||||
|
||||
# $(1) = Canonical board name all in lower case (e.g. coptercontrol)
|
||||
define EF_TEMPLATE
|
||||
.PHONY: ef_$(1)
|
||||
ef_$(1): ef_$(1)_bin
|
||||
|
||||
ef_$(1)_%: bl_$(1)_bin fw_$(1)_bin
|
||||
$(V1) mkdir -p $(BUILD_DIR)/ef_$(1)/dep
|
||||
$(V1) cd $(ROOT_DIR)/flight/EntireFlash && \
|
||||
$$(MAKE) -r --no-print-directory \
|
||||
BOARD_NAME=$(1) \
|
||||
TCHAIN_PREFIX="$(ARM_SDK_PREFIX)" \
|
||||
DFU_CMD="$(DFUUTIL_DIR)/bin/dfu-util" \
|
||||
$$*
|
||||
|
||||
.PHONY: ef_$(1)_clean
|
||||
ef_$(1)_clean:
|
||||
$(V0) @echo " CLEAN $$@"
|
||||
$(V1) $(RM) -fr $(BUILD_DIR)/ef_$(1)
|
||||
endef
|
||||
|
||||
# $(1) = Canonical board name all in lower case (e.g. coptercontrol)
|
||||
define BOARD_PHONY_TEMPLATE
|
||||
.PHONY: all_$(1)
|
||||
all_$(1): $$(filter fw_$(1), $$(FW_TARGETS))
|
||||
all_$(1): $$(filter bl_$(1), $$(BL_TARGETS))
|
||||
all_$(1): $$(filter bu_$(1), $$(BU_TARGETS))
|
||||
all_$(1): $$(filter ef_$(1), $$(EF_TARGETS))
|
||||
|
||||
.PHONY: all_$(1)_clean
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter fw_$(1), $$(FW_TARGETS)))
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter bl_$(1), $$(BL_TARGETS)))
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter ef_$(1), $$(EF_TARGETS)))
|
||||
endef
|
||||
|
||||
ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins
|
||||
ALL_BOARDS := coptercontrol pipxtreme revolution
|
||||
|
||||
# Friendly names of each board (used to find source tree)
|
||||
openpilot_friendly := OpenPilot
|
||||
coptercontrol_friendly := CopterControl
|
||||
pipxtreme_friendly := PipXtreme
|
||||
ins_friendly := INS
|
||||
ahrs_friendly := AHRS
|
||||
revolution_friendly := Revolution
|
||||
|
||||
# Start out assuming that we'll build fw, bl and bu for all boards
|
||||
FW_BOARDS := $(ALL_BOARDS)
|
||||
BL_BOARDS := $(ALL_BOARDS)
|
||||
BU_BOARDS := $(ALL_BOARDS)
|
||||
EF_BOARDS := $(ALL_BOARDS)
|
||||
|
||||
# FIXME: The INS build doesn't have a bootloader or bootloader
|
||||
# updater yet so we need to filter them out to prevent errors.
|
||||
BL_BOARDS := $(filter-out ins, $(BL_BOARDS))
|
||||
BU_BOARDS := $(filter-out ins, $(BU_BOARDS))
|
||||
|
||||
# FIXME: The CC bootloader updaters don't work anymore due to
|
||||
# differences between CC and CC3D
|
||||
BU_BOARDS := $(filter-out coptercontrol, $(BU_BOARDS))
|
||||
|
||||
# FIXME: PipXtreme bootloader updater doesn't work due to missing
|
||||
# definitions for LEDs
|
||||
BU_BOARDS := $(filter-out pipxtreme, $(BU_BOARDS))
|
||||
|
||||
# Generate the targets for whatever boards are left in each list
|
||||
FW_TARGETS := $(addprefix fw_, $(FW_BOARDS))
|
||||
BL_TARGETS := $(addprefix bl_, $(BL_BOARDS))
|
||||
BU_TARGETS := $(addprefix bu_, $(BU_BOARDS))
|
||||
EF_TARGETS := $(addprefix ef_, $(EF_BOARDS))
|
||||
|
||||
.PHONY: all_fw all_fw_clean
|
||||
all_fw: $(addsuffix _opfw, $(FW_TARGETS))
|
||||
@ -486,9 +669,13 @@ all_bl_clean: $(addsuffix _clean, $(BL_TARGETS))
|
||||
all_bu: $(addsuffix _opfw, $(BU_TARGETS))
|
||||
all_bu_clean: $(addsuffix _clean, $(BU_TARGETS))
|
||||
|
||||
.PHONY: all_ef all_ef_clean
|
||||
all_ef: $(EF_TARGETS))
|
||||
all_ef_clean: $(addsuffix _clean, $(EF_TARGETS))
|
||||
|
||||
.PHONY: all_flight all_flight_clean
|
||||
all_flight: all_fw all_bl all_bu
|
||||
all_flight_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))))
|
||||
@ -502,6 +689,9 @@ $(foreach board, $(ALL_BOARDS), $(eval $(call FW_TEMPLATE,$(board),$($(board)_fr
|
||||
# Expand the bootloader rules
|
||||
$(foreach board, $(ALL_BOARDS), $(eval $(call BL_TEMPLATE,$(board),$($(board)_friendly))))
|
||||
|
||||
# Expand the entire-flash rules
|
||||
$(foreach board, $(ALL_BOARDS), $(eval $(call EF_TEMPLATE,$(board),$($(board)_friendly))))
|
||||
|
||||
.PHONY: sim_posix
|
||||
sim_posix: sim_posix_elf
|
||||
|
||||
|
||||
@ -1,424 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot AHRS
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot AHRS project
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
TOP := $(realpath $(WHEREAMI)/../../)
|
||||
include $(TOP)/make/firmware-defs.mk
|
||||
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET := fw_$(BOARD_NAME)
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR := $(TOP)/build/$(TARGET)
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES for debugging
|
||||
DEBUG ?= NO
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= YES
|
||||
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
REMOVE_CMD = cs-rm
|
||||
else
|
||||
REMOVE_CMD = rm
|
||||
endif
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# Paths
|
||||
AHRS = ./
|
||||
AHRSINC = $(AHRS)/inc
|
||||
PIOS = ../PiOS
|
||||
PIOSINC = $(PIOS)/inc
|
||||
FLIGHTLIB = ../Libraries
|
||||
FLIGHTLIBINC = ../Libraries/inc
|
||||
PIOSSTM32F10X = $(PIOS)/STM32F10x
|
||||
PIOSCOMMON = $(PIOS)/Common
|
||||
PIOSBOARDS = $(PIOS)/Boards
|
||||
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
|
||||
STMLIBDIR = $(APPLIBDIR)
|
||||
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
|
||||
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
|
||||
STMSPDSRCDIR = $(STMSPDDIR)/src
|
||||
STMSPDINCDIR = $(STMSPDDIR)/inc
|
||||
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
|
||||
OPDIR = ../OpenPilot
|
||||
OPUAVOBJ = ../UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPSYSINC = $(OPDIR)/System/inc
|
||||
BOOT = ../Bootloaders/AHRS
|
||||
BOOTINC = $(BOOT)/inc
|
||||
HWDEFSINC = ../board_hw_defs/$(BOARD_NAME)
|
||||
|
||||
OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
## AHRS:
|
||||
SRC = ahrs.c
|
||||
SRC += pios_board.c
|
||||
SRC += ahrs_timer.c
|
||||
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
|
||||
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
|
||||
SRC += $(BOOT)/ahrs_spi_program_slave.c
|
||||
SRC += $(BOOT)/ahrs_slave_test.c
|
||||
SRC += $(BOOT)/ahrs_spi_program.c
|
||||
SRC += insgps13state.c
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
SRC += $(PIOSSTM32F10X)/pios_adc.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_sys.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_led.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_delay.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_i2c.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spi.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_exti.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_hmc5843.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
SRC += $(PIOSCOMMON)/pios_bl_helper.c
|
||||
|
||||
## CMSIS for STM32
|
||||
SRC += $(CMSISDIR)/core_cm3.c
|
||||
SRC += $(CMSISDIR)/system_stm32f10x.c
|
||||
|
||||
## Used parts of the STM-Library
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
|
||||
SRC += $(STMSPDSRCDIR)/misc.c
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
# will not be considered source files but generated files (assembler
|
||||
# output from the compiler), and will be deleted upon "make clean"!
|
||||
# Even though the DOS/Win* filesystem matches both .s and .S the same,
|
||||
# it will preserve the spelling of the filenames, and gcc itself does
|
||||
# care about how the name is spelled on its command-line.
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
|
||||
|
||||
# List Assembler source files here which must be assembled in ARM-Mode..
|
||||
ASRCARM =
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PIOSSTM32F10X)
|
||||
EXTRAINCDIRS += $(PIOSCOMMON)
|
||||
EXTRAINCDIRS += $(PIOSBOARDS)
|
||||
EXTRAINCDIRS += $(STMSPDINCDIR)
|
||||
EXTRAINCDIRS += $(CMSISDIR)
|
||||
EXTRAINCDIRS += $(AHRSINC)
|
||||
EXTRAINCDIRS += $(OPUAVSYNTHDIR)
|
||||
EXTRAINCDIRS += $(BOOTINC)
|
||||
EXTRAINCDIRS += $(HWDEFSINC)
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS += -O0
|
||||
CFLAGS += -DGENERAL_COV
|
||||
else
|
||||
CFLAGS += -Os
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
CDEFS += -DUSE_$(BOARD)
|
||||
CDEFS += -DIN_AHRS
|
||||
|
||||
# Place project-specific -D and/or -U options for
|
||||
# Assembler with preprocessor here.
|
||||
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
|
||||
ADEFS = -D__ASSEMBLY__
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
CFLAGS += -g$(DEBUGF)
|
||||
|
||||
CFLAGS += -ffast-math
|
||||
|
||||
CFLAGS += -mcpu=$(MCU)
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
|
||||
|
||||
# Define programs and commands.
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
# Default target.
|
||||
all: gccversion build
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
|
||||
|
||||
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino opfw
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
bino: $(OUTDIR)/$(TARGET).bin.o
|
||||
opfw: $(OUTDIR)/$(TARGET).opfw
|
||||
|
||||
# Display sizes of sections.
|
||||
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Install: install binary file with prefix/suffix into install directory
|
||||
install: $(OUTDIR)/$(TARGET).opfw
|
||||
ifneq ($(INSTALL_DIR),)
|
||||
@echo $(MSG_INSTALLING) $(call toprel, $<)
|
||||
$(V1) mkdir -p $(INSTALL_DIR)
|
||||
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
|
||||
else
|
||||
$(error INSTALL_DIR must be specified for $@)
|
||||
endif
|
||||
|
||||
# Target: clean project.
|
||||
clean: clean_list
|
||||
|
||||
clean_list :
|
||||
@echo $(MSG_CLEANING)
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
|
||||
$(V1) $(REMOVE) $(ALLOBJ)
|
||||
$(V1) $(REMOVE) $(LSTFILES)
|
||||
$(V1) $(REMOVE) $(DEPFILES)
|
||||
$(V1) $(REMOVE) $(SRC:.c=.s)
|
||||
$(V1) $(REMOVE) $(SRCARM:.c=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
|
||||
else
|
||||
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all build clean clean_list install
|
||||
1265
flight/AHRS/ahrs.c
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,46 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief PiOS configuration header.
|
||||
* - Central compile time config for the project.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef PIOS_CONFIG_H
|
||||
#define PIOS_CONFIG_H
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_SPI
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_USART
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_HMC5843
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_EXTI
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
#define PIOS_INCLUDE_IAP
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -33,7 +33,7 @@
|
||||
/* Prototype of PIOS_Board_Init() function */
|
||||
extern void PIOS_Board_Init(void);
|
||||
extern void FLASH_Download();
|
||||
void error(int);
|
||||
void error(void);
|
||||
|
||||
/* The ADDRESSES of the _binary_* symbols are the important
|
||||
* data. This is non-intuitive for _binary_size where you
|
||||
@ -50,14 +50,11 @@ int main() {
|
||||
|
||||
PIOS_SYS_Init();
|
||||
PIOS_Board_Init();
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
PIOS_DELAY_WaitmS(3000);
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
|
||||
/// Self overwrite check
|
||||
uint32_t base_address = SCB->VTOR;
|
||||
if ((0x08000000 + embedded_image_size) > base_address)
|
||||
error(PIOS_LED_HEARTBEAT);
|
||||
error();
|
||||
///
|
||||
FLASH_Unlock();
|
||||
|
||||
@ -82,7 +79,7 @@ int main() {
|
||||
}
|
||||
|
||||
if (fail == true)
|
||||
error(PIOS_LED_HEARTBEAT);
|
||||
error();
|
||||
|
||||
|
||||
///
|
||||
@ -90,7 +87,6 @@ int main() {
|
||||
/// Bootloader programing
|
||||
for (uint32_t offset = 0; offset < embedded_image_size/sizeof(uint32_t); ++offset) {
|
||||
bool result = false;
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
for (uint8_t retry = 0; retry < MAX_WRI_RETRYS; ++retry) {
|
||||
if (result == false) {
|
||||
result = (FLASH_ProgramWord(0x08000000 + (offset * 4), embedded_image_start[offset])
|
||||
@ -98,15 +94,9 @@ int main() {
|
||||
}
|
||||
}
|
||||
if (result == false)
|
||||
error(PIOS_LED_HEARTBEAT);
|
||||
error();
|
||||
}
|
||||
///
|
||||
for (uint8_t x = 0; x < 3; ++x) {
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
}
|
||||
|
||||
/// Invalidate the bootloader updater so we won't run
|
||||
/// the update again on the next power cycle.
|
||||
@ -119,11 +109,7 @@ int main() {
|
||||
|
||||
}
|
||||
|
||||
void error(int led) {
|
||||
void error(void) {
|
||||
for (;;) {
|
||||
PIOS_LED_On(led);
|
||||
PIOS_DELAY_WaitmS(500);
|
||||
PIOS_LED_Off(led);
|
||||
PIOS_DELAY_WaitmS(500);
|
||||
}
|
||||
}
|
||||
|
||||
@ -46,9 +46,4 @@ void PIOS_Board_Init(void) {
|
||||
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
}
|
||||
|
||||
@ -104,6 +104,8 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_iap.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
|
||||
|
||||
# PIOS USB related files (seperated to make code maintenance more easy)
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb.c
|
||||
@ -117,8 +119,6 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_board_info.c
|
||||
SRC += $(PIOSCOMMON)/pios_com_msg.c
|
||||
SRC += $(PIOSCOMMON)/pios_bl_helper.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
|
||||
## Libraries for flight calculations
|
||||
@ -414,7 +414,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
|
||||
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
|
||||
@ -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_CheckAvailable(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,20 @@ void PIOS_Board_Init(void) {
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // Revision 1
|
||||
PIOS_LED_Init(&pios_led_cfg_cc);
|
||||
break;
|
||||
case 0x02: // Revision 2
|
||||
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
|
||||
PIOS_LED_Init(&pios_led_cfg_cc3d);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB)
|
||||
@ -71,8 +83,15 @@ void PIOS_Board_Init(void) {
|
||||
PIOS_USB_DESC_HID_ONLY_Init();
|
||||
|
||||
uint32_t pios_usb_id;
|
||||
if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // Revision 1
|
||||
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
|
||||
break;
|
||||
case 0x02: // Revision 2
|
||||
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
|
||||
uint32_t pios_usb_hid_id;
|
||||
@ -90,3 +109,7 @@ void PIOS_Board_Init(void) {
|
||||
|
||||
board_init_complete = true;
|
||||
}
|
||||
|
||||
void PIOS_ADC_DMA_Handler()
|
||||
{
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -103,6 +103,8 @@ SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_iap.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
|
||||
|
||||
# PIOS USB related files (seperated to make code maintenance more easy)
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb.c
|
||||
@ -416,7 +418,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
|
||||
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
|
||||
@ -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
|
||||
@ -244,13 +247,12 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_crc.c
|
||||
SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c
|
||||
SRC += $(PIOSCOMMON)/pios_flash_w25x.c
|
||||
SRC += $(PIOSCOMMON)/pios_flash_jedec.c
|
||||
SRC += $(PIOSCOMMON)/pios_adxl345.c
|
||||
SRC += $(PIOSCOMMON)/pios_mpu6000.c
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
|
||||
SRC += $(PIOSCOMMON)/pios_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 +605,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_JTAG_CONFIG),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino opfw
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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
|
||||
|
||||
@ -43,6 +43,7 @@
|
||||
#include <manualcontrolsettings.h>
|
||||
#include <gcsreceiver.h>
|
||||
|
||||
|
||||
/* One slot per selectable receiver group.
|
||||
* eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
|
||||
* NOTE: No slot in this map for NONE.
|
||||
@ -66,25 +67,140 @@ uint32_t pios_com_vcp_id;
|
||||
uint32_t pios_com_gps_id;
|
||||
uint32_t pios_com_bridge_id;
|
||||
|
||||
/**
|
||||
* Configuration for MPU6000 chip
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
#include "pios_mpu6000.h"
|
||||
static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
|
||||
.vector = PIOS_MPU6000_IRQHandler,
|
||||
.line = EXTI_Line3,
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_3,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = EXTI3_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.exti = {
|
||||
.init = {
|
||||
.EXTI_Line = EXTI_Line3, // matches above GPIO pin
|
||||
.EXTI_Mode = EXTI_Mode_Interrupt,
|
||||
.EXTI_Trigger = EXTI_Trigger_Rising,
|
||||
.EXTI_LineCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
|
||||
.exti_cfg = &pios_exti_mpu6000_cfg,
|
||||
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
|
||||
// Clock at 8 khz, downsampled by 8 for 1khz
|
||||
.Smpl_rate_div = 15,
|
||||
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
|
||||
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
|
||||
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN,
|
||||
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
|
||||
.accel_range = PIOS_MPU6000_ACCEL_8G,
|
||||
.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
|
||||
.filter = PIOS_MPU6000_LOWPASS_256_HZ
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
|
||||
static const struct flashfs_cfg flashfs_w25x_cfg = {
|
||||
.table_magic = 0x85FB3C35,
|
||||
.obj_magic = 0x3015AE71,
|
||||
.obj_table_start = 0x00000010,
|
||||
.obj_table_end = 0x00001000,
|
||||
.sector_size = 0x00001000,
|
||||
};
|
||||
|
||||
static const struct pios_flash_jedec_cfg flash_w25x_cfg = {
|
||||
.sector_erase = 0x20,
|
||||
.chip_erase = 0x60
|
||||
};
|
||||
|
||||
static const struct flashfs_cfg flashfs_m25p_cfg = {
|
||||
.table_magic = 0x85FB3D35,
|
||||
.obj_magic = 0x3015A371,
|
||||
.obj_table_start = 0x00000010,
|
||||
.obj_table_end = 0x00010000,
|
||||
.sector_size = 0x00010000,
|
||||
};
|
||||
|
||||
static const struct pios_flash_jedec_cfg flash_m25p_cfg = {
|
||||
.sector_erase = 0xD8,
|
||||
.chip_erase = 0xC7
|
||||
};
|
||||
#include <pios_board_info.h>
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
int32_t init_test;
|
||||
void PIOS_Board_Init(void) {
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // Revision 1
|
||||
PIOS_LED_Init(&pios_led_cfg_cc);
|
||||
break;
|
||||
case 0x02: // Revision 2
|
||||
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
|
||||
PIOS_LED_Init(&pios_led_cfg_cc3d);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
/* Set up the SPI interface to the serial flash */
|
||||
if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // Revision 1
|
||||
if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
break;
|
||||
case 0x02: // Revision 2
|
||||
if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
PIOS_Flash_W25X_Init(pios_spi_flash_accel_id);
|
||||
PIOS_ADXL345_Attach(pios_spi_flash_accel_id);
|
||||
#endif
|
||||
|
||||
PIOS_FLASHFS_Init();
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // Revision 1
|
||||
PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg);
|
||||
PIOS_FLASHFS_Init(&flashfs_w25x_cfg);
|
||||
break;
|
||||
case 0x02: // Revision 2
|
||||
PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 0, &flash_m25p_cfg);
|
||||
PIOS_FLASHFS_Init(&flashfs_m25p_cfg);
|
||||
break;
|
||||
default:
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
@ -95,10 +211,6 @@ void PIOS_Board_Init(void) {
|
||||
PIOS_RTC_Init(&pios_rtc_main_cfg);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
HwSettingsInitialize();
|
||||
|
||||
#ifndef ERASE_FLASH
|
||||
@ -162,7 +274,17 @@ void PIOS_Board_Init(void) {
|
||||
}
|
||||
|
||||
uint32_t pios_usb_id;
|
||||
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
|
||||
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // Revision 1
|
||||
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
|
||||
break;
|
||||
case 0x02: // Revision 2
|
||||
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_CDC)
|
||||
|
||||
@ -575,8 +697,35 @@ void PIOS_Board_Init(void) {
|
||||
#else
|
||||
PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
|
||||
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */
|
||||
|
||||
PIOS_ADC_Init();
|
||||
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01:
|
||||
// Revision 1 with invensense gyros, start the ADC
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
PIOS_ADC_Init(&pios_adc_cfg);
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_ADXL345)
|
||||
PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0);
|
||||
#endif
|
||||
break;
|
||||
case 0x02:
|
||||
// Revision 2 with L3GD20 gyros, start a SPI interface and connect to it
|
||||
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
// Set up the SPI interface to the serial flash
|
||||
if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg);
|
||||
init_test = PIOS_MPU6000_Test();
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
/* Make sure we have at least one telemetry link configured or else fail initialization */
|
||||
|
||||
@ -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,52 +1,42 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_bl.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Main AHRS_BL header.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef AHRS_BL_H
|
||||
#define AHRS_BL_H
|
||||
|
||||
/* PIOS Includes */
|
||||
#include <pios.h>
|
||||
|
||||
/** Start programming
|
||||
returns: true if FLASH erased and ready to program
|
||||
*/
|
||||
bool
|
||||
StartProgramming(void);
|
||||
|
||||
/** Write a block to FLASH
|
||||
buffer contains the data to be written
|
||||
returns: true if FLASH programmed correctly
|
||||
*/
|
||||
bool
|
||||
WriteData(uint32_t offset, uint8_t *buffer, uint32_t size);
|
||||
|
||||
/** Read a block from FLASH
|
||||
returns: true if FLASH read correctly.
|
||||
Buffer is set to the read data
|
||||
*/
|
||||
bool
|
||||
ReadData(uint32_t offset, uint8_t *buffer, uint32_t size);
|
||||
|
||||
#endif /* AHRS_BL_H */
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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
|
||||
|
||||
@ -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?
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -1,45 +1,41 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS Control
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup AHRS_TIMER
|
||||
* @brief Sets up a simple timer that can be polled to estimate idle time
|
||||
* @addtogroup AltitudeModule Altitude Module
|
||||
* @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @file altitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief INSGPS Test Program
|
||||
* @brief Altitude module, reads temperature and pressure from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
/*
|
||||
* 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
|
||||
*
|
||||
* 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.,
|
||||
*
|
||||
* 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
|
||||
|
||||
#ifndef AHRS_TIMER
|
||||
#define AHRS_TIMER
|
||||
int32_t AltitudeInitialize();
|
||||
|
||||
#include <pios.h>
|
||||
#endif // ALTITUDE_H
|
||||
|
||||
#define TIMER_RATE (8e6 / 128)
|
||||
|
||||
void timer_start();
|
||||
uint32_t timer_count();
|
||||
uint32_t timer_rate();
|
||||
|
||||
#endif
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
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,9 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_spi_program_slave.h
|
||||
* @file examplemodperiodic.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief AHRS programming over SPI link - slave(AHRS) end.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -23,13 +23,9 @@
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef EXAMPLEMODPERIODIC_H
|
||||
#define EXAMPLEMODPERIODIC_H
|
||||
|
||||
#ifndef AHRS_SPI_PROGRAM_SLAVE_H
|
||||
#define AHRS_SPI_PROGRAM_SLAVE_H
|
||||
|
||||
/** Check if OpenPilot is trying to program AHRS
|
||||
* If so, it will program the FLASH then return
|
||||
* If not it just returns.
|
||||
*/
|
||||
void AhrsProgramReceive(uint32_t spi_id);
|
||||
#endif //AHRS_PROGRAM_SLAVE_H
|
||||
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,33 @@ 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;
|
||||
if(cc3d)
|
||||
retval = updateSensorsCC3D(&accels, &gyros);
|
||||
else
|
||||
retval = updateSensors(&accels, &gyros);
|
||||
|
||||
if(retval != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
else {
|
||||
// Only update attitude when sensor data is good
|
||||
updateAttitude(&attitudeRaw);
|
||||
AttitudeRawSet(&attitudeRaw);
|
||||
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];
|
||||
@ -252,9 +273,9 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
return -1;
|
||||
|
||||
// First sample is temperature
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
|
||||
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 +289,25 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
y += -accel_data.y;
|
||||
z += -accel_data.z;
|
||||
} while ( (i < 32) && (samples_remaining > 0) );
|
||||
attitudeRaw->gyrotemp[0] = samples_remaining;
|
||||
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 +321,100 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
|
||||
trim_samples++;
|
||||
// Store the digitally scaled version since that is what we use for bias
|
||||
trim_accels[0] += attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
|
||||
trim_accels[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 +424,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 +480,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
}
|
||||
|
||||
// Renomalize
|
||||
float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
q[0] = q[0] / qmag;
|
||||
q[1] = q[1] / qmag;
|
||||
q[2] = q[2] / qmag;
|
||||
|
||||
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);
|
||||
|
||||
@ -216,7 +216,7 @@ static float NMEA_real_to_float(char *nmea_real)
|
||||
}
|
||||
|
||||
/* Convert to float */
|
||||
return (((float)whole) + fract * pow(10, -fract_units));
|
||||
return (((float)whole) + fract * powf(10.0f, -fract_units));
|
||||
}
|
||||
|
||||
/*
|
||||
@ -239,37 +239,38 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool
|
||||
}
|
||||
|
||||
/* scale up the mmmm[mm] field apropriately depending on # of digits */
|
||||
/* not using 1eN notation because that forces fixed point and lost precision */
|
||||
switch (units) {
|
||||
case 0:
|
||||
/* no digits, value is zero so no scaling */
|
||||
break;
|
||||
case 1: /* m */
|
||||
num_m *= 1e6; /* m000000 */
|
||||
num_m *= 1000000; /* m000000 */
|
||||
break;
|
||||
case 2: /* mm */
|
||||
num_m *= 1e5; /* mm00000 */
|
||||
num_m *= 100000; /* mm00000 */
|
||||
break;
|
||||
case 3: /* mmm */
|
||||
num_m *= 1e4; /* mmm0000 */
|
||||
num_m *= 10000; /* mmm0000 */
|
||||
break;
|
||||
case 4: /* mmmm */
|
||||
num_m *= 1e3; /* mmmm000 */
|
||||
num_m *= 1000; /* mmmm000 */
|
||||
break;
|
||||
case 5: /* mmmmm */
|
||||
num_m *= 1e2; /* mmmmm00 */
|
||||
num_m *= 100; /* mmmmm00 */
|
||||
break;
|
||||
case 6: /* mmmmmm */
|
||||
num_m *= 1e1; /* mmmmmm0 */
|
||||
num_m *= 10; /* mmmmmm0 */
|
||||
break;
|
||||
default:
|
||||
/* unhandled format */
|
||||
num_m = 0;
|
||||
num_m = 0.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
*latlon = (num_DDDMM / 100) * 1e7; /* scale the whole degrees */
|
||||
*latlon += (num_DDDMM % 100) * 1e7 / 60; /* add in the scaled decimal whole minutes */
|
||||
*latlon += num_m / 60; /* add in the scaled decimal fractional minutes */
|
||||
*latlon = (num_DDDMM / 100) * 10000000; /* scale the whole degrees */
|
||||
*latlon += (num_DDDMM % 100) * 10000000 / 60; /* add in the scaled decimal whole minutes */
|
||||
*latlon += num_m / 60; /* add in the scaled decimal fractional minutes */
|
||||
|
||||
if (negative)
|
||||
*latlon *= -1;
|
||||
|
||||
@ -192,9 +192,9 @@ static void guidanceTask(void *parameters)
|
||||
NedAccelData accelData;
|
||||
NedAccelGet(&accelData);
|
||||
// Convert from m/s to cm/s
|
||||
accelData.North = accel_ned[0] * 100;
|
||||
accelData.East = accel_ned[1] * 100;
|
||||
accelData.Down = accel_ned[2] * 100;
|
||||
accelData.North = accel_ned[0];
|
||||
accelData.East = accel_ned[1];
|
||||
accelData.Down = accel_ned[2];
|
||||
NedAccelSet(&accelData);
|
||||
|
||||
|
||||
|
||||
@ -39,6 +39,7 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
|
||||
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
|
||||
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
|
||||
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
|
||||
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
|
||||
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
|
||||
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
|
||||
)
|
||||
|
||||
@ -44,20 +44,23 @@
|
||||
#include "flightstatus.h"
|
||||
#include "accessorydesired.h"
|
||||
#include "receiveractivity.h"
|
||||
#include "altitudeholddesired.h"
|
||||
#include "positionactual.h"
|
||||
#include "baroaltitude.h"
|
||||
|
||||
// Private constants
|
||||
#if defined(PIOS_MANUAL_STACK_SIZE)
|
||||
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 824
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
#endif
|
||||
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
#define UPDATE_PERIOD_MS 20
|
||||
#define THROTTLE_FAILSAFE -0.1
|
||||
#define FLIGHT_MODE_LIMIT 1.0/3.0
|
||||
#define THROTTLE_FAILSAFE -0.1f
|
||||
#define FLIGHT_MODE_LIMIT 1.0f/3.0f
|
||||
#define ARMED_TIME_MS 1000
|
||||
#define ARMED_THRESHOLD 0.50
|
||||
#define ARMED_THRESHOLD 0.50f
|
||||
//safe band to allow a bit of calibration error or trim offset (in microseconds)
|
||||
#define CONNECTION_OFFSET 150
|
||||
|
||||
@ -79,6 +82,7 @@ static portTickType lastSysTime;
|
||||
// Private functions
|
||||
static void updateActuatorDesired(ManualControlCommandData * cmd);
|
||||
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
||||
static void altitudeHoldDesired(ManualControlCommandData * cmd);
|
||||
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
|
||||
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
||||
static void setArmedIfChanged(uint8_t val);
|
||||
@ -197,7 +201,7 @@ static void manualControlTask(void *parameters)
|
||||
/* trying to fly via GCS and lost connection. fall back to transmitter */
|
||||
UAVObjMetadata metadata;
|
||||
ManualControlCommandGetMetadata(&metadata);
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
|
||||
ManualControlCommandSetMetadata(&metadata);
|
||||
}
|
||||
}
|
||||
@ -383,7 +387,13 @@ static void manualControlTask(void *parameters)
|
||||
updateStabilizationDesired(&cmd, &settings);
|
||||
break;
|
||||
case FLIGHTMODE_GUIDANCE:
|
||||
// TODO: Implement
|
||||
switch(flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
|
||||
altitudeHoldDesired(&cmd);
|
||||
break;
|
||||
default:
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -596,6 +606,52 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
|
||||
StabilizationDesiredSet(&stabilization);
|
||||
}
|
||||
|
||||
#if defined(REVOLUTION)
|
||||
// TODO: Need compile flag to exclude this from copter control
|
||||
static void altitudeHoldDesired(ManualControlCommandData * cmd)
|
||||
{
|
||||
const float DEADBAND_HIGH = 0.55;
|
||||
const float DEADBAND_LOW = 0.45;
|
||||
|
||||
static portTickType lastSysTime;
|
||||
static bool zeroed = false;
|
||||
portTickType thisSysTime;
|
||||
float dT;
|
||||
AltitudeHoldDesiredData altitudeHoldDesired;
|
||||
AltitudeHoldDesiredGet(&altitudeHoldDesired);
|
||||
|
||||
StabilizationSettingsData stabSettings;
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
|
||||
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
|
||||
altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
|
||||
|
||||
float currentDown;
|
||||
PositionActualDownGet(¤tDown);
|
||||
if(dT > 1) {
|
||||
// After not being in this mode for a while init at current height
|
||||
altitudeHoldDesired.Altitude = 0;
|
||||
zeroed = false;
|
||||
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
|
||||
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
|
||||
else if (cmd->Throttle < DEADBAND_LOW && zeroed)
|
||||
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
|
||||
else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) // Require the stick to enter the dead band before they can move height
|
||||
zeroed = true;
|
||||
|
||||
AltitudeHoldDesiredSet(&altitudeHoldDesired);
|
||||
}
|
||||
#else
|
||||
static void altitudeHoldDesired(ManualControlCommandData * cmd)
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
|
||||
}
|
||||
#endif
|
||||
/**
|
||||
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
|
||||
*/
|
||||
|
||||
@ -248,7 +248,7 @@ static bool Read(uint32_t start, uint8_t length, uint8_t * buffer)
|
||||
cmd[3] = (uint8_t) (start >> 8);
|
||||
cmd[4] = length;
|
||||
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0;
|
||||
}
|
||||
|
||||
static bool Write(uint32_t start, uint8_t length, const uint8_t * buffer)
|
||||
|
||||
46
flight/Modules/OveroSync/inc/overosync.h
Normal file
46
flight/Modules/OveroSync/inc/overosync.h
Normal file
@ -0,0 +1,46 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup TelemetryModule Telemetry Module
|
||||
* @brief Main telemetry module
|
||||
* Starts three tasks (RX, TX, and priority TX) that watch event queues
|
||||
* and handle all the telemetry of the UAVobjects
|
||||
* @{
|
||||
*
|
||||
* @file telemetry.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the telemetry module.
|
||||
* As with all modules only the initialize function is exposed all other
|
||||
* interactions with the module take place through the event queue and
|
||||
* objects.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef TELEMETRY_H
|
||||
#define TELEMETRY_H
|
||||
|
||||
int32_t TelemetryInitialize(void);
|
||||
|
||||
#endif // TELEMETRY_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
371
flight/Modules/OveroSync/overosync.c
Normal file
371
flight/Modules/OveroSync/overosync.c
Normal file
@ -0,0 +1,371 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup TelemetryModule Telemetry Module
|
||||
* @brief Main telemetry module
|
||||
* Starts three tasks (RX, TX, and priority TX) that watch event queues
|
||||
* and handle all the telemetry of the UAVobjects
|
||||
* @{
|
||||
*
|
||||
* @file telemetry.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Telemetry module, handles telemetry and UAVObject updates
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "overosync.h"
|
||||
#include "overosyncstats.h"
|
||||
#include "systemstats.h"
|
||||
|
||||
// Private constants
|
||||
#define OVEROSYNC_PACKET_SIZE 1024
|
||||
#define MAX_QUEUE_SIZE 10
|
||||
#define STACK_SIZE_BYTES 4096
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 0)
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xQueueHandle queue;
|
||||
static UAVTalkConnection uavTalkCon;
|
||||
static xTaskHandle overoSyncTaskHandle;
|
||||
volatile bool buffer_swap_failed;
|
||||
volatile uint32_t buffer_swap_timeval;
|
||||
|
||||
// Private functions
|
||||
static void overoSyncTask(void *parameters);
|
||||
static int32_t packData(uint8_t * data, int32_t length);
|
||||
static int32_t transmitData();
|
||||
static void transmitDataDone(bool crc_ok, uint8_t crc_val);
|
||||
static void registerObject(UAVObjHandle obj);
|
||||
|
||||
// External variables
|
||||
extern int32_t pios_spi_overo_id;
|
||||
|
||||
struct dma_transaction {
|
||||
uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
|
||||
uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
|
||||
};
|
||||
|
||||
struct overosync {
|
||||
struct dma_transaction transactions[2];
|
||||
uint32_t active_transaction_id;
|
||||
uint32_t loading_transaction_id;
|
||||
xSemaphoreHandle transaction_lock;
|
||||
xSemaphoreHandle buffer_lock;
|
||||
volatile bool transaction_done;
|
||||
uint32_t sent_bytes;
|
||||
uint32_t write_pointer;
|
||||
uint32_t sent_objects;
|
||||
uint32_t failed_objects;
|
||||
uint32_t received_objects;
|
||||
uint32_t framesync_error;
|
||||
};
|
||||
|
||||
struct overosync *overosync;
|
||||
|
||||
static void PIOS_OVERO_IRQHandler();
|
||||
|
||||
static const struct pios_exti_cfg pios_exti_overo_cfg __exti_config = {
|
||||
.vector = PIOS_OVERO_IRQHandler,
|
||||
.line = EXTI_Line15,
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_100MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = EXTI15_10_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.exti = {
|
||||
.init = {
|
||||
.EXTI_Line = EXTI_Line15, // matches above GPIO pin
|
||||
.EXTI_Mode = EXTI_Mode_Interrupt,
|
||||
.EXTI_Trigger = EXTI_Trigger_Rising,
|
||||
.EXTI_LineCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
/**
|
||||
* On the rising edge of NSS schedule a new transaction. This cannot be
|
||||
* done by the DMA complete because there is 150 us between that and the
|
||||
* Overo deasserting the CS line. We don't want to spin that long in an
|
||||
* isr
|
||||
*/
|
||||
void PIOS_OVERO_IRQHandler()
|
||||
{
|
||||
// transmitData must not block to get semaphore for when we get out of
|
||||
// frame and transaction is still running here. -1 indicates the transaction
|
||||
// semaphore is blocked and we are still in a transaction, thus a framesync
|
||||
// error occurred. This shouldn't happen. Race condition?
|
||||
if(transmitData() == -1)
|
||||
overosync->framesync_error++;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the telemetry module
|
||||
* \return -1 if initialisation failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t OveroSyncInitialize(void)
|
||||
{
|
||||
if(pios_spi_overo_id == 0)
|
||||
return -1;
|
||||
|
||||
OveroSyncStatsInitialize();
|
||||
|
||||
PIOS_EXTI_Init(&pios_exti_overo_cfg);
|
||||
// Create object queues
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Initialise UAVTalk
|
||||
uavTalkCon = UAVTalkInitialize(&packData);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the telemetry module
|
||||
* \return -1 if initialisation failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t OveroSyncStart(void)
|
||||
{
|
||||
if(pios_spi_overo_id == 0)
|
||||
return -1;
|
||||
|
||||
overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync));
|
||||
if(overosync == NULL)
|
||||
return -1;
|
||||
|
||||
overosync->transaction_lock = xSemaphoreCreateMutex();
|
||||
if(overosync->transaction_lock == NULL)
|
||||
return -1;
|
||||
|
||||
overosync->buffer_lock = xSemaphoreCreateMutex();
|
||||
if(overosync->buffer_lock == NULL)
|
||||
return -1;
|
||||
|
||||
overosync->active_transaction_id = 0;
|
||||
overosync->loading_transaction_id = 0;
|
||||
overosync->write_pointer = 0;
|
||||
overosync->sent_bytes = 0;
|
||||
overosync->framesync_error = 0;
|
||||
|
||||
// Process all registered objects and connect queue for updates
|
||||
UAVObjIterate(®isterObject);
|
||||
|
||||
// Start telemetry tasks
|
||||
xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle);
|
||||
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart)
|
||||
|
||||
/**
|
||||
* Register a new object, adds object to local list and connects the queue depending on the object's
|
||||
* telemetry settings.
|
||||
* \param[in] obj Object to connect
|
||||
*/
|
||||
static void registerObject(UAVObjHandle obj)
|
||||
{
|
||||
int32_t eventMask;
|
||||
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
if (UAVObjIsMetaobject(obj)) {
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, queue, eventMask);
|
||||
}
|
||||
|
||||
/**
|
||||
* Telemetry transmit task, regular priority
|
||||
*
|
||||
* Logic: We need to double buffer the DMA transfers. Pack the buffer until either
|
||||
* 1) it is full (and then we should record the number of missed events then)
|
||||
* 2) the current transaction is done (we should immediately schedule since we are slave)
|
||||
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
|
||||
* and then take the semaphrore
|
||||
*/
|
||||
static void overoSyncTask(void *parameters)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
// Kick off SPI transfers (once one is completed another will automatically transmit)
|
||||
overosync->transaction_done = true;
|
||||
overosync->sent_objects = 0;
|
||||
overosync->failed_objects = 0;
|
||||
overosync->received_objects = 0;
|
||||
|
||||
portTickType lastUpdateTime = xTaskGetTickCount();
|
||||
portTickType updateTime;
|
||||
|
||||
// Loop forever
|
||||
while (1) {
|
||||
// Wait for queue message
|
||||
if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
|
||||
// Process event. This calls transmitData
|
||||
UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
|
||||
|
||||
updateTime = xTaskGetTickCount();
|
||||
if(((portTickType) (updateTime - lastUpdateTime)) > 1000) {
|
||||
// Update stats. This will trigger a local send event too
|
||||
OveroSyncStatsData syncStats;
|
||||
syncStats.Send = overosync->sent_bytes;
|
||||
syncStats.Received = 0;
|
||||
syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE;
|
||||
syncStats.DroppedUpdates = overosync->failed_objects;
|
||||
OveroSyncStatsSet(&syncStats);
|
||||
overosync->failed_objects = 0;
|
||||
overosync->sent_bytes = 0;
|
||||
lastUpdateTime = updateTime;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void transmitDataDone(bool crc_ok, uint8_t crc_val)
|
||||
{
|
||||
uint8_t *rx_buffer;
|
||||
static signed portBASE_TYPE xHigherPriorityTaskWoken;
|
||||
|
||||
rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
|
||||
|
||||
// Release the semaphore and start another transaction (which grabs semaphore again but then
|
||||
// returns instantly). Because this is called by the DMA ISR we need to be aware of context
|
||||
// switches.
|
||||
xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
|
||||
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
||||
|
||||
overosync->transaction_done = true;
|
||||
|
||||
// Parse the data from overo
|
||||
for (uint32_t i = 0; rx_buffer[0] != 0 && i < sizeof(rx_buffer) ; i++)
|
||||
UAVTalkProcessInputStream(uavTalkCon, rx_buffer[i]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Transmit data buffer to the modem or USB port.
|
||||
* \param[in] data Data buffer to send
|
||||
* \param[in] length Length of buffer
|
||||
* \return -1 on failure
|
||||
* \return number of bytes transmitted on success
|
||||
*/
|
||||
uint32_t too_long = 0;
|
||||
static int32_t packData(uint8_t * data, int32_t length)
|
||||
{
|
||||
uint8_t *tx_buffer;
|
||||
|
||||
portTickType tickTime = xTaskGetTickCount();
|
||||
|
||||
// Get the lock for manipulating the buffer
|
||||
xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY);
|
||||
|
||||
// Check this packet will fit
|
||||
if ((overosync->write_pointer + length + sizeof(tickTime)) >
|
||||
sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
|
||||
overosync->failed_objects ++;
|
||||
xSemaphoreGive(overosync->buffer_lock);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Get offset into buffer and copy contents
|
||||
tx_buffer = overosync->transactions[overosync->loading_transaction_id].tx_buffer +
|
||||
overosync->write_pointer;
|
||||
memcpy(tx_buffer, &tickTime, sizeof(tickTime));
|
||||
memcpy(tx_buffer + sizeof(tickTime),data,length);
|
||||
overosync->write_pointer += length + sizeof(tickTime);
|
||||
overosync->sent_bytes += length;
|
||||
overosync->sent_objects++;
|
||||
|
||||
xSemaphoreGive(overosync->buffer_lock);
|
||||
|
||||
// When the NSS line rises while we are packing data then a transaction doesn't start
|
||||
// because that means we will be here very shortly afterwards (priority of task making that
|
||||
// not always perfectly true) schedule the transaction here.
|
||||
if (buffer_swap_failed && (PIOS_DELAY_DiffuS(buffer_swap_timeval) < 50)) {
|
||||
buffer_swap_failed = false;
|
||||
transmitData();
|
||||
} else if (buffer_swap_failed) {
|
||||
buffer_swap_failed = false;
|
||||
too_long++;
|
||||
}
|
||||
|
||||
return length;
|
||||
}
|
||||
|
||||
static int32_t transmitData()
|
||||
{
|
||||
uint8_t *tx_buffer, *rx_buffer;
|
||||
static signed portBASE_TYPE xHigherPriorityTaskWoken;
|
||||
|
||||
// Get this lock first so we don't swap buffers and then fail
|
||||
// to start
|
||||
if (xSemaphoreTake(overosync->transaction_lock, 0) == pdFALSE)
|
||||
return -1;
|
||||
|
||||
// Get lock to manipulate buffers
|
||||
if(xSemaphoreTake(overosync->buffer_lock, 0) == pdFALSE) {
|
||||
xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
|
||||
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
||||
buffer_swap_failed = true;
|
||||
buffer_swap_timeval = PIOS_DELAY_GetRaw();
|
||||
return -2;
|
||||
}
|
||||
|
||||
overosync->transaction_done = false;
|
||||
|
||||
// Swap buffers
|
||||
overosync->active_transaction_id = overosync->loading_transaction_id;
|
||||
overosync->loading_transaction_id = (overosync->loading_transaction_id + 1) %
|
||||
NELEMENTS(overosync->transactions);
|
||||
|
||||
tx_buffer = overosync->transactions[overosync->active_transaction_id].tx_buffer;
|
||||
rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
|
||||
|
||||
// Prepare the new loading buffer
|
||||
memset(overosync->transactions[overosync->loading_transaction_id].tx_buffer, 0xff,
|
||||
sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer));
|
||||
overosync->write_pointer = 0;
|
||||
|
||||
xSemaphoreGiveFromISR(overosync->buffer_lock, &xHigherPriorityTaskWoken);
|
||||
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
||||
|
||||
return PIOS_SPI_TransferBlock(pios_spi_overo_id, (uint8_t *) tx_buffer, (uint8_t *) rx_buffer, sizeof(overosync->transactions[overosync->active_transaction_id].tx_buffer), &transmitDataDone) == 0 ? 0 : -3;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
37
flight/Modules/Sensors/inc/sensors.h
Normal file
37
flight/Modules/Sensors/inc/sensors.h
Normal file
@ -0,0 +1,37 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup Sensors Sensors Module
|
||||
* @{
|
||||
*
|
||||
* @file attitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief Acquires sensor data and fuses it into attitude estimate for CC
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef SENSORS_H
|
||||
#define SENSORS_H
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
int32_t SensorsInitialize(void);
|
||||
|
||||
#endif // SENSORS_H
|
||||
480
flight/Modules/Sensors/sensors.c
Normal file
480
flight/Modules/Sensors/sensors.c
Normal file
@ -0,0 +1,480 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup Sensors
|
||||
* @brief Acquires sensor data
|
||||
* Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects
|
||||
* @{
|
||||
*
|
||||
* @file sensors.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
******************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input objects: None, takes sensor data via pios
|
||||
* Output objects: @ref Gyros @ref Accels @ref Magnetometer
|
||||
*
|
||||
* The module executes in its own thread.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
#include "attitude.h"
|
||||
#include "magnetometer.h"
|
||||
#include "accels.h"
|
||||
#include "gyros.h"
|
||||
#include "gyrosbias.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudesettings.h"
|
||||
#include "revocalibration.h"
|
||||
#include "flightstatus.h"
|
||||
#include "gpsposition.h"
|
||||
#include "baroaltitude.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
#include <pios_board_info.h>
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 1540
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
#define SENSOR_PERIOD 2
|
||||
|
||||
#define F_PI 3.14159265358979323846f
|
||||
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle sensorsTaskHandle;
|
||||
static bool gps_updated = false;
|
||||
static bool baro_updated = false;
|
||||
|
||||
// Private functions
|
||||
static void SensorsTask(void *parameters);
|
||||
static void settingsUpdatedCb(UAVObjEvent * objEv);
|
||||
static void sensorsUpdatedCb(UAVObjEvent * objEv);
|
||||
|
||||
// These values are initialized by settings but can be updated by the attitude algorithm
|
||||
static bool bias_correct_gyro = true;
|
||||
|
||||
static float mag_bias[3] = {0,0,0};
|
||||
static float mag_scale[3] = {0,0,0};
|
||||
static float accel_bias[3] = {0,0,0};
|
||||
static float accel_scale[3] = {0,0,0};
|
||||
|
||||
static float R[3][3] = {{0}};
|
||||
static int8_t rotate = 0;
|
||||
|
||||
/**
|
||||
* API for sensor fusion algorithms:
|
||||
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
|
||||
* Stores all the queues the algorithm will pull data from
|
||||
* FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
|
||||
* Update() -- queries queues and updates the attitude estiamte
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* Initialise the module. Called before the start function
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t SensorsInitialize(void)
|
||||
{
|
||||
GyrosInitialize();
|
||||
GyrosBiasInitialize();
|
||||
AccelsInitialize();
|
||||
MagnetometerInitialize();
|
||||
RevoCalibrationInitialize();
|
||||
AttitudeSettingsInitialize();
|
||||
|
||||
rotate = 0;
|
||||
|
||||
RevoCalibrationConnectCallback(&settingsUpdatedCb);
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Start the task. Expects all objects to be initialized by this point.
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t SensorsStart(void)
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(SensorsInitialize, SensorsStart)
|
||||
|
||||
int32_t accel_test;
|
||||
int32_t gyro_test;
|
||||
int32_t mag_test;
|
||||
//int32_t pressure_test;
|
||||
|
||||
|
||||
/**
|
||||
* The sensor task. This polls the gyros at 500 Hz and pumps that data to
|
||||
* stabilization and to the attitude loop
|
||||
*
|
||||
* This function has a lot of if/defs right now to allow these configurations:
|
||||
* 1. BMA180 accel and MPU6000 gyro
|
||||
* 2. MPU6000 gyro and accel
|
||||
* 3. BMA180 accel and L3GD20 gyro
|
||||
*/
|
||||
|
||||
uint32_t sensor_dt_us;
|
||||
static void SensorsTask(void *parameters)
|
||||
{
|
||||
portTickType lastSysTime;
|
||||
uint32_t accel_samples = 0;
|
||||
uint32_t gyro_samples = 0;
|
||||
int32_t accel_accum[3] = {0, 0, 0};
|
||||
int32_t gyro_accum[3] = {0,0,0};
|
||||
float gyro_scaling = 0;
|
||||
float accel_scaling = 0;
|
||||
static int32_t timeval;
|
||||
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
|
||||
|
||||
UAVObjEvent ev;
|
||||
settingsUpdatedCb(&ev);
|
||||
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01:
|
||||
#if defined(PIOS_INCLUDE_L3GD20)
|
||||
gyro_test = PIOS_L3GD20_Test();
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
accel_test = PIOS_BMA180_Test();
|
||||
#endif
|
||||
break;
|
||||
case 0x02:
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
gyro_test = PIOS_MPU6000_Test();
|
||||
accel_test = gyro_test;
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5883)
|
||||
mag_test = PIOS_HMC5883_Test();
|
||||
#endif
|
||||
|
||||
if(accel_test < 0 || gyro_test < 0 || mag_test < 0) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
while(1) {
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
|
||||
vTaskDelay(10);
|
||||
}
|
||||
}
|
||||
|
||||
// If debugging connect callback
|
||||
if(pios_com_aux_id != 0) {
|
||||
BaroAltitudeConnectCallback(&sensorsUpdatedCb);
|
||||
GPSPositionConnectCallback(&sensorsUpdatedCb);
|
||||
}
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
bool error = false;
|
||||
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
|
||||
while (1) {
|
||||
// TODO: add timeouts to the sensor reads and set an error if the fail
|
||||
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
if (error) {
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
error = false;
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
|
||||
}
|
||||
|
||||
int32_t read_good;
|
||||
int32_t count;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_accum[i] = 0;
|
||||
gyro_accum[i] = 0;
|
||||
}
|
||||
accel_samples = 0;
|
||||
gyro_samples = 0;
|
||||
|
||||
AccelsData accelsData;
|
||||
GyrosData gyrosData;
|
||||
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // L3GD20 + BMA180 board
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
{
|
||||
struct pios_bma180_data accel;
|
||||
|
||||
count = 0;
|
||||
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
|
||||
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
|
||||
if (error) {
|
||||
// Unfortunately if the BMA180 ever misses getting read, then it will not
|
||||
// trigger more interrupts. In this case we must force a read to kickstarts
|
||||
// it.
|
||||
struct pios_bma180_data data;
|
||||
PIOS_BMA180_ReadAccels(&data);
|
||||
continue;
|
||||
}
|
||||
while(read_good == 0) {
|
||||
count++;
|
||||
|
||||
accel_accum[0] += accel.x;
|
||||
accel_accum[1] += accel.y;
|
||||
accel_accum[2] += accel.z;
|
||||
|
||||
read_good = PIOS_BMA180_ReadFifo(&accel);
|
||||
}
|
||||
accel_samples = count;
|
||||
accel_scaling = PIOS_BMA180_GetScale();
|
||||
|
||||
// Get temp from last reading
|
||||
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
|
||||
}
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_L3GD20)
|
||||
{
|
||||
struct pios_l3gd20_data gyro;
|
||||
gyro_samples = 0;
|
||||
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
|
||||
|
||||
if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) {
|
||||
error = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
gyro_samples = 1;
|
||||
gyro_accum[0] += gyro.gyro_x;
|
||||
gyro_accum[1] += gyro.gyro_y;
|
||||
gyro_accum[2] += gyro.gyro_z;
|
||||
|
||||
gyro_scaling = PIOS_L3GD20_GetScale();
|
||||
|
||||
// Get temp from last reading
|
||||
gyrosData.temperature = gyro.temperature;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case 0x02: // MPU6000 board
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
{
|
||||
struct pios_mpu6000_data mpu6000_data;
|
||||
xQueueHandle queue = PIOS_MPU6000_GetQueue();
|
||||
|
||||
while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY)
|
||||
{
|
||||
gyro_accum[0] += mpu6000_data.gyro_x;
|
||||
gyro_accum[1] += mpu6000_data.gyro_y;
|
||||
gyro_accum[2] += mpu6000_data.gyro_z;
|
||||
|
||||
accel_accum[0] += mpu6000_data.accel_x;
|
||||
accel_accum[1] += mpu6000_data.accel_y;
|
||||
accel_accum[2] += mpu6000_data.accel_z;
|
||||
|
||||
gyro_samples ++;
|
||||
accel_samples ++;
|
||||
}
|
||||
|
||||
if (gyro_samples == 0) {
|
||||
PIOS_MPU6000_ReadGyros(&mpu6000_data);
|
||||
error = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
gyro_scaling = PIOS_MPU6000_GetScale();
|
||||
accel_scaling = PIOS_MPU6000_GetAccelScale();
|
||||
|
||||
gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
|
||||
accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
break;
|
||||
default:
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
// Scale the accels
|
||||
float accels[3] = {(float) accel_accum[1] / accel_samples,
|
||||
(float) accel_accum[0] / accel_samples,
|
||||
-(float) accel_accum[2] / accel_samples};
|
||||
float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
|
||||
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
|
||||
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]};
|
||||
if (rotate) {
|
||||
rot_mult(R, accels_out, accels);
|
||||
accelsData.x = accels[0];
|
||||
accelsData.y = accels[1];
|
||||
accelsData.z = accels[2];
|
||||
} else {
|
||||
accelsData.x = accels_out[0];
|
||||
accelsData.y = accels_out[1];
|
||||
accelsData.z = accels_out[2];
|
||||
}
|
||||
AccelsSet(&accelsData);
|
||||
|
||||
// Scale the gyros
|
||||
float gyros[3] = {(float) gyro_accum[1] / gyro_samples,
|
||||
(float) gyro_accum[0] / gyro_samples,
|
||||
-(float) gyro_accum[2] / gyro_samples};
|
||||
float gyros_out[3] = {gyros[0] * gyro_scaling,
|
||||
gyros[1] * gyro_scaling,
|
||||
gyros[2] * gyro_scaling};
|
||||
if (rotate) {
|
||||
rot_mult(R, gyros_out, gyros);
|
||||
gyrosData.x = gyros[0];
|
||||
gyrosData.y = gyros[1];
|
||||
gyrosData.z = gyros[2];
|
||||
} else {
|
||||
gyrosData.x = gyros_out[0];
|
||||
gyrosData.y = gyros_out[1];
|
||||
gyrosData.z = gyros_out[2];
|
||||
}
|
||||
|
||||
if (bias_correct_gyro) {
|
||||
// Apply bias correction to the gyros
|
||||
GyrosBiasData gyrosBias;
|
||||
GyrosBiasGet(&gyrosBias);
|
||||
gyrosData.x += gyrosBias.x;
|
||||
gyrosData.y += gyrosBias.y;
|
||||
gyrosData.z += gyrosBias.z;
|
||||
}
|
||||
GyrosSet(&gyrosData);
|
||||
|
||||
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
|
||||
// and make it average zero (weakly)
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5883)
|
||||
MagnetometerData mag;
|
||||
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
|
||||
int16_t values[3];
|
||||
PIOS_HMC5883_ReadMag(values);
|
||||
float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0],
|
||||
(float) values[0] * mag_scale[1] - mag_bias[1],
|
||||
-(float) values[2] * mag_scale[2] - mag_bias[2]};
|
||||
if (rotate) {
|
||||
float mag_out[3];
|
||||
rot_mult(R, mags, mag_out);
|
||||
mag.x = mag_out[0];
|
||||
mag.y = mag_out[1];
|
||||
mag.z = mag_out[2];
|
||||
} else {
|
||||
mag.x = mags[0];
|
||||
mag.y = mags[1];
|
||||
mag.z = mags[2];
|
||||
}
|
||||
MagnetometerSet(&mag);
|
||||
mag_update_time = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
#endif
|
||||
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
|
||||
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // L3GD20 + BMA180 board
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
break;
|
||||
case 0x02:
|
||||
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
|
||||
break;
|
||||
default:
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Indicate that these sensors have been updated
|
||||
*/
|
||||
static void sensorsUpdatedCb(UAVObjEvent * objEv)
|
||||
{
|
||||
if(objEv->obj == GPSPositionHandle())
|
||||
gps_updated = true;
|
||||
if(objEv->obj == BaroAltitudeHandle())
|
||||
baro_updated = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Locally cache some variables from the AtttitudeSettings object
|
||||
*/
|
||||
static void settingsUpdatedCb(UAVObjEvent * objEv) {
|
||||
RevoCalibrationData cal;
|
||||
RevoCalibrationGet(&cal);
|
||||
|
||||
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
|
||||
mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y];
|
||||
mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z];
|
||||
mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X];
|
||||
mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y];
|
||||
mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z];
|
||||
accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X];
|
||||
accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y];
|
||||
accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z];
|
||||
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
|
||||
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
|
||||
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
|
||||
|
||||
AttitudeSettingsData attitudeSettings;
|
||||
AttitudeSettingsGet(&attitudeSettings);
|
||||
|
||||
// Indicates not to expend cycles on rotation
|
||||
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
|
||||
attitudeSettings.BoardRotation[2] == 0) {
|
||||
rotate = 0;
|
||||
} else {
|
||||
float rotationQuat[4];
|
||||
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
|
||||
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
|
||||
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
|
||||
RPY2Quaternion(rpy, rotationQuat);
|
||||
Quaternion2R(rotationQuat, R);
|
||||
rotate = 1;
|
||||
}
|
||||
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -38,7 +38,7 @@
|
||||
#include "ratedesired.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attituderaw.h"
|
||||
#include "gyros.h"
|
||||
#include "flightstatus.h"
|
||||
#include "manualcontrol.h" // Just to get a macro
|
||||
#include "CoordinateConversions.h"
|
||||
@ -124,7 +124,7 @@ int32_t StabilizationInitialize()
|
||||
|
||||
// Listen for updates.
|
||||
// AttitudeActualConnectQueue(queue);
|
||||
AttitudeRawConnectQueue(queue);
|
||||
GyrosConnectQueue(queue);
|
||||
|
||||
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
|
||||
SettingsUpdatedCb(StabilizationSettingsHandle());
|
||||
@ -140,22 +140,20 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
|
||||
*/
|
||||
static void stabilizationTask(void* parameters)
|
||||
{
|
||||
portTickType lastSysTime;
|
||||
portTickType thisSysTime;
|
||||
UAVObjEvent ev;
|
||||
|
||||
uint32_t timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
ActuatorDesiredData actuatorDesired;
|
||||
StabilizationDesiredData stabDesired;
|
||||
RateDesiredData rateDesired;
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeRawData attitudeRaw;
|
||||
GyrosData gyrosData;
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
SettingsUpdatedCb((UAVObjEvent *) NULL);
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
ZeroPids();
|
||||
while(1) {
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
|
||||
@ -167,16 +165,13 @@ static void stabilizationTask(void* parameters)
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check how long since last update
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
|
||||
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
|
||||
lastSysTime = thisSysTime;
|
||||
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
AttitudeRawGet(&attitudeRaw);
|
||||
GyrosGet(&gyrosData);
|
||||
|
||||
#if defined(DIAGNOSTICS)
|
||||
RateDesiredGet(&rateDesired);
|
||||
@ -216,13 +211,13 @@ static void stabilizationTask(void* parameters)
|
||||
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
|
||||
stabDesired.Pitch - attitudeActual.Pitch,
|
||||
stabDesired.Yaw - attitudeActual.Yaw};
|
||||
local_error[2] = fmod(local_error[2] + 180, 360) - 180;
|
||||
local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
|
||||
#endif
|
||||
|
||||
|
||||
for(uint8_t i = 0; i < MAX_AXES; i++) {
|
||||
gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha);
|
||||
}
|
||||
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
|
||||
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
|
||||
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
|
||||
|
||||
float *attitudeDesiredAxis = &stabDesired.Roll;
|
||||
float *actuatorDesiredAxis = &actuatorDesired.Roll;
|
||||
@ -373,24 +368,24 @@ float ApplyPid(pid_type * pid, const float err)
|
||||
pid->lastErr = err;
|
||||
|
||||
// Scale up accumulator by 1000 while computing to avoid losing precision
|
||||
pid->iAccumulator += err * (pid->i * dT * 1000);
|
||||
if(pid->iAccumulator > (pid->iLim * 1000)) {
|
||||
pid->iAccumulator = pid->iLim * 1000;
|
||||
} else if (pid->iAccumulator < -(pid->iLim * 1000)) {
|
||||
pid->iAccumulator = -pid->iLim * 1000;
|
||||
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
|
||||
if(pid->iAccumulator > (pid->iLim * 1000.0f)) {
|
||||
pid->iAccumulator = pid->iLim * 1000.0f;
|
||||
} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
|
||||
pid->iAccumulator = -pid->iLim * 1000.0f;
|
||||
}
|
||||
return ((err * pid->p) + pid->iAccumulator / 1000 + (diff * pid->d / dT));
|
||||
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
|
||||
}
|
||||
|
||||
|
||||
static void ZeroPids(void)
|
||||
{
|
||||
for(int8_t ct = 0; ct < PID_MAX; ct++) {
|
||||
pids[ct].iAccumulator = 0;
|
||||
pids[ct].lastErr = 0;
|
||||
pids[ct].iAccumulator = 0.0f;
|
||||
pids[ct].lastErr = 0.0f;
|
||||
}
|
||||
for(uint8_t i = 0; i < 3; i++)
|
||||
axis_lock_accum[i] = 0;
|
||||
axis_lock_accum[i] = 0.0f;
|
||||
}
|
||||
|
||||
|
||||
@ -399,10 +394,10 @@ static void ZeroPids(void)
|
||||
*/
|
||||
static float bound(float val)
|
||||
{
|
||||
if(val < -1) {
|
||||
val = -1;
|
||||
} else if(val > 1) {
|
||||
val = 1;
|
||||
if(val < -1.0f) {
|
||||
val = -1.0f;
|
||||
} else if(val > 1.0f) {
|
||||
val = 1.0f;
|
||||
}
|
||||
return val;
|
||||
}
|
||||
@ -466,7 +461,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
if(settings.GyroTau < 0.0001)
|
||||
gyro_alpha = 0; // not trusting this to resolve to 0
|
||||
else
|
||||
gyro_alpha = exp(-fakeDt / settings.GyroTau);
|
||||
gyro_alpha = expf(-fakeDt / settings.GyroTau);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -384,7 +384,7 @@ static void updateStats()
|
||||
if (now > lastTickCount) {
|
||||
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
|
||||
stats.CPULoad =
|
||||
100 - (uint8_t) round(100.0 * ((float)idleCounter / ((float)dT / 1000.0)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
|
||||
100 - (uint8_t) roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
|
||||
} //else: TickCount has wrapped, do not calc now
|
||||
lastTickCount = now;
|
||||
idleCounterClear = 1;
|
||||
@ -457,11 +457,21 @@ static void updateSystemAlarms()
|
||||
EventGetStats(&evStats);
|
||||
UAVObjClearStats();
|
||||
EventClearStats();
|
||||
if (objStats.eventErrors > 0 || evStats.eventErrors > 0) {
|
||||
if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
|
||||
}
|
||||
|
||||
if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
|
||||
SystemStatsData sysStats;
|
||||
SystemStatsGet(&sysStats);
|
||||
sysStats.EventSystemWarningID = evStats.lastErrorID;
|
||||
sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
|
||||
sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;
|
||||
SystemStatsSet(&sysStats);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -73,7 +73,7 @@ static void telemetryTxTask(void *parameters);
|
||||
static void telemetryRxTask(void *parameters);
|
||||
static int32_t transmitData(uint8_t * data, int32_t length);
|
||||
static void registerObject(UAVObjHandle obj);
|
||||
static void updateObject(UAVObjHandle obj);
|
||||
static void updateObject(UAVObjHandle obj, int32_t eventType);
|
||||
static int32_t addObject(UAVObjHandle obj);
|
||||
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
|
||||
static void processObjEvent(UAVObjEvent * ev);
|
||||
@ -158,32 +158,34 @@ static void registerObject(UAVObjHandle obj)
|
||||
addObject(obj);
|
||||
|
||||
// Setup object for telemetry updates
|
||||
updateObject(obj);
|
||||
updateObject(obj, EV_NONE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update object's queue connections and timer, depending on object's settings
|
||||
* \param[in] obj Object to updates
|
||||
*/
|
||||
static void updateObject(UAVObjHandle obj)
|
||||
static void updateObject(UAVObjHandle obj, int32_t eventType)
|
||||
{
|
||||
UAVObjMetadata metadata;
|
||||
UAVObjUpdateMode updateMode;
|
||||
int32_t eventMask;
|
||||
|
||||
// Get metadata
|
||||
UAVObjGetMetadata(obj, &metadata);
|
||||
updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
|
||||
|
||||
// Setup object depending on update mode
|
||||
if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC) {
|
||||
if (updateMode == UPDATEMODE_PERIODIC) {
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
|
||||
// Connect queue
|
||||
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
if (UAVObjIsMetaobject(obj)) {
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, priorityQueue, eventMask);
|
||||
} else if (metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE) {
|
||||
} else if (updateMode == UPDATEMODE_ONCHANGE) {
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, 0);
|
||||
// Connect queue
|
||||
@ -192,7 +194,22 @@ static void updateObject(UAVObjHandle obj)
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, priorityQueue, eventMask);
|
||||
} else if (metadata.telemetryUpdateMode == UPDATEMODE_MANUAL) {
|
||||
} else if (updateMode == UPDATEMODE_THROTTLED) {
|
||||
if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) {
|
||||
// If we received a periodic update, we can change back to update on change
|
||||
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
// Set update period on initialization and metadata change
|
||||
if (eventType == EV_NONE)
|
||||
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
|
||||
} else {
|
||||
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
|
||||
eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
}
|
||||
if (UAVObjIsMetaobject(obj)) {
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, priorityQueue, eventMask);
|
||||
} else if (updateMode == UPDATEMODE_MANUAL) {
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, 0);
|
||||
// Connect queue
|
||||
@ -201,11 +218,6 @@ static void updateObject(UAVObjHandle obj)
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, priorityQueue, eventMask);
|
||||
} else if (metadata.telemetryUpdateMode == UPDATEMODE_NEVER) {
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, 0);
|
||||
// Disconnect queue
|
||||
UAVObjDisconnectQueue(obj, priorityQueue);
|
||||
}
|
||||
}
|
||||
|
||||
@ -215,6 +227,7 @@ static void updateObject(UAVObjHandle obj)
|
||||
static void processObjEvent(UAVObjEvent * ev)
|
||||
{
|
||||
UAVObjMetadata metadata;
|
||||
UAVObjUpdateMode updateMode;
|
||||
FlightTelemetryStatsData flightStats;
|
||||
int32_t retries;
|
||||
int32_t success;
|
||||
@ -226,16 +239,17 @@ static void processObjEvent(UAVObjEvent * ev)
|
||||
} else {
|
||||
// Only process event if connected to GCS or if object FlightTelemetryStats is updated
|
||||
FlightTelemetryStatsGet(&flightStats);
|
||||
// Get object metadata
|
||||
UAVObjGetMetadata(ev->obj, &metadata);
|
||||
updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
|
||||
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle()) {
|
||||
// Get object metadata
|
||||
UAVObjGetMetadata(ev->obj, &metadata);
|
||||
// Act on event
|
||||
retries = 0;
|
||||
success = -1;
|
||||
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
|
||||
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
|
||||
// Send update to GCS (with retries)
|
||||
while (retries < MAX_RETRIES && success == -1) {
|
||||
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
|
||||
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
|
||||
++retries;
|
||||
}
|
||||
// Update stats
|
||||
@ -257,9 +271,13 @@ static void processObjEvent(UAVObjEvent * ev)
|
||||
}
|
||||
// If this is a metaobject then make necessary telemetry updates
|
||||
if (UAVObjIsMetaobject(ev->obj)) {
|
||||
updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for
|
||||
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for
|
||||
}
|
||||
}
|
||||
if((updateMode == UPDATEMODE_THROTTLED) && !UAVObjIsMetaobject(ev->obj)) {
|
||||
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
|
||||
updateObject(ev->obj, ev->event);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -376,7 +394,7 @@ static int32_t addObject(UAVObjHandle obj)
|
||||
// Add object for periodic updates
|
||||
ev.obj = obj;
|
||||
ev.instId = UAVOBJ_ALL_INSTANCES;
|
||||
ev.event = EV_UPDATED_MANUAL;
|
||||
ev.event = EV_UPDATED_PERIODIC;
|
||||
return EventPeriodicQueueCreate(&ev, queue, 0);
|
||||
}
|
||||
|
||||
@ -394,7 +412,7 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
|
||||
// Add object for periodic updates
|
||||
ev.obj = obj;
|
||||
ev.instId = UAVOBJ_ALL_INSTANCES;
|
||||
ev.event = EV_UPDATED_MANUAL;
|
||||
ev.event = EV_UPDATED_PERIODIC;
|
||||
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
|
||||
}
|
||||
|
||||
@ -513,8 +531,8 @@ static void updateTelemetryStats()
|
||||
*/
|
||||
static void updateSettings()
|
||||
{
|
||||
|
||||
if (telemetryPort) {
|
||||
|
||||
// Retrieve settings
|
||||
uint8_t speed;
|
||||
HwSettingsTelemetrySpeedGet(&speed);
|
||||
|
||||
@ -1,665 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot project build PiOS and the AP.
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES to compile for debugging
|
||||
DEBUG ?= YES
|
||||
|
||||
# Include objects that are just nice information to show
|
||||
DIAGNOSTICS ?= YES
|
||||
|
||||
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
|
||||
ENABLE_DEBUG_PINS ?= NO
|
||||
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
#
|
||||
USE_BOOTLOADER ?= NO
|
||||
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= NO
|
||||
|
||||
# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
|
||||
TCHAIN_PREFIX ?= ""
|
||||
|
||||
# Remove command is different for Code Sourcery on Windows
|
||||
REMOVE_CMD ?= rm
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# YES enables -mthumb option to flags for source-files listed
|
||||
# in SRC and CPPSRC
|
||||
USE_THUMB_MODE = YES
|
||||
|
||||
# List of modules to include
|
||||
OPTMODULES = CameraStab GPS
|
||||
MODULES = Telemetry Actuator Stabilization Guidance ManualControl
|
||||
#MODULES = Telemetry ManualControl Actuator Attitude Stabilization
|
||||
#MODULES = Telemetry Example
|
||||
#MODULES = Telemetry MK/MKSerial
|
||||
PYMODULES = FlightPlan
|
||||
|
||||
#MODULES += Osd/OsdEtStd
|
||||
|
||||
|
||||
# MCU name, submodel and board
|
||||
# - MCU used for compiler-option (-mtune)
|
||||
# - MODEL used for linker-script name (-T) and passed as define
|
||||
# - BOARD just passed as define (optional)
|
||||
MCU = i686
|
||||
#CHIP = STM32F103RET
|
||||
#BOARD = STM3210E_OP
|
||||
MODEL = HD
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
BOOT_MODEL = $(MODEL)_BL
|
||||
|
||||
else
|
||||
BOOT_MODEL = $(MODEL)_NB
|
||||
endif
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR = ../../build/sitl_posix
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET = OpenPilot
|
||||
|
||||
# Paths
|
||||
OPSYSTEM = ./System
|
||||
OPSYSTEMINC = $(OPSYSTEM)/inc
|
||||
OPUAVTALK = ../UAVTalk
|
||||
OPUAVTALKINC = $(OPUAVTALK)/inc
|
||||
OPUAVOBJ = ../UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPTESTS = ./Tests
|
||||
OPMODULEDIR = ../Modules
|
||||
FLIGHTLIB = ../Libraries
|
||||
FLIGHTLIBINC = $(FLIGHTLIB)/inc
|
||||
PIOS = ../PiOS.posix
|
||||
PIOSINC = $(PIOS)/inc
|
||||
PIOSPOSIX = $(PIOS)/posix
|
||||
APPLIBDIR = $(PIOSPOSIX)/Libraries
|
||||
RTOSDIR = $(APPLIBDIR)/FreeRTOS
|
||||
RTOSSRCDIR = $(RTOSDIR)/Source
|
||||
RTOSINCDIR = $(RTOSSRCDIR)/include
|
||||
DOXYGENDIR = ../Doc/Doxygen
|
||||
AHRSBOOTLOADER = ../Bootloaders/AHRS/
|
||||
AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
|
||||
PYMITE = $(FLIGHTLIB)/PyMite
|
||||
PYMITELIB = $(PYMITE)/lib
|
||||
PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
|
||||
PYMITETOOLS = $(PYMITE)/tools
|
||||
PYMITEVM = $(PYMITE)/vm
|
||||
PYMITEINC = $(PYMITEVM)
|
||||
PYMITEINC += $(PYMITEPLAT)
|
||||
PYMITEINC += $(OUTDIR)
|
||||
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
|
||||
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
|
||||
|
||||
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
|
||||
UAVOBJPYTHONSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/python
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
MODNAMES = $(notdir ${MODULES})
|
||||
MODNAMES += $(notdir ${OPTMODULES})
|
||||
|
||||
ifndef TESTAPP
|
||||
|
||||
## PyMite files
|
||||
SRC += $(OUTDIR)/pmlib_img.c
|
||||
SRC += $(OUTDIR)/pmlib_nat.c
|
||||
SRC += $(OUTDIR)/pmlibusr_img.c
|
||||
SRC += $(OUTDIR)/pmlibusr_nat.c
|
||||
PYSRC += $(wildcard ${PYMITEVM}/*.c)
|
||||
PYSRC += $(wildcard ${PYMITEPLAT}/*.c)
|
||||
PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += $(PYSRC)
|
||||
|
||||
## MODULES
|
||||
SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += ${OUTDIR}/InitMods.c
|
||||
## OPENPILOT CORE:
|
||||
SRC += ${OPMODULEDIR}/System/systemmod.c
|
||||
SRC += $(OPSYSTEM)/openpilot.c
|
||||
SRC += $(OPSYSTEM)/pios_board_posix.c
|
||||
SRC += $(OPSYSTEM)/alarms.c
|
||||
SRC += $(OPUAVTALK)/uavtalk.c
|
||||
SRC += $(OPUAVOBJ)/uavobjectmanager.c
|
||||
SRC += $(OPUAVOBJ)/eventdispatcher.c
|
||||
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
|
||||
else
|
||||
## TESTCODE
|
||||
SRC += $(OPTESTS)/test_common.c
|
||||
SRC += $(OPTESTS)/$(TESTAPP).c
|
||||
endif
|
||||
|
||||
|
||||
|
||||
## UAVOBJECTS
|
||||
ifndef TESTAPP
|
||||
#include $(UAVOBJSYNTHDIR)/Makefile.inc
|
||||
include ./UAVObjects.inc
|
||||
SRC += $(UAVOBJSRC)
|
||||
CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
|
||||
endif
|
||||
|
||||
## PIOS Hardware (posix)
|
||||
SRC += $(PIOSPOSIX)/pios_crc.c
|
||||
SRC += $(PIOSPOSIX)/pios_sys.c
|
||||
SRC += $(PIOSPOSIX)/pios_led.c
|
||||
SRC += $(PIOSPOSIX)/pios_irq.c
|
||||
SRC += $(PIOSPOSIX)/pios_delay.c
|
||||
SRC += $(PIOSPOSIX)/pios_sdcard.c
|
||||
SRC += $(PIOSPOSIX)/pios_udp.c
|
||||
SRC += $(PIOSPOSIX)/pios_com.c
|
||||
SRC += $(PIOSPOSIX)/pios_servo.c
|
||||
SRC += $(PIOSPOSIX)/pios_wdg.c
|
||||
SRC += $(PIOSPOSIX)/pios_debug.c
|
||||
|
||||
SRC += $(PIOSPOSIX)/pios_rcvr.c
|
||||
|
||||
## Libraries for flight calculations
|
||||
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
SRC += $(FLIGHTLIB)/taskmonitor.c
|
||||
## RTOS and RTOS Portable
|
||||
SRC += $(RTOSSRCDIR)/list.c
|
||||
SRC += $(RTOSSRCDIR)/queue.c
|
||||
UNAME := $(shell uname)
|
||||
ifeq ($(UNAME), Linux)
|
||||
SRC += $(RTOSSRCDIR)/tasks_linux.c
|
||||
SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port_linux.c
|
||||
else
|
||||
SRC += $(RTOSSRCDIR)/tasks_posix.c
|
||||
SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port_posix.c
|
||||
endif
|
||||
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
|
||||
|
||||
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS = $(OPSYSTEM)
|
||||
EXTRAINCDIRS += $(OPSYSTEMINC)
|
||||
EXTRAINCDIRS += $(OPUAVTALK)
|
||||
EXTRAINCDIRS += $(OPUAVTALKINC)
|
||||
EXTRAINCDIRS += $(OPUAVOBJ)
|
||||
EXTRAINCDIRS += $(OPUAVOBJINC)
|
||||
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PIOSPOSIX)
|
||||
EXTRAINCDIRS += $(RTOSINCDIR)
|
||||
EXTRAINCDIRS += $(APPLIBDIR)
|
||||
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/Posix
|
||||
EXTRAINCDIRS += $(PYMITEINC)
|
||||
|
||||
EXTRAINCDIRS += ${foreach MOD, ${PYMODULES} ${OPTMODULES} ${MODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
|
||||
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
OPT = 0
|
||||
else
|
||||
OPT = s
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
#DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
ifeq ($(ENABLE_DEBUG_PINS), YES)
|
||||
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
|
||||
endif
|
||||
ifeq ($(ENABLE_AUX_UART), YES)
|
||||
CDEFS += -DPIOS_ENABLE_AUX_UART
|
||||
endif
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
CDEFS += -DUSE_BOOTLOADER
|
||||
endif
|
||||
|
||||
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS = -g$(DEBUGF) -DDEBUG
|
||||
endif
|
||||
|
||||
ifeq ($(DIAGNOSTICS),YES)
|
||||
CFLAGS = -DDIAGNOSTICS
|
||||
endif
|
||||
|
||||
CFLAGS += $(CFLAGS_UAVOBJECTS)
|
||||
CFLAGS += -DARCH_POSIX
|
||||
CFLAGS += -O$(OPT)
|
||||
CFLAGS += -mtune=$(MCU)
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mtune=$(MCU) -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
LDFLAGS += -lpthread
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
|
||||
|
||||
|
||||
# Define programs and commands.
|
||||
CC = $(TCHAIN_PREFIX)gcc
|
||||
CPP = $(TCHAIN_PREFIX)g++
|
||||
AR = $(TCHAIN_PREFIX)ar
|
||||
OBJCOPY = $(TCHAIN_PREFIX)objcopy
|
||||
OBJDUMP = $(TCHAIN_PREFIX)objdump
|
||||
SIZE = $(TCHAIN_PREFIX)size
|
||||
NM = $(TCHAIN_PREFIX)nm
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
PYTHON = python
|
||||
###SHELL = sh
|
||||
###COPY = cp
|
||||
|
||||
|
||||
|
||||
# Define Messages
|
||||
# English
|
||||
MSG_ERRORS_NONE = Errors: none
|
||||
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
|
||||
MSG_END = ${quote}-------- end --------${quote}
|
||||
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
|
||||
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
|
||||
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
|
||||
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
|
||||
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
|
||||
MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
|
||||
MSG_LINKING = ${quote}**** Linking :${quote}
|
||||
MSG_COMPILING = ${quote}**** Compiling C :${quote}
|
||||
MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
|
||||
MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
|
||||
MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
|
||||
MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
|
||||
MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
|
||||
MSG_CLEANING = ${quote}Cleaning project:${quote}
|
||||
MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
|
||||
MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
|
||||
MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
|
||||
MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
# Default target.
|
||||
#all: begin gccversion sizebefore build sizeafter finished end
|
||||
#all: begin gencode gccversion build sizeafter finished end
|
||||
all: elf
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# Test if quotes are needed for the echo-command
|
||||
result = ${shell echo "test"}
|
||||
ifeq (${result}, test)
|
||||
quote = '
|
||||
else
|
||||
quote =
|
||||
endif
|
||||
|
||||
# Generate code for module initialization
|
||||
${OUTDIR}/InitMods.c: Makefile.posix
|
||||
@echo ${MSG_MODINIT}
|
||||
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Start(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void StartModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Start();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
|
||||
# Generate code for PyMite
|
||||
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py)
|
||||
@echo $(MSG_PYMITEINIT) $(call toprel, $@)
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
|
||||
|
||||
# Eye candy.
|
||||
begin:
|
||||
## @echo
|
||||
@echo $(MSG_BEGIN)
|
||||
|
||||
finished:
|
||||
## @echo $(MSG_ERRORS_NONE)
|
||||
|
||||
end:
|
||||
@echo $(MSG_END)
|
||||
## @echo
|
||||
|
||||
# Display sizes of sections.
|
||||
ELFSIZE = $(SIZE) -A $(OUTDIR)/$(TARGET).elf
|
||||
##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
|
||||
sizebefore:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
|
||||
|
||||
sizeafter:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
|
||||
@echo $(MSG_SIZE_AFTER)
|
||||
$(ELFSIZE)
|
||||
|
||||
# Display compiler version information.
|
||||
gccversion :
|
||||
@$(CC) --version
|
||||
# @echo $(ALLOBJ)
|
||||
|
||||
# Program the device.
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
# Program the device with OP Upload Tool".
|
||||
program: $(OUTDIR)/$(TARGET).bin
|
||||
@echo ${quote}Programming with OP Upload Tool${quote}
|
||||
../../ground/src/experimental/upload-build-desktop/debug/OPUploadTool -d 0 -p $(OUTDIR)/$(TARGET).bin
|
||||
else
|
||||
ifeq ($(FLASH_TOOL),OPENOCD)
|
||||
# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
|
||||
program: $(OUTDIR)/$(TARGET).elf
|
||||
@echo ${quote}Programming with OPENOCD${quote}
|
||||
$(OOCD_EXE) $(OOCD_CL)
|
||||
endif
|
||||
endif
|
||||
|
||||
# Create final output file (.hex) from ELF output file.
|
||||
%.hex: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O ihex $< $@
|
||||
|
||||
# Create final output file (.bin) from ELF output file.
|
||||
%.bin: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O binary $< $@
|
||||
|
||||
# Create extended listing file/disassambly from ELF output file.
|
||||
# using objdump testing: option -C
|
||||
%.lss: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_EXTENDED_LISTING) $@
|
||||
$(OBJDUMP) -h -S -C -r $< > $@
|
||||
# $(OBJDUMP) -x -S $< > $@
|
||||
|
||||
# Create a symbol table from ELF output file.
|
||||
%.sym: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_SYMBOL_TABLE) $@
|
||||
$(NM) -n $< > $@
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
.SECONDARY : $(TARGET).elf
|
||||
.PRECIOUS : $(ALLOBJ)
|
||||
%.elf: $(ALLOBJ)
|
||||
@echo $(MSG_LINKING) $@
|
||||
# use $(CC) for C-only projects or $(CPP) for C++-projects:
|
||||
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
define ASSEMBLE_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
define ASSEMBLE_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
|
||||
$(CC) -c $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
define COMPILE_C_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
define COMPILE_C_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
define COMPILE_CPP_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
define COMPILE_CPP_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(SRC:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC) $< to $@
|
||||
$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(SRCARM:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC_ARM) $< to $@
|
||||
$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Target: clean project.
|
||||
clean: begin clean_list finished end
|
||||
|
||||
clean_list :
|
||||
## @echo
|
||||
@echo $(MSG_CLEANING)
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(REMOVE) $(wildcard $(OUTDIR)/*.c)
|
||||
$(REMOVE) $(wildcard $(OUTDIR)/*.h)
|
||||
$(REMOVE) $(ALLOBJ)
|
||||
$(REMOVE) $(LSTFILES)
|
||||
$(REMOVE) $(DEPFILES)
|
||||
$(REMOVE) $(SRC:.c=.s)
|
||||
$(REMOVE) $(SRCARM:.c=.s)
|
||||
$(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(OUTDIR) 2>NUL)
|
||||
else
|
||||
$(shell mkdir $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(OUTDIR)\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all begin finish end sizebefore sizeafter gccversion \
|
||||
build elf hex bin lss sym clean clean_list program
|
||||
|
||||
@ -1,637 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot project build PiOS and the AP.
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES to compile for debugging
|
||||
DEBUG ?= YES
|
||||
|
||||
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
|
||||
ENABLE_DEBUG_PINS ?= NO
|
||||
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
#
|
||||
USE_BOOTLOADER ?= NO
|
||||
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= NO
|
||||
|
||||
# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
|
||||
TCHAIN_PREFIX ?= mingw32-
|
||||
|
||||
# Remove command is different for Code Sourcery on Windows
|
||||
REMOVE_CMD ?= rm
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# YES enables -mthumb option to flags for source-files listed
|
||||
# in SRC and CPPSRC
|
||||
USE_THUMB_MODE = YES
|
||||
|
||||
# List of modules to include
|
||||
MODULES = Telemetry Actuator Stabilization Guidance ManualControl FlightPlan
|
||||
#MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization
|
||||
#MODULES = Telemetry Example
|
||||
#MODULES = Telemetry MK/MKSerial
|
||||
|
||||
#MODULES += Osd/OsdEtStd
|
||||
|
||||
|
||||
# MCU name, submodel and board
|
||||
# - MCU used for compiler-option (-mtune)
|
||||
# - MODEL used for linker-script name (-T) and passed as define
|
||||
# - BOARD just passed as define (optional)
|
||||
MCU = i686
|
||||
#CHIP = STM32F103RET
|
||||
#BOARD = STM3210E_OP
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
MODEL = HD_BL
|
||||
|
||||
else
|
||||
MODEL = HD_NB
|
||||
endif
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR = ../../build/sitl_win32
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET = OpenPilot
|
||||
|
||||
# Paths
|
||||
OPSYSTEM = ./System
|
||||
OPSYSTEMINC = $(OPSYSTEM)/inc
|
||||
OPUAVTALK = ../UAVTalk
|
||||
OPUAVTALKINC = $(OPUAVTALK)/inc
|
||||
OPUAVOBJ = ../UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPTESTS = ./Tests
|
||||
OPMODULEDIR = ../Modules
|
||||
FLIGHTLIB = ../Libraries
|
||||
FLIGHTLIBINC = ../Libraries/inc
|
||||
PIOS = ../PiOS.win32
|
||||
PIOSINC = $(PIOS)/inc
|
||||
PIOSWIN32 = $(PIOS)/win32
|
||||
APPLIBDIR = $(PIOSWIN32)/Libraries
|
||||
RTOSDIR = $(APPLIBDIR)/FreeRTOS
|
||||
RTOSSRCDIR = $(RTOSDIR)/Source
|
||||
RTOSINCDIR = $(RTOSSRCDIR)/include
|
||||
DOXYGENDIR = ../Doc/Doxygen
|
||||
PYMITE = $(FLIGHTLIB)/PyMite
|
||||
PYMITELIB = $(PYMITE)/lib
|
||||
PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
|
||||
PYMITETOOLS = $(PYMITE)/tools
|
||||
PYMITEVM = $(PYMITE)/vm
|
||||
PYMITEINC = $(PYMITEVM)
|
||||
PYMITEINC += $(PYMITEPLAT)
|
||||
PYMITEINC += $(OUTDIR)
|
||||
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
|
||||
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
|
||||
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
MODNAMES = $(notdir ${MODULES})
|
||||
|
||||
ifndef TESTAPP
|
||||
|
||||
## PyMite files
|
||||
SRC += $(OUTDIR)/pmlib_img.c
|
||||
SRC += $(OUTDIR)/pmlib_nat.c
|
||||
SRC += $(OUTDIR)/pmlibusr_img.c
|
||||
SRC += $(OUTDIR)/pmlibusr_nat.c
|
||||
SRC += $(wildcard ${PYMITEVM}/*.c)
|
||||
SRC += $(wildcard ${PYMITEPLAT}/*.c)
|
||||
|
||||
## MODULES
|
||||
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += ${OUTDIR}/InitMods.c
|
||||
## OPENPILOT CORE:
|
||||
SRC += ${OPMODULEDIR}/System/systemmod.c
|
||||
SRC += $(OPSYSTEM)/openpilot.c
|
||||
SRC += $(OPSYSTEM)/pios_board_posix.c
|
||||
SRC += $(OPSYSTEM)/alarms.c
|
||||
SRC += $(OPSYSTEM)/taskmonitor.c
|
||||
SRC += $(OPUAVTALK)/uavtalk.c
|
||||
SRC += $(OPUAVOBJ)/uavobjectmanager.c
|
||||
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
|
||||
SRC += $(OPUAVOBJ)/eventdispatcher.c
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
|
||||
else
|
||||
## TESTCODE
|
||||
SRC += $(OPTESTS)/test_common.c
|
||||
SRC += $(OPTESTS)/$(TESTAPP).c
|
||||
endif
|
||||
|
||||
|
||||
## UAVOBJECTS
|
||||
ifndef TESTAPP
|
||||
#include $(UAVOBJSYNTHDIR)/Makefile.inc
|
||||
include ./UAVObjects.inc
|
||||
SRC += $(UAVOBJSRC)
|
||||
CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
|
||||
endif
|
||||
|
||||
## PIOS Hardware (win32)
|
||||
SRC += $(PIOSWIN32)/pios_sys.c
|
||||
SRC += $(PIOSWIN32)/pios_led.c
|
||||
SRC += $(PIOSWIN32)/pios_delay.c
|
||||
SRC += $(PIOSWIN32)/pios_sdcard.c
|
||||
SRC += $(PIOSWIN32)/pios_udp.c
|
||||
SRC += $(PIOSWIN32)/pios_com.c
|
||||
SRC += $(PIOSWIN32)/pios_servo.c
|
||||
SRC += $(PIOSWIN32)/pios_wdg.c
|
||||
SRC += $(PIOSWIN32)/pios_crc.c
|
||||
#
|
||||
## RTOS
|
||||
SRC += $(RTOSSRCDIR)/list.c
|
||||
SRC += $(RTOSSRCDIR)/queue.c
|
||||
SRC += $(RTOSSRCDIR)/tasks.c
|
||||
#
|
||||
## RTOS Portable
|
||||
SRC += $(RTOSSRCDIR)/portable/GCC/Win32/port.c
|
||||
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
|
||||
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS = $(PIOSINC)
|
||||
EXTRAINCDIRS += $(OPSYSTEM)
|
||||
EXTRAINCDIRS += $(OPSYSTEMINC)
|
||||
EXTRAINCDIRS += $(OPUAVTALK)
|
||||
EXTRAINCDIRS += $(OPUAVTALKINC)
|
||||
EXTRAINCDIRS += $(OPUAVOBJ)
|
||||
EXTRAINCDIRS += $(OPUAVOBJINC)
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSWIN32)
|
||||
EXTRAINCDIRS += $(MININIDIR)
|
||||
EXTRAINCDIRS += $(RTOSINCDIR)
|
||||
EXTRAINCDIRS += $(APPLIBDIR)
|
||||
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/Win32
|
||||
EXTRAINCDIRS += $(FLIGHTLIB)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PYMITEINC)
|
||||
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
|
||||
|
||||
EXTRAINCDIRS += ${foreach MOD, ${MODULES}, ${OPMODULEDIR}/${MOD}/inc} ${OPMODULEDIR}/System/inc
|
||||
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS = WS2_32 Winmm
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
OPT = 0
|
||||
else
|
||||
OPT = s
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
#DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
ifeq ($(ENABLE_DEBUG_PINS), YES)
|
||||
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
|
||||
endif
|
||||
ifeq ($(ENABLE_AUX_UART), YES)
|
||||
CDEFS += -DPIOS_ENABLE_AUX_UART
|
||||
endif
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
CDEFS += -DUSE_BOOTLOADER
|
||||
endif
|
||||
|
||||
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS = -g
|
||||
endif
|
||||
|
||||
CFLAGS += $(CFLAGS_UAVOBJECTS)
|
||||
CFLAGS += -DARCH_WIN32
|
||||
CFLAGS += -O$(OPT)
|
||||
CFLAGS += -mtune=$(MCU)
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mtune=$(MCU) -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
#LDFLAGS += -lpthread
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
#LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lgcc
|
||||
|
||||
|
||||
|
||||
|
||||
# Define programs and commands.
|
||||
CC = $(TCHAIN_PREFIX)gcc
|
||||
CPP = $(TCHAIN_PREFIX)g++
|
||||
AR = ar
|
||||
OBJCOPY = objcopy
|
||||
OBJDUMP = objdump
|
||||
SIZE = size
|
||||
NM = nm
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
PYTHON = python
|
||||
###SHELL = sh
|
||||
###COPY = cp
|
||||
|
||||
|
||||
|
||||
# Define Messages
|
||||
# English
|
||||
MSG_ERRORS_NONE = Errors: none
|
||||
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
|
||||
MSG_END = ${quote}-------- end --------${quote}
|
||||
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
|
||||
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
|
||||
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
|
||||
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
|
||||
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
|
||||
MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
|
||||
MSG_LINKING = ${quote}**** Linking :${quote}
|
||||
MSG_COMPILING = ${quote}**** Compiling C :${quote}
|
||||
MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
|
||||
MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
|
||||
MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
|
||||
MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
|
||||
MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
|
||||
MSG_CLEANING = ${quote}Cleaning project:${quote}
|
||||
MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
|
||||
MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
|
||||
MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
|
||||
MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
exe: $(OUTDIR)/$(TARGET).exe
|
||||
|
||||
# Default target.
|
||||
#all: begin gccversion sizebefore build sizeafter finished end
|
||||
#all: begin gccversion build sizeafter finished end
|
||||
#all: elf
|
||||
all: gencode exe
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# Test if quotes are needed for the echo-command
|
||||
result = ${shell echo "test"}
|
||||
ifeq (${result}, test)
|
||||
quote = '
|
||||
else
|
||||
quote =
|
||||
endif
|
||||
|
||||
# Generate intermediate code
|
||||
gencode: ${OUTDIR}/InitMods.c $(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h
|
||||
|
||||
${OUTDIR}/InitMods.c: Makefile.win32
|
||||
@echo ${MSG_MODINIT}
|
||||
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
|
||||
#Generate code for PyMite
|
||||
$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h $(OPMODULEDIR)/FlightPlan/flightplan.c: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py)
|
||||
@echo ${MSG_PYMITEINIT}
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
|
||||
|
||||
# Eye candy.
|
||||
begin:
|
||||
## @echo
|
||||
@echo $(MSG_BEGIN)
|
||||
|
||||
finished:
|
||||
## @echo $(MSG_ERRORS_NONE)
|
||||
|
||||
end:
|
||||
@echo $(MSG_END)
|
||||
## @echo
|
||||
|
||||
# Display sizes of sections.
|
||||
ELFSIZE = $(SIZE) -A $(OUTDIR)/$(TARGET).elf
|
||||
##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
|
||||
sizebefore:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
|
||||
|
||||
sizeafter:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
|
||||
@echo $(MSG_SIZE_AFTER)
|
||||
$(ELFSIZE)
|
||||
|
||||
# Display compiler version information.
|
||||
gccversion :
|
||||
@$(CC) --version
|
||||
# @echo $(ALLOBJ)
|
||||
|
||||
# Program the device.
|
||||
ifeq ($(FLASH_TOOL),OPENOCD)
|
||||
# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
|
||||
program: $(OUTDIR)/$(TARGET).elf
|
||||
@echo ${quote}Programming with OPENOCD${quote}
|
||||
$(OOCD_EXE) $(OOCD_CL)
|
||||
endif
|
||||
|
||||
# Create final output file (.hex) from ELF output file.
|
||||
%.hex: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O ihex $< $@
|
||||
|
||||
# Create final output file (.bin) from ELF output file.
|
||||
%.bin: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O binary $< $@
|
||||
|
||||
# Create extended listing file/disassambly from ELF output file.
|
||||
# using objdump testing: option -C
|
||||
%.lss: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_EXTENDED_LISTING) $@
|
||||
$(OBJDUMP) -h -S -C -r $< > $@
|
||||
# $(OBJDUMP) -x -S $< > $@
|
||||
|
||||
# Create a symbol table from ELF output file.
|
||||
%.sym: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_SYMBOL_TABLE) $@
|
||||
$(NM) -n $< > $@
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
.SECONDARY : $(TARGET).elf
|
||||
.PRECIOUS : $(ALLOBJ)
|
||||
%.elf: $(ALLOBJ)
|
||||
@echo $(MSG_LINKING) $@
|
||||
# use $(CC) for C-only projects or $(CPP) for C++-projects:
|
||||
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
|
||||
# Link: create EXE output file from object files.
|
||||
.SECONDARY : $(TARGET).exe
|
||||
.PRECIOUS : $(ALLOBJ)
|
||||
%.exe: $(ALLOBJ)
|
||||
@echo $(MSG_LINKING) $@
|
||||
# use $(CC) for C-only projects or $(CPP) for C++-projects:
|
||||
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
define ASSEMBLE_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
define ASSEMBLE_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
|
||||
$(CC) -c $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
define COMPILE_C_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
define COMPILE_C_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
define COMPILE_CPP_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
define COMPILE_CPP_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(SRC:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC) $< to $@
|
||||
$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(SRCARM:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC_ARM) $< to $@
|
||||
$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Target: clean project.
|
||||
clean: begin clean_list finished end
|
||||
|
||||
clean_list :
|
||||
## @echo
|
||||
@echo $(MSG_CLEANING)
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).exe
|
||||
$(REMOVE) $(ALLOBJ)
|
||||
$(REMOVE) $(LSTFILES)
|
||||
$(REMOVE) $(DEPFILES)
|
||||
$(REMOVE) $(SRC:.c=.s)
|
||||
$(REMOVE) $(SRCARM:.c=.s)
|
||||
$(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(OUTDIR) 2>NUL)
|
||||
else
|
||||
$(shell mkdir $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(OUTDIR)\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all begin gencode finish end sizebefore sizeafter gccversion \
|
||||
build exe elf hex bin lss sym clean clean_list program
|
||||
|
||||
@ -1,99 +0,0 @@
|
||||
|
||||
#ifndef FREERTOS_CONFIG_H
|
||||
#define FREERTOS_CONFIG_H
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Application specific definitions.
|
||||
*
|
||||
* These definitions should be adjusted for your particular hardware and
|
||||
* application requirements.
|
||||
*
|
||||
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
|
||||
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
|
||||
*
|
||||
* See http://www.freertos.org/a00110.html.
|
||||
*----------------------------------------------------------*/
|
||||
|
||||
/**
|
||||
* @addtogroup PIOS PIOS
|
||||
* @{
|
||||
* @addtogroup FreeRTOS FreeRTOS
|
||||
* @{
|
||||
*/
|
||||
|
||||
/* Notes: We use 5 task priorities */
|
||||
|
||||
#define configUSE_PREEMPTION 1
|
||||
#define configUSE_IDLE_HOOK 1
|
||||
#define configUSE_TICK_HOOK 0
|
||||
#define configUSE_MALLOC_FAILED_HOOK 1
|
||||
#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 )
|
||||
#define configTICK_RATE_HZ ( ( portTickType ) 1000 )
|
||||
#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 )
|
||||
#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 )
|
||||
#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 41 * 1024 ) )
|
||||
#define configMAX_TASK_NAME_LEN ( 16 )
|
||||
#define configUSE_TRACE_FACILITY 0
|
||||
#define configUSE_16_BIT_TICKS 0
|
||||
#define configIDLE_SHOULD_YIELD 0
|
||||
#define configUSE_MUTEXES 1
|
||||
#define configUSE_RECURSIVE_MUTEXES 1
|
||||
#define configUSE_COUNTING_SEMAPHORES 0
|
||||
#define configUSE_ALTERNATIVE_API 0
|
||||
//#define configCHECK_FOR_STACK_OVERFLOW 2
|
||||
#define configQUEUE_REGISTRY_SIZE 10
|
||||
|
||||
/* Co-routine definitions. */
|
||||
#define configUSE_CO_ROUTINES 0
|
||||
#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
|
||||
|
||||
/* Set the following definitions to 1 to include the API function, or zero
|
||||
to exclude the API function. */
|
||||
|
||||
#define INCLUDE_vTaskPrioritySet 1
|
||||
#define INCLUDE_uxTaskPriorityGet 1
|
||||
#define INCLUDE_vTaskDelete 1
|
||||
#define INCLUDE_vTaskCleanUpResources 0
|
||||
#define INCLUDE_vTaskSuspend 1
|
||||
#define INCLUDE_vTaskDelayUntil 1
|
||||
#define INCLUDE_vTaskDelay 1
|
||||
#define INCLUDE_xTaskGetSchedulerState 1
|
||||
#define INCLUDE_xTaskGetCurrentTaskHandle 1
|
||||
#define INCLUDE_uxTaskGetStackHighWaterMark 1
|
||||
|
||||
/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255
|
||||
(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */
|
||||
#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */
|
||||
#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */
|
||||
|
||||
/* This is the value being used as per the ST library which permits 16
|
||||
priority values, 0 to 15. This must correspond to the
|
||||
configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest
|
||||
NVIC value of 255. */
|
||||
#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15
|
||||
|
||||
/* Enable run time stats collection */
|
||||
#if defined(DIAGNOSTICS)
|
||||
#define configCHECK_FOR_STACK_OVERFLOW 2
|
||||
|
||||
#define configGENERATE_RUN_TIME_STATS 1
|
||||
#define INCLUDE_uxTaskGetRunTime 1
|
||||
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
|
||||
do {\
|
||||
(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\
|
||||
(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
|
||||
} while(0)
|
||||
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
|
||||
#else
|
||||
#define configCHECK_FOR_STACK_OVERFLOW 1
|
||||
#endif
|
||||
|
||||
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
|
||||
#define CHECK_IRQ_STACK
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
#endif /* FREERTOS_CONFIG_H */
|
||||
@ -1,91 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_board.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board hardware for the OpenPilot Version 1.1 hardware.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PIOS_BOARD_H
|
||||
#define PIOS_BOARD_H
|
||||
|
||||
|
||||
|
||||
|
||||
//------------------------
|
||||
// PIOS_LED
|
||||
//------------------------
|
||||
//#define PIOS_LED_LED1_GPIO_PORT GPIOC
|
||||
//#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12
|
||||
//#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
//#define PIOS_LED_LED2_GPIO_PORT GPIOC
|
||||
//#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13
|
||||
//#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define PIOS_LED_NUM 2
|
||||
//#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT }
|
||||
//#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN }
|
||||
//#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK }
|
||||
|
||||
|
||||
//-------------------------
|
||||
// COM
|
||||
//
|
||||
// See also pios_board_posix.c
|
||||
//-------------------------
|
||||
//#define PIOS_USART_TX_BUFFER_SIZE 256
|
||||
#define PIOS_COM_BUFFER_SIZE 1024
|
||||
#define PIOS_COM_MAX_DEVS 256
|
||||
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
|
||||
|
||||
extern uint32_t pios_com_telem_rf_id;
|
||||
extern uint32_t pios_com_telem_usb_id;
|
||||
extern uint32_t pios_com_gps_id;
|
||||
extern uint32_t pios_com_aux_id;
|
||||
extern uint32_t pios_com_spectrum_id;
|
||||
|
||||
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
|
||||
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
|
||||
#define PIOS_COM_GPS (pios_com_gps_id)
|
||||
|
||||
#ifdef PIOS_ENABLE_AUX_UART
|
||||
#define PIOS_COM_AUX (pios_com_aux_id)
|
||||
#define PIOS_COM_DEBUG (PIOS_COM_AUX
|
||||
#endif
|
||||
|
||||
/**
|
||||
* glue macros for file IO
|
||||
* STM32 uses DOSFS for file IO
|
||||
*/
|
||||
#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL
|
||||
|
||||
#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL
|
||||
|
||||
#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length
|
||||
|
||||
#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file)
|
||||
|
||||
|
||||
|
||||
#define PIOS_FCLOSE(file) fclose(file)
|
||||
|
||||
#define PIOS_FUNLINK(file) unlink((char*)filename)
|
||||
|
||||
#endif /* PIOS_BOARD_H */
|
||||
@ -1,78 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief PiOS configuration header.
|
||||
* Central compile time config for the project.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PIOS_CONFIG_POSIX_H
|
||||
#define PIOS_CONFIG_POSIX_H
|
||||
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_SDCARD
|
||||
#define PIOS_INCLUDE_FREERTOS
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_GPS
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_TELEMETRY_RF
|
||||
#define PIOS_INCLUDE_UDP
|
||||
#define PIOS_INCLUDE_SERVO
|
||||
#define PIOS_INCLUDE_RCVR
|
||||
|
||||
#define PIOS_RCVR_MAX_CHANNELS 12
|
||||
#define PIOS_RCVR_MAX_DEVS 3
|
||||
|
||||
/* Defaults for Logging */
|
||||
#define LOG_FILENAME "PIOS.LOG"
|
||||
#define STARTUP_LOG_ENABLED 1
|
||||
|
||||
#define TELEM_QUEUE_SIZE 20
|
||||
#define PIOS_TELEM_STACK_SIZE 2048
|
||||
|
||||
/* Alarm Thresholds */
|
||||
#define HEAP_LIMIT_WARNING 4000
|
||||
#define HEAP_LIMIT_CRITICAL 1000
|
||||
#define IRQSTACK_LIMIT_WARNING 150
|
||||
#define IRQSTACK_LIMIT_CRITICAL 80
|
||||
#define CPULOAD_LIMIT_WARNING 80
|
||||
#define CPULOAD_LIMIT_CRITICAL 95
|
||||
|
||||
/* Stabilization options */
|
||||
#define PIOS_QUATERNION_STABILIZATION
|
||||
|
||||
/* Alarm Thresholds */
|
||||
#define HEAP_LIMIT_WARNING 4000
|
||||
#define HEAP_LIMIT_CRITICAL 1000
|
||||
#define IRQSTACK_LIMIT_WARNING 150
|
||||
#define IRQSTACK_LIMIT_CRITICAL 80
|
||||
#define CPULOAD_LIMIT_WARNING 80
|
||||
#define CPULOAD_LIMIT_CRITICAL 95
|
||||
|
||||
/* GPS options */
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
#endif /* PIOS_CONFIG_POSIX_H */
|
||||
@ -1,351 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @brief These files are the core system files of OpenPilot.
|
||||
* They are the ground layer just above PiOS. In practice, OpenPilot actually starts
|
||||
* in the main() function of openpilot.c
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @brief This is where the OP firmware starts. Those files also define the compile-time
|
||||
* options of the firmware.
|
||||
* @{
|
||||
* @file openpilot.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up and runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
#include "uavobjectsinit.h"
|
||||
#include "systemmod.h"
|
||||
|
||||
/* Task Priorities */
|
||||
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
#define INCLUDE_TEST_TASKS 0
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static uint8_t sdcard_available;
|
||||
#endif
|
||||
FILEINFO File;
|
||||
char Buffer[1024];
|
||||
uint32_t Cache;
|
||||
|
||||
/* Function Prototypes */
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskTick(void *pvParameters);
|
||||
static void TaskTesting(void *pvParameters);
|
||||
static void TaskHIDTest(void *pvParameters);
|
||||
static void TaskServos(void *pvParameters);
|
||||
static void TaskSDCard(void *pvParameters);
|
||||
#endif
|
||||
int32_t CONSOLE_Parse(uint8_t port, char c);
|
||||
void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
|
||||
|
||||
/* Prototype of PIOS_Board_Init() function */
|
||||
extern void PIOS_Board_Init(void);
|
||||
extern void Stack_Change(void);
|
||||
static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change")));
|
||||
|
||||
/**
|
||||
* OpenPilot Main function:
|
||||
*
|
||||
* Initialize PiOS<BR>
|
||||
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
|
||||
* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
|
||||
* If something goes wrong, blink LED1 and LED2 every 100ms
|
||||
*
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
/* NOTE: Do NOT modify the following start-up sequence */
|
||||
/* Any new initialization functions should be added in OpenPilotInit() */
|
||||
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
/* Architecture dependant Hardware and
|
||||
* core subsystem initialisation
|
||||
* (see pios_board.c for your arch)
|
||||
* */
|
||||
PIOS_Board_Init();
|
||||
|
||||
/* Initialize modules */
|
||||
MODULE_INITIALISE_ALL
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
/* Create test tasks */
|
||||
xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
|
||||
xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
|
||||
xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
|
||||
xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
|
||||
#endif
|
||||
|
||||
/* swap the stack to use the IRQ stack (does nothing in sim mode) */
|
||||
Stack_Change_Weak();
|
||||
|
||||
/* Start the FreeRTOS scheduler, which should never return.
|
||||
*
|
||||
* NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls
|
||||
* (schedules) function files (modules). These functions never return from their
|
||||
* while loops, which explains why each module has a while(1){} segment. Thus,
|
||||
* the OpenPilot software actually starts at the vTaskStartScheduler() function,
|
||||
* even though this is somewhat obscure.
|
||||
*
|
||||
* In addition, there are many main() functions in the OpenPilot firmware source tree
|
||||
* This is because each main() refers to a separate hardware platform. Of course,
|
||||
* C only allows one main(), so only the relevant main() function is compiled when
|
||||
* making a specific firmware.
|
||||
*
|
||||
*/
|
||||
vTaskStartScheduler();
|
||||
|
||||
/* If all is well we will never reach here as the scheduler will now be running. */
|
||||
|
||||
/* Do some indication to user that something bad just happened */
|
||||
while (1) {
|
||||
#if defined(PIOS_LED_HEARTBEAT)
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
#endif /* PIOS_LED_HEARTBEAT */
|
||||
PIOS_DELAY_WaitmS(100);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskTesting(void *pvParameters)
|
||||
{
|
||||
portTickType xDelay = 250 / portTICK_RATE_MS;
|
||||
portTickType xTimeout = 10 / portTICK_RATE_MS;
|
||||
|
||||
//PIOS_BMP085_Init();
|
||||
|
||||
for(;;)
|
||||
{
|
||||
/* This blocks the task until the BMP085 EOC */
|
||||
/*
|
||||
PIOS_BMP085_StartADC(TemperatureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetTemperature());
|
||||
|
||||
PIOS_BMP085_StartADC(PressureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetPressure());
|
||||
*/
|
||||
|
||||
#if defined(PIOS_INCLUDE_DSM)
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_DSM_Get(0), PIOS_DSM_Get(1), PIOS_DSM_Get(2), PIOS_DSM_Get(3), PIOS_DSM_Get(4), PIOS_DSM_Get(5), PIOS_DSM_Get(6), PIOS_DSM_Get(7));
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_SBUS)
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SBus_Get(0), PIOS_SBus_Get(1), PIOS_SBus_Get(2), PIOS_SBus_Get(3), PIOS_SBus_Get(4), PIOS_SBus_Get(5), PIOS_SBus_Get(6), PIOS_SBus_Get(7));
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_PWM)
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PWM_Get(0), PIOS_PWM_Get(1), PIOS_PWM_Get(2), PIOS_PWM_Get(3), PIOS_PWM_Get(4), PIOS_PWM_Get(5), PIOS_PWM_Get(6), PIOS_PWM_Get(7));
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PPM_Get(0), PIOS_PPM_Get(1), PIOS_PPM_Get(2), PIOS_PPM_Get(3), PIOS_PPM_Get(4), PIOS_PPM_Get(5), PIOS_PPM_Get(6), PIOS_PPM_Get(7));
|
||||
#endif
|
||||
|
||||
/* This blocks the task until there is something on the buffer */
|
||||
/*xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
|
||||
int32_t len = PIOS_COM_ReceiveBufferUsed(COM_USART1);
|
||||
for(int32_t i = 0; i < len; i++) {
|
||||
PIOS_COM_SendFormattedString(COM_DEBUG_USART, ">%c\r", PIOS_COM_ReceiveBuffer(COM_USART1));
|
||||
}*/
|
||||
|
||||
//int32_t state = PIOS_USB_CableConnected();
|
||||
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "State: %d\r", state);
|
||||
|
||||
//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x57, (uint8_t *)50, 1);
|
||||
|
||||
/* Test ADC pins */
|
||||
//temp = ((1.43 - ((Vsense / 4096) * 3.3)) / 4.3) + 25;
|
||||
//uint32_t vsense = PIOS_ADC_PinGet(0);
|
||||
//uint32_t Temp = (1.42 - vsense * 3.3 / 4096) * 1000 / 4.35 + 25;
|
||||
//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "Temp: %d, CS_I: %d, CS_V: %d, 5v: %d\r", PIOS_ADC_PinGet(0), PIOS_ADC_PinGet(1), PIOS_ADC_PinGet(2), PIOS_ADC_PinGet(3));
|
||||
//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "AUX1: %d, AUX2: %d, AUX3: %d\r", PIOS_ADC_PinGet(4), PIOS_ADC_PinGet(5), PIOS_ADC_PinGet(6));
|
||||
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskHIDTest(void *pvParameters)
|
||||
{
|
||||
uint8_t byte;
|
||||
uint8_t line_buffer[128];
|
||||
uint16_t line_ix = 0;
|
||||
|
||||
for(;;)
|
||||
{
|
||||
/* HID Loopback Test */
|
||||
#if 0
|
||||
if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) != 0) {
|
||||
byte = PIOS_COM_ReceiveBuffer(COM_USB_HID);
|
||||
if(byte == '\r' || byte == '\n' || byte == 0) {
|
||||
PIOS_COM_SendFormattedString(COM_USB_HID, "RX: %s\r", line_buffer);
|
||||
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
|
||||
line_ix = 0;
|
||||
} else if(line_ix < sizeof(line_buffer)) {
|
||||
line_buffer[line_ix++] = byte;
|
||||
line_buffer[line_ix] = 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* HID Loopback Test */
|
||||
if(PIOS_COM_ReceiveBufferUsed(COM_USART2) != 0) {
|
||||
byte = PIOS_COM_ReceiveBuffer(COM_USART2);
|
||||
#if 0
|
||||
if(byte == '\r' || byte == '\n' || byte == 0) {
|
||||
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
|
||||
line_ix = 0;
|
||||
} else if(line_ix < sizeof(line_buffer)) {
|
||||
line_buffer[line_ix++] = byte;
|
||||
line_buffer[line_ix] = 0;
|
||||
}
|
||||
#endif
|
||||
PIOS_COM_SendChar(COM_DEBUG_USART, (char)byte);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskServos(void *pvParameters)
|
||||
{
|
||||
/* For testing servo outputs */
|
||||
portTickType xDelay;
|
||||
|
||||
/* Used to test servos, cycles all servos from one side to the other */
|
||||
for(;;) {
|
||||
/*xDelay = 250 / portTICK_RATE_MS;
|
||||
PIOS_Servo_Set(0, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(1, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(2, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(3, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(4, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(5, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(6, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(7, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
|
||||
PIOS_Servo_Set(7, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(6, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(5, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(4, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(3, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(2, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(1, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(0, 1000);
|
||||
vTaskDelay(xDelay);*/
|
||||
|
||||
xDelay = 1 / portTICK_RATE_MS;
|
||||
for(int i = 1000; i < 2000; i++) {
|
||||
PIOS_Servo_Set(0, i);
|
||||
PIOS_Servo_Set(1, i);
|
||||
PIOS_Servo_Set(2, i);
|
||||
PIOS_Servo_Set(3, i);
|
||||
PIOS_Servo_Set(4, i);
|
||||
PIOS_Servo_Set(5, i);
|
||||
PIOS_Servo_Set(6, i);
|
||||
PIOS_Servo_Set(7, i);
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
for(int i = 2000; i > 1000; i--) {
|
||||
PIOS_Servo_Set(0, i);
|
||||
PIOS_Servo_Set(1, i);
|
||||
PIOS_Servo_Set(2, i);
|
||||
PIOS_Servo_Set(3, i);
|
||||
PIOS_Servo_Set(4, i);
|
||||
PIOS_Servo_Set(5, i);
|
||||
PIOS_Servo_Set(6, i);
|
||||
PIOS_Servo_Set(7, i);
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskSDCard(void *pvParameters)
|
||||
{
|
||||
uint16_t second_delay_ctr = 0;
|
||||
portTickType xLastExecutionTime;
|
||||
|
||||
/* Initialise the xLastExecutionTime variable on task entry */
|
||||
xLastExecutionTime = xTaskGetTickCount();
|
||||
|
||||
for(;;) {
|
||||
vTaskDelayUntil(&xLastExecutionTime, 1 / portTICK_RATE_MS);
|
||||
|
||||
/* Each second: */
|
||||
/* Check if SD card is available */
|
||||
/* High-speed access if SD card was previously available */
|
||||
if(++second_delay_ctr >= 1000) {
|
||||
second_delay_ctr = 0;
|
||||
|
||||
uint8_t prev_sdcard_available = sdcard_available;
|
||||
sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available);
|
||||
|
||||
if(sdcard_available && !prev_sdcard_available) {
|
||||
/* SD Card has been connected! */
|
||||
/* Switch to mass storage device */
|
||||
MSD_Init(0);
|
||||
} else if(!sdcard_available && prev_sdcard_available) {
|
||||
/* Re-init USB for HID */
|
||||
PIOS_USB_Init(1);
|
||||
/* SD Card disconnected! */
|
||||
}
|
||||
}
|
||||
|
||||
/* Each millisecond: */
|
||||
/* Handle USB access if device is available */
|
||||
if(sdcard_available) {
|
||||
MSD_Periodic_mS();
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
@ -1,440 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
*
|
||||
* @file pios_board.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board specific static initializers for hardware for the OpenPilot board.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/* Pull in the board-specific static HW definitions.
|
||||
* Including .c files is a bit ugly but this allows all of
|
||||
* the HW definitions to be const and static to limit their
|
||||
* scope.
|
||||
*
|
||||
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
|
||||
*/
|
||||
#include "board_hw_defs.c"
|
||||
|
||||
#include <pios.h>
|
||||
#include <openpilot.h>
|
||||
#include <uavobjectsinit.h>
|
||||
#include <hwsettings.h>
|
||||
#include <manualcontrolsettings.h>
|
||||
|
||||
//#define I2C_DEBUG_PIN 0
|
||||
//#define USART_GPS_DEBUG_PIN 1
|
||||
|
||||
/* One slot per selectable receiver group.
|
||||
* eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
|
||||
* NOTE: No slot in this map for NONE.
|
||||
*/
|
||||
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
|
||||
|
||||
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
|
||||
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
|
||||
|
||||
#define PIOS_COM_GPS_RX_BUF_LEN 96
|
||||
|
||||
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
|
||||
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
|
||||
|
||||
#define PIOS_COM_BRIDGE_RX_BUF_LEN 65
|
||||
#define PIOS_COM_BRIDGE_TX_BUF_LEN 12
|
||||
|
||||
uint32_t pios_com_telem_rf_id;
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
uint32_t pios_com_vcp_id;
|
||||
uint32_t pios_com_gps_id;
|
||||
uint32_t pios_com_aux_id;
|
||||
uint32_t pios_com_dsm_id;
|
||||
|
||||
#include "ahrs_spi_comm.h"
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
void PIOS_Board_Init(void) {
|
||||
|
||||
/* Remap AFIO pin */
|
||||
//GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
|
||||
|
||||
#ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS
|
||||
PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
|
||||
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
/* Set up the SPI interface to the SD card */
|
||||
if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
/* Enable and mount the SDCard */
|
||||
PIOS_SDCARD_Init(pios_spi_sdcard_id);
|
||||
PIOS_SDCARD_MountFS(0);
|
||||
#endif /* PIOS_INCLUDE_SPI */
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
UAVObjInitialize();
|
||||
|
||||
#if defined(PIOS_INCLUDE_RTC)
|
||||
/* Initialize the real-time clock and its associated tick */
|
||||
PIOS_RTC_Init(&pios_rtc_main_cfg);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_LED)
|
||||
PIOS_LED_Init(&pios_led_cfg);
|
||||
#endif /* PIOS_INCLUDE_LED */
|
||||
|
||||
HwSettingsInitialize();
|
||||
|
||||
PIOS_WDG_Init();
|
||||
|
||||
/* Initialize the alarms library */
|
||||
AlarmsInitialize();
|
||||
|
||||
PIOS_IAP_Init();
|
||||
uint16_t boot_count = PIOS_IAP_ReadBootCount();
|
||||
if (boot_count < 3) {
|
||||
PIOS_IAP_WriteBootCount(++boot_count);
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT);
|
||||
} else {
|
||||
/* Too many failed boot attempts, force hwsettings to defaults */
|
||||
HwSettingsSetDefaults(HwSettingsHandle(), 0);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
|
||||
/* Initialize the task monitor library */
|
||||
TaskMonitorInitialize();
|
||||
|
||||
/* Set up pulse timers */
|
||||
PIOS_TIM_InitClock(&tim_1_cfg);
|
||||
PIOS_TIM_InitClock(&tim_3_cfg);
|
||||
PIOS_TIM_InitClock(&tim_5_cfg);
|
||||
PIOS_TIM_InitClock(&tim_4_cfg);
|
||||
PIOS_TIM_InitClock(&tim_8_cfg);
|
||||
|
||||
/* Prepare the AHRS Comms upper layer protocol */
|
||||
AhrsInitComms();
|
||||
|
||||
/* Set up the SPI interface to the AHRS */
|
||||
if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
/* Bind the AHRS comms layer to the AHRS SPI link */
|
||||
AhrsConnect(pios_spi_ahrs_id);
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB)
|
||||
/* Initialize board specific USB data */
|
||||
PIOS_USB_BOARD_DATA_Init();
|
||||
|
||||
/* Flags to determine if various USB interfaces are advertised */
|
||||
bool usb_hid_present = false;
|
||||
bool usb_cdc_present = false;
|
||||
|
||||
uint8_t hwsettings_usb_devicetype;
|
||||
HwSettingsUSB_DeviceTypeGet(&hwsettings_usb_devicetype);
|
||||
|
||||
switch (hwsettings_usb_devicetype) {
|
||||
case HWSETTINGS_USB_DEVICETYPE_HIDONLY:
|
||||
if (PIOS_USB_DESC_HID_ONLY_Init()) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
usb_hid_present = true;
|
||||
break;
|
||||
case HWSETTINGS_USB_DEVICETYPE_HIDVCP:
|
||||
if (PIOS_USB_DESC_HID_CDC_Init()) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
usb_hid_present = true;
|
||||
usb_cdc_present = true;
|
||||
break;
|
||||
case HWSETTINGS_USB_DEVICETYPE_VCPONLY:
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_usb_id;
|
||||
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_CDC)
|
||||
/* Configure the USB VCP port */
|
||||
uint8_t hwsettings_usb_vcpport;
|
||||
HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport);
|
||||
|
||||
if (!usb_cdc_present) {
|
||||
/* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */
|
||||
hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED;
|
||||
}
|
||||
|
||||
switch (hwsettings_usb_vcpport) {
|
||||
case HWSETTINGS_USB_VCPPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_USB_VCPPORT_USBTELEMETRY:
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
{
|
||||
uint32_t pios_usb_cdc_id;
|
||||
if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
|
||||
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
PIOS_Assert(tx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
|
||||
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
|
||||
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
break;
|
||||
case HWSETTINGS_USB_VCPPORT_COMBRIDGE:
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
{
|
||||
uint32_t pios_usb_cdc_id;
|
||||
if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
|
||||
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
PIOS_Assert(tx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
|
||||
rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
|
||||
tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
break;
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_USB_CDC */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_HID)
|
||||
/* Configure the usb HID port */
|
||||
uint8_t hwsettings_usb_hidport;
|
||||
HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport);
|
||||
|
||||
if (!usb_hid_present) {
|
||||
/* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */
|
||||
hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED;
|
||||
}
|
||||
|
||||
switch (hwsettings_usb_hidport) {
|
||||
case HWSETTINGS_USB_HIDPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_USB_HIDPORT_USBTELEMETRY:
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
{
|
||||
uint32_t pios_usb_hid_id;
|
||||
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
|
||||
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
PIOS_Assert(tx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id,
|
||||
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
|
||||
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
break;
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB_HID */
|
||||
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
|
||||
/* Configure the main IO port */
|
||||
uint8_t hwsettings_op_mainport;
|
||||
HwSettingsOP_MainPortGet(&hwsettings_op_mainport);
|
||||
|
||||
switch (hwsettings_op_mainport) {
|
||||
case HWSETTINGS_OP_MAINPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_OP_MAINPORT_TELEMETRY:
|
||||
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
|
||||
{
|
||||
uint32_t pios_usart_telem_rf_id;
|
||||
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
|
||||
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
PIOS_Assert(tx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
|
||||
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
|
||||
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
|
||||
break;
|
||||
}
|
||||
|
||||
/* Configure the flexi port */
|
||||
uint8_t hwsettings_op_flexiport;
|
||||
HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport);
|
||||
|
||||
switch (hwsettings_op_flexiport) {
|
||||
case HWSETTINGS_OP_FLEXIPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_OP_FLEXIPORT_GPS:
|
||||
#if defined(PIOS_INCLUDE_GPS)
|
||||
{
|
||||
uint32_t pios_usart_gps_id;
|
||||
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
|
||||
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
|
||||
NULL, 0)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_GPS */
|
||||
break;
|
||||
}
|
||||
|
||||
#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
|
||||
PIOS_Servo_Init(&pios_servo_cfg);
|
||||
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */
|
||||
|
||||
PIOS_ADC_Init();
|
||||
PIOS_GPIO_Init();
|
||||
|
||||
/* Configure the rcvr port */
|
||||
uint8_t hwsettings_rcvrport;
|
||||
HwSettingsOP_RcvrPortGet(&hwsettings_rcvrport);
|
||||
|
||||
|
||||
switch (hwsettings_rcvrport) {
|
||||
case HWSETTINGS_OP_RCVRPORT_DISABLED:
|
||||
break;
|
||||
case HWSETTINGS_OP_RCVRPORT_DEBUG:
|
||||
/* Not supported yet */
|
||||
break;
|
||||
case HWSETTINGS_OP_RCVRPORT_DSM2:
|
||||
case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
|
||||
case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
|
||||
#if defined(PIOS_INCLUDE_DSM)
|
||||
{
|
||||
enum pios_dsm_proto proto;
|
||||
switch (hwsettings_rcvrport) {
|
||||
case HWSETTINGS_OP_RCVRPORT_DSM2:
|
||||
proto = PIOS_DSM_PROTO_DSM2;
|
||||
break;
|
||||
case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
|
||||
proto = PIOS_DSM_PROTO_DSMX10BIT;
|
||||
break;
|
||||
case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
|
||||
proto = PIOS_DSM_PROTO_DSMX11BIT;
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
break;
|
||||
}
|
||||
|
||||
uint32_t pios_usart_dsm_id;
|
||||
if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_dsm_id;
|
||||
if (PIOS_DSM_Init(&pios_dsm_id,
|
||||
&pios_dsm_cfg,
|
||||
&pios_usart_com_driver,
|
||||
pios_usart_dsm_id,
|
||||
proto, 0)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint32_t pios_dsm_rcvr_id;
|
||||
if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case HWSETTINGS_OP_RCVRPORT_PWM:
|
||||
#if defined(PIOS_INCLUDE_PWM)
|
||||
{
|
||||
uint32_t pios_pwm_id;
|
||||
PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg);
|
||||
|
||||
uint32_t pios_pwm_rcvr_id;
|
||||
if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_PWM */
|
||||
break;
|
||||
case HWSETTINGS_OP_RCVRPORT_PPM:
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
{
|
||||
uint32_t pios_ppm_id;
|
||||
PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
|
||||
|
||||
uint32_t pios_ppm_rcvr_id;
|
||||
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_PPM */
|
||||
break;
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
|
||||
/* Make sure we have at least one telemetry link configured or else fail initialization */
|
||||
PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
@ -1,197 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_board.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board specific static initializers for hardware for the OpenPilot board.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include <pios.h>
|
||||
#include <pios_udp_priv.h>
|
||||
#include <pios_com_priv.h>
|
||||
#include <openpilot.h>
|
||||
#include <uavobjectsinit.h>
|
||||
|
||||
#include "hwsettings.h"
|
||||
#include "attituderaw.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "positionactual.h"
|
||||
#include "velocityactual.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
|
||||
#if defined(PIOS_INCLUDE_RCVR)
|
||||
#include "pios_rcvr_priv.h"
|
||||
|
||||
/* One slot per selectable receiver group.
|
||||
* eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
|
||||
* NOTE: No slot in this map for NONE.
|
||||
*/
|
||||
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
|
||||
|
||||
#endif /* PIOS_INCLUDE_RCVR */
|
||||
|
||||
|
||||
void Stack_Change() {
|
||||
}
|
||||
|
||||
void Stack_Change_Weak() {
|
||||
}
|
||||
|
||||
|
||||
const struct pios_udp_cfg pios_udp_telem_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9000,
|
||||
};
|
||||
const struct pios_udp_cfg pios_udp_gps_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9001,
|
||||
};
|
||||
const struct pios_udp_cfg pios_udp_debug_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9002,
|
||||
};
|
||||
|
||||
#ifdef PIOS_COM_AUX
|
||||
/*
|
||||
* AUX USART
|
||||
*/
|
||||
const struct pios_udp_cfg pios_udp_aux_cfg = {
|
||||
.ip = "0.0.0.0",
|
||||
.port = 9003,
|
||||
};
|
||||
#endif
|
||||
|
||||
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
|
||||
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
|
||||
#define PIOS_COM_GPS_RX_BUF_LEN 96
|
||||
|
||||
/*
|
||||
* Board specific number of devices.
|
||||
*/
|
||||
/*
|
||||
struct pios_udp_dev pios_udp_devs[] = {
|
||||
#define PIOS_UDP_TELEM 0
|
||||
{
|
||||
.cfg = &pios_udp0_cfg,
|
||||
},
|
||||
#define PIOS_UDP_GPS 1
|
||||
{
|
||||
.cfg = &pios_udp1_cfg,
|
||||
},
|
||||
#define PIOS_UDP_LOCAL 2
|
||||
{
|
||||
.cfg = &pios_udp2_cfg,
|
||||
},
|
||||
#ifdef PIOS_COM_AUX
|
||||
#define PIOS_UDP_AUX 3
|
||||
{
|
||||
.cfg = &pios_udp3_cfg,
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
||||
uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
|
||||
*/
|
||||
/*
|
||||
* COM devices
|
||||
*/
|
||||
|
||||
/*
|
||||
* Board specific number of devices.
|
||||
*/
|
||||
extern const struct pios_com_driver pios_serial_com_driver;
|
||||
extern const struct pios_com_driver pios_udp_com_driver;
|
||||
|
||||
uint32_t pios_com_telem_rf_id;
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
uint32_t pios_com_gps_id;
|
||||
uint32_t pios_com_aux_id;
|
||||
uint32_t pios_com_spectrum_id;
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core systems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
void PIOS_Board_Init(void) {
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
UAVObjInitialize();
|
||||
UAVObjectsInitializeAll();
|
||||
|
||||
/* Initialize the alarms library */
|
||||
AlarmsInitialize();
|
||||
|
||||
/* Initialize the task monitor library */
|
||||
TaskMonitorInitialize();
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
|
||||
{
|
||||
uint32_t pios_udp_telem_rf_id;
|
||||
if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
|
||||
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
PIOS_Assert(tx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
|
||||
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
|
||||
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
|
||||
|
||||
#if defined(PIOS_INCLUDE_GPS)
|
||||
{
|
||||
uint32_t pios_udp_gps_id;
|
||||
if (PIOS_UDP_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id,
|
||||
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
|
||||
NULL, 0)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_GPS */
|
||||
#endif
|
||||
|
||||
// Initialize these here as posix has no AHRSComms
|
||||
AttitudeRawInitialize();
|
||||
AttitudeActualInitialize();
|
||||
VelocityActualInitialize();
|
||||
PositionActualInitialize();
|
||||
HwSettingsInitialize();
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
@ -1,100 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file openpilot.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up and runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
|
||||
/* Task Priorities */
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
|
||||
/* Function Prototypes */
|
||||
static void TaskTesting(void *pvParameters);
|
||||
|
||||
/**
|
||||
* Test Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
// Init
|
||||
PIOS_SYS_Init();
|
||||
PIOS_DELAY_Init();
|
||||
PIOS_COM_Init();
|
||||
PIOS_ADC_Init();
|
||||
PIOS_I2C_Init();
|
||||
xTaskCreate(TaskTesting, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
|
||||
// Should not get here
|
||||
PIOS_DEBUG_Assert(0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void TaskTesting(void *pvParameters)
|
||||
{
|
||||
portTickType xDelay = 500 / portTICK_RATE_MS;
|
||||
portTickType xTimeout = 10 / portTICK_RATE_MS;
|
||||
|
||||
PIOS_BMP085_Init();
|
||||
|
||||
for(;;)
|
||||
{
|
||||
PIOS_LED_Toggle(LED2);
|
||||
|
||||
int16_t temp;
|
||||
int32_t pressure;
|
||||
|
||||
PIOS_BMP085_StartADC(TemperatureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
|
||||
PIOS_BMP085_StartADC(PressureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
|
||||
temp = PIOS_BMP085_GetTemperature();
|
||||
pressure = PIOS_BMP085_GetPressure();
|
||||
|
||||
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_TELEM_RF, "%3u,%4u\r", temp, pressure);
|
||||
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Idle hook function
|
||||
*/
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
|
||||
|
||||
}
|
||||
@ -1,89 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file test_cpuload.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Calibration for the CPU load calculation
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* This file is used to calculate the number of counts we get when
|
||||
* the CPU is fully idle (no load). This is used by systemmod.c to
|
||||
* calculate the CPU load. This calibration should be run whenever
|
||||
* changes are made in the FreeRTOS configuration or on the compiler
|
||||
* optimisation parameters.
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
// Local constants
|
||||
#define BENCHMARK_DURATION_MS 10000
|
||||
|
||||
// Local functions
|
||||
static void testTask(void *pvParameters);
|
||||
|
||||
// Variables
|
||||
static uint32_t idleCounter = 0;
|
||||
static uint32_t idleCounterClear = 0;
|
||||
|
||||
int main()
|
||||
{
|
||||
PIOS_SYS_Init();
|
||||
|
||||
// Create test task
|
||||
xTaskCreate(testTask, (signed portCHAR *)"Test", 1000 , NULL, 1, NULL);
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void testTask(void *pvParameters)
|
||||
{
|
||||
uint32_t countsPerSecond = 0;
|
||||
|
||||
while (1)
|
||||
{
|
||||
// Delay until enough required test duration
|
||||
vTaskDelay( BENCHMARK_DURATION_MS / portTICK_RATE_MS );
|
||||
|
||||
// Calculate counts per second, set breakpoint here
|
||||
countsPerSecond = idleCounter / (BENCHMARK_DURATION_MS/1000);
|
||||
|
||||
// Reset and start again - do not clear idleCounter directly!
|
||||
// SET BREAKPOINT HERE and read the countsPerSecond variable
|
||||
// this should be used to update IDLE_COUNTS_PER_SEC_AT_NO_LOAD in systemmod.c
|
||||
idleCounterClear = 1;
|
||||
}
|
||||
}
|
||||
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
if (idleCounterClear == 0)
|
||||
{
|
||||
++idleCounter;
|
||||
}
|
||||
else
|
||||
{
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user