mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Remove references to deprecated targets
This commit is contained in:
parent
cce351bb9a
commit
d5eb38a065
2
Makefile
2
Makefile
@ -432,7 +432,7 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bl_$(1), $$(BL_TARGETS)))
|
||||
all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
|
||||
endef
|
||||
|
||||
ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins revolution
|
||||
ALL_BOARDS := coptercontrol pipxtreme ins revolution
|
||||
|
||||
# Friendly names of each board (used to find source tree)
|
||||
openpilot_friendly := OpenPilot
|
||||
|
@ -1,422 +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
|
||||
|
||||
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
|
||||
SRC += $(PIOSSTM32F10X)/pios_iap.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_hmc5843.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.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)
|
||||
|
||||
# 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(LED1); // not keeping up
|
||||
else
|
||||
PIOS_LED_Off(LED1);
|
||||
previous_conversion = total_conversion_blocks;
|
||||
|
||||
|
||||
// Dump raw buffer
|
||||
result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, &framing[0], 16); // framing header
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
|
||||
result +=
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX,
|
||||
(uint8_t *) & valid_data_buffer[0],
|
||||
adc_oversampling *
|
||||
PIOS_ADC_NUM_PINS *
|
||||
sizeof(valid_data_buffer[0]));
|
||||
if (result == 0)
|
||||
PIOS_LED_Off(LED1);
|
||||
else {
|
||||
PIOS_LED_On(LED1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(PIP_DUMP_RAW)
|
||||
|
||||
#define MAX_OVERSAMPLING PIOS_ADC_MAX_OVERSAMPLING
|
||||
|
||||
void print_ahrs_raw()
|
||||
{
|
||||
int16_t accel_x[MAX_OVERSAMPLING], accel_y[MAX_OVERSAMPLING], accel_z[MAX_OVERSAMPLING];
|
||||
int16_t gyro_x[MAX_OVERSAMPLING], gyro_y[MAX_OVERSAMPLING], gyro_z[MAX_OVERSAMPLING];
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
int16_t mag[3];
|
||||
int16_t mag_x, mag_y, mag_z;
|
||||
#endif
|
||||
|
||||
static int previous_conversion = 0;
|
||||
|
||||
uint8_t framing[3] = {0xD2, 0x73, 0x00};
|
||||
|
||||
// wait for new raw samples
|
||||
while (previous_conversion == total_conversion_blocks);
|
||||
if ((previous_conversion + 1) != total_conversion_blocks)
|
||||
PIOS_LED_On(LED1); // we are not keeping up
|
||||
previous_conversion = total_conversion_blocks;
|
||||
|
||||
// fetch the buffer address for the new samples
|
||||
int16_t *valid_data_buffer = PIOS_ADC_GetRawBuffer();
|
||||
|
||||
// fetch number of raw samples in the buffer (per channel)
|
||||
int over_sampling = PIOS_ADC_GetOverSampling();
|
||||
|
||||
framing[2] = over_sampling;
|
||||
|
||||
// copy the raw samples into their own buffers
|
||||
for (uint16_t i = 0, j = 0; i < over_sampling; i++, j += PIOS_ADC_NUM_CHANNELS)
|
||||
{
|
||||
accel_x[i] = valid_data_buffer[ACCEL_RAW_X_IDX + j];
|
||||
accel_y[i] = valid_data_buffer[ACCEL_RAW_Y_IDX + j];
|
||||
accel_z[i] = valid_data_buffer[ACCEL_RAW_Z_IDX + j];
|
||||
|
||||
gyro_x[i] = valid_data_buffer[GYRO_RAW_X_IDX + j];
|
||||
gyro_y[i] = valid_data_buffer[GYRO_RAW_Y_IDX + j];
|
||||
gyro_z[i] = valid_data_buffer[GYRO_RAW_Z_IDX + j];
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if (PIOS_HMC5843_NewDataAvailable())
|
||||
{
|
||||
PIOS_HMC5843_ReadMag(mag);
|
||||
|
||||
mag_x = mag[MAG_RAW_X_IDX];
|
||||
mag_y = mag[MAG_RAW_Y_IDX];
|
||||
mag_z = mag[MAG_RAW_Z_IDX];
|
||||
}
|
||||
#endif
|
||||
|
||||
// send the raw samples
|
||||
int result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, framing, sizeof(framing));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_x, over_sampling * sizeof(accel_x[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_y, over_sampling * sizeof(accel_y[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_z, over_sampling * sizeof(accel_z[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_x, over_sampling * sizeof(gyro_x[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_y, over_sampling * sizeof(gyro_y[0]));
|
||||
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0]));
|
||||
|
||||
if (result != 0)
|
||||
PIOS_LED_On(LED1); // all data not sent
|
||||
else
|
||||
PIOS_LED_Off(LED1);
|
||||
}
|
||||
#endif
|
||||
|
||||
extern void PIOS_Board_Init(void);
|
||||
|
||||
/**
|
||||
* @brief AHRS Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
gps_data.quality = -1;
|
||||
uint32_t up_time_real = 0;
|
||||
uint32_t up_time = 0;
|
||||
uint32_t last_up_time = 0;
|
||||
static int8_t last_ahrs_algorithm;
|
||||
uint32_t last_counter_idle_start = 0;
|
||||
uint32_t last_counter_idle_end = 0;
|
||||
uint32_t idle_counts = 0;
|
||||
uint32_t running_counts = 0;
|
||||
uint32_t counter_val = 0;
|
||||
ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE;
|
||||
|
||||
reset_values();
|
||||
|
||||
PIOS_Board_Init();
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
// Get 3 ID bytes
|
||||
strcpy((char *)mag_data.id, "ZZZ");
|
||||
PIOS_HMC5843_ReadID(mag_data.id);
|
||||
#endif
|
||||
|
||||
while(!AhrsLinkReady()) {
|
||||
AhrsPoll();
|
||||
}
|
||||
/* we didn't connect the callbacks before because we have to wait
|
||||
for all data to be up to date before doing anything*/
|
||||
|
||||
AHRSCalibrationConnectCallback(calibration_callback);
|
||||
GPSPositionConnectCallback(gps_callback);
|
||||
BaroAltitudeConnectCallback(altitude_callback);
|
||||
AHRSSettingsConnectCallback(settings_callback);
|
||||
HomeLocationConnectCallback(homelocation_callback);
|
||||
FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
|
||||
|
||||
calibration_callback(AHRSCalibrationHandle()); //force an update
|
||||
|
||||
#if defined(DUMP_RAW) || defined(PIP_DUMP_RAW)
|
||||
while (1) {
|
||||
AhrsPoll();
|
||||
print_ahrs_raw();
|
||||
}
|
||||
#endif
|
||||
|
||||
timer_start();
|
||||
|
||||
/******************* Main EKF loop ****************************/
|
||||
while(1) {
|
||||
|
||||
|
||||
AhrsPoll();
|
||||
AhrsStatusData status;
|
||||
AhrsStatusGet(&status);
|
||||
status.CPULoad = ((float)running_counts /
|
||||
(float)(idle_counts + running_counts)) * 100;
|
||||
status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
|
||||
status.RunningTimePerCyle = running_counts / (timer_rate() / 10000);
|
||||
status.DroppedUpdates = ekf_too_slow;
|
||||
up_time = timer_count();
|
||||
if(up_time >= last_up_time) // normal condition
|
||||
up_time_real += ((up_time - last_up_time) * 1000) / timer_rate();
|
||||
else
|
||||
up_time_real += ((0xFFFF - last_up_time + up_time) * 1000) / timer_rate();
|
||||
last_up_time = up_time;
|
||||
status.RunningTime = up_time_real;
|
||||
AhrsStatusSet(&status);
|
||||
|
||||
// Alive signal
|
||||
if (((total_conversion_blocks % 100) & 0xFFFE) == 0)
|
||||
PIOS_LED_Toggle(LED1);
|
||||
|
||||
// Delay for valid data
|
||||
|
||||
counter_val = timer_count();
|
||||
running_counts = counter_val - last_counter_idle_end;
|
||||
last_counter_idle_start = counter_val;
|
||||
|
||||
// This function blocks till data avilable
|
||||
get_accel_gyro_data();
|
||||
|
||||
// Get any mag data available
|
||||
process_mag_data();
|
||||
|
||||
counter_val = timer_count();
|
||||
idle_counts = counter_val - last_counter_idle_start;
|
||||
last_counter_idle_end = counter_val;
|
||||
|
||||
|
||||
if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
|
||||
ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) ||
|
||||
ACCEL_OOB(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
|
||||
GYRO_OOB(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z)) {
|
||||
// If any values are NaN or huge don't update
|
||||
//TODO: add field to ahrs status to track number of these events
|
||||
continue;
|
||||
}
|
||||
|
||||
print_ekf_binary();
|
||||
|
||||
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
|
||||
if (ahrs_algorithm != last_ahrs_algorithm)
|
||||
ins_init_algorithm();
|
||||
last_ahrs_algorithm = ahrs_algorithm;
|
||||
|
||||
switch(ahrs_algorithm) {
|
||||
case AHRSSETTINGS_ALGORITHM_SIMPLE:
|
||||
simple_update();
|
||||
break;
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
|
||||
ins_outdoor_update();
|
||||
break;
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR:
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
|
||||
ins_indoor_update();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the accel and gyro data from whichever source when available
|
||||
*
|
||||
* This function will act as the HAL for the new INS sensors
|
||||
*/
|
||||
bool get_accel_gyro_data()
|
||||
{
|
||||
float accel[6];
|
||||
float gyro[6];
|
||||
uint16_t spin_count = 1;
|
||||
while(fifoBuf_getUsed(&adc_fifo_buffer) < (sizeof(accel) + sizeof(gyro))) {
|
||||
if(spin_count++ == 0)
|
||||
AhrsPoll();
|
||||
}
|
||||
|
||||
fifoBuf_getData(&adc_fifo_buffer, &accel[0], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &gyro[0], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &accel[3], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &gyro[3], sizeof(float) * 3);
|
||||
|
||||
accel_data.filtered.x = (accel[0] + accel[3]) / 2;
|
||||
accel_data.filtered.y = (accel[1] + accel[4]) / 2;
|
||||
accel_data.filtered.z = (accel[2] + accel[5]) / 2;
|
||||
gyro_data.filtered.x = (gyro[0] + gyro[3]) / 2;
|
||||
gyro_data.filtered.y = (gyro[1] + gyro[4]) / 2;
|
||||
gyro_data.filtered.z = (gyro[2] + gyro[5]) / 2;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Perform calibration of a 3-axis field sensor using an affine transformation
|
||||
* matrix.
|
||||
*
|
||||
* Computes result = scale * arg.
|
||||
*
|
||||
* @param result[out] The three-axis resultant field.
|
||||
* @param scale[in] The 4x4 affine transformation matrix. The fourth row is implicitly
|
||||
* [0 0 0 1]
|
||||
* @param arg[in] The 3-axis input field. The 'w' component is implicitly 1.
|
||||
*/
|
||||
void calibration(float result[3], float scale[3][4], float arg[3])
|
||||
{
|
||||
for (int row = 0; row < 3; ++row) {
|
||||
result[row] = 0.0f;
|
||||
int col;
|
||||
for (col = 0; col < 3; ++col) {
|
||||
result[row] += scale[row][col] * arg[col];
|
||||
}
|
||||
// fourth column: arg has an implicit w value of 1.0f.
|
||||
result[row] += scale[row][col];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Scale an affine transformation matrix by a rotation, defined by a
|
||||
* rotation vector. scale <- rotation * scale
|
||||
*
|
||||
* @param scale[in,out] The affine transformation matrix to be rotated
|
||||
* @param rotation[in] The rotation vector defining the rotation
|
||||
*/
|
||||
void affine_rotate(float scale[3][4], float rotation[3])
|
||||
{
|
||||
// Rotate the scale factor matrix in-place
|
||||
float rmatrix[3][3];
|
||||
Rv2Rot(rotation, rmatrix);
|
||||
|
||||
float ret[3][4];
|
||||
for (int row = 0; row < 3; ++row) {
|
||||
for (int col = 0; col < 4; ++col) {
|
||||
ret[row][col] = 0.0f;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
ret[row][col] += rmatrix[row][i] * scale[i][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
// copy output to argument
|
||||
for (int row = 0; row < 3; ++row) {
|
||||
for (int col = 0; col < 4; ++col) {
|
||||
scale[row][col] = ret[row][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Downsample the analog data
|
||||
* @return none
|
||||
*
|
||||
* Tried to make as much of the filtering fixed point when possible. Need to account
|
||||
* for offset for each sample before the multiplication if filter not a boxcar. Could
|
||||
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
|
||||
* data structures @ref accel_data and @ref gyro_data.
|
||||
*
|
||||
* The accel_data values are converted into a coordinate system where X is forwards along
|
||||
* the fuselage, Y is along right the wing, and Z is down.
|
||||
*/
|
||||
void adc_callback(float * downsampled_data)
|
||||
{
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
#if 0
|
||||
// Get the Y data. Third byte in. Convert to m/s
|
||||
float accel_filtered[3];
|
||||
accel_filtered[1] = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_filtered[1] += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_filtered[1] /= (float) fir_coeffs[adc_oversampling];
|
||||
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
|
||||
accel_filtered[0] = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_filtered[0] += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_filtered[0] /= (float) fir_coeffs[adc_oversampling];
|
||||
|
||||
// Get the Z data. Third byte in. Convert to m/s
|
||||
accel_filtered[2] = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_filtered[2] += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_filtered[2] /= (float) fir_coeffs[adc_oversampling];
|
||||
|
||||
float accel_scaled[3];
|
||||
calibration(accel_scaled, accel_data.calibration.scale, accel_filtered);
|
||||
accel_data.filtered.x = accel_scaled[0];
|
||||
accel_data.filtered.y = accel_scaled[1];
|
||||
accel_data.filtered.z = accel_scaled[2];
|
||||
#else
|
||||
float accel[3], gyro[3];
|
||||
|
||||
float raw_accel[3] = {
|
||||
downsampled_data[ACCEL_RAW_X_IDX],
|
||||
downsampled_data[ACCEL_RAW_Y_IDX],
|
||||
-downsampled_data[ACCEL_RAW_Z_IDX]
|
||||
};
|
||||
|
||||
// Accel data is (y,x,z) in first third and fifth byte. Convert to m/s^2
|
||||
calibration(accel, accel_data.calibration.scale, raw_accel);
|
||||
|
||||
// Gyro data is (x,y,z) in second, fifth and seventh byte. Convert to rad/s
|
||||
gyro[0] = ( ( downsampled_data[GYRO_RAW_X_IDX] + gyro_data.calibration.tempcompfactor[0] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
|
||||
gyro[1] = ( ( downsampled_data[GYRO_RAW_Y_IDX] + gyro_data.calibration.tempcompfactor[1] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
|
||||
gyro[2] = ( ( downsampled_data[GYRO_RAW_Z_IDX] + gyro_data.calibration.tempcompfactor[2] * downsampled_data[GYRO_TEMP_RAW_Z_IDX] ) * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
|
||||
|
||||
#endif
|
||||
#if 0
|
||||
static float gravity_tracking[3] = {0,0,0};
|
||||
const float tau = 0.9999;
|
||||
gravity_tracking[0] = tau * gravity_tracking[0] + (1-tau) * accel[0];
|
||||
gravity_tracking[1] = tau * gravity_tracking[1] + (1-tau) * accel[1];
|
||||
gravity_tracking[2] = tau * gravity_tracking[2] + (1-tau) * (accel[2] + 9.81);
|
||||
if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
|
||||
accel[0] -= gravity_tracking[0];
|
||||
accel[1] -= gravity_tracking[1];
|
||||
accel[2] -= gravity_tracking[2];
|
||||
}
|
||||
#endif
|
||||
if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
|
||||
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
|
||||
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
|
||||
} else {
|
||||
ekf_too_slow++;
|
||||
}
|
||||
|
||||
AttitudeRawData raw;
|
||||
|
||||
raw.gyrotemp[0] = downsampled_data[GYRO_TEMP_RAW_XY_IDX];
|
||||
raw.gyrotemp[1] = downsampled_data[GYRO_TEMP_RAW_Z_IDX];
|
||||
|
||||
raw.gyros[0] = gyro[0] * RAD_TO_DEG;
|
||||
raw.gyros[1] = gyro[1] * RAD_TO_DEG;
|
||||
raw.gyros[2] = gyro[2] * RAD_TO_DEG;
|
||||
|
||||
raw.accels[0] = accel[0];
|
||||
raw.accels[1] = accel[1];
|
||||
raw.accels[2] = accel[2];
|
||||
|
||||
raw.magnetometers[0] = mag_data.scaled.axis[0];
|
||||
raw.magnetometers[1] = mag_data.scaled.axis[1];
|
||||
raw.magnetometers[2] = mag_data.scaled.axis[2];
|
||||
|
||||
if (settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE)
|
||||
{
|
||||
raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
|
||||
raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
|
||||
raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
|
||||
|
||||
raw.accels[0] -= Nav.accel_bias[0];
|
||||
raw.accels[1] -= Nav.accel_bias[1];
|
||||
raw.accels[2] -= Nav.accel_bias[2];
|
||||
}
|
||||
|
||||
AttitudeRawSet(&raw);
|
||||
|
||||
total_conversion_blocks++;
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
/**
|
||||
* @brief Get the mag data from the I2C sensor and load into structure
|
||||
* @return none
|
||||
*
|
||||
* This function also considers if the home location is set and has a valid
|
||||
* magnetic field before updating the mag data to prevent data being used that
|
||||
* cannot be interpreted. In addition the mag data is not used for the first
|
||||
* five seconds to allow the filter to start to converge
|
||||
*/
|
||||
void process_mag_data()
|
||||
{
|
||||
// Get magnetic readings
|
||||
// For now don't use mags until the magnetic field is set AND until 5 seconds
|
||||
// after initialization otherwise it seems to have problems
|
||||
// TODO: Follow up this initialization issue
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
if (PIOS_HMC5843_NewDataAvailable()) {
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
|
||||
// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
|
||||
// Only use if magnetic length reasonable
|
||||
float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
|
||||
|
||||
mag_data.updated = (home.Set == HOMELOCATION_SET_TRUE) &&
|
||||
((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) &&
|
||||
((mag_data.raw.axis[MAG_RAW_X_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Y_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Z_IDX] != 0)) &&
|
||||
((Blen < mag_len * (1 + INSGPS_MAGTOL)) && (Blen > mag_len * (1 - INSGPS_MAGTOL)));
|
||||
}
|
||||
}
|
||||
#else
|
||||
void process_mag_data() { }
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Assumes board is not moving computes biases and variances of sensors
|
||||
* @returns None
|
||||
*
|
||||
* All data is stored in global structures. This function should be called from OP when
|
||||
* aircraft is in stable state and then the data stored to SD card.
|
||||
*
|
||||
* After this function the bias for each sensor will be the mean value. This doesn't make
|
||||
* sense for the z accel so make sure 6 point calibration is also run and those values set
|
||||
* after these read.
|
||||
*/
|
||||
#define NBIAS 100
|
||||
#define NVAR 500
|
||||
void calibrate_sensors()
|
||||
{
|
||||
int i,j;
|
||||
float accel_bias[3] = {0, 0, 0};
|
||||
float gyro_bias[3] = {0, 0, 0};
|
||||
float mag_bias[3] = {0, 0, 0};
|
||||
|
||||
|
||||
for (i = 0, j = 0; i < NBIAS; i++) {
|
||||
|
||||
get_accel_gyro_data();
|
||||
|
||||
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
|
||||
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
|
||||
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
|
||||
accel_bias[0] += accel_data.filtered.x / NBIAS;
|
||||
accel_bias[1] += accel_data.filtered.y / NBIAS;
|
||||
accel_bias[2] += accel_data.filtered.z / NBIAS;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
mag_bias[0] += mag_data.scaled.axis[0];
|
||||
mag_bias[1] += mag_data.scaled.axis[1];
|
||||
mag_bias[2] += mag_data.scaled.axis[2];
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
mag_bias[0] /= j;
|
||||
mag_bias[1] /= j;
|
||||
mag_bias[2] /= j;
|
||||
|
||||
gyro_data.calibration.variance[0] = 0;
|
||||
gyro_data.calibration.variance[1] = 0;
|
||||
gyro_data.calibration.variance[2] = 0;
|
||||
mag_data.calibration.variance[0] = 0;
|
||||
mag_data.calibration.variance[1] = 0;
|
||||
mag_data.calibration.variance[2] = 0;
|
||||
accel_data.calibration.variance[0] = 0;
|
||||
accel_data.calibration.variance[1] = 0;
|
||||
accel_data.calibration.variance[2] = 0;
|
||||
|
||||
for (i = 0, j = 0; j < NVAR; j++) {
|
||||
get_accel_gyro_data();
|
||||
|
||||
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
|
||||
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
|
||||
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
|
||||
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
|
||||
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
|
||||
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
|
||||
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
|
||||
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
mag_data.calibration.variance[0] /= j;
|
||||
mag_data.calibration.variance[1] /= j;
|
||||
mag_data.calibration.variance[2] /= j;
|
||||
|
||||
gyro_data.calibration.bias[0] -= gyro_bias[0];
|
||||
gyro_data.calibration.bias[1] -= gyro_bias[1];
|
||||
gyro_data.calibration.bias[2] -= gyro_bias[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Populate fields with initial values
|
||||
*/
|
||||
void reset_values()
|
||||
{
|
||||
accel_data.calibration.scale[0][1] = 0;
|
||||
accel_data.calibration.scale[1][0] = 0;
|
||||
accel_data.calibration.scale[0][2] = 0;
|
||||
accel_data.calibration.scale[2][0] = 0;
|
||||
accel_data.calibration.scale[1][2] = 0;
|
||||
accel_data.calibration.scale[2][1] = 0;
|
||||
|
||||
accel_data.calibration.scale[0][0] = 0.0359;
|
||||
accel_data.calibration.scale[1][1] = 0.0359;
|
||||
accel_data.calibration.scale[2][2] = 0.0359;
|
||||
accel_data.calibration.scale[0][3] = -73.5;
|
||||
accel_data.calibration.scale[1][3] = -73.5;
|
||||
accel_data.calibration.scale[2][3] = -73.5;
|
||||
|
||||
accel_data.calibration.variance[0] = 1e-4;
|
||||
accel_data.calibration.variance[1] = 1e-4;
|
||||
accel_data.calibration.variance[2] = 1e-4;
|
||||
|
||||
gyro_data.calibration.scale[0] = -0.014;
|
||||
gyro_data.calibration.scale[1] = 0.014;
|
||||
gyro_data.calibration.scale[2] = -0.014;
|
||||
gyro_data.calibration.bias[0] = -24;
|
||||
gyro_data.calibration.bias[1] = -24;
|
||||
gyro_data.calibration.bias[2] = -24;
|
||||
gyro_data.calibration.variance[0] = 1;
|
||||
gyro_data.calibration.variance[1] = 1;
|
||||
gyro_data.calibration.variance[2] = 1;
|
||||
mag_data.calibration.scale[0] = 1;
|
||||
mag_data.calibration.scale[1] = 1;
|
||||
mag_data.calibration.scale[2] = 1;
|
||||
mag_data.calibration.bias[0] = 0;
|
||||
mag_data.calibration.bias[1] = 0;
|
||||
mag_data.calibration.bias[2] = 0;
|
||||
mag_data.calibration.variance[0] = 50;
|
||||
mag_data.calibration.variance[1] = 50;
|
||||
mag_data.calibration.variance[2] = 50;
|
||||
}
|
||||
|
||||
|
||||
void send_attitude(void)
|
||||
{
|
||||
AttitudeActualData attitude;
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
attitude.q1 = attitude_data.quaternion.q1;
|
||||
attitude.q2 = attitude_data.quaternion.q2;
|
||||
attitude.q3 = attitude_data.quaternion.q3;
|
||||
attitude.q4 = attitude_data.quaternion.q4;
|
||||
float rpy[3];
|
||||
Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
|
||||
attitude.Roll = rpy[0] + settings.RollBias;
|
||||
attitude.Pitch = rpy[1] + settings.PitchBias;
|
||||
attitude.Yaw = rpy[2] + settings.YawBias;
|
||||
if(attitude.Yaw > 360)
|
||||
attitude.Yaw -= 360;
|
||||
AttitudeActualSet(&attitude);
|
||||
}
|
||||
|
||||
void send_velocity(void)
|
||||
{
|
||||
VelocityActualData velocityActual;
|
||||
VelocityActualGet(&velocityActual);
|
||||
|
||||
// convert into cm
|
||||
velocityActual.North = Nav.Vel[0] * 100;
|
||||
velocityActual.East = Nav.Vel[1] * 100;
|
||||
velocityActual.Down = Nav.Vel[2] * 100;
|
||||
|
||||
VelocityActualSet(&velocityActual);
|
||||
}
|
||||
|
||||
void send_position(void)
|
||||
{
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
|
||||
// convert into cm
|
||||
positionActual.North = Nav.Pos[0] * 100;
|
||||
positionActual.East = Nav.Pos[1] * 100;
|
||||
positionActual.Down = Nav.Pos[2] * 100;
|
||||
|
||||
PositionActualSet(&positionActual);
|
||||
}
|
||||
|
||||
void send_calibration(void)
|
||||
{
|
||||
AHRSCalibrationData cal;
|
||||
AHRSCalibrationGet(&cal);
|
||||
for(int ct=0; ct<3; ct++)
|
||||
{
|
||||
cal.accel_var[ct] = accel_data.calibration.variance[ct];
|
||||
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
|
||||
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
|
||||
cal.mag_var[ct] = mag_data.calibration.variance[ct];
|
||||
}
|
||||
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
|
||||
AHRSCalibrationSet(&cal);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief AHRS calibration callback
|
||||
*
|
||||
* Called when the OP board sets the calibration
|
||||
*/
|
||||
void calibration_callback(AhrsObjHandle obj)
|
||||
{
|
||||
AHRSCalibrationData cal;
|
||||
AHRSCalibrationGet(&cal);
|
||||
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
|
||||
|
||||
accel_data.calibration.scale[0][1] = cal.accel_ortho[0];
|
||||
accel_data.calibration.scale[1][0] = cal.accel_ortho[0];
|
||||
|
||||
accel_data.calibration.scale[0][2] = cal.accel_ortho[1];
|
||||
accel_data.calibration.scale[2][0] = cal.accel_ortho[1];
|
||||
|
||||
accel_data.calibration.scale[1][2] = cal.accel_ortho[2];
|
||||
accel_data.calibration.scale[2][1] = cal.accel_ortho[2];
|
||||
|
||||
#if 0
|
||||
// TODO: Enable after v1.0 feature freeze.
|
||||
float rotation[3] = { cal.accel_rotation[0],
|
||||
cal.accel_rotation[1],
|
||||
cal.accel_rotation[2],
|
||||
};
|
||||
|
||||
affine_rotate(accel_data.calibration.scale, rotation);
|
||||
#endif
|
||||
|
||||
for(int ct=0; ct<3; ct++)
|
||||
{
|
||||
accel_data.calibration.scale[ct][ct] = cal.accel_scale[ct];
|
||||
accel_data.calibration.scale[ct][3] = cal.accel_bias[ct];
|
||||
accel_data.calibration.variance[ct] = cal.accel_var[ct];
|
||||
|
||||
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
|
||||
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
|
||||
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
|
||||
#if 1
|
||||
gyro_data.calibration.tempcompfactor[ct] = cal.gyro_tempcompfactor[ct];
|
||||
#endif
|
||||
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
|
||||
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
|
||||
mag_data.calibration.variance[ct] = cal.mag_var[ct];
|
||||
}
|
||||
// Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to
|
||||
// one in code
|
||||
float mag_var[3] = {mag_data.calibration.variance[0] / INSGPS_MAGLEN / INSGPS_MAGLEN,
|
||||
mag_data.calibration.variance[1] / INSGPS_MAGLEN / INSGPS_MAGLEN,
|
||||
mag_data.calibration.variance[2] / INSGPS_MAGLEN / INSGPS_MAGLEN};
|
||||
INSSetMagVar(mag_var);
|
||||
INSSetAccelVar(accel_data.calibration.variance);
|
||||
INSSetGyroVar(gyro_data.calibration.variance);
|
||||
}
|
||||
else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE) {
|
||||
calibrate_sensors();
|
||||
send_calibration();
|
||||
}
|
||||
|
||||
INSSetPosVelVar(cal.pos_var, cal.vel_var);
|
||||
|
||||
}
|
||||
|
||||
void gps_callback(AhrsObjHandle obj)
|
||||
{
|
||||
GPSPositionData pos;
|
||||
GPSPositionGet(&pos);
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
// convert from cm back to meters
|
||||
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
|
||||
// put in local NED frame
|
||||
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
|
||||
|
||||
gps_data.heading = pos.Heading;
|
||||
gps_data.groundspeed = pos.Groundspeed;
|
||||
gps_data.quality = 1; /* currently unused */
|
||||
gps_data.updated = true;
|
||||
|
||||
// if poor don't use this update
|
||||
if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
|
||||
(pos.Satellites < INSGPS_GPS_MINSAT) ||
|
||||
(pos.PDOP >= INSGPS_GPS_MINPDOP) ||
|
||||
(home.Set == FALSE) ||
|
||||
((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
|
||||
{
|
||||
gps_data.quality = 0;
|
||||
gps_data.updated = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void altitude_callback(AhrsObjHandle obj)
|
||||
{
|
||||
BaroAltitudeData alt;
|
||||
BaroAltitudeGet(&alt);
|
||||
altitude_data.altitude = alt.Altitude;
|
||||
altitude_data.updated = true;
|
||||
}
|
||||
|
||||
void settings_callback(AhrsObjHandle obj)
|
||||
{
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
ahrs_algorithm = settings.Algorithm;
|
||||
|
||||
if(settings.Downsampling != adc_oversampling) {
|
||||
adc_oversampling = settings.Downsampling;
|
||||
PIOS_ADC_Config(adc_oversampling);
|
||||
}
|
||||
}
|
||||
|
||||
void homelocation_callback(AhrsObjHandle obj)
|
||||
{
|
||||
HomeLocationData data;
|
||||
HomeLocationGet(&data);
|
||||
|
||||
mag_len = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2));
|
||||
float Be[3] = {data.Be[0] / mag_len, data.Be[1] / mag_len, data.Be[2] / mag_len};
|
||||
|
||||
INSSetMagNorth(Be);
|
||||
}
|
||||
|
||||
void firmwareiapobj_callback(AhrsObjHandle obj)
|
||||
{
|
||||
const struct pios_board_info * bdinfo = &pios_board_info_blob;
|
||||
|
||||
FirmwareIAPObjData firmwareIAPObj;
|
||||
FirmwareIAPObjGet(&firmwareIAPObj);
|
||||
if(firmwareIAPObj.ArmReset==0)
|
||||
reset_count=0;
|
||||
if(firmwareIAPObj.ArmReset==1)
|
||||
{
|
||||
|
||||
if((firmwareIAPObj.BoardType==bdinfo->board_type) || (firmwareIAPObj.BoardType==0xFF))
|
||||
{
|
||||
|
||||
++reset_count;
|
||||
if(reset_count>2)
|
||||
{
|
||||
PIOS_IAP_SetRequest1();
|
||||
PIOS_IAP_SetRequest2();
|
||||
PIOS_SYS_Reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(firmwareIAPObj.BoardType==bdinfo->board_type && firmwareIAPObj.crc!=PIOS_BL_HELPER_CRC_Memory_Calc())
|
||||
{
|
||||
PIOS_BL_HELPER_FLASH_Read_Description(firmwareIAPObj.Description,bdinfo->desc_size);
|
||||
firmwareIAPObj.crc=PIOS_BL_HELPER_CRC_Memory_Calc();
|
||||
firmwareIAPObj.BoardRevision=bdinfo->board_rev;
|
||||
FirmwareIAPObjSet(&firmwareIAPObj);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
@ -1,61 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_TIMER AHRS Timer
|
||||
* @brief Sets up a simple timer that can be polled to estimate idle time
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief INSGPS Test Program
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "ahrs_timer.h"
|
||||
|
||||
void timer_start()
|
||||
{
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,
|
||||
ENABLE);
|
||||
PWR_BackupAccessCmd(ENABLE);
|
||||
|
||||
RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128);
|
||||
RCC_RTCCLKCmd(ENABLE);
|
||||
RTC_WaitForLastTask();
|
||||
RTC_WaitForSynchro();
|
||||
RTC_WaitForLastTask();
|
||||
RTC_SetPrescaler(0); // counting at 8e6 / 128
|
||||
RTC_WaitForLastTask();
|
||||
RTC_SetCounter(0);
|
||||
RTC_WaitForLastTask();
|
||||
}
|
||||
|
||||
uint32_t timer_count()
|
||||
{
|
||||
return RTC_GetCounter();
|
||||
}
|
||||
|
||||
uint32_t timer_rate()
|
||||
{
|
||||
return TIMER_RATE;
|
||||
}
|
@ -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,45 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS Control
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_TIMER
|
||||
* @brief Sets up a simple timer that can be polled to estimate idle time
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief INSGPS Test Program
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef AHRS_TIMER
|
||||
#define AHRS_TIMER
|
||||
|
||||
#include <pios.h>
|
||||
|
||||
#define TIMER_RATE (8e6 / 128)
|
||||
|
||||
void timer_start();
|
||||
uint32_t timer_count();
|
||||
uint32_t timer_rate();
|
||||
|
||||
#endif
|
@ -1,93 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS
|
||||
* @{
|
||||
* @addtogroup INSGPS
|
||||
* @{
|
||||
* @brief INSGPS is a joint attitude and position estimation EKF
|
||||
*
|
||||
* @file insgps.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the INSGPS exposed functionality.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef INSGPS_H_
|
||||
#define INSGPS_H_
|
||||
|
||||
#include "stdint.h"
|
||||
|
||||
/**
|
||||
* @addtogroup Constants
|
||||
* @{
|
||||
*/
|
||||
#define POS_SENSORS 0x007
|
||||
#define HORIZ_SENSORS 0x018
|
||||
#define VERT_SENSORS 0x020
|
||||
#define MAG_SENSORS 0x1C0
|
||||
#define BARO_SENSOR 0x200
|
||||
|
||||
#define FULL_SENSORS 0x3FF
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
// Exposed Function Prototypes
|
||||
void INSGPSInit();
|
||||
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
|
||||
void INSCovariancePrediction(float dT);
|
||||
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
|
||||
|
||||
void INSResetP(float PDiag[13]);
|
||||
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
|
||||
void INSSetPosVelVar(float PosVar, float VelVar);
|
||||
void INSSetGyroBias(float gyro_bias[3]);
|
||||
void INSSetAccelVar(float accel_var[3]);
|
||||
void INSSetGyroVar(float gyro_var[3]);
|
||||
void INSSetMagNorth(float B[3]);
|
||||
void INSSetMagVar(float scaled_mag_var[3]);
|
||||
void INSPosVelReset(float pos[3], float vel[3]);
|
||||
|
||||
void MagCorrection(float mag_data[3]);
|
||||
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt);
|
||||
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
|
||||
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
|
||||
void VelBaroCorrection(float Vel[3], float BaroAlt);
|
||||
|
||||
uint16_t ins_get_num_states();
|
||||
|
||||
// Nav structure containing current solution
|
||||
struct NavStruct {
|
||||
float Pos[3]; // Position in meters and relative to a local NED frame
|
||||
float Vel[3]; // Velocity in meters and in NED
|
||||
float q[4]; // unit quaternion rotation relative to NED
|
||||
float gyro_bias[3];
|
||||
float accel_bias[3];
|
||||
} Nav;
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
#endif /* EKF_H_ */
|
@ -1,45 +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
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
@ -1,1644 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS
|
||||
* @{
|
||||
* @addtogroup INSGPS
|
||||
* @{
|
||||
* @brief INSGPS is a joint attitude and position estimation EKF
|
||||
*
|
||||
* @file insgps.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief An INS/GPS algorithm implemented with an EKF.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "insgps.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
// constants/macros/typdefs
|
||||
#define NUMX 13 // number of states, X is the state vector
|
||||
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
|
||||
#define NUMV 10 // number of measurements, v is the measurement noise vector
|
||||
#define NUMU 6 // number of deterministic inputs, U is the input vector
|
||||
|
||||
#if defined(GENERAL_COV)
|
||||
// This might trick people so I have a note here. There is a slower but bigger version of the
|
||||
// code here but won't fit when debugging disabled (requires -Os)
|
||||
#define COVARIANCE_PREDICTION_GENERAL
|
||||
#endif
|
||||
|
||||
// Private functions
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX]);
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed);
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW]);
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
|
||||
// Private variables
|
||||
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
|
||||
// global to init to zero and maintain zero elements
|
||||
float Be[3]; // local magnetic unit vector in NED frame
|
||||
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
|
||||
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
|
||||
float K[NUMX][NUMV]; // feedback gain matrix
|
||||
|
||||
// ************* Exposed Functions ****************
|
||||
// *************************************************
|
||||
|
||||
uint16_t ins_get_num_states()
|
||||
{
|
||||
return NUMX;
|
||||
}
|
||||
|
||||
void INSGPSInit() //pretty much just a place holder for now
|
||||
{
|
||||
Be[0] = 1;
|
||||
Be[1] = 0;
|
||||
Be[2] = 0; // local magnetic unit vector
|
||||
|
||||
for (int i = 0; i < NUMX; i++) {
|
||||
for (int j = 0; j < NUMX; j++) {
|
||||
P[i][j] = 0; // zero all terms
|
||||
}
|
||||
}
|
||||
|
||||
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
|
||||
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
|
||||
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
|
||||
P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
|
||||
|
||||
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
|
||||
X[6] = 1;
|
||||
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
|
||||
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
|
||||
|
||||
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
|
||||
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
|
||||
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
|
||||
|
||||
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
|
||||
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
|
||||
R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
|
||||
R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
|
||||
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
|
||||
R[9] = .05; // High freq altimeter noise variance (m^2)
|
||||
}
|
||||
|
||||
void INSResetP(float PDiag[NUMX])
|
||||
{
|
||||
uint8_t i,j;
|
||||
|
||||
// if PDiag[i] nonzero then clear row and column and set diagonal element
|
||||
for (i=0;i<NUMX;i++){
|
||||
if (PDiag != 0){
|
||||
for (j=0;j<NUMX;j++)
|
||||
P[i][j]=P[j][i]=0;
|
||||
P[i][i]=PDiag[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
|
||||
{
|
||||
/* Note: accel_bias not used in 13 state INS */
|
||||
X[0] = pos[0];
|
||||
X[1] = pos[1];
|
||||
X[2] = pos[2];
|
||||
X[3] = vel[0];
|
||||
X[4] = vel[1];
|
||||
X[5] = vel[2];
|
||||
X[6] = q[0];
|
||||
X[7] = q[1];
|
||||
X[8] = q[2];
|
||||
X[9] = q[3];
|
||||
X[10] = gyro_bias[0];
|
||||
X[11] = gyro_bias[1];
|
||||
X[12] = gyro_bias[2];
|
||||
}
|
||||
|
||||
void INSPosVelReset(float pos[3], float vel[3])
|
||||
{
|
||||
for (int i = 0; i < 6; i++) {
|
||||
for(int j = i; j < NUMX; j++) {
|
||||
P[i][j] = 0; // zero the first 6 rows and columns
|
||||
P[j][i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
|
||||
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
|
||||
|
||||
X[0] = pos[0];
|
||||
X[1] = pos[1];
|
||||
X[2] = pos[2];
|
||||
X[3] = vel[0];
|
||||
X[4] = vel[1];
|
||||
X[5] = vel[2];
|
||||
}
|
||||
|
||||
void INSSetPosVelVar(float PosVar, float VelVar)
|
||||
{
|
||||
R[0] = PosVar;
|
||||
R[1] = PosVar;
|
||||
R[2] = PosVar;
|
||||
R[3] = VelVar;
|
||||
R[4] = VelVar;
|
||||
// R[5] = PosVar; // Don't change vertical velocity, not measured
|
||||
}
|
||||
|
||||
void INSSetGyroBias(float gyro_bias[3])
|
||||
{
|
||||
X[10] = gyro_bias[0];
|
||||
X[11] = gyro_bias[1];
|
||||
X[12] = gyro_bias[2];
|
||||
}
|
||||
|
||||
void INSSetAccelVar(float accel_var[3])
|
||||
{
|
||||
Q[3] = accel_var[0];
|
||||
Q[4] = accel_var[1];
|
||||
Q[5] = accel_var[2];
|
||||
}
|
||||
|
||||
void INSSetGyroVar(float gyro_var[3])
|
||||
{
|
||||
Q[0] = gyro_var[0];
|
||||
Q[1] = gyro_var[1];
|
||||
Q[2] = gyro_var[2];
|
||||
}
|
||||
|
||||
void INSSetMagVar(float scaled_mag_var[3])
|
||||
{
|
||||
R[6] = scaled_mag_var[0];
|
||||
R[7] = scaled_mag_var[1];
|
||||
R[8] = scaled_mag_var[2];
|
||||
}
|
||||
|
||||
void INSSetMagNorth(float B[3])
|
||||
{
|
||||
Be[0] = B[0];
|
||||
Be[1] = B[1];
|
||||
Be[2] = B[2];
|
||||
}
|
||||
|
||||
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
{
|
||||
float U[6];
|
||||
float qmag;
|
||||
|
||||
// rate gyro inputs in units of rad/s
|
||||
U[0] = gyro_data[0];
|
||||
U[1] = gyro_data[1];
|
||||
U[2] = gyro_data[2];
|
||||
|
||||
// accelerometer inputs in units of m/s
|
||||
U[3] = accel_data[0];
|
||||
U[4] = accel_data[1];
|
||||
U[5] = accel_data[2];
|
||||
|
||||
// EKF prediction step
|
||||
LinearizeFG(X, U, F, G);
|
||||
RungeKutta(X, U, dT);
|
||||
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
|
||||
X[6] /= qmag;
|
||||
X[7] /= qmag;
|
||||
X[8] /= qmag;
|
||||
X[9] /= qmag;
|
||||
//CovariancePrediction(F,G,Q,dT,P);
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
Nav.Pos[1] = X[1];
|
||||
Nav.Pos[2] = X[2];
|
||||
Nav.Vel[0] = X[3];
|
||||
Nav.Vel[1] = X[4];
|
||||
Nav.Vel[2] = X[5];
|
||||
Nav.q[0] = X[6];
|
||||
Nav.q[1] = X[7];
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
Nav.gyro_bias[0] = X[10];
|
||||
Nav.gyro_bias[1] = X[11];
|
||||
Nav.gyro_bias[2] = X[12];
|
||||
}
|
||||
|
||||
void INSCovariancePrediction(float dT)
|
||||
{
|
||||
CovariancePrediction(F, G, Q, dT, P);
|
||||
}
|
||||
|
||||
float zeros[3] = { 0, 0, 0 };
|
||||
|
||||
void MagCorrection(float mag_data[3])
|
||||
{
|
||||
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
|
||||
}
|
||||
|
||||
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(mag_data, zeros, Vel, BaroAlt,
|
||||
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
|
||||
BARO_SENSOR);
|
||||
}
|
||||
|
||||
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(zeros, Pos, Vel, BaroAlt,
|
||||
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
}
|
||||
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt)
|
||||
{
|
||||
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
|
||||
}
|
||||
|
||||
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
|
||||
{
|
||||
INSCorrection(mag_data, Pos, Vel, zeros[0],
|
||||
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
|
||||
}
|
||||
|
||||
void VelBaroCorrection(float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(zeros, zeros, Vel, BaroAlt,
|
||||
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
}
|
||||
|
||||
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt, uint16_t SensorsUsed)
|
||||
{
|
||||
float Z[10], Y[10];
|
||||
float Bmag, qmag;
|
||||
|
||||
// GPS Position in meters and in local NED frame
|
||||
Z[0] = Pos[0];
|
||||
Z[1] = Pos[1];
|
||||
Z[2] = Pos[2];
|
||||
|
||||
// GPS Velocity in meters and in local NED frame
|
||||
Z[3] = Vel[0];
|
||||
Z[4] = Vel[1];
|
||||
Z[5] = Vel[2];
|
||||
|
||||
// magnetometer data in any units (use unit vector) and in body frame
|
||||
Bmag =
|
||||
sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
|
||||
mag_data[2] * mag_data[2]);
|
||||
Z[6] = mag_data[0] / Bmag;
|
||||
Z[7] = mag_data[1] / Bmag;
|
||||
Z[8] = mag_data[2] / Bmag;
|
||||
|
||||
// barometric altimeter in meters and in local NED frame
|
||||
Z[9] = BaroAlt;
|
||||
|
||||
// EKF correction step
|
||||
LinearizeH(X, Be, H);
|
||||
MeasurementEq(X, Be, Y);
|
||||
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
|
||||
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
|
||||
X[6] /= qmag;
|
||||
X[7] /= qmag;
|
||||
X[8] /= qmag;
|
||||
X[9] /= qmag;
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
Nav.Pos[1] = X[1];
|
||||
Nav.Pos[2] = X[2];
|
||||
Nav.Vel[0] = X[3];
|
||||
Nav.Vel[1] = X[4];
|
||||
Nav.Vel[2] = X[5];
|
||||
Nav.q[0] = X[6];
|
||||
Nav.q[1] = X[7];
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
}
|
||||
|
||||
// ************* CovariancePrediction *************
|
||||
// Does the prediction step of the Kalman filter for the covariance matrix
|
||||
// Output, Pnew, overwrites P, the input covariance
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
|
||||
// Q is the discrete time covariance of process noise
|
||||
// Q is vector of the diagonal for a square matrix with
|
||||
// dimensions equal to the number of disturbance noise variables
|
||||
// The General Method is very inefficient,not taking advantage of the sparse F and G
|
||||
// The first Method is very specific to this implementation
|
||||
// ************************************************
|
||||
|
||||
#ifdef COVARIANCE_PREDICTION_GENERAL
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float Dummy[NUMX][NUMX], dTsq;
|
||||
uint8_t i, j, k;
|
||||
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
|
||||
|
||||
dTsq = dT * dT;
|
||||
|
||||
for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
|
||||
for (j = 0; j < NUMX; j++) {
|
||||
Dummy[i][j] = P[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
Dummy[i][j] += F[i][k] * P[k][j];
|
||||
}
|
||||
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
|
||||
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
|
||||
P[i][j] = Dummy[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
|
||||
for (k = 0; k < NUMW; k++)
|
||||
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
|
||||
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float D[NUMX][NUMX], T, Tsq;
|
||||
uint8_t i, j;
|
||||
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
|
||||
|
||||
T = dT;
|
||||
Tsq = dT * dT;
|
||||
|
||||
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
|
||||
for (j = i; j < NUMX; j++)
|
||||
D[i][j] = P[i][j];
|
||||
|
||||
// Brute force calculation of the elements of P
|
||||
P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0];
|
||||
P[0][1] = P[1][0] =
|
||||
D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1];
|
||||
P[0][2] = P[2][0] =
|
||||
D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2];
|
||||
P[0][3] = P[3][0] =
|
||||
(F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] +
|
||||
F[3][9] * D[3][9]) * Tsq + (D[3][3] + F[3][6] * D[0][6] +
|
||||
F[3][7] * D[0][7] +
|
||||
F[3][8] * D[0][8] +
|
||||
F[3][9] * D[0][9]) * T + D[0][3];
|
||||
P[0][4] = P[4][0] =
|
||||
(F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] +
|
||||
F[4][9] * D[3][9]) * Tsq + (D[3][4] + F[4][6] * D[0][6] +
|
||||
F[4][7] * D[0][7] +
|
||||
F[4][8] * D[0][8] +
|
||||
F[4][9] * D[0][9]) * T + D[0][4];
|
||||
P[0][5] = P[5][0] =
|
||||
(F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] +
|
||||
F[5][9] * D[3][9]) * Tsq + (D[3][5] + F[5][6] * D[0][6] +
|
||||
F[5][7] * D[0][7] +
|
||||
F[5][8] * D[0][8] +
|
||||
F[5][9] * D[0][9]) * T + D[0][5];
|
||||
P[0][6] = P[6][0] =
|
||||
(F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] +
|
||||
F[6][10] * D[3][10] + F[6][11] * D[3][11] +
|
||||
F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] +
|
||||
F[6][8] * D[0][8] +
|
||||
F[6][9] * D[0][9] +
|
||||
F[6][10] * D[0][10] +
|
||||
F[6][11] * D[0][11] +
|
||||
F[6][12] * D[0][12]) * T +
|
||||
D[0][6];
|
||||
P[0][7] = P[7][0] =
|
||||
(F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] +
|
||||
F[7][10] * D[3][10] + F[7][11] * D[3][11] +
|
||||
F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] +
|
||||
F[7][8] * D[0][8] +
|
||||
F[7][9] * D[0][9] +
|
||||
F[7][10] * D[0][10] +
|
||||
F[7][11] * D[0][11] +
|
||||
F[7][12] * D[0][12]) * T +
|
||||
D[0][7];
|
||||
P[0][8] = P[8][0] =
|
||||
(F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] +
|
||||
F[8][10] * D[3][10] + F[8][11] * D[3][11] +
|
||||
F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] +
|
||||
F[8][7] * D[0][7] +
|
||||
F[8][9] * D[0][9] +
|
||||
F[8][10] * D[0][10] +
|
||||
F[8][11] * D[0][11] +
|
||||
F[8][12] * D[0][12]) * T +
|
||||
D[0][8];
|
||||
P[0][9] = P[9][0] =
|
||||
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
|
||||
F[9][10] * D[3][10] + F[9][11] * D[3][11] +
|
||||
F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] +
|
||||
F[9][7] * D[0][7] +
|
||||
F[9][8] * D[0][8] +
|
||||
F[9][10] * D[0][10] +
|
||||
F[9][11] * D[0][11] +
|
||||
F[9][12] * D[0][12]) * T +
|
||||
D[0][9];
|
||||
P[0][10] = P[10][0] = D[3][10] * T + D[0][10];
|
||||
P[0][11] = P[11][0] = D[3][11] * T + D[0][11];
|
||||
P[0][12] = P[12][0] = D[3][12] * T + D[0][12];
|
||||
P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1];
|
||||
P[1][2] = P[2][1] =
|
||||
D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2];
|
||||
P[1][3] = P[3][1] =
|
||||
(F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] +
|
||||
F[3][9] * D[4][9]) * Tsq + (D[3][4] + F[3][6] * D[1][6] +
|
||||
F[3][7] * D[1][7] +
|
||||
F[3][8] * D[1][8] +
|
||||
F[3][9] * D[1][9]) * T + D[1][3];
|
||||
P[1][4] = P[4][1] =
|
||||
(F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] +
|
||||
F[4][9] * D[4][9]) * Tsq + (D[4][4] + F[4][6] * D[1][6] +
|
||||
F[4][7] * D[1][7] +
|
||||
F[4][8] * D[1][8] +
|
||||
F[4][9] * D[1][9]) * T + D[1][4];
|
||||
P[1][5] = P[5][1] =
|
||||
(F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] +
|
||||
F[5][9] * D[4][9]) * Tsq + (D[4][5] + F[5][6] * D[1][6] +
|
||||
F[5][7] * D[1][7] +
|
||||
F[5][8] * D[1][8] +
|
||||
F[5][9] * D[1][9]) * T + D[1][5];
|
||||
P[1][6] = P[6][1] =
|
||||
(F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] +
|
||||
F[6][10] * D[4][10] + F[6][11] * D[4][11] +
|
||||
F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] +
|
||||
F[6][8] * D[1][8] +
|
||||
F[6][9] * D[1][9] +
|
||||
F[6][10] * D[1][10] +
|
||||
F[6][11] * D[1][11] +
|
||||
F[6][12] * D[1][12]) * T +
|
||||
D[1][6];
|
||||
P[1][7] = P[7][1] =
|
||||
(F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] +
|
||||
F[7][10] * D[4][10] + F[7][11] * D[4][11] +
|
||||
F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] +
|
||||
F[7][8] * D[1][8] +
|
||||
F[7][9] * D[1][9] +
|
||||
F[7][10] * D[1][10] +
|
||||
F[7][11] * D[1][11] +
|
||||
F[7][12] * D[1][12]) * T +
|
||||
D[1][7];
|
||||
P[1][8] = P[8][1] =
|
||||
(F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] +
|
||||
F[8][10] * D[4][10] + F[8][11] * D[4][11] +
|
||||
F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] +
|
||||
F[8][7] * D[1][7] +
|
||||
F[8][9] * D[1][9] +
|
||||
F[8][10] * D[1][10] +
|
||||
F[8][11] * D[1][11] +
|
||||
F[8][12] * D[1][12]) * T +
|
||||
D[1][8];
|
||||
P[1][9] = P[9][1] =
|
||||
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
|
||||
F[9][10] * D[4][10] + F[9][11] * D[4][11] +
|
||||
F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] +
|
||||
F[9][7] * D[1][7] +
|
||||
F[9][8] * D[1][8] +
|
||||
F[9][10] * D[1][10] +
|
||||
F[9][11] * D[1][11] +
|
||||
F[9][12] * D[1][12]) * T +
|
||||
D[1][9];
|
||||
P[1][10] = P[10][1] = D[4][10] * T + D[1][10];
|
||||
P[1][11] = P[11][1] = D[4][11] * T + D[1][11];
|
||||
P[1][12] = P[12][1] = D[4][12] * T + D[1][12];
|
||||
P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2];
|
||||
P[2][3] = P[3][2] =
|
||||
(F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] +
|
||||
F[3][9] * D[5][9]) * Tsq + (D[3][5] + F[3][6] * D[2][6] +
|
||||
F[3][7] * D[2][7] +
|
||||
F[3][8] * D[2][8] +
|
||||
F[3][9] * D[2][9]) * T + D[2][3];
|
||||
P[2][4] = P[4][2] =
|
||||
(F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] +
|
||||
F[4][9] * D[5][9]) * Tsq + (D[4][5] + F[4][6] * D[2][6] +
|
||||
F[4][7] * D[2][7] +
|
||||
F[4][8] * D[2][8] +
|
||||
F[4][9] * D[2][9]) * T + D[2][4];
|
||||
P[2][5] = P[5][2] =
|
||||
(F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] +
|
||||
F[5][9] * D[5][9]) * Tsq + (D[5][5] + F[5][6] * D[2][6] +
|
||||
F[5][7] * D[2][7] +
|
||||
F[5][8] * D[2][8] +
|
||||
F[5][9] * D[2][9]) * T + D[2][5];
|
||||
P[2][6] = P[6][2] =
|
||||
(F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] +
|
||||
F[6][10] * D[5][10] + F[6][11] * D[5][11] +
|
||||
F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] +
|
||||
F[6][8] * D[2][8] +
|
||||
F[6][9] * D[2][9] +
|
||||
F[6][10] * D[2][10] +
|
||||
F[6][11] * D[2][11] +
|
||||
F[6][12] * D[2][12]) * T +
|
||||
D[2][6];
|
||||
P[2][7] = P[7][2] =
|
||||
(F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] +
|
||||
F[7][10] * D[5][10] + F[7][11] * D[5][11] +
|
||||
F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] +
|
||||
F[7][8] * D[2][8] +
|
||||
F[7][9] * D[2][9] +
|
||||
F[7][10] * D[2][10] +
|
||||
F[7][11] * D[2][11] +
|
||||
F[7][12] * D[2][12]) * T +
|
||||
D[2][7];
|
||||
P[2][8] = P[8][2] =
|
||||
(F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] +
|
||||
F[8][10] * D[5][10] + F[8][11] * D[5][11] +
|
||||
F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] +
|
||||
F[8][7] * D[2][7] +
|
||||
F[8][9] * D[2][9] +
|
||||
F[8][10] * D[2][10] +
|
||||
F[8][11] * D[2][11] +
|
||||
F[8][12] * D[2][12]) * T +
|
||||
D[2][8];
|
||||
P[2][9] = P[9][2] =
|
||||
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
|
||||
F[9][10] * D[5][10] + F[9][11] * D[5][11] +
|
||||
F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] +
|
||||
F[9][7] * D[2][7] +
|
||||
F[9][8] * D[2][8] +
|
||||
F[9][10] * D[2][10] +
|
||||
F[9][11] * D[2][11] +
|
||||
F[9][12] * D[2][12]) * T +
|
||||
D[2][9];
|
||||
P[2][10] = P[10][2] = D[5][10] * T + D[2][10];
|
||||
P[2][11] = P[11][2] = D[5][11] * T + D[2][11];
|
||||
P[2][12] = P[12][2] = D[5][12] * T + D[2][12];
|
||||
P[3][3] =
|
||||
(Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] +
|
||||
Q[5] * G[3][5] * G[3][5] + F[3][9] * (F[3][9] * D[9][9] +
|
||||
F[3][6] * D[6][9] +
|
||||
F[3][7] * D[7][9] +
|
||||
F[3][8] * D[8][9]) +
|
||||
F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
|
||||
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
|
||||
F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
|
||||
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
|
||||
F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
|
||||
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
|
||||
(2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] +
|
||||
2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9]) * T + D[3][3];
|
||||
P[3][4] = P[4][3] =
|
||||
(F[4][9] *
|
||||
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
|
||||
F[3][8] * D[8][9]) + F[4][6] * (F[3][6] * D[6][6] +
|
||||
F[3][7] * D[6][7] +
|
||||
F[3][8] * D[6][8] +
|
||||
F[3][9] * D[6][9]) +
|
||||
F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
|
||||
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
|
||||
F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
|
||||
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
|
||||
G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] +
|
||||
G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] +
|
||||
F[4][6] * D[3][6] +
|
||||
F[3][7] * D[4][7] +
|
||||
F[4][7] * D[3][7] +
|
||||
F[3][8] * D[4][8] +
|
||||
F[4][8] * D[3][8] +
|
||||
F[3][9] * D[4][9] +
|
||||
F[4][9] * D[3][9]) * T +
|
||||
D[3][4];
|
||||
P[3][5] = P[5][3] =
|
||||
(F[5][9] *
|
||||
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
|
||||
F[3][8] * D[8][9]) + F[5][6] * (F[3][6] * D[6][6] +
|
||||
F[3][7] * D[6][7] +
|
||||
F[3][8] * D[6][8] +
|
||||
F[3][9] * D[6][9]) +
|
||||
F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
|
||||
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
|
||||
F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
|
||||
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
|
||||
G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] +
|
||||
G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] +
|
||||
F[5][6] * D[3][6] +
|
||||
F[3][7] * D[5][7] +
|
||||
F[5][7] * D[3][7] +
|
||||
F[3][8] * D[5][8] +
|
||||
F[5][8] * D[3][8] +
|
||||
F[3][9] * D[5][9] +
|
||||
F[5][9] * D[3][9]) * T +
|
||||
D[3][5];
|
||||
P[3][6] = P[6][3] =
|
||||
(F[6][9] *
|
||||
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
|
||||
F[3][8] * D[8][9]) + F[6][10] * (F[3][9] * D[9][10] +
|
||||
F[3][6] * D[6][10] +
|
||||
F[3][7] * D[7][10] +
|
||||
F[3][8] * D[8][10]) +
|
||||
F[6][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
|
||||
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
|
||||
F[6][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
|
||||
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
|
||||
F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
|
||||
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
|
||||
F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
|
||||
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
|
||||
(F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] +
|
||||
F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] +
|
||||
F[6][9] * D[3][9] + F[6][10] * D[3][10] +
|
||||
F[6][11] * D[3][11] + F[6][12] * D[3][12]) * T + D[3][6];
|
||||
P[3][7] = P[7][3] =
|
||||
(F[7][9] *
|
||||
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
|
||||
F[3][8] * D[8][9]) + F[7][10] * (F[3][9] * D[9][10] +
|
||||
F[3][6] * D[6][10] +
|
||||
F[3][7] * D[7][10] +
|
||||
F[3][8] * D[8][10]) +
|
||||
F[7][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
|
||||
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
|
||||
F[7][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
|
||||
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
|
||||
F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
|
||||
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
|
||||
F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
|
||||
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
|
||||
(F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] +
|
||||
F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] +
|
||||
F[7][9] * D[3][9] + F[7][10] * D[3][10] +
|
||||
F[7][11] * D[3][11] + F[7][12] * D[3][12]) * T + D[3][7];
|
||||
P[3][8] = P[8][3] =
|
||||
(F[8][9] *
|
||||
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
|
||||
F[3][8] * D[8][9]) + F[8][10] * (F[3][9] * D[9][10] +
|
||||
F[3][6] * D[6][10] +
|
||||
F[3][7] * D[7][10] +
|
||||
F[3][8] * D[8][10]) +
|
||||
F[8][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
|
||||
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
|
||||
F[8][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
|
||||
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
|
||||
F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
|
||||
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
|
||||
F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
|
||||
F[3][8] * D[7][8] + F[3][9] * D[7][9])) * Tsq +
|
||||
(F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] +
|
||||
F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] +
|
||||
F[8][9] * D[3][9] + F[8][10] * D[3][10] +
|
||||
F[8][11] * D[3][11] + F[8][12] * D[3][12]) * T + D[3][8];
|
||||
P[3][9] = P[9][3] =
|
||||
(F[9][10] *
|
||||
(F[3][9] * D[9][10] + F[3][6] * D[6][10] +
|
||||
F[3][7] * D[7][10] + F[3][8] * D[8][10]) +
|
||||
F[9][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
|
||||
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
|
||||
F[9][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
|
||||
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
|
||||
F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
|
||||
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
|
||||
F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
|
||||
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
|
||||
F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
|
||||
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
|
||||
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
|
||||
F[3][9] * D[9][9] + F[9][10] * D[3][10] +
|
||||
F[9][11] * D[3][11] + F[9][12] * D[3][12] +
|
||||
F[3][6] * D[6][9] + F[3][7] * D[7][9] +
|
||||
F[3][8] * D[8][9]) * T + D[3][9];
|
||||
P[3][10] = P[10][3] =
|
||||
(F[3][9] * D[9][10] + F[3][6] * D[6][10] + F[3][7] * D[7][10] +
|
||||
F[3][8] * D[8][10]) * T + D[3][10];
|
||||
P[3][11] = P[11][3] =
|
||||
(F[3][9] * D[9][11] + F[3][6] * D[6][11] + F[3][7] * D[7][11] +
|
||||
F[3][8] * D[8][11]) * T + D[3][11];
|
||||
P[3][12] = P[12][3] =
|
||||
(F[3][9] * D[9][12] + F[3][6] * D[6][12] + F[3][7] * D[7][12] +
|
||||
F[3][8] * D[8][12]) * T + D[3][12];
|
||||
P[4][4] =
|
||||
(Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] +
|
||||
Q[5] * G[4][5] * G[4][5] + F[4][9] * (F[4][9] * D[9][9] +
|
||||
F[4][6] * D[6][9] +
|
||||
F[4][7] * D[7][9] +
|
||||
F[4][8] * D[8][9]) +
|
||||
F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
|
||||
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
|
||||
F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
|
||||
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
|
||||
F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
|
||||
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
|
||||
(2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] +
|
||||
2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9]) * T + D[4][4];
|
||||
P[4][5] = P[5][4] =
|
||||
(F[5][9] *
|
||||
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
|
||||
F[4][8] * D[8][9]) + F[5][6] * (F[4][6] * D[6][6] +
|
||||
F[4][7] * D[6][7] +
|
||||
F[4][8] * D[6][8] +
|
||||
F[4][9] * D[6][9]) +
|
||||
F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
|
||||
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
|
||||
F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
|
||||
F[4][8] * D[8][8] + F[4][9] * D[8][9]) +
|
||||
G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] +
|
||||
G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] +
|
||||
F[5][6] * D[4][6] +
|
||||
F[4][7] * D[5][7] +
|
||||
F[5][7] * D[4][7] +
|
||||
F[4][8] * D[5][8] +
|
||||
F[5][8] * D[4][8] +
|
||||
F[4][9] * D[5][9] +
|
||||
F[5][9] * D[4][9]) * T +
|
||||
D[4][5];
|
||||
P[4][6] = P[6][4] =
|
||||
(F[6][9] *
|
||||
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
|
||||
F[4][8] * D[8][9]) + F[6][10] * (F[4][9] * D[9][10] +
|
||||
F[4][6] * D[6][10] +
|
||||
F[4][7] * D[7][10] +
|
||||
F[4][8] * D[8][10]) +
|
||||
F[6][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
|
||||
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
|
||||
F[6][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
|
||||
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
|
||||
F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
|
||||
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
|
||||
F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
|
||||
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
|
||||
(F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] +
|
||||
F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] +
|
||||
F[6][9] * D[4][9] + F[6][10] * D[4][10] +
|
||||
F[6][11] * D[4][11] + F[6][12] * D[4][12]) * T + D[4][6];
|
||||
P[4][7] = P[7][4] =
|
||||
(F[7][9] *
|
||||
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
|
||||
F[4][8] * D[8][9]) + F[7][10] * (F[4][9] * D[9][10] +
|
||||
F[4][6] * D[6][10] +
|
||||
F[4][7] * D[7][10] +
|
||||
F[4][8] * D[8][10]) +
|
||||
F[7][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
|
||||
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
|
||||
F[7][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
|
||||
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
|
||||
F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
|
||||
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
|
||||
F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
|
||||
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
|
||||
(F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] +
|
||||
F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] +
|
||||
F[7][9] * D[4][9] + F[7][10] * D[4][10] +
|
||||
F[7][11] * D[4][11] + F[7][12] * D[4][12]) * T + D[4][7];
|
||||
P[4][8] = P[8][4] =
|
||||
(F[8][9] *
|
||||
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
|
||||
F[4][8] * D[8][9]) + F[8][10] * (F[4][9] * D[9][10] +
|
||||
F[4][6] * D[6][10] +
|
||||
F[4][7] * D[7][10] +
|
||||
F[4][8] * D[8][10]) +
|
||||
F[8][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
|
||||
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
|
||||
F[8][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
|
||||
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
|
||||
F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
|
||||
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
|
||||
F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
|
||||
F[4][8] * D[7][8] + F[4][9] * D[7][9])) * Tsq +
|
||||
(F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] +
|
||||
F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] +
|
||||
F[8][9] * D[4][9] + F[8][10] * D[4][10] +
|
||||
F[8][11] * D[4][11] + F[8][12] * D[4][12]) * T + D[4][8];
|
||||
P[4][9] = P[9][4] =
|
||||
(F[9][10] *
|
||||
(F[4][9] * D[9][10] + F[4][6] * D[6][10] +
|
||||
F[4][7] * D[7][10] + F[4][8] * D[8][10]) +
|
||||
F[9][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
|
||||
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
|
||||
F[9][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
|
||||
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
|
||||
F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
|
||||
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
|
||||
F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
|
||||
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
|
||||
F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
|
||||
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
|
||||
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
|
||||
F[4][9] * D[9][9] + F[9][10] * D[4][10] +
|
||||
F[9][11] * D[4][11] + F[9][12] * D[4][12] +
|
||||
F[4][6] * D[6][9] + F[4][7] * D[7][9] +
|
||||
F[4][8] * D[8][9]) * T + D[4][9];
|
||||
P[4][10] = P[10][4] =
|
||||
(F[4][9] * D[9][10] + F[4][6] * D[6][10] + F[4][7] * D[7][10] +
|
||||
F[4][8] * D[8][10]) * T + D[4][10];
|
||||
P[4][11] = P[11][4] =
|
||||
(F[4][9] * D[9][11] + F[4][6] * D[6][11] + F[4][7] * D[7][11] +
|
||||
F[4][8] * D[8][11]) * T + D[4][11];
|
||||
P[4][12] = P[12][4] =
|
||||
(F[4][9] * D[9][12] + F[4][6] * D[6][12] + F[4][7] * D[7][12] +
|
||||
F[4][8] * D[8][12]) * T + D[4][12];
|
||||
P[5][5] =
|
||||
(Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] +
|
||||
Q[5] * G[5][5] * G[5][5] + F[5][9] * (F[5][9] * D[9][9] +
|
||||
F[5][6] * D[6][9] +
|
||||
F[5][7] * D[7][9] +
|
||||
F[5][8] * D[8][9]) +
|
||||
F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
|
||||
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
|
||||
F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
|
||||
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
|
||||
F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
|
||||
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
|
||||
(2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] +
|
||||
2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9]) * T + D[5][5];
|
||||
P[5][6] = P[6][5] =
|
||||
(F[6][9] *
|
||||
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
|
||||
F[5][8] * D[8][9]) + F[6][10] * (F[5][9] * D[9][10] +
|
||||
F[5][6] * D[6][10] +
|
||||
F[5][7] * D[7][10] +
|
||||
F[5][8] * D[8][10]) +
|
||||
F[6][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
|
||||
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
|
||||
F[6][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
|
||||
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
|
||||
F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
|
||||
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
|
||||
F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
|
||||
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
|
||||
(F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] +
|
||||
F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] +
|
||||
F[6][9] * D[5][9] + F[6][10] * D[5][10] +
|
||||
F[6][11] * D[5][11] + F[6][12] * D[5][12]) * T + D[5][6];
|
||||
P[5][7] = P[7][5] =
|
||||
(F[7][9] *
|
||||
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
|
||||
F[5][8] * D[8][9]) + F[7][10] * (F[5][9] * D[9][10] +
|
||||
F[5][6] * D[6][10] +
|
||||
F[5][7] * D[7][10] +
|
||||
F[5][8] * D[8][10]) +
|
||||
F[7][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
|
||||
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
|
||||
F[7][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
|
||||
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
|
||||
F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
|
||||
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
|
||||
F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
|
||||
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
|
||||
(F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] +
|
||||
F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] +
|
||||
F[7][9] * D[5][9] + F[7][10] * D[5][10] +
|
||||
F[7][11] * D[5][11] + F[7][12] * D[5][12]) * T + D[5][7];
|
||||
P[5][8] = P[8][5] =
|
||||
(F[8][9] *
|
||||
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
|
||||
F[5][8] * D[8][9]) + F[8][10] * (F[5][9] * D[9][10] +
|
||||
F[5][6] * D[6][10] +
|
||||
F[5][7] * D[7][10] +
|
||||
F[5][8] * D[8][10]) +
|
||||
F[8][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
|
||||
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
|
||||
F[8][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
|
||||
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
|
||||
F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
|
||||
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
|
||||
F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
|
||||
F[5][8] * D[7][8] + F[5][9] * D[7][9])) * Tsq +
|
||||
(F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] +
|
||||
F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] +
|
||||
F[8][9] * D[5][9] + F[8][10] * D[5][10] +
|
||||
F[8][11] * D[5][11] + F[8][12] * D[5][12]) * T + D[5][8];
|
||||
P[5][9] = P[9][5] =
|
||||
(F[9][10] *
|
||||
(F[5][9] * D[9][10] + F[5][6] * D[6][10] +
|
||||
F[5][7] * D[7][10] + F[5][8] * D[8][10]) +
|
||||
F[9][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
|
||||
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
|
||||
F[9][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
|
||||
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
|
||||
F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
|
||||
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
|
||||
F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
|
||||
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
|
||||
F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
|
||||
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
|
||||
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
|
||||
F[5][9] * D[9][9] + F[9][10] * D[5][10] +
|
||||
F[9][11] * D[5][11] + F[9][12] * D[5][12] +
|
||||
F[5][6] * D[6][9] + F[5][7] * D[7][9] +
|
||||
F[5][8] * D[8][9]) * T + D[5][9];
|
||||
P[5][10] = P[10][5] =
|
||||
(F[5][9] * D[9][10] + F[5][6] * D[6][10] + F[5][7] * D[7][10] +
|
||||
F[5][8] * D[8][10]) * T + D[5][10];
|
||||
P[5][11] = P[11][5] =
|
||||
(F[5][9] * D[9][11] + F[5][6] * D[6][11] + F[5][7] * D[7][11] +
|
||||
F[5][8] * D[8][11]) * T + D[5][11];
|
||||
P[5][12] = P[12][5] =
|
||||
(F[5][9] * D[9][12] + F[5][6] * D[6][12] + F[5][7] * D[7][12] +
|
||||
F[5][8] * D[8][12]) * T + D[5][12];
|
||||
P[6][6] =
|
||||
(Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] +
|
||||
Q[2] * G[6][2] * G[6][2] + F[6][9] * (F[6][9] * D[9][9] +
|
||||
F[6][10] * D[9][10] +
|
||||
F[6][11] * D[9][11] +
|
||||
F[6][12] * D[9][12] +
|
||||
F[6][7] * D[7][9] +
|
||||
F[6][8] * D[8][9]) +
|
||||
F[6][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
|
||||
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
|
||||
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
|
||||
F[6][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
|
||||
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
|
||||
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
|
||||
F[6][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
|
||||
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
|
||||
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
|
||||
F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
|
||||
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
|
||||
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
|
||||
F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
|
||||
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
|
||||
F[6][11] * D[8][11] + F[6][12] * D[8][12])) * Tsq +
|
||||
(2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] +
|
||||
2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] +
|
||||
2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T +
|
||||
D[6][6];
|
||||
P[6][7] = P[7][6] =
|
||||
(F[7][9] *
|
||||
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
|
||||
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
|
||||
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
|
||||
F[7][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
|
||||
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
|
||||
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
|
||||
F[7][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
|
||||
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
|
||||
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
|
||||
F[7][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
|
||||
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
|
||||
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
|
||||
F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
|
||||
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
|
||||
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
|
||||
F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
|
||||
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
|
||||
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
|
||||
G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] +
|
||||
G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] +
|
||||
F[6][7] * D[7][7] +
|
||||
F[6][8] * D[7][8] +
|
||||
F[7][8] * D[6][8] +
|
||||
F[6][9] * D[7][9] +
|
||||
F[7][9] * D[6][9] +
|
||||
F[6][10] * D[7][10] +
|
||||
F[7][10] * D[6][10] +
|
||||
F[6][11] * D[7][11] +
|
||||
F[7][11] * D[6][11] +
|
||||
F[6][12] * D[7][12] +
|
||||
F[7][12] * D[6][12]) * T +
|
||||
D[6][7];
|
||||
P[6][8] = P[8][6] =
|
||||
(F[8][9] *
|
||||
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
|
||||
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
|
||||
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
|
||||
F[8][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
|
||||
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
|
||||
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
|
||||
F[8][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
|
||||
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
|
||||
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
|
||||
F[8][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
|
||||
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
|
||||
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
|
||||
F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
|
||||
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
|
||||
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
|
||||
F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
|
||||
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
|
||||
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
|
||||
G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] +
|
||||
G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] +
|
||||
F[8][6] * D[6][6] +
|
||||
F[8][7] * D[6][7] +
|
||||
F[6][8] * D[8][8] +
|
||||
F[6][9] * D[8][9] +
|
||||
F[8][9] * D[6][9] +
|
||||
F[6][10] * D[8][10] +
|
||||
F[8][10] * D[6][10] +
|
||||
F[6][11] * D[8][11] +
|
||||
F[8][11] * D[6][11] +
|
||||
F[6][12] * D[8][12] +
|
||||
F[8][12] * D[6][12]) * T +
|
||||
D[6][8];
|
||||
P[6][9] = P[9][6] =
|
||||
(F[9][10] *
|
||||
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
|
||||
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
|
||||
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
|
||||
F[9][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
|
||||
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
|
||||
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
|
||||
F[9][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
|
||||
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
|
||||
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
|
||||
F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
|
||||
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
|
||||
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
|
||||
F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
|
||||
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
|
||||
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
|
||||
F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
|
||||
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
|
||||
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
|
||||
G[9][0] * G[6][0] * Q[0] + G[9][1] * G[6][1] * Q[1] +
|
||||
G[9][2] * G[6][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] +
|
||||
F[9][7] * D[6][7] +
|
||||
F[9][8] * D[6][8] +
|
||||
F[6][9] * D[9][9] +
|
||||
F[9][10] * D[6][10] +
|
||||
F[6][10] * D[9][10] +
|
||||
F[9][11] * D[6][11] +
|
||||
F[6][11] * D[9][11] +
|
||||
F[9][12] * D[6][12] +
|
||||
F[6][12] * D[9][12] +
|
||||
F[6][7] * D[7][9] +
|
||||
F[6][8] * D[8][9]) * T +
|
||||
D[6][9];
|
||||
P[6][10] = P[10][6] =
|
||||
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
|
||||
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
|
||||
F[6][7] * D[7][10] + F[6][8] * D[8][10]) * T + D[6][10];
|
||||
P[6][11] = P[11][6] =
|
||||
(F[6][9] * D[9][11] + F[6][10] * D[10][11] +
|
||||
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
|
||||
F[6][7] * D[7][11] + F[6][8] * D[8][11]) * T + D[6][11];
|
||||
P[6][12] = P[12][6] =
|
||||
(F[6][9] * D[9][12] + F[6][10] * D[10][12] +
|
||||
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
|
||||
F[6][7] * D[7][12] + F[6][8] * D[8][12]) * T + D[6][12];
|
||||
P[7][7] =
|
||||
(Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] +
|
||||
Q[2] * G[7][2] * G[7][2] + F[7][9] * (F[7][9] * D[9][9] +
|
||||
F[7][10] * D[9][10] +
|
||||
F[7][11] * D[9][11] +
|
||||
F[7][12] * D[9][12] +
|
||||
F[7][6] * D[6][9] +
|
||||
F[7][8] * D[8][9]) +
|
||||
F[7][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
|
||||
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
|
||||
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
|
||||
F[7][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
|
||||
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
|
||||
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
|
||||
F[7][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
|
||||
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
|
||||
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
|
||||
F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
|
||||
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
|
||||
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
|
||||
F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
|
||||
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
|
||||
F[7][11] * D[8][11] + F[7][12] * D[8][12])) * Tsq +
|
||||
(2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] +
|
||||
2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] +
|
||||
2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T +
|
||||
D[7][7];
|
||||
P[7][8] = P[8][7] =
|
||||
(F[8][9] *
|
||||
(F[7][9] * D[9][9] + F[7][10] * D[9][10] +
|
||||
F[7][11] * D[9][11] + F[7][12] * D[9][12] +
|
||||
F[7][6] * D[6][9] + F[7][8] * D[8][9]) +
|
||||
F[8][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
|
||||
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
|
||||
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
|
||||
F[8][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
|
||||
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
|
||||
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
|
||||
F[8][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
|
||||
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
|
||||
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
|
||||
F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
|
||||
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
|
||||
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
|
||||
F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
|
||||
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
|
||||
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
|
||||
G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] +
|
||||
G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] +
|
||||
F[8][6] * D[6][7] +
|
||||
F[8][7] * D[7][7] +
|
||||
F[7][8] * D[8][8] +
|
||||
F[7][9] * D[8][9] +
|
||||
F[8][9] * D[7][9] +
|
||||
F[7][10] * D[8][10] +
|
||||
F[8][10] * D[7][10] +
|
||||
F[7][11] * D[8][11] +
|
||||
F[8][11] * D[7][11] +
|
||||
F[7][12] * D[8][12] +
|
||||
F[8][12] * D[7][12]) * T +
|
||||
D[7][8];
|
||||
P[7][9] = P[9][7] =
|
||||
(F[9][10] *
|
||||
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
|
||||
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
|
||||
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
|
||||
F[9][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
|
||||
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
|
||||
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
|
||||
F[9][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
|
||||
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
|
||||
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
|
||||
F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
|
||||
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
|
||||
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
|
||||
F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
|
||||
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
|
||||
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
|
||||
F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
|
||||
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
|
||||
F[7][11] * D[8][11] + F[7][12] * D[8][12]) +
|
||||
G[9][0] * G[7][0] * Q[0] + G[9][1] * G[7][1] * Q[1] +
|
||||
G[9][2] * G[7][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] +
|
||||
F[9][7] * D[7][7] +
|
||||
F[9][8] * D[7][8] +
|
||||
F[7][9] * D[9][9] +
|
||||
F[9][10] * D[7][10] +
|
||||
F[7][10] * D[9][10] +
|
||||
F[9][11] * D[7][11] +
|
||||
F[7][11] * D[9][11] +
|
||||
F[9][12] * D[7][12] +
|
||||
F[7][12] * D[9][12] +
|
||||
F[7][6] * D[6][9] +
|
||||
F[7][8] * D[8][9]) * T +
|
||||
D[7][9];
|
||||
P[7][10] = P[10][7] =
|
||||
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
|
||||
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
|
||||
F[7][6] * D[6][10] + F[7][8] * D[8][10]) * T + D[7][10];
|
||||
P[7][11] = P[11][7] =
|
||||
(F[7][9] * D[9][11] + F[7][10] * D[10][11] +
|
||||
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
|
||||
F[7][6] * D[6][11] + F[7][8] * D[8][11]) * T + D[7][11];
|
||||
P[7][12] = P[12][7] =
|
||||
(F[7][9] * D[9][12] + F[7][10] * D[10][12] +
|
||||
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
|
||||
F[7][6] * D[6][12] + F[7][8] * D[8][12]) * T + D[7][12];
|
||||
P[8][8] =
|
||||
(Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] +
|
||||
Q[2] * G[8][2] * G[8][2] + F[8][9] * (F[8][9] * D[9][9] +
|
||||
F[8][10] * D[9][10] +
|
||||
F[8][11] * D[9][11] +
|
||||
F[8][12] * D[9][12] +
|
||||
F[8][6] * D[6][9] +
|
||||
F[8][7] * D[7][9]) +
|
||||
F[8][10] * (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
|
||||
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
|
||||
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
|
||||
F[8][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
|
||||
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
|
||||
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
|
||||
F[8][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
|
||||
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
|
||||
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
|
||||
F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
|
||||
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
|
||||
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
|
||||
F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
|
||||
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
|
||||
F[8][11] * D[7][11] + F[8][12] * D[7][12])) * Tsq +
|
||||
(2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] +
|
||||
2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] +
|
||||
2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T +
|
||||
D[8][8];
|
||||
P[8][9] = P[9][8] =
|
||||
(F[9][10] *
|
||||
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
|
||||
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
|
||||
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
|
||||
F[9][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
|
||||
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
|
||||
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
|
||||
F[9][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
|
||||
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
|
||||
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
|
||||
F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
|
||||
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
|
||||
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
|
||||
F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
|
||||
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
|
||||
F[8][11] * D[7][11] + F[8][12] * D[7][12]) +
|
||||
F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] +
|
||||
F[8][9] * D[8][9] + F[8][10] * D[8][10] +
|
||||
F[8][11] * D[8][11] + F[8][12] * D[8][12]) +
|
||||
G[9][0] * G[8][0] * Q[0] + G[9][1] * G[8][1] * Q[1] +
|
||||
G[9][2] * G[8][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] +
|
||||
F[9][7] * D[7][8] +
|
||||
F[9][8] * D[8][8] +
|
||||
F[8][9] * D[9][9] +
|
||||
F[9][10] * D[8][10] +
|
||||
F[8][10] * D[9][10] +
|
||||
F[9][11] * D[8][11] +
|
||||
F[8][11] * D[9][11] +
|
||||
F[9][12] * D[8][12] +
|
||||
F[8][12] * D[9][12] +
|
||||
F[8][6] * D[6][9] +
|
||||
F[8][7] * D[7][9]) * T +
|
||||
D[8][9];
|
||||
P[8][10] = P[10][8] =
|
||||
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
|
||||
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
|
||||
F[8][6] * D[6][10] + F[8][7] * D[7][10]) * T + D[8][10];
|
||||
P[8][11] = P[11][8] =
|
||||
(F[8][9] * D[9][11] + F[8][10] * D[10][11] +
|
||||
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
|
||||
F[8][6] * D[6][11] + F[8][7] * D[7][11]) * T + D[8][11];
|
||||
P[8][12] = P[12][8] =
|
||||
(F[8][9] * D[9][12] + F[8][10] * D[10][12] +
|
||||
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
|
||||
F[8][6] * D[6][12] + F[8][7] * D[7][12]) * T + D[8][12];
|
||||
P[9][9] =
|
||||
(Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] +
|
||||
Q[2] * G[9][2] * G[9][2] + F[9][10] * (F[9][10] * D[10][10] +
|
||||
F[9][11] * D[10][11] +
|
||||
F[9][12] * D[10][12] +
|
||||
F[9][6] * D[6][10] +
|
||||
F[9][7] * D[7][10] +
|
||||
F[9][8] * D[8][10]) +
|
||||
F[9][11] * (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
|
||||
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
|
||||
F[9][7] * D[7][11] + F[9][8] * D[8][11]) +
|
||||
F[9][12] * (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
|
||||
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
|
||||
F[9][7] * D[7][12] + F[9][8] * D[8][12]) +
|
||||
F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] +
|
||||
F[9][8] * D[6][8] + F[9][10] * D[6][10] +
|
||||
F[9][11] * D[6][11] + F[9][12] * D[6][12]) +
|
||||
F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] +
|
||||
F[9][8] * D[7][8] + F[9][10] * D[7][10] +
|
||||
F[9][11] * D[7][11] + F[9][12] * D[7][12]) +
|
||||
F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] +
|
||||
F[9][8] * D[8][8] + F[9][10] * D[8][10] +
|
||||
F[9][11] * D[8][11] + F[9][12] * D[8][12])) * Tsq +
|
||||
(2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] +
|
||||
2 * F[9][12] * D[9][12] + 2 * F[9][6] * D[6][9] +
|
||||
2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9]) * T + D[9][9];
|
||||
P[9][10] = P[10][9] =
|
||||
(F[9][10] * D[10][10] + F[9][11] * D[10][11] +
|
||||
F[9][12] * D[10][12] + F[9][6] * D[6][10] +
|
||||
F[9][7] * D[7][10] + F[9][8] * D[8][10]) * T + D[9][10];
|
||||
P[9][11] = P[11][9] =
|
||||
(F[9][10] * D[10][11] + F[9][11] * D[11][11] +
|
||||
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
|
||||
F[9][7] * D[7][11] + F[9][8] * D[8][11]) * T + D[9][11];
|
||||
P[9][12] = P[12][9] =
|
||||
(F[9][10] * D[10][12] + F[9][11] * D[11][12] +
|
||||
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
|
||||
F[9][7] * D[7][12] + F[9][8] * D[8][12]) * T + D[9][12];
|
||||
P[10][10] = Q[6] * Tsq + D[10][10];
|
||||
P[10][11] = P[11][10] = D[10][11];
|
||||
P[10][12] = P[12][10] = D[10][12];
|
||||
P[11][11] = Q[7] * Tsq + D[11][11];
|
||||
P[11][12] = P[12][11] = D[11][12];
|
||||
P[12][12] = Q[8] * Tsq + D[12][12];
|
||||
}
|
||||
#endif
|
||||
|
||||
// ************* SerialUpdate *******************
|
||||
// Does the update step of the Kalman filter for the covariance and estimate
|
||||
// Outputs are Xnew & Pnew, and are written over P and X
|
||||
// Z is actual measurement, Y is predicted measurement
|
||||
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
|
||||
// where K=P*H'*inv[H*P*H'+R]
|
||||
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
|
||||
// i.e. the measurment noises are uncorrelated.
|
||||
// It therefore uses a serial update that requires no matrix inversion by
|
||||
// processing the measurements one at a time.
|
||||
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
|
||||
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
|
||||
// The SensorsUsed variable is a bitwise mask indicating which sensors
|
||||
// should be used in the update.
|
||||
// ************************************************
|
||||
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed)
|
||||
{
|
||||
float HP[NUMX], HPHR, Error;
|
||||
uint8_t i, j, k, m;
|
||||
|
||||
for (m = 0; m < NUMV; m++) {
|
||||
|
||||
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
|
||||
|
||||
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
|
||||
HP[j] = 0;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
HP[j] += H[m][k] * P[k][j];
|
||||
}
|
||||
HPHR = R[m]; // Find HPHR = H*P*H' + R
|
||||
for (k = 0; k < NUMX; k++)
|
||||
HPHR += HP[k] * H[m][k];
|
||||
|
||||
for (k = 0; k < NUMX; k++)
|
||||
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
|
||||
|
||||
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
|
||||
for (j = i; j < NUMX; j++)
|
||||
P[i][j] = P[j][i] =
|
||||
P[i][j] - K[i][m] * HP[j];
|
||||
}
|
||||
|
||||
Error = Z[m] - Y[m];
|
||||
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
|
||||
X[i] = X[i] + K[i][m] * Error;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ************* RungeKutta **********************
|
||||
// Does a 4th order Runge Kutta numerical integration step
|
||||
// Output, Xnew, is written over X
|
||||
// NOTE the algorithm assumes time invariant state equations and
|
||||
// constant inputs over integration step
|
||||
// ************************************************
|
||||
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
{
|
||||
|
||||
float dT2 =
|
||||
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NUMX; i++)
|
||||
Xlast[i] = X[i]; // make a working copy
|
||||
|
||||
StateEq(X, U, K1); // k1 = f(x,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT2 * K1[i];
|
||||
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT2 * K2[i];
|
||||
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT * K3[i];
|
||||
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
|
||||
|
||||
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] =
|
||||
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
|
||||
K4[i]) / 6;
|
||||
}
|
||||
|
||||
// ************* Model Specific Stuff ***************************
|
||||
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
|
||||
//
|
||||
// State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
|
||||
// Deterministic Inputs = [AngularVel Accel]
|
||||
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
|
||||
//
|
||||
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
|
||||
// Inputs to Measurement = [EarthFrameMagField]
|
||||
//
|
||||
// Notes: Pos and Vel in earth frame
|
||||
// AngularVel and Accel in body frame
|
||||
// MagFields are unit vectors
|
||||
// Xdot is output of StateEq()
|
||||
// F and G are outputs of LinearizeFG(), all elements not set should be zero
|
||||
// y is output of OutputEq()
|
||||
// H is output of LinearizeH(), all elements not set should be zero
|
||||
// ************************************************
|
||||
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
|
||||
{
|
||||
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
|
||||
|
||||
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
|
||||
ax = U[3];
|
||||
ay = U[4];
|
||||
az = U[5]; // NO BIAS STATES ON ACCELS
|
||||
wx = U[0] - X[10];
|
||||
wy = U[1] - X[11];
|
||||
wz = U[2] - X[12]; // subtract the biases on gyros
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// Pdot = V
|
||||
Xdot[0] = X[3];
|
||||
Xdot[1] = X[4];
|
||||
Xdot[2] = X[5];
|
||||
|
||||
// Vdot = Reb*a
|
||||
Xdot[3] =
|
||||
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
|
||||
q0 * q3) *
|
||||
ay + 2 * (q1 * q3 + q0 * q2) * az;
|
||||
Xdot[4] =
|
||||
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
|
||||
q3 * q3) * ay + 2 * (q2 * q3 -
|
||||
q0 * q1) *
|
||||
az;
|
||||
Xdot[5] =
|
||||
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
|
||||
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
|
||||
|
||||
// qdot = Q*w
|
||||
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
|
||||
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
|
||||
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
|
||||
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
|
||||
|
||||
// best guess is that bias stays constant
|
||||
Xdot[10] = Xdot[11] = Xdot[12] = 0;
|
||||
}
|
||||
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW])
|
||||
{
|
||||
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
|
||||
|
||||
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
|
||||
ax = U[3];
|
||||
ay = U[4];
|
||||
az = U[5]; // NO BIAS STATES ON ACCELS
|
||||
wx = U[0] - X[10];
|
||||
wy = U[1] - X[11];
|
||||
wz = U[2] - X[12]; // subtract the biases on gyros
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// Pdot = V
|
||||
F[0][3] = F[1][4] = F[2][5] = 1;
|
||||
|
||||
// dVdot/dq
|
||||
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
|
||||
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
|
||||
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
|
||||
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
|
||||
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
|
||||
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
|
||||
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
|
||||
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
|
||||
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
|
||||
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
|
||||
// dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
|
||||
// F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
|
||||
// F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
|
||||
// F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
|
||||
|
||||
// dqdot/dq
|
||||
F[6][6] = 0;
|
||||
F[6][7] = -wx / 2;
|
||||
F[6][8] = -wy / 2;
|
||||
F[6][9] = -wz / 2;
|
||||
F[7][6] = wx / 2;
|
||||
F[7][7] = 0;
|
||||
F[7][8] = wz / 2;
|
||||
F[7][9] = -wy / 2;
|
||||
F[8][6] = wy / 2;
|
||||
F[8][7] = -wz / 2;
|
||||
F[8][8] = 0;
|
||||
F[8][9] = wx / 2;
|
||||
F[9][6] = wz / 2;
|
||||
F[9][7] = wy / 2;
|
||||
F[9][8] = -wx / 2;
|
||||
F[9][9] = 0;
|
||||
|
||||
// dqdot/dwbias
|
||||
F[6][10] = q1 / 2;
|
||||
F[6][11] = q2 / 2;
|
||||
F[6][12] = q3 / 2;
|
||||
F[7][10] = -q0 / 2;
|
||||
F[7][11] = q3 / 2;
|
||||
F[7][12] = -q2 / 2;
|
||||
F[8][10] = -q3 / 2;
|
||||
F[8][11] = -q0 / 2;
|
||||
F[8][12] = q1 / 2;
|
||||
F[9][10] = q2 / 2;
|
||||
F[9][11] = -q1 / 2;
|
||||
F[9][12] = -q0 / 2;
|
||||
|
||||
// dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
|
||||
G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
|
||||
G[3][4] = 2 * (-q1 * q2 + q0 * q3);
|
||||
G[3][5] = -2 * (q1 * q3 + q0 * q2);
|
||||
G[4][3] = -2 * (q1 * q2 + q0 * q3);
|
||||
G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
|
||||
G[4][5] = 2 * (-q2 * q3 + q0 * q1);
|
||||
G[5][3] = 2 * (-q1 * q3 + q0 * q2);
|
||||
G[5][4] = -2 * (q2 * q3 + q0 * q1);
|
||||
G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
|
||||
|
||||
// dqdot/dnw
|
||||
G[6][0] = q1 / 2;
|
||||
G[6][1] = q2 / 2;
|
||||
G[6][2] = q3 / 2;
|
||||
G[7][0] = -q0 / 2;
|
||||
G[7][1] = q3 / 2;
|
||||
G[7][2] = -q2 / 2;
|
||||
G[8][0] = -q3 / 2;
|
||||
G[8][1] = -q0 / 2;
|
||||
G[8][2] = q1 / 2;
|
||||
G[9][0] = q2 / 2;
|
||||
G[9][1] = -q1 / 2;
|
||||
G[9][2] = -q0 / 2;
|
||||
|
||||
// dwbias = random walk noise
|
||||
G[10][6] = G[11][7] = G[12][8] = 1;
|
||||
// dabias = random walk noise
|
||||
// G[13][9]=G[14][10]=G[15][11]=1; // NO BIAS STATES ON ACCELS
|
||||
}
|
||||
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
|
||||
{
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// first six outputs are P and V
|
||||
Y[0] = X[0];
|
||||
Y[1] = X[1];
|
||||
Y[2] = X[2];
|
||||
Y[3] = X[3];
|
||||
Y[4] = X[4];
|
||||
Y[5] = X[5];
|
||||
|
||||
// Bb=Rbe*Be
|
||||
Y[6] =
|
||||
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
|
||||
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
|
||||
q0 * q2) * Be[2];
|
||||
Y[7] =
|
||||
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
|
||||
q2 * q2 - q3 * q3) * Be[1] +
|
||||
2 * (q2 * q3 + q0 * q1) * Be[2];
|
||||
Y[8] =
|
||||
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
|
||||
q0 * q1) * Be[1] +
|
||||
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
|
||||
|
||||
// Alt = -Pz
|
||||
Y[9] = -X[2];
|
||||
}
|
||||
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
|
||||
{
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// dP/dP=I;
|
||||
H[0][0] = H[1][1] = H[2][2] = 1;
|
||||
// dV/dV=I;
|
||||
H[3][3] = H[4][4] = H[5][5] = 1;
|
||||
|
||||
// dBb/dq
|
||||
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
|
||||
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
|
||||
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
|
||||
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
|
||||
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
|
||||
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
|
||||
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
|
||||
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
|
||||
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
|
||||
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
|
||||
// dAlt/dPz = -1
|
||||
H[9][2] = -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -1,866 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS
|
||||
* @{
|
||||
* @addtogroup INSGPS
|
||||
* @{
|
||||
* @brief INSGPS is a joint attitude and position estimation EKF
|
||||
*
|
||||
* @file insgps.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief An INS/GPS algorithm implemented with an EKF.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "insgps.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
// constants/macros/typdefs
|
||||
#define NUMX 16 // number of states, X is the state vector
|
||||
#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector
|
||||
#define NUMV 10 // number of measurements, v is the measurement noise vector
|
||||
#define NUMU 6 // number of deterministic inputs, U is the input vector
|
||||
|
||||
#if defined(GENERAL_COV)
|
||||
// This might trick people so I have a note here. There is a slower but bigger version of the
|
||||
// code here but won't fit when debugging disabled (requires -Os)
|
||||
#define COVARIANCE_PREDICTION_GENERAL
|
||||
#endif
|
||||
|
||||
// Private functions
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX]);
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed);
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW]);
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
|
||||
// Private variables
|
||||
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
|
||||
// global to init to zero and maintain zero elements
|
||||
float Be[3]; // local magnetic unit vector in NED frame
|
||||
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
|
||||
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
|
||||
float K[NUMX][NUMV]; // feedback gain matrix
|
||||
|
||||
// ************* Exposed Functions ****************
|
||||
// *************************************************
|
||||
|
||||
uint16_t ins_get_num_states()
|
||||
{
|
||||
return NUMX;
|
||||
}
|
||||
|
||||
void INSGPSInit() //pretty much just a place holder for now
|
||||
{
|
||||
Be[0] = 1;
|
||||
Be[1] = 0;
|
||||
Be[2] = 0; // local magnetic unit vector
|
||||
|
||||
for (int i = 0; i < NUMX; i++) {
|
||||
for (int j = 0; j < NUMX; j++) {
|
||||
P[i][j] = 0; // zero all terms
|
||||
}
|
||||
}
|
||||
|
||||
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
|
||||
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
|
||||
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
|
||||
P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
|
||||
|
||||
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
|
||||
X[6] = 1;
|
||||
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
|
||||
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
|
||||
|
||||
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
|
||||
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
|
||||
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
|
||||
Q[9] = Q[10] = Q[11] = 2e-20; // accel bias random walk variance (m/s^3)^2
|
||||
|
||||
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
|
||||
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
|
||||
R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
|
||||
R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
|
||||
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
|
||||
R[9] = .05; // High freq altimeter noise variance (m^2)
|
||||
}
|
||||
|
||||
void INSResetP(float PDiag[NUMX])
|
||||
{
|
||||
uint8_t i,j;
|
||||
|
||||
// if PDiag[i] nonzero then clear row and column and set diagonal element
|
||||
for (i=0;i<NUMX;i++){
|
||||
if (PDiag != 0){
|
||||
for (j=0;j<NUMX;j++)
|
||||
P[i][j]=P[j][i]=0;
|
||||
P[i][i]=PDiag[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
|
||||
{
|
||||
Nav.Pos[0] = X[0] = pos[0];
|
||||
Nav.Pos[1] = X[1] = pos[1];
|
||||
Nav.Pos[2] = X[2] = pos[2];
|
||||
Nav.Vel[0] = X[3] = vel[0];
|
||||
Nav.Vel[1] = X[4] = vel[1];
|
||||
Nav.Vel[2] = X[5] = vel[2];
|
||||
Nav.q[0] = X[6] = q[0];
|
||||
Nav.q[1] = X[7] = q[1];
|
||||
Nav.q[2] = X[8] = q[2];
|
||||
Nav.q[3] = X[9] = q[3];
|
||||
Nav.gyro_bias[0] = X[10] = gyro_bias[0];
|
||||
Nav.gyro_bias[1] = X[11] = gyro_bias[1];
|
||||
Nav.gyro_bias[2] = X[12] = gyro_bias[2];
|
||||
Nav.accel_bias[0] = X[13] = accel_bias[0];
|
||||
Nav.accel_bias[1] = X[14] = accel_bias[1];
|
||||
Nav.accel_bias[2] = X[15] = accel_bias[2];
|
||||
}
|
||||
|
||||
void INSPosVelReset(float pos[3], float vel[3])
|
||||
{
|
||||
for (int i = 0; i < 6; i++) {
|
||||
for(int j = i; j < NUMX; j++) {
|
||||
P[i][j] = 0; // zero the first 6 rows and columns
|
||||
P[j][i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
|
||||
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
|
||||
|
||||
X[0] = pos[0];
|
||||
X[1] = pos[1];
|
||||
X[2] = pos[2];
|
||||
X[3] = vel[0];
|
||||
X[4] = vel[1];
|
||||
X[5] = vel[2];
|
||||
}
|
||||
|
||||
void INSSetPosVelVar(float PosVar, float VelVar)
|
||||
{
|
||||
R[0] = PosVar;
|
||||
R[1] = PosVar;
|
||||
R[2] = PosVar;
|
||||
R[3] = VelVar;
|
||||
R[4] = VelVar;
|
||||
// R[5] = PosVar; // Don't change vertical velocity, not measured
|
||||
}
|
||||
|
||||
void INSSetGyroBias(float gyro_bias[3])
|
||||
{
|
||||
X[10] = gyro_bias[0];
|
||||
X[11] = gyro_bias[1];
|
||||
X[12] = gyro_bias[2];
|
||||
}
|
||||
|
||||
void INSSetAccelVar(float accel_var[3])
|
||||
{
|
||||
Q[3] = accel_var[0];
|
||||
Q[4] = accel_var[1];
|
||||
Q[5] = accel_var[2];
|
||||
}
|
||||
|
||||
void INSSetGyroVar(float gyro_var[3])
|
||||
{
|
||||
Q[0] = gyro_var[0];
|
||||
Q[1] = gyro_var[1];
|
||||
Q[2] = gyro_var[2];
|
||||
}
|
||||
|
||||
void INSSetMagVar(float scaled_mag_var[3])
|
||||
{
|
||||
R[6] = scaled_mag_var[0];
|
||||
R[7] = scaled_mag_var[1];
|
||||
R[8] = scaled_mag_var[2];
|
||||
}
|
||||
|
||||
void INSSetMagNorth(float B[3])
|
||||
{
|
||||
Be[0] = B[0];
|
||||
Be[1] = B[1];
|
||||
Be[2] = B[2];
|
||||
}
|
||||
|
||||
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
{
|
||||
float U[6];
|
||||
float qmag;
|
||||
|
||||
// rate gyro inputs in units of rad/s
|
||||
U[0] = gyro_data[0];
|
||||
U[1] = gyro_data[1];
|
||||
U[2] = gyro_data[2];
|
||||
|
||||
// accelerometer inputs in units of m/s
|
||||
U[3] = accel_data[0];
|
||||
U[4] = accel_data[1];
|
||||
U[5] = accel_data[2];
|
||||
|
||||
// EKF prediction step
|
||||
LinearizeFG(X, U, F, G);
|
||||
RungeKutta(X, U, dT);
|
||||
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
|
||||
X[6] /= qmag;
|
||||
X[7] /= qmag;
|
||||
X[8] /= qmag;
|
||||
X[9] /= qmag;
|
||||
//CovariancePrediction(F,G,Q,dT,P);
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
Nav.Pos[1] = X[1];
|
||||
Nav.Pos[2] = X[2];
|
||||
Nav.Vel[0] = X[3];
|
||||
Nav.Vel[1] = X[4];
|
||||
Nav.Vel[2] = X[5];
|
||||
Nav.q[0] = X[6];
|
||||
Nav.q[1] = X[7];
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
Nav.gyro_bias[0] = X[10];
|
||||
Nav.gyro_bias[1] = X[11];
|
||||
Nav.gyro_bias[2] = X[12];
|
||||
}
|
||||
|
||||
void INSCovariancePrediction(float dT)
|
||||
{
|
||||
CovariancePrediction(F, G, Q, dT, P);
|
||||
}
|
||||
|
||||
float zeros[3] = { 0, 0, 0 };
|
||||
|
||||
void MagCorrection(float mag_data[3])
|
||||
{
|
||||
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
|
||||
}
|
||||
|
||||
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(mag_data, zeros, Vel, BaroAlt,
|
||||
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
|
||||
BARO_SENSOR);
|
||||
}
|
||||
|
||||
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(zeros, Pos, Vel, BaroAlt,
|
||||
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
}
|
||||
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt)
|
||||
{
|
||||
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
|
||||
}
|
||||
|
||||
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
|
||||
{
|
||||
INSCorrection(mag_data, Pos, Vel, zeros[0],
|
||||
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
|
||||
}
|
||||
|
||||
void VelBaroCorrection(float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(zeros, zeros, Vel, BaroAlt,
|
||||
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
}
|
||||
|
||||
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt, uint16_t SensorsUsed)
|
||||
{
|
||||
float Z[10], Y[10];
|
||||
float Bmag, qmag;
|
||||
|
||||
// GPS Position in meters and in local NED frame
|
||||
Z[0] = Pos[0];
|
||||
Z[1] = Pos[1];
|
||||
Z[2] = Pos[2];
|
||||
|
||||
// GPS Velocity in meters and in local NED frame
|
||||
Z[3] = Vel[0];
|
||||
Z[4] = Vel[1];
|
||||
Z[5] = Vel[2];
|
||||
|
||||
// magnetometer data in any units (use unit vector) and in body frame
|
||||
Bmag =
|
||||
sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
|
||||
mag_data[2] * mag_data[2]);
|
||||
Z[6] = mag_data[0] / Bmag;
|
||||
Z[7] = mag_data[1] / Bmag;
|
||||
Z[8] = mag_data[2] / Bmag;
|
||||
|
||||
// barometric altimeter in meters and in local NED frame
|
||||
Z[9] = BaroAlt;
|
||||
|
||||
// EKF correction step
|
||||
LinearizeH(X, Be, H);
|
||||
MeasurementEq(X, Be, Y);
|
||||
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
|
||||
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
|
||||
X[6] /= qmag;
|
||||
X[7] /= qmag;
|
||||
X[8] /= qmag;
|
||||
X[9] /= qmag;
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
Nav.Pos[1] = X[1];
|
||||
Nav.Pos[2] = X[2];
|
||||
Nav.Vel[0] = X[3];
|
||||
Nav.Vel[1] = X[4];
|
||||
Nav.Vel[2] = X[5];
|
||||
Nav.q[0] = X[6];
|
||||
Nav.q[1] = X[7];
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
Nav.gyro_bias[0] = X[10];
|
||||
Nav.gyro_bias[1] = X[11];
|
||||
Nav.gyro_bias[2] = X[12];
|
||||
Nav.accel_bias[0] = X[13];
|
||||
Nav.accel_bias[1] = X[14];
|
||||
Nav.accel_bias[2] = X[15];
|
||||
}
|
||||
|
||||
// ************* CovariancePrediction *************
|
||||
// Does the prediction step of the Kalman filter for the covariance matrix
|
||||
// Output, Pnew, overwrites P, the input covariance
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
|
||||
// Q is the discrete time covariance of process noise
|
||||
// Q is vector of the diagonal for a square matrix with
|
||||
// dimensions equal to the number of disturbance noise variables
|
||||
// The General Method is very inefficient,not taking advantage of the sparse F and G
|
||||
// The first Method is very specific to this implementation
|
||||
// ************************************************
|
||||
|
||||
#ifdef COVARIANCE_PREDICTION_GENERAL
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float Dummy[NUMX][NUMX], dTsq;
|
||||
uint8_t i, j, k;
|
||||
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
|
||||
|
||||
dTsq = dT * dT;
|
||||
|
||||
for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
|
||||
for (j = 0; j < NUMX; j++) {
|
||||
Dummy[i][j] = P[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
Dummy[i][j] += F[i][k] * P[k][j];
|
||||
}
|
||||
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
|
||||
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
|
||||
P[i][j] = Dummy[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
|
||||
for (k = 0; k < NUMW; k++)
|
||||
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
|
||||
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float D[NUMX][NUMX], T, Tsq;
|
||||
uint8_t i, j;
|
||||
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
|
||||
|
||||
T = dT;
|
||||
Tsq = dT * dT;
|
||||
|
||||
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
|
||||
for (j = i; j < NUMX; j++)
|
||||
D[i][j] = P[i][j];
|
||||
|
||||
// Brute force calculation of the elements of P
|
||||
P[0][0] = D[3][3]*Tsq + (2*D[0][3])*T + D[0][0];
|
||||
P[0][1] = P[1][0] = D[3][4]*Tsq + (D[0][4] + D[1][3])*T + D[0][1];
|
||||
P[0][2] = P[2][0] = D[3][5]*Tsq + (D[0][5] + D[2][3])*T + D[0][2];
|
||||
P[0][3] = P[3][0] = (F[3][6]*D[3][6] + F[3][7]*D[3][7] + F[3][8]*D[3][8] + F[3][9]*D[3][9] + F[3][13]*D[3][13] + F[3][14]*D[3][14] + F[3][15]*D[3][15])*Tsq + (D[3][3] + F[3][6]*D[0][6] + F[3][7]*D[0][7] + F[3][8]*D[0][8] + F[3][9]*D[0][9] + F[3][13]*D[0][13] + F[3][14]*D[0][14] + F[3][15]*D[0][15])*T + D[0][3];
|
||||
P[0][4] = P[4][0] = (F[4][6]*D[3][6] + F[4][7]*D[3][7] + F[4][8]*D[3][8] + F[4][9]*D[3][9] + F[4][13]*D[3][13] + F[4][14]*D[3][14] + F[4][15]*D[3][15])*Tsq + (D[3][4] + F[4][6]*D[0][6] + F[4][7]*D[0][7] + F[4][8]*D[0][8] + F[4][9]*D[0][9] + F[4][13]*D[0][13] + F[4][14]*D[0][14] + F[4][15]*D[0][15])*T + D[0][4];
|
||||
P[0][5] = P[5][0] = (F[5][6]*D[3][6] + F[5][7]*D[3][7] + F[5][8]*D[3][8] + F[5][9]*D[3][9] + F[5][13]*D[3][13] + F[5][14]*D[3][14] + F[5][15]*D[3][15])*Tsq + (D[3][5] + F[5][6]*D[0][6] + F[5][7]*D[0][7] + F[5][8]*D[0][8] + F[5][9]*D[0][9] + F[5][13]*D[0][13] + F[5][14]*D[0][14] + F[5][15]*D[0][15])*T + D[0][5];
|
||||
P[0][6] = P[6][0] = (F[6][7]*D[3][7] + F[6][8]*D[3][8] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*Tsq + (D[3][6] + F[6][7]*D[0][7] + F[6][8]*D[0][8] + F[6][9]*D[0][9] + F[6][10]*D[0][10] + F[6][11]*D[0][11] + F[6][12]*D[0][12])*T + D[0][6];
|
||||
P[0][7] = P[7][0] = (F[7][6]*D[3][6] + F[7][8]*D[3][8] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*Tsq + (D[3][7] + F[7][6]*D[0][6] + F[7][8]*D[0][8] + F[7][9]*D[0][9] + F[7][10]*D[0][10] + F[7][11]*D[0][11] + F[7][12]*D[0][12])*T + D[0][7];
|
||||
P[0][8] = P[8][0] = (F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*Tsq + (D[3][8] + F[8][6]*D[0][6] + F[8][7]*D[0][7] + F[8][9]*D[0][9] + F[8][10]*D[0][10] + F[8][11]*D[0][11] + F[8][12]*D[0][12])*T + D[0][8];
|
||||
P[0][9] = P[9][0] = (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12])*Tsq + (D[3][9] + F[9][6]*D[0][6] + F[9][7]*D[0][7] + F[9][8]*D[0][8] + F[9][10]*D[0][10] + F[9][11]*D[0][11] + F[9][12]*D[0][12])*T + D[0][9];
|
||||
P[0][10] = P[10][0] = D[3][10]*T + D[0][10];
|
||||
P[0][11] = P[11][0] = D[3][11]*T + D[0][11];
|
||||
P[0][12] = P[12][0] = D[3][12]*T + D[0][12];
|
||||
P[0][13] = P[13][0] = D[3][13]*T + D[0][13];
|
||||
P[0][14] = P[14][0] = D[3][14]*T + D[0][14];
|
||||
P[0][15] = P[15][0] = D[3][15]*T + D[0][15];
|
||||
P[1][1] = D[4][4]*Tsq + (2*D[1][4])*T + D[1][1];
|
||||
P[1][2] = P[2][1] = D[4][5]*Tsq + (D[1][5] + D[2][4])*T + D[1][2];
|
||||
P[1][3] = P[3][1] = (F[3][6]*D[4][6] + F[3][7]*D[4][7] + F[3][8]*D[4][8] + F[3][9]*D[4][9] + F[3][13]*D[4][13] + F[3][14]*D[4][14] + F[3][15]*D[4][15])*Tsq + (D[3][4] + F[3][6]*D[1][6] + F[3][7]*D[1][7] + F[3][8]*D[1][8] + F[3][9]*D[1][9] + F[3][13]*D[1][13] + F[3][14]*D[1][14] + F[3][15]*D[1][15])*T + D[1][3];
|
||||
P[1][4] = P[4][1] = (F[4][6]*D[4][6] + F[4][7]*D[4][7] + F[4][8]*D[4][8] + F[4][9]*D[4][9] + F[4][13]*D[4][13] + F[4][14]*D[4][14] + F[4][15]*D[4][15])*Tsq + (D[4][4] + F[4][6]*D[1][6] + F[4][7]*D[1][7] + F[4][8]*D[1][8] + F[4][9]*D[1][9] + F[4][13]*D[1][13] + F[4][14]*D[1][14] + F[4][15]*D[1][15])*T + D[1][4];
|
||||
P[1][5] = P[5][1] = (F[5][6]*D[4][6] + F[5][7]*D[4][7] + F[5][8]*D[4][8] + F[5][9]*D[4][9] + F[5][13]*D[4][13] + F[5][14]*D[4][14] + F[5][15]*D[4][15])*Tsq + (D[4][5] + F[5][6]*D[1][6] + F[5][7]*D[1][7] + F[5][8]*D[1][8] + F[5][9]*D[1][9] + F[5][13]*D[1][13] + F[5][14]*D[1][14] + F[5][15]*D[1][15])*T + D[1][5];
|
||||
P[1][6] = P[6][1] = (F[6][7]*D[4][7] + F[6][8]*D[4][8] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*Tsq + (D[4][6] + F[6][7]*D[1][7] + F[6][8]*D[1][8] + F[6][9]*D[1][9] + F[6][10]*D[1][10] + F[6][11]*D[1][11] + F[6][12]*D[1][12])*T + D[1][6];
|
||||
P[1][7] = P[7][1] = (F[7][6]*D[4][6] + F[7][8]*D[4][8] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*Tsq + (D[4][7] + F[7][6]*D[1][6] + F[7][8]*D[1][8] + F[7][9]*D[1][9] + F[7][10]*D[1][10] + F[7][11]*D[1][11] + F[7][12]*D[1][12])*T + D[1][7];
|
||||
P[1][8] = P[8][1] = (F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*Tsq + (D[4][8] + F[8][6]*D[1][6] + F[8][7]*D[1][7] + F[8][9]*D[1][9] + F[8][10]*D[1][10] + F[8][11]*D[1][11] + F[8][12]*D[1][12])*T + D[1][8];
|
||||
P[1][9] = P[9][1] = (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12])*Tsq + (D[4][9] + F[9][6]*D[1][6] + F[9][7]*D[1][7] + F[9][8]*D[1][8] + F[9][10]*D[1][10] + F[9][11]*D[1][11] + F[9][12]*D[1][12])*T + D[1][9];
|
||||
P[1][10] = P[10][1] = D[4][10]*T + D[1][10];
|
||||
P[1][11] = P[11][1] = D[4][11]*T + D[1][11];
|
||||
P[1][12] = P[12][1] = D[4][12]*T + D[1][12];
|
||||
P[1][13] = P[13][1] = D[4][13]*T + D[1][13];
|
||||
P[1][14] = P[14][1] = D[4][14]*T + D[1][14];
|
||||
P[1][15] = P[15][1] = D[4][15]*T + D[1][15];
|
||||
P[2][2] = D[5][5]*Tsq + (2*D[2][5])*T + D[2][2];
|
||||
P[2][3] = P[3][2] = (F[3][6]*D[5][6] + F[3][7]*D[5][7] + F[3][8]*D[5][8] + F[3][9]*D[5][9] + F[3][13]*D[5][13] + F[3][14]*D[5][14] + F[3][15]*D[5][15])*Tsq + (D[3][5] + F[3][6]*D[2][6] + F[3][7]*D[2][7] + F[3][8]*D[2][8] + F[3][9]*D[2][9] + F[3][13]*D[2][13] + F[3][14]*D[2][14] + F[3][15]*D[2][15])*T + D[2][3];
|
||||
P[2][4] = P[4][2] = (F[4][6]*D[5][6] + F[4][7]*D[5][7] + F[4][8]*D[5][8] + F[4][9]*D[5][9] + F[4][13]*D[5][13] + F[4][14]*D[5][14] + F[4][15]*D[5][15])*Tsq + (D[4][5] + F[4][6]*D[2][6] + F[4][7]*D[2][7] + F[4][8]*D[2][8] + F[4][9]*D[2][9] + F[4][13]*D[2][13] + F[4][14]*D[2][14] + F[4][15]*D[2][15])*T + D[2][4];
|
||||
P[2][5] = P[5][2] = (F[5][6]*D[5][6] + F[5][7]*D[5][7] + F[5][8]*D[5][8] + F[5][9]*D[5][9] + F[5][13]*D[5][13] + F[5][14]*D[5][14] + F[5][15]*D[5][15])*Tsq + (D[5][5] + F[5][6]*D[2][6] + F[5][7]*D[2][7] + F[5][8]*D[2][8] + F[5][9]*D[2][9] + F[5][13]*D[2][13] + F[5][14]*D[2][14] + F[5][15]*D[2][15])*T + D[2][5];
|
||||
P[2][6] = P[6][2] = (F[6][7]*D[5][7] + F[6][8]*D[5][8] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*Tsq + (D[5][6] + F[6][7]*D[2][7] + F[6][8]*D[2][8] + F[6][9]*D[2][9] + F[6][10]*D[2][10] + F[6][11]*D[2][11] + F[6][12]*D[2][12])*T + D[2][6];
|
||||
P[2][7] = P[7][2] = (F[7][6]*D[5][6] + F[7][8]*D[5][8] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*Tsq + (D[5][7] + F[7][6]*D[2][6] + F[7][8]*D[2][8] + F[7][9]*D[2][9] + F[7][10]*D[2][10] + F[7][11]*D[2][11] + F[7][12]*D[2][12])*T + D[2][7];
|
||||
P[2][8] = P[8][2] = (F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*Tsq + (D[5][8] + F[8][6]*D[2][6] + F[8][7]*D[2][7] + F[8][9]*D[2][9] + F[8][10]*D[2][10] + F[8][11]*D[2][11] + F[8][12]*D[2][12])*T + D[2][8];
|
||||
P[2][9] = P[9][2] = (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12])*Tsq + (D[5][9] + F[9][6]*D[2][6] + F[9][7]*D[2][7] + F[9][8]*D[2][8] + F[9][10]*D[2][10] + F[9][11]*D[2][11] + F[9][12]*D[2][12])*T + D[2][9];
|
||||
P[2][10] = P[10][2] = D[5][10]*T + D[2][10];
|
||||
P[2][11] = P[11][2] = D[5][11]*T + D[2][11];
|
||||
P[2][12] = P[12][2] = D[5][12]*T + D[2][12];
|
||||
P[2][13] = P[13][2] = D[5][13]*T + D[2][13];
|
||||
P[2][14] = P[14][2] = D[5][14]*T + D[2][14];
|
||||
P[2][15] = P[15][2] = D[5][15]*T + D[2][15];
|
||||
P[3][3] = (Q[3]*G[3][3]*G[3][3] + Q[4]*G[3][4]*G[3][4] + Q[5]*G[3][5]*G[3][5] + F[3][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[3][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[3][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[3][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[3][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[3][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[3][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (2*F[3][6]*D[3][6] + 2*F[3][7]*D[3][7] + 2*F[3][8]*D[3][8] + 2*F[3][9]*D[3][9] + 2*F[3][13]*D[3][13] + 2*F[3][14]*D[3][14] + 2*F[3][15]*D[3][15])*T + D[3][3];
|
||||
P[3][4] = P[4][3] = (F[4][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[4][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[4][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[4][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[4][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[4][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[4][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[4][3]*Q[3] + G[3][4]*G[4][4]*Q[4] + G[3][5]*G[4][5]*Q[5])*Tsq + (F[3][6]*D[4][6] + F[4][6]*D[3][6] + F[3][7]*D[4][7] + F[4][7]*D[3][7] + F[3][8]*D[4][8] + F[4][8]*D[3][8] + F[3][9]*D[4][9] + F[4][9]*D[3][9] + F[3][13]*D[4][13] + F[4][13]*D[3][13] + F[3][14]*D[4][14] + F[4][14]*D[3][14] + F[3][15]*D[4][15] + F[4][15]*D[3][15])*T + D[3][4];
|
||||
P[3][5] = P[5][3] = (F[5][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[5][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[5][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[5][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[5][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[5][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[5][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[5][3]*Q[3] + G[3][4]*G[5][4]*Q[4] + G[3][5]*G[5][5]*Q[5])*Tsq + (F[3][6]*D[5][6] + F[5][6]*D[3][6] + F[3][7]*D[5][7] + F[5][7]*D[3][7] + F[3][8]*D[5][8] + F[5][8]*D[3][8] + F[3][9]*D[5][9] + F[5][9]*D[3][9] + F[3][13]*D[5][13] + F[5][13]*D[3][13] + F[3][14]*D[5][14] + F[5][14]*D[3][14] + F[3][15]*D[5][15] + F[5][15]*D[3][15])*T + D[3][5];
|
||||
P[3][6] = P[6][3] = (F[6][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[6][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[6][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[6][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[6][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[6][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[6][7]*D[3][7] + F[3][8]*D[6][8] + F[6][8]*D[3][8] + F[3][9]*D[6][9] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15])*T + D[3][6];
|
||||
P[3][7] = P[7][3] = (F[7][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[7][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[7][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[7][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[7][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[7][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][7] + F[7][6]*D[3][6] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[7][8]*D[3][8] + F[3][9]*D[7][9] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15])*T + D[3][7];
|
||||
P[3][8] = P[8][3] = (F[8][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[8][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[8][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[8][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[8][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[8][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]))*Tsq + (F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15])*T + D[3][8];
|
||||
P[3][9] = P[9][3] = (F[9][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[9][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[9][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[9][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[9][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[9][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[3][9]*D[9][9] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9])*T + D[3][9];
|
||||
P[3][10] = P[10][3] = (F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10])*T + D[3][10];
|
||||
P[3][11] = P[11][3] = (F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11])*T + D[3][11];
|
||||
P[3][12] = P[12][3] = (F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12])*T + D[3][12];
|
||||
P[3][13] = P[13][3] = (F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13])*T + D[3][13];
|
||||
P[3][14] = P[14][3] = (F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14])*T + D[3][14];
|
||||
P[3][15] = P[15][3] = (F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15])*T + D[3][15];
|
||||
P[4][4] = (Q[3]*G[4][3]*G[4][3] + Q[4]*G[4][4]*G[4][4] + Q[5]*G[4][5]*G[4][5] + F[4][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[4][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[4][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[4][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[4][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[4][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[4][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (2*F[4][6]*D[4][6] + 2*F[4][7]*D[4][7] + 2*F[4][8]*D[4][8] + 2*F[4][9]*D[4][9] + 2*F[4][13]*D[4][13] + 2*F[4][14]*D[4][14] + 2*F[4][15]*D[4][15])*T + D[4][4];
|
||||
P[4][5] = P[5][4] = (F[5][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[5][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[5][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[5][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[5][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[5][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[5][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + G[4][3]*G[5][3]*Q[3] + G[4][4]*G[5][4]*Q[4] + G[4][5]*G[5][5]*Q[5])*Tsq + (F[4][6]*D[5][6] + F[5][6]*D[4][6] + F[4][7]*D[5][7] + F[5][7]*D[4][7] + F[4][8]*D[5][8] + F[5][8]*D[4][8] + F[4][9]*D[5][9] + F[5][9]*D[4][9] + F[4][13]*D[5][13] + F[5][13]*D[4][13] + F[4][14]*D[5][14] + F[5][14]*D[4][14] + F[4][15]*D[5][15] + F[5][15]*D[4][15])*T + D[4][5];
|
||||
P[4][6] = P[6][4] = (F[6][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[6][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[6][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[6][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[6][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[6][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[6][7]*D[4][7] + F[4][8]*D[6][8] + F[6][8]*D[4][8] + F[4][9]*D[6][9] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15])*T + D[4][6];
|
||||
P[4][7] = P[7][4] = (F[7][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[7][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[7][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[7][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[7][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[7][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][7] + F[7][6]*D[4][6] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[7][8]*D[4][8] + F[4][9]*D[7][9] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15])*T + D[4][7];
|
||||
P[4][8] = P[8][4] = (F[8][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[8][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[8][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[8][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[8][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[8][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]))*Tsq + (F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15])*T + D[4][8];
|
||||
P[4][9] = P[9][4] = (F[9][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[9][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[9][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[9][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[9][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[9][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[4][9]*D[9][9] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9])*T + D[4][9];
|
||||
P[4][10] = P[10][4] = (F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10])*T + D[4][10];
|
||||
P[4][11] = P[11][4] = (F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11])*T + D[4][11];
|
||||
P[4][12] = P[12][4] = (F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12])*T + D[4][12];
|
||||
P[4][13] = P[13][4] = (F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13])*T + D[4][13];
|
||||
P[4][14] = P[14][4] = (F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14])*T + D[4][14];
|
||||
P[4][15] = P[15][4] = (F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15])*T + D[4][15];
|
||||
P[5][5] = (Q[3]*G[5][3]*G[5][3] + Q[4]*G[5][4]*G[5][4] + Q[5]*G[5][5]*G[5][5] + F[5][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[5][13]*(F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13]) + F[5][14]*(F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14]) + F[5][15]*(F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15]) + F[5][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[5][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[5][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (2*F[5][6]*D[5][6] + 2*F[5][7]*D[5][7] + 2*F[5][8]*D[5][8] + 2*F[5][9]*D[5][9] + 2*F[5][13]*D[5][13] + 2*F[5][14]*D[5][14] + 2*F[5][15]*D[5][15])*T + D[5][5];
|
||||
P[5][6] = P[6][5] = (F[6][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[6][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[6][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[6][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[6][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[6][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[6][7]*D[5][7] + F[5][8]*D[6][8] + F[6][8]*D[5][8] + F[5][9]*D[6][9] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15])*T + D[5][6];
|
||||
P[5][7] = P[7][5] = (F[7][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[7][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[7][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[7][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[7][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[7][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][7] + F[7][6]*D[5][6] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[7][8]*D[5][8] + F[5][9]*D[7][9] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15])*T + D[5][7];
|
||||
P[5][8] = P[8][5] = (F[8][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[8][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[8][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[8][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[8][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[8][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]))*Tsq + (F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15])*T + D[5][8];
|
||||
P[5][9] = P[9][5] = (F[9][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[9][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[9][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[9][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[9][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[9][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[5][9]*D[9][9] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9])*T + D[5][9];
|
||||
P[5][10] = P[10][5] = (F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10])*T + D[5][10];
|
||||
P[5][11] = P[11][5] = (F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11])*T + D[5][11];
|
||||
P[5][12] = P[12][5] = (F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12])*T + D[5][12];
|
||||
P[5][13] = P[13][5] = (F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13])*T + D[5][13];
|
||||
P[5][14] = P[14][5] = (F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14])*T + D[5][14];
|
||||
P[5][15] = P[15][5] = (F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15])*T + D[5][15];
|
||||
P[6][6] = (Q[0]*G[6][0]*G[6][0] + Q[1]*G[6][1]*G[6][1] + Q[2]*G[6][2]*G[6][2] + F[6][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[6][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[6][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[6][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[6][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[6][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]))*Tsq + (2*F[6][7]*D[6][7] + 2*F[6][8]*D[6][8] + 2*F[6][9]*D[6][9] + 2*F[6][10]*D[6][10] + 2*F[6][11]*D[6][11] + 2*F[6][12]*D[6][12])*T + D[6][6];
|
||||
P[6][7] = P[7][6] = (F[7][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[7][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[7][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[7][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[7][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[7][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[6][0]*G[7][0]*Q[0] + G[6][1]*G[7][1]*Q[1] + G[6][2]*G[7][2]*Q[2])*Tsq + (F[7][6]*D[6][6] + F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[7][8]*D[6][8] + F[6][9]*D[7][9] + F[7][9]*D[6][9] + F[6][10]*D[7][10] + F[7][10]*D[6][10] + F[6][11]*D[7][11] + F[7][11]*D[6][11] + F[6][12]*D[7][12] + F[7][12]*D[6][12])*T + D[6][7];
|
||||
P[6][8] = P[8][6] = (F[8][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[8][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[8][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[8][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[8][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[8][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + G[6][0]*G[8][0]*Q[0] + G[6][1]*G[8][1]*Q[1] + G[6][2]*G[8][2]*Q[2])*Tsq + (F[6][7]*D[7][8] + F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[8][9]*D[6][9] + F[6][10]*D[8][10] + F[8][10]*D[6][10] + F[6][11]*D[8][11] + F[8][11]*D[6][11] + F[6][12]*D[8][12] + F[8][12]*D[6][12])*T + D[6][8];
|
||||
P[6][9] = P[9][6] = (F[9][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[9][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[9][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[9][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[9][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[9][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[9][0]*G[6][0]*Q[0] + G[9][1]*G[6][1]*Q[1] + G[9][2]*G[6][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][9]*D[9][9] + F[9][10]*D[6][10] + F[6][10]*D[9][10] + F[9][11]*D[6][11] + F[6][11]*D[9][11] + F[9][12]*D[6][12] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9])*T + D[6][9];
|
||||
P[6][10] = P[10][6] = (F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10])*T + D[6][10];
|
||||
P[6][11] = P[11][6] = (F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11])*T + D[6][11];
|
||||
P[6][12] = P[12][6] = (F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12])*T + D[6][12];
|
||||
P[6][13] = P[13][6] = (F[6][9]*D[9][13] + F[6][10]*D[10][13] + F[6][11]*D[11][13] + F[6][12]*D[12][13] + F[6][7]*D[7][13] + F[6][8]*D[8][13])*T + D[6][13];
|
||||
P[6][14] = P[14][6] = (F[6][9]*D[9][14] + F[6][10]*D[10][14] + F[6][11]*D[11][14] + F[6][12]*D[12][14] + F[6][7]*D[7][14] + F[6][8]*D[8][14])*T + D[6][14];
|
||||
P[6][15] = P[15][6] = (F[6][9]*D[9][15] + F[6][10]*D[10][15] + F[6][11]*D[11][15] + F[6][12]*D[12][15] + F[6][7]*D[7][15] + F[6][8]*D[8][15])*T + D[6][15];
|
||||
P[7][7] = (Q[0]*G[7][0]*G[7][0] + Q[1]*G[7][1]*G[7][1] + Q[2]*G[7][2]*G[7][2] + F[7][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[7][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[7][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[7][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[7][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[7][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]))*Tsq + (2*F[7][6]*D[6][7] + 2*F[7][8]*D[7][8] + 2*F[7][9]*D[7][9] + 2*F[7][10]*D[7][10] + 2*F[7][11]*D[7][11] + 2*F[7][12]*D[7][12])*T + D[7][7];
|
||||
P[7][8] = P[8][7] = (F[8][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[8][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[8][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[8][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[8][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[8][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + G[7][0]*G[8][0]*Q[0] + G[7][1]*G[8][1]*Q[1] + G[7][2]*G[8][2]*Q[2])*Tsq + (F[7][6]*D[6][8] + F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[8][9]*D[7][9] + F[7][10]*D[8][10] + F[8][10]*D[7][10] + F[7][11]*D[8][11] + F[8][11]*D[7][11] + F[7][12]*D[8][12] + F[8][12]*D[7][12])*T + D[7][8];
|
||||
P[7][9] = P[9][7] = (F[9][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[9][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[9][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[9][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[9][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[9][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + G[9][0]*G[7][0]*Q[0] + G[9][1]*G[7][1]*Q[1] + G[9][2]*G[7][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][9]*D[9][9] + F[9][10]*D[7][10] + F[7][10]*D[9][10] + F[9][11]*D[7][11] + F[7][11]*D[9][11] + F[9][12]*D[7][12] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9])*T + D[7][9];
|
||||
P[7][10] = P[10][7] = (F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10])*T + D[7][10];
|
||||
P[7][11] = P[11][7] = (F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11])*T + D[7][11];
|
||||
P[7][12] = P[12][7] = (F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12])*T + D[7][12];
|
||||
P[7][13] = P[13][7] = (F[7][9]*D[9][13] + F[7][10]*D[10][13] + F[7][11]*D[11][13] + F[7][12]*D[12][13] + F[7][6]*D[6][13] + F[7][8]*D[8][13])*T + D[7][13];
|
||||
P[7][14] = P[14][7] = (F[7][9]*D[9][14] + F[7][10]*D[10][14] + F[7][11]*D[11][14] + F[7][12]*D[12][14] + F[7][6]*D[6][14] + F[7][8]*D[8][14])*T + D[7][14];
|
||||
P[7][15] = P[15][7] = (F[7][9]*D[9][15] + F[7][10]*D[10][15] + F[7][11]*D[11][15] + F[7][12]*D[12][15] + F[7][6]*D[6][15] + F[7][8]*D[8][15])*T + D[7][15];
|
||||
P[8][8] = (Q[0]*G[8][0]*G[8][0] + Q[1]*G[8][1]*G[8][1] + Q[2]*G[8][2]*G[8][2] + F[8][9]*(F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[8][11]*D[9][11] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9]) + F[8][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[8][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[8][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[8][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[8][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]))*Tsq + (2*F[8][6]*D[6][8] + 2*F[8][7]*D[7][8] + 2*F[8][9]*D[8][9] + 2*F[8][10]*D[8][10] + 2*F[8][11]*D[8][11] + 2*F[8][12]*D[8][12])*T + D[8][8];
|
||||
P[8][9] = P[9][8] = (F[9][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[9][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[9][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[9][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[9][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[9][8]*(F[8][6]*D[6][8] + F[8][7]*D[7][8] + F[8][9]*D[8][9] + F[8][10]*D[8][10] + F[8][11]*D[8][11] + F[8][12]*D[8][12]) + G[9][0]*G[8][0]*Q[0] + G[9][1]*G[8][1]*Q[1] + G[9][2]*G[8][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][9]*D[9][9] + F[9][10]*D[8][10] + F[8][10]*D[9][10] + F[9][11]*D[8][11] + F[8][11]*D[9][11] + F[9][12]*D[8][12] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9])*T + D[8][9];
|
||||
P[8][10] = P[10][8] = (F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10])*T + D[8][10];
|
||||
P[8][11] = P[11][8] = (F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11])*T + D[8][11];
|
||||
P[8][12] = P[12][8] = (F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12])*T + D[8][12];
|
||||
P[8][13] = P[13][8] = (F[8][9]*D[9][13] + F[8][10]*D[10][13] + F[8][11]*D[11][13] + F[8][12]*D[12][13] + F[8][6]*D[6][13] + F[8][7]*D[7][13])*T + D[8][13];
|
||||
P[8][14] = P[14][8] = (F[8][9]*D[9][14] + F[8][10]*D[10][14] + F[8][11]*D[11][14] + F[8][12]*D[12][14] + F[8][6]*D[6][14] + F[8][7]*D[7][14])*T + D[8][14];
|
||||
P[8][15] = P[15][8] = (F[8][9]*D[9][15] + F[8][10]*D[10][15] + F[8][11]*D[11][15] + F[8][12]*D[12][15] + F[8][6]*D[6][15] + F[8][7]*D[7][15])*T + D[8][15];
|
||||
P[9][9] = (Q[0]*G[9][0]*G[9][0] + Q[1]*G[9][1]*G[9][1] + Q[2]*G[9][2]*G[9][2] + F[9][10]*(F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10]) + F[9][11]*(F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11]) + F[9][12]*(F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12]) + F[9][6]*(F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[9][10]*D[6][10] + F[9][11]*D[6][11] + F[9][12]*D[6][12]) + F[9][7]*(F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[9][10]*D[7][10] + F[9][11]*D[7][11] + F[9][12]*D[7][12]) + F[9][8]*(F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[9][10]*D[8][10] + F[9][11]*D[8][11] + F[9][12]*D[8][12]))*Tsq + (2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12] + 2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9])*T + D[9][9];
|
||||
P[9][10] = P[10][9] = (F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10])*T + D[9][10];
|
||||
P[9][11] = P[11][9] = (F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11])*T + D[9][11];
|
||||
P[9][12] = P[12][9] = (F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12])*T + D[9][12];
|
||||
P[9][13] = P[13][9] = (F[9][10]*D[10][13] + F[9][11]*D[11][13] + F[9][12]*D[12][13] + F[9][6]*D[6][13] + F[9][7]*D[7][13] + F[9][8]*D[8][13])*T + D[9][13];
|
||||
P[9][14] = P[14][9] = (F[9][10]*D[10][14] + F[9][11]*D[11][14] + F[9][12]*D[12][14] + F[9][6]*D[6][14] + F[9][7]*D[7][14] + F[9][8]*D[8][14])*T + D[9][14];
|
||||
P[9][15] = P[15][9] = (F[9][10]*D[10][15] + F[9][11]*D[11][15] + F[9][12]*D[12][15] + F[9][6]*D[6][15] + F[9][7]*D[7][15] + F[9][8]*D[8][15])*T + D[9][15];
|
||||
P[10][10] = Q[6]*Tsq + D[10][10];
|
||||
P[10][11] = P[11][10] = D[10][11];
|
||||
P[10][12] = P[12][10] = D[10][12];
|
||||
P[10][13] = P[13][10] = D[10][13];
|
||||
P[10][14] = P[14][10] = D[10][14];
|
||||
P[10][15] = P[15][10] = D[10][15];
|
||||
P[11][11] = Q[7]*Tsq + D[11][11];
|
||||
P[11][12] = P[12][11] = D[11][12];
|
||||
P[11][13] = P[13][11] = D[11][13];
|
||||
P[11][14] = P[14][11] = D[11][14];
|
||||
P[11][15] = P[15][11] = D[11][15];
|
||||
P[12][12] = Q[8]*Tsq + D[12][12];
|
||||
P[12][13] = P[13][12] = D[12][13];
|
||||
P[12][14] = P[14][12] = D[12][14];
|
||||
P[12][15] = P[15][12] = D[12][15];
|
||||
P[13][13] = Q[9]*Tsq + D[13][13];
|
||||
P[13][14] = P[14][13] = D[13][14];
|
||||
P[13][15] = P[15][13] = D[13][15];
|
||||
P[14][14] = Q[10]*Tsq + D[14][14];
|
||||
P[14][15] = P[15][14] = D[14][15];
|
||||
P[15][15] = Q[11]*Tsq + D[14][15];
|
||||
}
|
||||
#endif
|
||||
|
||||
// ************* SerialUpdate *******************
|
||||
// Does the update step of the Kalman filter for the covariance and estimate
|
||||
// Outputs are Xnew & Pnew, and are written over P and X
|
||||
// Z is actual measurement, Y is predicted measurement
|
||||
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
|
||||
// where K=P*H'*inv[H*P*H'+R]
|
||||
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
|
||||
// i.e. the measurment noises are uncorrelated.
|
||||
// It therefore uses a serial update that requires no matrix inversion by
|
||||
// processing the measurements one at a time.
|
||||
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
|
||||
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
|
||||
// The SensorsUsed variable is a bitwise mask indicating which sensors
|
||||
// should be used in the update.
|
||||
// ************************************************
|
||||
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed)
|
||||
{
|
||||
float HP[NUMX], HPHR, Error;
|
||||
uint8_t i, j, k, m;
|
||||
|
||||
for (m = 0; m < NUMV; m++) {
|
||||
|
||||
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
|
||||
|
||||
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
|
||||
HP[j] = 0;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
HP[j] += H[m][k] * P[k][j];
|
||||
}
|
||||
HPHR = R[m]; // Find HPHR = H*P*H' + R
|
||||
for (k = 0; k < NUMX; k++)
|
||||
HPHR += HP[k] * H[m][k];
|
||||
|
||||
for (k = 0; k < NUMX; k++)
|
||||
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
|
||||
|
||||
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
|
||||
for (j = i; j < NUMX; j++)
|
||||
P[i][j] = P[j][i] =
|
||||
P[i][j] - K[i][m] * HP[j];
|
||||
}
|
||||
|
||||
Error = Z[m] - Y[m];
|
||||
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
|
||||
X[i] = X[i] + K[i][m] * Error;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ************* RungeKutta **********************
|
||||
// Does a 4th order Runge Kutta numerical integration step
|
||||
// Output, Xnew, is written over X
|
||||
// NOTE the algorithm assumes time invariant state equations and
|
||||
// constant inputs over integration step
|
||||
// ************************************************
|
||||
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
{
|
||||
|
||||
float dT2 =
|
||||
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NUMX; i++)
|
||||
Xlast[i] = X[i]; // make a working copy
|
||||
|
||||
StateEq(X, U, K1); // k1 = f(x,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT2 * K1[i];
|
||||
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT2 * K2[i];
|
||||
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT * K3[i];
|
||||
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
|
||||
|
||||
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] =
|
||||
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
|
||||
K4[i]) / 6;
|
||||
}
|
||||
|
||||
// ************* Model Specific Stuff ***************************
|
||||
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
|
||||
//
|
||||
// State Variables = [Pos Vel Quaternion GyroBias AccelBias]
|
||||
// Deterministic Inputs = [AngularVel Accel]
|
||||
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise]
|
||||
//
|
||||
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
|
||||
// Inputs to Measurement = [EarthFrameMagField]
|
||||
//
|
||||
// Notes: Pos and Vel in earth frame
|
||||
// AngularVel and Accel in body frame
|
||||
// MagFields are unit vectors
|
||||
// Xdot is output of StateEq()
|
||||
// F and G are outputs of LinearizeFG(), all elements not set should be zero
|
||||
// y is output of OutputEq()
|
||||
// H is output of LinearizeH(), all elements not set should be zero
|
||||
// ************************************************
|
||||
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
|
||||
{
|
||||
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
|
||||
|
||||
ax=U[3]-X[13];
|
||||
ay=U[4]-X[14];
|
||||
az=U[5]-X[15]; // subtract the biases on accels
|
||||
wx = U[0] - X[10];
|
||||
wy = U[1] - X[11];
|
||||
wz = U[2] - X[12]; // subtract the biases on gyros
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// Pdot = V
|
||||
Xdot[0] = X[3];
|
||||
Xdot[1] = X[4];
|
||||
Xdot[2] = X[5];
|
||||
|
||||
// Vdot = Reb*a
|
||||
Xdot[3] =
|
||||
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
|
||||
q0 * q3) *
|
||||
ay + 2 * (q1 * q3 + q0 * q2) * az;
|
||||
Xdot[4] =
|
||||
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
|
||||
q3 * q3) * ay + 2 * (q2 * q3 -
|
||||
q0 * q1) *
|
||||
az;
|
||||
Xdot[5] =
|
||||
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
|
||||
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
|
||||
|
||||
// qdot = Q*w
|
||||
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
|
||||
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
|
||||
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
|
||||
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
|
||||
|
||||
// best guess is that bias stays constant
|
||||
Xdot[10] = Xdot[11] = Xdot[12] = 0;
|
||||
Xdot[13] = Xdot[14] = Xdot[15] = 0;
|
||||
}
|
||||
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW])
|
||||
{
|
||||
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
|
||||
|
||||
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
|
||||
ax = U[3];
|
||||
ay = U[4];
|
||||
az = U[5]; // NO BIAS STATES ON ACCELS
|
||||
wx = U[0] - X[10];
|
||||
wy = U[1] - X[11];
|
||||
wz = U[2] - X[12]; // subtract the biases on gyros
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// Pdot = V
|
||||
F[0][3] = F[1][4] = F[2][5] = 1;
|
||||
|
||||
// dVdot/dq
|
||||
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
|
||||
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
|
||||
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
|
||||
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
|
||||
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
|
||||
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
|
||||
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
|
||||
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
|
||||
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
|
||||
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
|
||||
// dVdot/dabias & dVdot/dna
|
||||
F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
|
||||
F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
|
||||
F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
|
||||
|
||||
// dqdot/dq
|
||||
F[6][6] = 0;
|
||||
F[6][7] = -wx / 2;
|
||||
F[6][8] = -wy / 2;
|
||||
F[6][9] = -wz / 2;
|
||||
F[7][6] = wx / 2;
|
||||
F[7][7] = 0;
|
||||
F[7][8] = wz / 2;
|
||||
F[7][9] = -wy / 2;
|
||||
F[8][6] = wy / 2;
|
||||
F[8][7] = -wz / 2;
|
||||
F[8][8] = 0;
|
||||
F[8][9] = wx / 2;
|
||||
F[9][6] = wz / 2;
|
||||
F[9][7] = wy / 2;
|
||||
F[9][8] = -wx / 2;
|
||||
F[9][9] = 0;
|
||||
|
||||
// dqdot/dwbias
|
||||
F[6][10] = q1 / 2;
|
||||
F[6][11] = q2 / 2;
|
||||
F[6][12] = q3 / 2;
|
||||
F[7][10] = -q0 / 2;
|
||||
F[7][11] = q3 / 2;
|
||||
F[7][12] = -q2 / 2;
|
||||
F[8][10] = -q3 / 2;
|
||||
F[8][11] = -q0 / 2;
|
||||
F[8][12] = q1 / 2;
|
||||
F[9][10] = q2 / 2;
|
||||
F[9][11] = -q1 / 2;
|
||||
F[9][12] = -q0 / 2;
|
||||
|
||||
// dVdot/dna - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
|
||||
//G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3); G[3][5]=-2*(q1*q3+q0*q2);
|
||||
//G[4][3]=-2*(q1*q2+q0*q3); G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; G[4][5]=2*(-q2*q3+q0*q1);
|
||||
//G[5][3]=2*(-q1*q3+q0*q2); G[5][4]=-2*(q2*q3+q0*q1); G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
|
||||
|
||||
// dqdot/dnw
|
||||
G[6][0] = q1 / 2;
|
||||
G[6][1] = q2 / 2;
|
||||
G[6][2] = q3 / 2;
|
||||
G[7][0] = -q0 / 2;
|
||||
G[7][1] = q3 / 2;
|
||||
G[7][2] = -q2 / 2;
|
||||
G[8][0] = -q3 / 2;
|
||||
G[8][1] = -q0 / 2;
|
||||
G[8][2] = q1 / 2;
|
||||
G[9][0] = q2 / 2;
|
||||
G[9][1] = -q1 / 2;
|
||||
G[9][2] = -q0 / 2;
|
||||
|
||||
// dwbias = random walk noise
|
||||
G[10][6] = G[11][7] = G[12][8] = 1;
|
||||
// dabias = random walk noise
|
||||
G[13][9] = G[14][10] = G[15][11] = 1;
|
||||
}
|
||||
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
|
||||
{
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// first six outputs are P and V
|
||||
Y[0] = X[0];
|
||||
Y[1] = X[1];
|
||||
Y[2] = X[2];
|
||||
Y[3] = X[3];
|
||||
Y[4] = X[4];
|
||||
Y[5] = X[5];
|
||||
|
||||
// Bb=Rbe*Be
|
||||
Y[6] =
|
||||
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
|
||||
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
|
||||
q0 * q2) * Be[2];
|
||||
Y[7] =
|
||||
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
|
||||
q2 * q2 - q3 * q3) * Be[1] +
|
||||
2 * (q2 * q3 + q0 * q1) * Be[2];
|
||||
Y[8] =
|
||||
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
|
||||
q0 * q1) * Be[1] +
|
||||
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
|
||||
|
||||
// Alt = -Pz
|
||||
Y[9] = -X[2];
|
||||
}
|
||||
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
|
||||
{
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// dP/dP=I;
|
||||
H[0][0] = H[1][1] = H[2][2] = 1;
|
||||
// dV/dV=I;
|
||||
H[3][3] = H[4][4] = H[5][5] = 1;
|
||||
|
||||
// dBb/dq
|
||||
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
|
||||
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
|
||||
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
|
||||
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
|
||||
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
|
||||
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
|
||||
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
|
||||
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
|
||||
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
|
||||
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
|
||||
// dAlt/dPz = -1
|
||||
H[9][2] = -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -1,473 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_board.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board specific static initializers for hardware for the AHRS board.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include <pios.h>
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
|
||||
#include <pios_spi_priv.h>
|
||||
|
||||
/* OP Interface
|
||||
*
|
||||
* NOTE: Leave this declared as const data so that it ends up in the
|
||||
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
|
||||
*/
|
||||
void PIOS_SPI_op_irq_handler(void);
|
||||
void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
|
||||
void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
|
||||
static const struct pios_spi_cfg pios_spi_op_cfg = {
|
||||
.regs = SPI2,
|
||||
.init = {
|
||||
.SPI_Mode = SPI_Mode_Slave,
|
||||
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
|
||||
.SPI_DataSize = SPI_DataSize_8b,
|
||||
.SPI_NSS = SPI_NSS_Hard,
|
||||
.SPI_FirstBit = SPI_FirstBit_MSB,
|
||||
.SPI_CRCPolynomial = 7,
|
||||
.SPI_CPOL = SPI_CPOL_High,
|
||||
.SPI_CPHA = SPI_CPHA_2Edge,
|
||||
},
|
||||
.use_crc = TRUE,
|
||||
.dma = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
|
||||
.irq = {
|
||||
.flags =
|
||||
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
|
||||
DMA1_FLAG_GL4),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA1_Channel4,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr =
|
||||
(uint32_t) & (SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc =
|
||||
DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize =
|
||||
DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize =
|
||||
DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA1_Channel5,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr =
|
||||
(uint32_t) & (SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralDST,
|
||||
.DMA_PeripheralInc =
|
||||
DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize =
|
||||
DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize =
|
||||
DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
},
|
||||
.ssel = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_12,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_14,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
uint32_t pios_spi_op_id;
|
||||
void PIOS_SPI_op_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_SPI_IRQ_Handler(pios_spi_op_id);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_SPI */
|
||||
|
||||
/*
|
||||
* ADC system
|
||||
*/
|
||||
#include "pios_adc_priv.h"
|
||||
extern void PIOS_ADC_handler(void);
|
||||
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
|
||||
// Remap the ADC DMA handler to this one
|
||||
static const struct pios_adc_cfg pios_adc_cfg = {
|
||||
.dma = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
.irq = {
|
||||
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.channel = DMA1_Channel1,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR,
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
|
||||
.DMA_Mode = DMA_Mode_Circular,
|
||||
.DMA_Priority = DMA_Priority_High,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
}
|
||||
},
|
||||
.half_flag = DMA1_IT_HT1,
|
||||
.full_flag = DMA1_IT_TC1,
|
||||
};
|
||||
|
||||
struct pios_adc_dev pios_adc_devs[] = {
|
||||
{
|
||||
.cfg = &pios_adc_cfg,
|
||||
.callback_function = NULL,
|
||||
},
|
||||
};
|
||||
|
||||
uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
|
||||
|
||||
void PIOS_ADC_handler() {
|
||||
PIOS_ADC_DMA_Handler();
|
||||
}
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_USART)
|
||||
#include <pios_usart_priv.h>
|
||||
|
||||
/*
|
||||
* AUX USART
|
||||
*/
|
||||
static const struct pios_usart_cfg pios_usart_aux_cfg = {
|
||||
.regs = USART3,
|
||||
.init = {
|
||||
.USART_BaudRate = 230400,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl =
|
||||
USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART3_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_INCLUDE_USART */
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
|
||||
#include <pios_com_priv.h>
|
||||
|
||||
#define PIOS_COM_AUX_TX_BUF_LEN 192
|
||||
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
|
||||
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
|
||||
#include <pios_i2c_priv.h>
|
||||
|
||||
/*
|
||||
* I2C Adapters
|
||||
*/
|
||||
|
||||
void PIOS_I2C_main_adapter_ev_irq_handler(void);
|
||||
void PIOS_I2C_main_adapter_er_irq_handler(void);
|
||||
void I2C1_EV_IRQHandler()
|
||||
__attribute__ ((alias("PIOS_I2C_main_adapter_ev_irq_handler")));
|
||||
void I2C1_ER_IRQHandler()
|
||||
__attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler")));
|
||||
|
||||
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
|
||||
.regs = I2C1,
|
||||
.init = {
|
||||
.I2C_Mode = I2C_Mode_I2C,
|
||||
.I2C_OwnAddress1 = 0,
|
||||
.I2C_Ack = I2C_Ack_Enable,
|
||||
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
|
||||
.I2C_DutyCycle = I2C_DutyCycle_2,
|
||||
.I2C_ClockSpeed = 200000, /* bits/s */
|
||||
},
|
||||
.transfer_timeout_ms = 50,
|
||||
.scl = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_OD,
|
||||
},
|
||||
},
|
||||
.sda = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_OD,
|
||||
},
|
||||
},
|
||||
.event = {
|
||||
.flags = 0, /* FIXME: check this */
|
||||
.init = {
|
||||
.NVIC_IRQChannel = I2C1_EV_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.error = {
|
||||
.flags = 0, /* FIXME: check this */
|
||||
.init = {
|
||||
.NVIC_IRQChannel = I2C1_ER_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
uint32_t pios_i2c_main_adapter_id;
|
||||
void PIOS_I2C_main_adapter_ev_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id);
|
||||
}
|
||||
|
||||
void PIOS_I2C_main_adapter_er_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
|
||||
#if defined(PIOS_ENABLE_DEBUG_PINS)
|
||||
|
||||
static const struct stm32_gpio pios_debug_pins[] = {
|
||||
{
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
{
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_ENABLE_DEBUG_PINS */
|
||||
|
||||
#include "pios_bmp085.h"
|
||||
static const struct pios_bmp085_cfg pios_bmp085_cfg = {
|
||||
.drdy = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_2,
|
||||
.GPIO_Speed = GPIO_Speed_100MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN,
|
||||
.GPIO_OType = GPIO_OType_OD,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.eoc_exti = {
|
||||
// .pin_source = GPIO_PinSource2,
|
||||
// .port_source = GPIO_PortSourceGPIOC,
|
||||
.init = {
|
||||
.EXTI_Line = EXTI_Line2, // matches above GPIO pin
|
||||
.EXTI_Mode = EXTI_Mode_Interrupt,
|
||||
.EXTI_Trigger = EXTI_Trigger_Rising,
|
||||
.EXTI_LineCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.eoc_irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = EXTI15_10_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.xclr = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_1,
|
||||
.GPIO_Speed = GPIO_Speed_100MHz,
|
||||
.GPIO_Mode = GPIO_Mode_OUT,
|
||||
.GPIO_OType = GPIO_OType_PP,
|
||||
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
||||
},
|
||||
},
|
||||
.oversampling = 3,
|
||||
};
|
||||
|
||||
extern const struct pios_com_driver pios_usart_com_driver;
|
||||
|
||||
uint32_t pios_com_aux_id;
|
||||
uint8_t adc_fifo_buf[sizeof(float) * 6 * 4] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
void PIOS_Board_Init(void) {
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
PIOS_LED_On(LED1);
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Communication system */
|
||||
#if !defined(PIOS_ENABLE_DEBUG_PINS)
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
{
|
||||
uint32_t pios_usart_aux_id;
|
||||
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
|
||||
NULL, 0,
|
||||
pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
#endif
|
||||
|
||||
/* IAP System Setup */
|
||||
PIOS_IAP_Init();
|
||||
|
||||
/* ADC system */
|
||||
PIOS_ADC_Init();
|
||||
extern uint8_t adc_oversampling;
|
||||
PIOS_ADC_Config(adc_oversampling);
|
||||
extern void adc_callback(float *);
|
||||
PIOS_ADC_SetCallback(adc_callback);
|
||||
|
||||
/* ADC buffer */
|
||||
extern t_fifo_buffer adc_fifo_buffer;
|
||||
fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf));
|
||||
|
||||
/* Setup the Accelerometer FS (Full-Scale) GPIO */
|
||||
PIOS_GPIO_Enable(0);
|
||||
SET_ACCEL_6G;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
/* Magnetic sensor system */
|
||||
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
PIOS_HMC5843_Init();
|
||||
#endif
|
||||
|
||||
PIOS_BMP085_Init(&pios_bmp085_cfg);
|
||||
|
||||
#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,590 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot project build PiOS and the AP.
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
TOP := $(realpath $(WHEREAMI)/../../)
|
||||
include $(TOP)/make/firmware-defs.mk
|
||||
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET := fw_$(BOARD_NAME)
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR := $(TOP)/build/$(TARGET)
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES to compile for debugging
|
||||
DEBUG ?= YES
|
||||
|
||||
# Include objects that are just nice information to show
|
||||
DIAGNOSTICS ?= YES
|
||||
|
||||
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
|
||||
ENABLE_DEBUG_PINS ?= NO
|
||||
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= YES
|
||||
|
||||
# Remove command is different for Code Sourcery on Windows
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
REMOVE_CMD = cs-rm
|
||||
else
|
||||
REMOVE_CMD = rm
|
||||
endif
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
|
||||
# List of modules to include
|
||||
|
||||
OPTMODULES = CameraStab GPS
|
||||
MODULES = Actuator ManualControl Altitude AHRSComms Stabilization Guidance FirmwareIAP
|
||||
PYMODULES = FlightPlan
|
||||
#MODULES += Osd/OsdEtStd
|
||||
#MODULES += Guidance
|
||||
MODULES += Telemetry
|
||||
|
||||
# Paths
|
||||
OPSYSTEM = ./System
|
||||
OPSYSTEMINC = $(OPSYSTEM)/inc
|
||||
OPUAVTALK = ../UAVTalk
|
||||
OPUAVTALKINC = $(OPUAVTALK)/inc
|
||||
OPUAVOBJ = ../UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPTESTS = ./Tests
|
||||
OPMODULEDIR = ../Modules
|
||||
FLIGHTLIB = ../Libraries
|
||||
FLIGHTLIBINC = $(FLIGHTLIB)/inc
|
||||
PIOS = ../PiOS
|
||||
PIOSINC = $(PIOS)/inc
|
||||
PIOSSTM32F10X = $(PIOS)/STM32F10x
|
||||
PIOSCOMMON = $(PIOS)/Common
|
||||
PIOSBOARDS = $(PIOS)/Boards
|
||||
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
|
||||
STMLIBDIR = $(APPLIBDIR)
|
||||
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
|
||||
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
|
||||
STMSPDSRCDIR = $(STMSPDDIR)/src
|
||||
STMSPDINCDIR = $(STMSPDDIR)/inc
|
||||
STMUSBSRCDIR = $(STMUSBDIR)/src
|
||||
STMUSBINCDIR = $(STMUSBDIR)/inc
|
||||
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
|
||||
DOSFSDIR = $(APPLIBDIR)/dosfs
|
||||
MSDDIR = $(APPLIBDIR)/msd
|
||||
RTOSDIR = $(APPLIBDIR)/FreeRTOS
|
||||
RTOSSRCDIR = $(RTOSDIR)/Source
|
||||
RTOSINCDIR = $(RTOSSRCDIR)/include
|
||||
DOXYGENDIR = ../Doc/Doxygen
|
||||
AHRSBOOTLOADER = ../Bootloaders/AHRS/
|
||||
AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
|
||||
PYMITE = $(FLIGHTLIB)/PyMite
|
||||
PYMITELIB = $(PYMITE)/lib
|
||||
PYMITEPLAT = $(PYMITE)/platform/openpilot
|
||||
PYMITETOOLS = $(PYMITE)/tools
|
||||
PYMITEVM = $(PYMITE)/vm
|
||||
PYMITEINC = $(PYMITEVM)
|
||||
PYMITEINC += $(PYMITEPLAT)
|
||||
PYMITEINC += $(OUTDIR)
|
||||
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
|
||||
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
|
||||
|
||||
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
ifndef TESTAPP
|
||||
|
||||
## PyMite files and modules
|
||||
SRC += $(OUTDIR)/pmlib_img.c
|
||||
SRC += $(OUTDIR)/pmlib_nat.c
|
||||
SRC += $(OUTDIR)/pmlibusr_img.c
|
||||
SRC += $(OUTDIR)/pmlibusr_nat.c
|
||||
PYSRC += $(wildcard ${PYMITEVM}/*.c)
|
||||
PYSRC += $(wildcard ${PYMITEPLAT}/*.c)
|
||||
PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += $(PYSRC)
|
||||
|
||||
## MODULES
|
||||
SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
## OPENPILOT CORE:
|
||||
SRC += ${OPMODULEDIR}/System/systemmod.c
|
||||
SRC += $(OPSYSTEM)/openpilot.c
|
||||
SRC += $(OPSYSTEM)/pios_board.c
|
||||
SRC += $(OPSYSTEM)/alarms.c
|
||||
SRC += $(OPSYSTEM)/taskmonitor.c
|
||||
SRC += $(OPUAVTALK)/uavtalk.c
|
||||
SRC += $(OPUAVOBJ)/uavobjectmanager.c
|
||||
SRC += $(OPUAVOBJ)/eventdispatcher.c
|
||||
else
|
||||
## TESTCODE
|
||||
SRC += $(OPTESTS)/test_common.c
|
||||
SRC += $(OPTESTS)/$(TESTAPP).c
|
||||
endif
|
||||
|
||||
|
||||
|
||||
## UAVOBJECTS
|
||||
ifndef TESTAPP
|
||||
#include $(UAVOBJSYNTHDIR)/Makefile.inc
|
||||
include ./UAVObjects.inc
|
||||
SRC += $(UAVOBJSRC)
|
||||
endif
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
SRC += $(PIOSSTM32F10X)/pios_sys.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_led.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_delay.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_adc.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_servo.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_i2c.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spi.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_ppm.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_pwm.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_dsm.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_sbus.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_tim.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_exti.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_rtc.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_wdg.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_iap.c
|
||||
|
||||
|
||||
# PIOS USB related files (seperated to make code maintenance more easy)
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_desc.c
|
||||
#SRC += $(PIOSSTM32F10X)/pios_usb_hid_endp.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
|
||||
#SRC += $(PIOSSTM32F10X)/pios_bmp085.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_crc.c
|
||||
SRC += $(PIOSCOMMON)/pios_sdcard.c
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_hcsr04.c
|
||||
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
|
||||
SRC += $(PIOSCOMMON)/pios_rcvr.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
|
||||
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
|
||||
## Libraries for flight calculations
|
||||
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
|
||||
## CMSIS for STM32
|
||||
SRC += $(CMSISDIR)/core_cm3.c
|
||||
SRC += $(CMSISDIR)/system_stm32f10x.c
|
||||
|
||||
## Used parts of the STM-Library
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_iwdg.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
|
||||
SRC += $(STMSPDSRCDIR)/misc.c
|
||||
|
||||
## STM32 USB Library
|
||||
SRC += $(STMUSBSRCDIR)/usb_core.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_init.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_int.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_mem.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_regs.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_sil.c
|
||||
|
||||
## RTOS
|
||||
SRC += $(RTOSSRCDIR)/list.c
|
||||
SRC += $(RTOSSRCDIR)/queue.c
|
||||
SRC += $(RTOSSRCDIR)/tasks.c
|
||||
|
||||
## RTOS Portable
|
||||
SRC += $(RTOSSRCDIR)/portable/GCC/ARM_CM3/port.c
|
||||
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_2.c
|
||||
|
||||
## Dosfs file system
|
||||
SRC += $(DOSFSDIR)/dosfs.c
|
||||
SRC += $(DOSFSDIR)/dfs_sdcard.c
|
||||
|
||||
## AHRS boot loader comms
|
||||
SRC += $(AHRSBOOTLOADER)/ahrs_spi_program_master.c
|
||||
SRC += $(AHRSBOOTLOADER)/ahrs_spi_program.c
|
||||
|
||||
## Mass Storage Device
|
||||
#SRC += $(MSDDIR)/msd.c
|
||||
#SRC += $(MSDDIR)/msd_bot.c
|
||||
#SRC += $(MSDDIR)/msd_desc.c
|
||||
#SRC += $(MSDDIR)/msd_memory.c
|
||||
#SRC += $(MSDDIR)/msd_scsi.c
|
||||
#SRC += $(MSDDIR)/msd_scsi_data.c
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
# will not be considered source files but generated files (assembler
|
||||
# output from the compiler), and will be deleted upon "make clean"!
|
||||
# Even though the DOS/Win* filesystem matches both .s and .S the same,
|
||||
# it will preserve the spelling of the filenames, and gcc itself does
|
||||
# care about how the name is spelled on its command-line.
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
|
||||
|
||||
# List Assembler source files here which must be assembled in ARM-Mode..
|
||||
ASRCARM =
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS = $(OPSYSTEM)
|
||||
EXTRAINCDIRS += $(OPSYSTEMINC)
|
||||
EXTRAINCDIRS += $(OPUAVTALK)
|
||||
EXTRAINCDIRS += $(OPUAVTALKINC)
|
||||
EXTRAINCDIRS += $(OPUAVOBJ)
|
||||
EXTRAINCDIRS += $(OPUAVOBJINC)
|
||||
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(FLIGHTLIBINC)
|
||||
EXTRAINCDIRS += $(PIOSSTM32F10X)
|
||||
EXTRAINCDIRS += $(PIOSCOMMON)
|
||||
EXTRAINCDIRS += $(PIOSBOARDS)
|
||||
EXTRAINCDIRS += $(STMSPDINCDIR)
|
||||
EXTRAINCDIRS += $(STMUSBINCDIR)
|
||||
EXTRAINCDIRS += $(CMSISDIR)
|
||||
EXTRAINCDIRS += $(DOSFSDIR)
|
||||
EXTRAINCDIRS += $(MSDDIR)
|
||||
EXTRAINCDIRS += $(RTOSINCDIR)
|
||||
EXTRAINCDIRS += $(APPLIBDIR)
|
||||
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
|
||||
EXTRAINCDIRS += $(AHRSBOOTLOADERINC)
|
||||
EXTRAINCDIRS += $(PYMITEINC)
|
||||
|
||||
EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES} ${PYMODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
|
||||
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
OPT = 0
|
||||
else
|
||||
OPT = s
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
CDEFS += -DUSE_$(BOARD)
|
||||
ifeq ($(ENABLE_DEBUG_PINS), YES)
|
||||
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
|
||||
endif
|
||||
ifeq ($(ENABLE_AUX_UART), YES)
|
||||
CDEFS += -DPIOS_ENABLE_AUX_UART
|
||||
endif
|
||||
|
||||
# Place project-specific -D and/or -U options for
|
||||
# Assembler with preprocessor here.
|
||||
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
|
||||
ADEFS = -D__ASSEMBLY__
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS = -DDEBUG
|
||||
endif
|
||||
|
||||
ifeq ($(DIAGNOSTICS),YES)
|
||||
CFLAGS = -DDIAGNOSTICS
|
||||
endif
|
||||
|
||||
CFLAGS += -g$(DEBUGF)
|
||||
CFLAGS += -O$(OPT)
|
||||
CFLAGS += -mcpu=$(MCU)
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mcpu=$(MCU) -mthumb -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
|
||||
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
|
||||
|
||||
# Define programs and commands.
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
PYTHON = python
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
# Default target.
|
||||
all: gccversion build
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# Generate intermediate code
|
||||
|
||||
# Generate code for PyMite
|
||||
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py)
|
||||
@echo $(MSG_PYMITEINIT) $(call toprel, $@)
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
|
||||
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
|
||||
|
||||
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
|
||||
|
||||
# Add jtag targets (program and wipe)
|
||||
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
|
||||
|
||||
.PHONY: elf lss sym hex bin bino opfw
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
bino: $(OUTDIR)/$(TARGET).bin.o
|
||||
opfw: $(OUTDIR)/$(TARGET).opfw
|
||||
|
||||
# Display sizes of sections.
|
||||
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Install: install binary file with prefix/suffix into install directory
|
||||
install: $(OUTDIR)/$(TARGET).opfw
|
||||
ifneq ($(INSTALL_DIR),)
|
||||
@echo $(MSG_INSTALLING) $(call toprel, $<)
|
||||
$(V1) mkdir -p $(INSTALL_DIR)
|
||||
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
|
||||
else
|
||||
$(error INSTALL_DIR must be specified for $@)
|
||||
endif
|
||||
|
||||
# Target: clean project.
|
||||
clean: clean_list
|
||||
|
||||
clean_list :
|
||||
@echo $(MSG_CLEANING)
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
|
||||
$(V1) $(REMOVE) $(wildcard $(OUTDIR)/*.c)
|
||||
$(V1) $(REMOVE) $(wildcard $(OUTDIR)/*.h)
|
||||
$(V1) $(REMOVE) $(ALLOBJ)
|
||||
$(V1) $(REMOVE) $(LSTFILES)
|
||||
$(V1) $(REMOVE) $(DEPFILES)
|
||||
$(V1) $(REMOVE) $(SRC:.c=.s)
|
||||
$(V1) $(REMOVE) $(SRCARM:.c=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
|
||||
else
|
||||
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir -p $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all build clean clean_list install
|
@ -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 += $(OPSYSTEM)/taskmonitor.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
|
||||
## 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,210 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
|
||||
* @brief OpenPilot System libraries are available to all OP modules.
|
||||
* @{
|
||||
* @file alarms.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Library for setting and clearing system alarms
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "alarms.h"
|
||||
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xSemaphoreHandle lock;
|
||||
|
||||
// Private functions
|
||||
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
|
||||
|
||||
/**
|
||||
* Initialize the alarms library
|
||||
*/
|
||||
int32_t AlarmsInitialize(void)
|
||||
{
|
||||
SystemAlarmsInitialize();
|
||||
lock = xSemaphoreCreateRecursiveMutex();
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set an alarm
|
||||
* @param alarm The system alarm to be modified
|
||||
* @param severity The alarm severity
|
||||
* @return 0 if success, -1 if an error
|
||||
*/
|
||||
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
|
||||
{
|
||||
SystemAlarmsData alarms;
|
||||
|
||||
// Check that this is a valid alarm
|
||||
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
|
||||
|
||||
// Read alarm and update its severity only if it was changed
|
||||
SystemAlarmsGet(&alarms);
|
||||
if ( alarms.Alarm[alarm] != severity )
|
||||
{
|
||||
alarms.Alarm[alarm] = severity;
|
||||
SystemAlarmsSet(&alarms);
|
||||
}
|
||||
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Get an alarm
|
||||
* @param alarm The system alarm to be read
|
||||
* @return Alarm severity
|
||||
*/
|
||||
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
|
||||
{
|
||||
SystemAlarmsData alarms;
|
||||
|
||||
// Check that this is a valid alarm
|
||||
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Read alarm
|
||||
SystemAlarmsGet(&alarms);
|
||||
return alarms.Alarm[alarm];
|
||||
}
|
||||
|
||||
/**
|
||||
* Set an alarm to it's default value
|
||||
* @param alarm The system alarm to be modified
|
||||
* @return 0 if success, -1 if an error
|
||||
*/
|
||||
int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm)
|
||||
{
|
||||
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT);
|
||||
}
|
||||
|
||||
/**
|
||||
* Default all alarms
|
||||
*/
|
||||
void AlarmsDefaultAll()
|
||||
{
|
||||
uint32_t n;
|
||||
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
|
||||
{
|
||||
AlarmsDefault(n);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear an alarm
|
||||
* @param alarm The system alarm to be modified
|
||||
* @return 0 if success, -1 if an error
|
||||
*/
|
||||
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm)
|
||||
{
|
||||
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear all alarms
|
||||
*/
|
||||
void AlarmsClearAll()
|
||||
{
|
||||
uint32_t n;
|
||||
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
|
||||
{
|
||||
AlarmsClear(n);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with the given or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
int32_t AlarmsHasWarnings()
|
||||
{
|
||||
return hasSeverity(SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with error or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
int32_t AlarmsHasErrors()
|
||||
{
|
||||
return hasSeverity(SYSTEMALARMS_ALARM_ERROR);
|
||||
};
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with critical or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
int32_t AlarmsHasCritical()
|
||||
{
|
||||
return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL);
|
||||
};
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with the given or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
|
||||
{
|
||||
SystemAlarmsData alarms;
|
||||
uint32_t n;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
|
||||
|
||||
// Read alarms
|
||||
SystemAlarmsGet(&alarms);
|
||||
|
||||
// Go through alarms and check if any are of the given severity or higher
|
||||
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
|
||||
{
|
||||
if ( alarms.Alarm[n] >= severity)
|
||||
{
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
// If this point is reached then no alarms found
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 0;
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
@ -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,50 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
|
||||
* @{
|
||||
* @file alarms.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the alarm library
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ALARMS_H
|
||||
#define ALARMS_H
|
||||
|
||||
#include "systemalarms.h"
|
||||
#define SYSTEMALARMS_ALARM_DEFAULT SYSTEMALARMS_ALARM_UNINITIALISED
|
||||
|
||||
int32_t AlarmsInitialize(void);
|
||||
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity);
|
||||
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm);
|
||||
int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm);
|
||||
void AlarmsDefaultAll();
|
||||
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm);
|
||||
void AlarmsClearAll();
|
||||
int32_t AlarmsHasWarnings();
|
||||
int32_t AlarmsHasErrors();
|
||||
int32_t AlarmsHasCritical();
|
||||
|
||||
#endif // ALARMS_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -1,39 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
*
|
||||
* @file op_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief OpenPilot configuration header.
|
||||
* Compile time config for OpenPilot Application
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef OP_CONFIG_H
|
||||
#define OP_CONFIG_H
|
||||
|
||||
#endif /* OP_CONFIG_H */
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -1,53 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file openpilot.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Main OpenPilot header.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef OPENPILOT_H
|
||||
#define OPENPILOT_H
|
||||
|
||||
|
||||
/* PIOS Includes */
|
||||
#include <pios.h>
|
||||
|
||||
/* OpenPilot Libraries */
|
||||
#include "op_config.h"
|
||||
#include "utlist.h"
|
||||
#include "uavobjectmanager.h"
|
||||
#include "eventdispatcher.h"
|
||||
#include "alarms.h"
|
||||
#include "taskmonitor.h"
|
||||
#include "uavtalk.h"
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
#endif /* OPENPILOT_H */
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
*
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief PiOS configuration header.
|
||||
* Central compile time config for the project.
|
||||
* In particular, pios_config.h is where you define which PiOS libraries
|
||||
* and features are included in the firmware.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PIOS_CONFIG_H
|
||||
#define PIOS_CONFIG_H
|
||||
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_LED
|
||||
|
||||
#define PIOS_INCLUDE_RCVR
|
||||
|
||||
#define PIOS_INCLUDE_DSM
|
||||
//#define PIOS_INCLUDE_SBUS
|
||||
#define PIOS_INCLUDE_PWM
|
||||
#define PIOS_INCLUDE_PPM
|
||||
|
||||
#define PIOS_INCLUDE_TELEMETRY_RF
|
||||
|
||||
#define PIOS_INCLUDE_SERVO
|
||||
#define PIOS_INCLUDE_SPI
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_USART
|
||||
##define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_BMP085
|
||||
//#define PIOS_INCLUDE_HCSR04
|
||||
#define PIOS_INCLUDE_OPAHRS
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_GPS
|
||||
#define PIOS_INCLUDE_SDCARD
|
||||
#define PIOS_INCLUDE_SETTINGS
|
||||
#define PIOS_INCLUDE_FREERTOS
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_EXTI
|
||||
#define PIOS_INCLUDE_RTC
|
||||
#define PIOS_INCLUDE_WDG
|
||||
#define PIOS_INCLUDE_I2C_ESC
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
|
||||
/* Defaults for Logging */
|
||||
#define LOG_FILENAME "PIOS.LOG"
|
||||
#define STARTUP_LOG_ENABLED 1
|
||||
|
||||
/* Enable a priority queue in telemetry */
|
||||
#define PIOS_TELEM_PRIORITY_QUEUE
|
||||
|
||||
/* Alarm Thresholds */
|
||||
#define HEAP_LIMIT_WARNING 4000
|
||||
#define HEAP_LIMIT_CRITICAL 1000
|
||||
#define IRQSTACK_LIMIT_WARNING 150
|
||||
#define IRQSTACK_LIMIT_CRITICAL 80
|
||||
#define CPULOAD_LIMIT_WARNING 80
|
||||
#define CPULOAD_LIMIT_CRITICAL 95
|
||||
|
||||
/* Stabilization options */
|
||||
#define PIOS_QUATERNION_STABILIZATION
|
||||
|
||||
/* GPS options */
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
/* PIOS Initcall infrastructure */
|
||||
#define PIOS_INCLUDE_INITCALL
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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,43 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
|
||||
* @{
|
||||
* @file taskmonitor.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the task monitoring library
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef TASKMONITOR_H
|
||||
#define TASKMONITOR_H
|
||||
|
||||
#include "taskinfo.h"
|
||||
|
||||
int32_t TaskMonitorInitialize(void);
|
||||
int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle);
|
||||
int32_t TaskMonitorRemove(TaskInfoRunningElem task);
|
||||
void TaskMonitorUpdateAll(void);
|
||||
|
||||
#endif // TASKMONITOR_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -1,339 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @brief These files are the core system files of OpenPilot.
|
||||
* They are the ground layer just above PiOS. In practice, OpenPilot actually starts
|
||||
* in the main() function of openpilot.c
|
||||
* @{
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @brief This is where the OP firmware starts. Those files also define the compile-time
|
||||
* options of the firmware.
|
||||
* @{
|
||||
* @file openpilot.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up and runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
#include "uavobjectsinit.h"
|
||||
#include "systemmod.h"
|
||||
|
||||
/* Task Priorities */
|
||||
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
#define INCLUDE_TEST_TASKS 0
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static uint8_t sdcard_available;
|
||||
#endif
|
||||
FILEINFO File;
|
||||
char Buffer[1024];
|
||||
uint32_t Cache;
|
||||
|
||||
/* Function Prototypes */
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskTick(void *pvParameters);
|
||||
static void TaskTesting(void *pvParameters);
|
||||
static void TaskHIDTest(void *pvParameters);
|
||||
static void TaskServos(void *pvParameters);
|
||||
static void TaskSDCard(void *pvParameters);
|
||||
#endif
|
||||
int32_t CONSOLE_Parse(uint8_t port, char c);
|
||||
void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
|
||||
|
||||
/* Prototype of PIOS_Board_Init() function */
|
||||
extern void PIOS_Board_Init(void);
|
||||
extern void Stack_Change(void);
|
||||
static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change")));
|
||||
|
||||
/**
|
||||
* OpenPilot Main function:
|
||||
*
|
||||
* Initialize PiOS<BR>
|
||||
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
|
||||
* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
|
||||
* If something goes wrong, blink LED1 and LED2 every 100ms
|
||||
*
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
/* NOTE: Do NOT modify the following start-up sequence */
|
||||
/* Any new initialization functions should be added in OpenPilotInit() */
|
||||
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
/* Architecture dependant Hardware and
|
||||
* core subsystem initialisation
|
||||
* (see pios_board.c for your arch)
|
||||
* */
|
||||
PIOS_Board_Init();
|
||||
|
||||
/* Initialize modules */
|
||||
MODULE_INITIALISE_ALL
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
/* Create test tasks */
|
||||
xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
|
||||
xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
|
||||
xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
|
||||
xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
|
||||
#endif
|
||||
|
||||
/* swap the stack to use the IRQ stack (does nothing in sim mode) */
|
||||
Stack_Change_Weak();
|
||||
|
||||
/* Start the FreeRTOS scheduler which should never returns.*/
|
||||
vTaskStartScheduler();
|
||||
|
||||
/* If all is well we will never reach here as the scheduler will now be running. */
|
||||
|
||||
/* Do some indication to user that something bad just happened */
|
||||
PIOS_LED_Off(LED1); \
|
||||
for(;;) { \
|
||||
PIOS_LED_Toggle(LED1); \
|
||||
PIOS_DELAY_WaitmS(100); \
|
||||
};
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#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,1306 +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
|
||||
*/
|
||||
|
||||
#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
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
|
||||
#include <pios_spi_priv.h>
|
||||
|
||||
/* MicroSD Interface
|
||||
*
|
||||
* NOTE: Leave this declared as const data so that it ends up in the
|
||||
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
|
||||
*/
|
||||
void PIOS_SPI_sdcard_irq_handler(void);
|
||||
void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
|
||||
void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
|
||||
static const struct pios_spi_cfg pios_spi_sdcard_cfg = {
|
||||
.regs = SPI1,
|
||||
.init = {
|
||||
.SPI_Mode = SPI_Mode_Master,
|
||||
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
|
||||
.SPI_DataSize = SPI_DataSize_8b,
|
||||
.SPI_NSS = SPI_NSS_Soft,
|
||||
.SPI_FirstBit = SPI_FirstBit_MSB,
|
||||
.SPI_CRCPolynomial = 7,
|
||||
.SPI_CPOL = SPI_CPOL_High,
|
||||
.SPI_CPHA = SPI_CPHA_2Edge,
|
||||
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, /* Maximum divider (ie. slowest clock rate) */
|
||||
},
|
||||
.dma = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
|
||||
.irq = {
|
||||
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA1_Channel2,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA1_Channel3,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralDST,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
},
|
||||
.ssel = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_4,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_5,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
/* AHRS Interface
|
||||
*
|
||||
* NOTE: Leave this declared as const data so that it ends up in the
|
||||
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
|
||||
*/
|
||||
void PIOS_SPI_ahrs_irq_handler(void);
|
||||
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
|
||||
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
|
||||
static const struct pios_spi_cfg pios_spi_ahrs_cfg = {
|
||||
.regs = SPI2,
|
||||
.init = {
|
||||
.SPI_Mode = SPI_Mode_Master,
|
||||
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
|
||||
.SPI_DataSize = SPI_DataSize_8b,
|
||||
.SPI_NSS = SPI_NSS_Soft,
|
||||
.SPI_FirstBit = SPI_FirstBit_MSB,
|
||||
.SPI_CRCPolynomial = 7,
|
||||
.SPI_CPOL = SPI_CPOL_High,
|
||||
.SPI_CPHA = SPI_CPHA_2Edge,
|
||||
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16,
|
||||
},
|
||||
.use_crc = TRUE,
|
||||
.dma = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
|
||||
.irq = {
|
||||
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA1_Channel4,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_High,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA1_Channel5,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralDST,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_High,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
},
|
||||
.ssel = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_12,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_14,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static uint32_t pios_spi_sdcard_id;
|
||||
void PIOS_SPI_sdcard_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id);
|
||||
}
|
||||
|
||||
uint32_t pios_spi_ahrs_id;
|
||||
void PIOS_SPI_ahrs_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_SPI_IRQ_Handler(pios_spi_ahrs_id);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_SPI */
|
||||
|
||||
/*
|
||||
* ADC system
|
||||
*/
|
||||
#include "pios_adc_priv.h"
|
||||
extern void PIOS_ADC_handler(void);
|
||||
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
|
||||
// Remap the ADC DMA handler to this one
|
||||
static const struct pios_adc_cfg pios_adc_cfg = {
|
||||
.dma = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
.irq = {
|
||||
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.channel = DMA1_Channel1,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR,
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
|
||||
.DMA_Mode = DMA_Mode_Circular,
|
||||
.DMA_Priority = DMA_Priority_Low,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
}
|
||||
},
|
||||
.half_flag = DMA1_IT_HT1,
|
||||
.full_flag = DMA1_IT_TC1,
|
||||
};
|
||||
|
||||
struct pios_adc_dev pios_adc_devs[] = {
|
||||
{
|
||||
.cfg = &pios_adc_cfg,
|
||||
.callback_function = NULL,
|
||||
},
|
||||
};
|
||||
|
||||
uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
|
||||
|
||||
void PIOS_ADC_handler() {
|
||||
PIOS_ADC_DMA_Handler();
|
||||
}
|
||||
|
||||
#include "pios_tim_priv.h"
|
||||
|
||||
static const TIM_TimeBaseInitTypeDef tim_4_8_time_base = {
|
||||
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
|
||||
.TIM_ClockDivision = TIM_CKD_DIV1,
|
||||
.TIM_CounterMode = TIM_CounterMode_Up,
|
||||
.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
|
||||
.TIM_RepetitionCounter = 0x0000,
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_4_cfg = {
|
||||
.timer = TIM4,
|
||||
.time_base_init = &tim_4_8_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_8_cfg = {
|
||||
.timer = TIM8,
|
||||
.time_base_init = &tim_4_8_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM8_CC_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const TIM_TimeBaseInitTypeDef tim_1_3_5_time_base = {
|
||||
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
|
||||
.TIM_ClockDivision = TIM_CKD_DIV1,
|
||||
.TIM_CounterMode = TIM_CounterMode_Up,
|
||||
.TIM_Period = 0xFFFF,
|
||||
.TIM_RepetitionCounter = 0x0000,
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_1_cfg = {
|
||||
.timer = TIM1,
|
||||
.time_base_init = &tim_1_3_5_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM1_CC_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_3_cfg = {
|
||||
.timer = TIM3,
|
||||
.time_base_init = &tim_1_3_5_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM3_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_tim_clock_cfg tim_5_cfg = {
|
||||
.timer = TIM5,
|
||||
.time_base_init = &tim_1_3_5_time_base,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = TIM5_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#if defined(PIOS_INCLUDE_USART)
|
||||
|
||||
#include "pios_usart_priv.h"
|
||||
|
||||
/*
|
||||
* Telemetry USART
|
||||
*/
|
||||
static const struct pios_usart_cfg pios_usart_telem_cfg = {
|
||||
.regs = USART2,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART2_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_3,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_2,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
/*
|
||||
* GPS USART
|
||||
*/
|
||||
static const struct pios_usart_cfg pios_usart_gps_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_PartialRemap_USART3,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART3_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#ifdef PIOS_COM_AUX
|
||||
/*
|
||||
* AUX USART
|
||||
*/
|
||||
static const struct pios_usart_cfg pios_usart_aux_cfg = {
|
||||
.regs = USART1,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.remap = GPIO_Remap_USART1,
|
||||
.rx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_RTC)
|
||||
/*
|
||||
* Realtime Clock (RTC)
|
||||
*/
|
||||
#include <pios_rtc_priv.h>
|
||||
|
||||
void PIOS_RTC_IRQ_Handler (void);
|
||||
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
|
||||
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
|
||||
.clksrc = RCC_RTCCLKSource_HSE_Div128,
|
||||
.prescaler = 100,
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = RTC_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
void PIOS_RTC_IRQ_Handler (void)
|
||||
{
|
||||
PIOS_RTC_irq_handler ();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_DSM)
|
||||
/*
|
||||
* Spektrum/JR DSM USART
|
||||
*/
|
||||
#include <pios_dsm_priv.h>
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_dsm_cfg = {
|
||||
.regs = USART1,
|
||||
.init = {
|
||||
.USART_BaudRate = 115200,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx,
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_dsm_cfg pios_dsm_cfg = {
|
||||
.bind = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_COM_DSM */
|
||||
|
||||
#if defined(PIOS_INCLUDE_SBUS)
|
||||
#error PIOS_INCLUDE_SBUS not implemented
|
||||
#endif /* PIOS_INCLUDE_SBUS */
|
||||
|
||||
#endif /* PIOS_INCLUDE_USART */
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
|
||||
#include "pios_com_priv.h"
|
||||
|
||||
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
|
||||
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
|
||||
|
||||
#define PIOS_COM_GPS_RX_BUF_LEN 96
|
||||
|
||||
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
|
||||
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
|
||||
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
|
||||
/**
|
||||
* Pios servo configuration structures
|
||||
*/
|
||||
#include <pios_servo_priv.h>
|
||||
static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
|
||||
{
|
||||
.timer = TIM4,
|
||||
.timer_chan = TIM_Channel_1,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM4,
|
||||
.timer_chan = TIM_Channel_2,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM4,
|
||||
.timer_chan = TIM_Channel_3,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_8,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM4,
|
||||
.timer_chan = TIM_Channel_4,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM8,
|
||||
.timer_chan = TIM_Channel_1,
|
||||
.pin = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM8,
|
||||
.timer_chan = TIM_Channel_2,
|
||||
.pin = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM8,
|
||||
.timer_chan = TIM_Channel_3,
|
||||
.pin = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_8,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM8,
|
||||
.timer_chan = TIM_Channel_4,
|
||||
.pin = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
const struct pios_servo_cfg pios_servo_cfg = {
|
||||
.tim_oc_init = {
|
||||
.TIM_OCMode = TIM_OCMode_PWM1,
|
||||
.TIM_OutputState = TIM_OutputState_Enable,
|
||||
.TIM_OutputNState = TIM_OutputNState_Disable,
|
||||
.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
|
||||
.TIM_OCPolarity = TIM_OCPolarity_High,
|
||||
.TIM_OCNPolarity = TIM_OCPolarity_High,
|
||||
.TIM_OCIdleState = TIM_OCIdleState_Reset,
|
||||
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
|
||||
},
|
||||
.channels = pios_tim_servoport_all_pins,
|
||||
.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
* PWM Inputs
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
|
||||
#include <pios_pwm_priv.h>
|
||||
static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
|
||||
{
|
||||
.timer = TIM1,
|
||||
.timer_chan = TIM_Channel_2,
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM1,
|
||||
.timer_chan = TIM_Channel_3,
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM5,
|
||||
.timer_chan = TIM_Channel_1,
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_0,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM1,
|
||||
.timer_chan = TIM_Channel_1,
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_8,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM3,
|
||||
.timer_chan = TIM_Channel_4,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_1,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM3,
|
||||
.timer_chan = TIM_Channel_3,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_0,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.timer = TIM3,
|
||||
.timer_chan = TIM_Channel_1,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_4,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
.remap = GPIO_PartialRemap_TIM3,
|
||||
},
|
||||
{
|
||||
.timer = TIM3,
|
||||
.timer_chan = TIM_Channel_2,
|
||||
.pin = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_5,
|
||||
.GPIO_Mode = GPIO_Mode_IPD,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
},
|
||||
},
|
||||
.remap = GPIO_PartialRemap_TIM3,
|
||||
},
|
||||
};
|
||||
|
||||
const struct pios_pwm_cfg pios_pwm_cfg = {
|
||||
.tim_ic_init = {
|
||||
.TIM_ICPolarity = TIM_ICPolarity_Rising,
|
||||
.TIM_ICSelection = TIM_ICSelection_DirectTI,
|
||||
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
|
||||
.TIM_ICFilter = 0x0,
|
||||
},
|
||||
.channels = pios_tim_rcvrport_all_channels,
|
||||
.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
|
||||
};
|
||||
#endif
|
||||
|
||||
/*
|
||||
* PPM Input
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
#include <pios_ppm_priv.h>
|
||||
static const struct pios_ppm_cfg pios_ppm_cfg = {
|
||||
.tim_ic_init = {
|
||||
.TIM_ICPolarity = TIM_ICPolarity_Rising,
|
||||
.TIM_ICSelection = TIM_ICSelection_DirectTI,
|
||||
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
|
||||
.TIM_ICFilter = 0x0,
|
||||
.TIM_Channel = TIM_Channel_2,
|
||||
},
|
||||
/* Use only the first channel for ppm */
|
||||
.channels = &pios_tim_rcvrport_all_channels[0],
|
||||
.num_channels = 1,
|
||||
};
|
||||
|
||||
#endif //PPM
|
||||
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
|
||||
#include <pios_i2c_priv.h>
|
||||
|
||||
/*
|
||||
* I2C Adapters
|
||||
*/
|
||||
|
||||
void PIOS_I2C_main_adapter_ev_irq_handler(void);
|
||||
void PIOS_I2C_main_adapter_er_irq_handler(void);
|
||||
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
|
||||
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
|
||||
|
||||
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
|
||||
.regs = I2C2,
|
||||
.init = {
|
||||
.I2C_Mode = I2C_Mode_I2C,
|
||||
.I2C_OwnAddress1 = 0,
|
||||
.I2C_Ack = I2C_Ack_Enable,
|
||||
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
|
||||
.I2C_DutyCycle = I2C_DutyCycle_2,
|
||||
.I2C_ClockSpeed = 400000, /* bits/s */
|
||||
},
|
||||
.transfer_timeout_ms = 50,
|
||||
.scl = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_OD,
|
||||
},
|
||||
},
|
||||
.sda = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_OD,
|
||||
},
|
||||
},
|
||||
.event = {
|
||||
.flags = 0, /* FIXME: check this */
|
||||
.init = {
|
||||
.NVIC_IRQChannel = I2C2_EV_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.error = {
|
||||
.flags = 0, /* FIXME: check this */
|
||||
.init = {
|
||||
.NVIC_IRQChannel = I2C2_ER_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
uint32_t pios_i2c_main_adapter_id;
|
||||
void PIOS_I2C_main_adapter_ev_irq_handler(void)
|
||||
{
|
||||
#ifdef I2C_DEBUG_PIN
|
||||
PIOS_DEBUG_PinHigh(I2C_DEBUG_PIN);
|
||||
#endif
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id);
|
||||
#ifdef I2C_DEBUG_PIN
|
||||
PIOS_DEBUG_PinLow(I2C_DEBUG_PIN);
|
||||
#endif
|
||||
}
|
||||
|
||||
void PIOS_I2C_main_adapter_er_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
|
||||
#if defined(PIOS_ENABLE_DEBUG_PINS)
|
||||
|
||||
static const struct stm32_gpio pios_debug_pins[] = {
|
||||
#define PIOS_DEBUG_PIN_SERVO_1 0
|
||||
{
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
#define PIOS_DEBUG_PIN_SERVO_2 1
|
||||
{
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
#define PIOS_DEBUG_PIN_SERVO_3 2
|
||||
{
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_8,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
#define PIOS_DEBUG_PIN_SERVO_4 3
|
||||
{
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
#define PIOS_DEBUG_PIN_SERVO_5 4
|
||||
{
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
#define PIOS_DEBUG_PIN_SERVO_6 5
|
||||
{
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
#define PIOS_DEBUG_PIN_SERVO_7 6
|
||||
{
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_8,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
#define PIOS_DEBUG_PIN_SERVO_8 7
|
||||
{
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_9,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* PIOS_ENABLE_DEBUG_PINS */
|
||||
|
||||
#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 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_HID)
|
||||
#include "pios_usb_hid_priv.h"
|
||||
|
||||
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_USB_HID */
|
||||
|
||||
extern const struct pios_com_driver pios_usb_com_driver;
|
||||
|
||||
uint32_t pios_com_telem_rf_id;
|
||||
uint32_t pios_com_telem_usb_id;
|
||||
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();
|
||||
|
||||
HwSettingsInitialize();
|
||||
|
||||
#if defined(PIOS_INCLUDE_RTC)
|
||||
/* Initialize the real-time clock and its associated tick */
|
||||
PIOS_RTC_Init(&pios_rtc_main_cfg);
|
||||
#endif
|
||||
|
||||
/* Initialize the alarms library */
|
||||
AlarmsInitialize();
|
||||
|
||||
/* 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);
|
||||
|
||||
/* 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_USB_HID)
|
||||
uint32_t pios_usb_hid_id;
|
||||
PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
|
||||
#if defined(PIOS_INCLUDE_COM)
|
||||
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
|
||||
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
|
||||
PIOS_Assert(rx_buffer);
|
||||
PIOS_Assert(tx_buffer);
|
||||
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
|
||||
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
|
||||
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
#endif /* PIOS_INCLUDE_USB_HID */
|
||||
|
||||
#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 */
|
||||
PIOS_IAP_Init();
|
||||
PIOS_WDG_Init();
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
@ -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,150 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
|
||||
* @{
|
||||
* @file taskmonitor.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Task monitoring library
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#include "openpilot.h"
|
||||
//#include "taskmonitor.h"
|
||||
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xSemaphoreHandle lock;
|
||||
static xTaskHandle handles[TASKINFO_RUNNING_NUMELEM];
|
||||
static uint32_t lastMonitorTime;
|
||||
|
||||
// Private functions
|
||||
|
||||
/**
|
||||
* Initialize library
|
||||
*/
|
||||
int32_t TaskMonitorInitialize(void)
|
||||
{
|
||||
lock = xSemaphoreCreateRecursiveMutex();
|
||||
memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
|
||||
lastMonitorTime = 0;
|
||||
#if defined(DIAGNOSTICS)
|
||||
lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Register a task handle with the library
|
||||
*/
|
||||
int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
|
||||
{
|
||||
if (task < TASKINFO_RUNNING_NUMELEM)
|
||||
{
|
||||
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
|
||||
handles[task] = handle;
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Remove a task handle from the library
|
||||
*/
|
||||
int32_t TaskMonitorRemove(TaskInfoRunningElem task)
|
||||
{
|
||||
if (task < TASKINFO_RUNNING_NUMELEM)
|
||||
{
|
||||
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
|
||||
handles[task] = 0;
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the status of all tasks
|
||||
*/
|
||||
void TaskMonitorUpdateAll(void)
|
||||
{
|
||||
#if defined(DIAGNOSTICS)
|
||||
TaskInfoData data;
|
||||
int n;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
|
||||
|
||||
#if ( configGENERATE_RUN_TIME_STATS == 1 )
|
||||
uint32_t currentTime;
|
||||
uint32_t deltaTime;
|
||||
|
||||
/*
|
||||
* Calculate the amount of elapsed run time between the last time we
|
||||
* measured and now. Scale so that we can convert task run times
|
||||
* directly to percentages.
|
||||
*/
|
||||
currentTime = portGET_RUN_TIME_COUNTER_VALUE();
|
||||
deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */
|
||||
lastMonitorTime = currentTime;
|
||||
#endif
|
||||
|
||||
// Update all task information
|
||||
for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)
|
||||
{
|
||||
if (handles[n] != 0)
|
||||
{
|
||||
data.Running[n] = TASKINFO_RUNNING_TRUE;
|
||||
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
|
||||
data.StackRemaining[n] = 10000;
|
||||
#else
|
||||
data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;
|
||||
#if ( configGENERATE_RUN_TIME_STATS == 1 )
|
||||
/* Generate run time stats */
|
||||
data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
data.Running[n] = TASKINFO_RUNNING_FALSE;
|
||||
data.StackRemaining[n] = 0;
|
||||
data.RunningTime[n] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Update object
|
||||
TaskInfoSet(&data);
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
#endif
|
||||
}
|
@ -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,37 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file test_common.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up ans runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
|
||||
/**
|
||||
* Called by the RTOS when a stack overflow is detected.
|
||||
*/
|
||||
void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed portCHAR *pcTaskName )
|
||||
{
|
||||
PIOS_DEBUG_Panic("STACK OVERFLOW");
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
@ -1,282 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file openpilot.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up ans runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* I2C Test: communicate with external PCF8570 ram chip
|
||||
* For this test to function, PCF8570 chips need to be attached to the I2C port
|
||||
*
|
||||
* Blinking Blue LED: No errors detected, test continues
|
||||
* Blinking Red LED: Error detected, test stopped
|
||||
*
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
|
||||
//#define USE_DEBUG_PINS
|
||||
|
||||
#define DEVICE_1_ADDRESS 0x50
|
||||
#define DEVICE_2_ADDRESS 0x51
|
||||
|
||||
#ifdef USE_DEBUG_PINS
|
||||
#define DEBUG_PIN_TASK1_WAIT 0
|
||||
#define DEBUG_PIN_TASK1_LOCKED 1
|
||||
#define DEBUG_PIN_TASK2_WAIT 2
|
||||
#define DEBUG_PIN_TASK2_LOCKED 3
|
||||
#define DEBUG_PIN_TRANSFER 4
|
||||
#define DEBUG_PIN_IDLE 6
|
||||
#define DEBUG_PIN_ERROR 7
|
||||
#define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
|
||||
#define DebugPinLow(x) PIOS_DEBUG_PinLow(x)
|
||||
#else
|
||||
#define DebugPinHigh(x)
|
||||
#define DebugPinLow(x)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#define MAX_LOCK_WAIT 2 // Time in ms that a thread can normally block I2C
|
||||
|
||||
/* Task Priorities */
|
||||
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
|
||||
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
|
||||
/* Function Prototypes */
|
||||
static void Task1(void *pvParameters);
|
||||
static void Task2(void *pvParameters);
|
||||
|
||||
|
||||
/**
|
||||
* OpenPilot Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
// Init
|
||||
PIOS_SYS_Init();
|
||||
PIOS_DEBUG_Init();
|
||||
PIOS_DELAY_Init();
|
||||
PIOS_COM_Init();
|
||||
PIOS_USB_Init(0);
|
||||
PIOS_I2C_Init();
|
||||
|
||||
// Both leds off
|
||||
PIOS_LED_Off(LED1);
|
||||
PIOS_LED_Off(LED2);
|
||||
|
||||
// Create task
|
||||
xTaskCreate(Task1, (signed portCHAR *)"Task1", 1024 , NULL, 1, NULL);
|
||||
xTaskCreate(Task2, (signed portCHAR *)"Task2", 1024 , NULL, 2, NULL);
|
||||
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
|
||||
/* If all is well we will never reach here as the scheduler will now be running. */
|
||||
/* If we do get here, it will most likely be because we ran out of heap space. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void OnError(void)
|
||||
{
|
||||
PIOS_LED_Off(LED1);
|
||||
while(1)
|
||||
{
|
||||
DebugPinHigh(DEBUG_PIN_ERROR);
|
||||
PIOS_LED_Toggle(LED2);
|
||||
vTaskDelay(50 / portTICK_RATE_MS);
|
||||
DebugPinLow(DEBUG_PIN_ERROR);
|
||||
}
|
||||
}
|
||||
//
|
||||
// This is a low priority task that will continuously access one I2C device
|
||||
// Frequently it will release the I2C device so that others can also use I2C
|
||||
//
|
||||
static void Task1(void *pvParameters)
|
||||
{
|
||||
int i = 0;
|
||||
|
||||
|
||||
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
|
||||
{
|
||||
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20\xB0\xB1\xB2", 4) != 0)
|
||||
OnError();
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
|
||||
for(;;)
|
||||
{
|
||||
i++;
|
||||
if (i==100)
|
||||
{
|
||||
PIOS_LED_Toggle(LED1);
|
||||
i = 0;
|
||||
}
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TASK1_WAIT);
|
||||
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
|
||||
{
|
||||
uint8_t buf[20];
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK1_WAIT);
|
||||
DebugPinHigh(DEBUG_PIN_TASK1_LOCKED);
|
||||
|
||||
// Write A0 A1 A2 at address 0x10
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10\xA0\xA1\xA2", 4) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
|
||||
// Read 3 bytes at address 0x20 and check
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20", 1) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
if (memcmp(buf, "\xB0\xB1\xB2",3) != 0)
|
||||
OnError();
|
||||
|
||||
// Read 3 bytes at address 0x10 and check
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
if (memcmp(buf, "\xA0\xA1\xA2",3) != 0)
|
||||
OnError();
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK1_LOCKED);
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// This is a high priority task that will periodically perform some actions on the second I2C device
|
||||
// Most of the time it will have to wait for the other task task to release I2C
|
||||
static void Task2(void *pvParameters)
|
||||
{
|
||||
portTickType xLastExecutionTime;
|
||||
|
||||
xLastExecutionTime = xTaskGetTickCount();
|
||||
uint32_t count = 0;
|
||||
|
||||
for(;;)
|
||||
{
|
||||
uint8_t buf[20];
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
|
||||
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
|
||||
{
|
||||
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
|
||||
|
||||
// Write value of count to address 0x10
|
||||
buf[0] = 0x10; // The address
|
||||
memcpy(&buf[1], &count, 4); // The data to write
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_2_ADDRESS<<1, buf, 5) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
|
||||
vTaskDelay(2 / portTICK_RATE_MS);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
|
||||
if (PIOS_I2C_LockDevice(1 / portTICK_RATE_MS))
|
||||
{
|
||||
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
|
||||
|
||||
// Read at address 0x10 and check
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_2_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_2_ADDRESS<<1, buf, 4) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (memcmp(buf, &count, 4) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
|
||||
vTaskDelay(5 / portTICK_RATE_MS);
|
||||
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Idle hook function
|
||||
*/
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
DebugPinHigh(DEBUG_PIN_IDLE);
|
||||
DebugPinLow(DEBUG_PIN_IDLE);
|
||||
}
|
@ -1,174 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file test_uavobjects.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Tests for the UAVObject libraries
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "exampleobject1.h"
|
||||
#include "uavobjectsinit.h"
|
||||
|
||||
// Local functions
|
||||
static void testTask(void *pvParameters);
|
||||
static void eventCallback(UAVObjEvent* ev);
|
||||
static void eventCallbackPeriodic(UAVObjEvent* ev);
|
||||
|
||||
// Variables
|
||||
int testDelay = 0;
|
||||
|
||||
int main()
|
||||
{
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* SPI Init */
|
||||
PIOS_SPI_Init();
|
||||
|
||||
/* Enables the SDCard */
|
||||
PIOS_SDCARD_Init();
|
||||
PIOS_SDCARD_MountFS(0);
|
||||
|
||||
// Turn on both LEDs
|
||||
PIOS_LED_On(LED1);
|
||||
PIOS_LED_On(LED2);
|
||||
|
||||
// Create test task
|
||||
xTaskCreate(testTask, (signed portCHAR *)"ObjTest", 1000 , NULL, 1, NULL);
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void testTask(void *pvParameters)
|
||||
{
|
||||
ExampleObject1Data data;
|
||||
|
||||
// Initialize object and manager
|
||||
EventDispatcherInitialize();
|
||||
UAVObjInitialize();
|
||||
UAVObjectsInitializeAll();
|
||||
|
||||
// Set data
|
||||
ExampleObject1Get(&data);
|
||||
data.Field1 = 1;
|
||||
data.Field2 = 2;
|
||||
ExampleObject1Set(&data);
|
||||
|
||||
// Get data
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1Get(&data);
|
||||
|
||||
// Benchmark time is takes to get and set data (both operations)
|
||||
int tickStart = xTaskGetTickCount();
|
||||
for (int n = 0; n < 10000; ++n)
|
||||
{
|
||||
ExampleObject1Set(&data);
|
||||
ExampleObject1Get(&data);
|
||||
}
|
||||
int delay = xTaskGetTickCount() - tickStart;
|
||||
|
||||
// Create a new instance and get/set its data
|
||||
uint16_t instId = ExampleObject1CreateInstance();
|
||||
ExampleObject1InstSet(instId, &data);
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1InstGet(instId, &data);
|
||||
|
||||
// Test pack/unpack
|
||||
uint8_t buffer[EXAMPLEOBJECT1_NUMBYTES];
|
||||
memset(buffer, 0, EXAMPLEOBJECT1_NUMBYTES);
|
||||
UAVObjPack(ExampleObject1Handle(), 0, buffer);
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1Set(&data);
|
||||
UAVObjUnpack(ExampleObject1Handle(), 0, buffer);
|
||||
ExampleObject1Get(&data);
|
||||
|
||||
// Test object saving/loading to SD card
|
||||
UAVObjSave(ExampleObject1Handle(), 0);
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1Set(&data);
|
||||
UAVObjLoad(ExampleObject1Handle(), 0);
|
||||
ExampleObject1Get(&data);
|
||||
|
||||
// Retrieve object handle by ID
|
||||
UAVObjHandle handle = UAVObjGetByID(EXAMPLEOBJECT1_OBJID);
|
||||
const char* name = UAVObjGetName(handle);
|
||||
|
||||
// Get/Set the metadata
|
||||
UAVObjMetadata mdata;
|
||||
ExampleObject1GetMetadata(&mdata);
|
||||
mdata.gcsTelemetryAcked = 0;
|
||||
ExampleObject1SetMetadata(&mdata);
|
||||
memset(&mdata, 0, sizeof(mdata));
|
||||
ExampleObject1GetMetadata(&mdata);
|
||||
|
||||
// Test callbacks
|
||||
ExampleObject1ConnectCallback(eventCallback);
|
||||
ExampleObject1Set(&data);
|
||||
|
||||
// Test queue events
|
||||
xQueueHandle queue;
|
||||
queue = xQueueCreate(10, sizeof(UAVObjEvent));
|
||||
ExampleObject1ConnectQueue(queue);
|
||||
|
||||
// Test periodic events
|
||||
UAVObjEvent ev;
|
||||
ev.event = 0;
|
||||
ev.instId = 0;
|
||||
ev.obj = ExampleObject1Handle();
|
||||
EventPeriodicCallbackCreate(&ev, eventCallbackPeriodic, 500);
|
||||
EventPeriodicQueueCreate(&ev, queue, 250);
|
||||
|
||||
// Done testing
|
||||
while (1)
|
||||
{
|
||||
if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
|
||||
{
|
||||
name = UAVObjGetName(ev.obj);
|
||||
PIOS_LED_Toggle(LED2);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void eventCallback(UAVObjEvent* ev)
|
||||
{
|
||||
const char* name = UAVObjGetName(ev->obj);
|
||||
}
|
||||
|
||||
static void eventCallbackPeriodic(UAVObjEvent* ev)
|
||||
{
|
||||
static int lastUpdate;
|
||||
testDelay = xTaskGetTickCount() - lastUpdate;
|
||||
lastUpdate = xTaskGetTickCount();
|
||||
const char* name = UAVObjGetName(ev->obj);
|
||||
PIOS_LED_Toggle(LED1);
|
||||
//ExampleObject1Updated();
|
||||
}
|
||||
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
}
|
@ -1,76 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot
|
||||
#
|
||||
# Makefile for OpenPilot UAVObject code
|
||||
#
|
||||
# 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
|
||||
#####
|
||||
|
||||
# These are the UAVObjects supposed to be build as part of the OpenPilot target
|
||||
# (all architectures)
|
||||
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
UAVOBJSRCFILENAMES += actuatordesired
|
||||
UAVOBJSRCFILENAMES += actuatorsettings
|
||||
UAVOBJSRCFILENAMES += inssettings
|
||||
UAVOBJSRCFILENAMES += insstatus
|
||||
UAVOBJSRCFILENAMES += attitudeactual
|
||||
UAVOBJSRCFILENAMES += attituderaw
|
||||
UAVOBJSRCFILENAMES += baroaltitude
|
||||
UAVOBJSRCFILENAMES += flightbatterysettings
|
||||
UAVOBJSRCFILENAMES += firmwareiapobj
|
||||
UAVOBJSRCFILENAMES += flightbatterystate
|
||||
UAVOBJSRCFILENAMES += flightplancontrol
|
||||
UAVOBJSRCFILENAMES += flightplansettings
|
||||
UAVOBJSRCFILENAMES += flightplanstatus
|
||||
UAVOBJSRCFILENAMES += flighttelemetrystats
|
||||
UAVOBJSRCFILENAMES += gcstelemetrystats
|
||||
UAVOBJSRCFILENAMES += gpsposition
|
||||
UAVOBJSRCFILENAMES += gpssatellites
|
||||
UAVOBJSRCFILENAMES += gpstime
|
||||
UAVOBJSRCFILENAMES += guidancesettings
|
||||
UAVOBJSRCFILENAMES += homelocation
|
||||
UAVOBJSRCFILENAMES += i2cstats
|
||||
UAVOBJSRCFILENAMES += manualcontrolcommand
|
||||
UAVOBJSRCFILENAMES += manualcontrolsettings
|
||||
UAVOBJSRCFILENAMES += mixersettings
|
||||
UAVOBJSRCFILENAMES += mixerstatus
|
||||
UAVOBJSRCFILENAMES += nedaccel
|
||||
UAVOBJSRCFILENAMES += objectpersistence
|
||||
UAVOBJSRCFILENAMES += positionactual
|
||||
UAVOBJSRCFILENAMES += positiondesired
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += sonaraltitude
|
||||
UAVOBJSRCFILENAMES += stabilizationdesired
|
||||
UAVOBJSRCFILENAMES += stabilizationsettings
|
||||
UAVOBJSRCFILENAMES += systemalarms
|
||||
UAVOBJSRCFILENAMES += systemsettings
|
||||
UAVOBJSRCFILENAMES += systemstats
|
||||
UAVOBJSRCFILENAMES += taskinfo
|
||||
UAVOBJSRCFILENAMES += velocityactual
|
||||
UAVOBJSRCFILENAMES += velocitydesired
|
||||
UAVOBJSRCFILENAMES += watchdogstatus
|
||||
UAVOBJSRCFILENAMES += flightstatus
|
||||
UAVOBJSRCFILENAMES += hwsettings
|
||||
UAVOBJSRCFILENAMES += receiveractivity
|
||||
UAVOBJSRCFILENAMES += cameradesired
|
||||
UAVOBJSRCFILENAMES += camerastabsettings
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
@ -8,9 +8,7 @@
|
||||
|
||||
/* Begin PBXFileReference section */
|
||||
65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = "<group>"; };
|
||||
6502584212CA4D2600583CDF /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps13state.c; path = ../../AHRS/insgps13state.c; sourceTree = SOURCE_ROOT; };
|
||||
65078B09136FCEE600536549 /* flightstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightstatus.xml; sourceTree = "<group>"; };
|
||||
6509C7E912CA57DC002E5DC2 /* insgps16state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps16state.c; path = ../../AHRS/insgps16state.c; sourceTree = SOURCE_ROOT; };
|
||||
650D8E2112DFE16400D05CC9 /* actuator.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = actuator.c; sourceTree = "<group>"; };
|
||||
650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = "<group>"; };
|
||||
650D8E2512DFE16400D05CC9 /* ahrs_comms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_comms.c; sourceTree = "<group>"; };
|
||||
@ -85,9 +83,7 @@
|
||||
6534B55A14763566003DF47C /* pios_flash_m25p16.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flash_m25p16.c; sourceTree = "<group>"; };
|
||||
6534B55B1476D3A8003DF47C /* pios_ms5611.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_ms5611.h; sourceTree = "<group>"; };
|
||||
6536D47B1307962C0042A298 /* stabilizationdesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = stabilizationdesired.xml; sourceTree = "<group>"; };
|
||||
6536D4881307AB950042A298 /* UAVObjects.inc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.pascal; path = UAVObjects.inc; sourceTree = "<group>"; };
|
||||
65408AA812BB1648004DACC5 /* i2cstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = i2cstats.xml; sourceTree = "<group>"; };
|
||||
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
|
||||
6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = "<group>"; };
|
||||
6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = "<group>"; };
|
||||
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
|
||||
@ -116,8 +112,6 @@
|
||||
657CF024121F49CD007A1FBE /* WMMInternal.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = WMMInternal.h; sourceTree = "<group>"; };
|
||||
657FF86A12EA8BFB00801617 /* pios_pwm_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_pwm_priv.h; sourceTree = "<group>"; };
|
||||
6581785414A65B9B0007885F /* pios_bl_helper.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_bl_helper.c; sourceTree = "<group>"; };
|
||||
6589A972131DDE93006BD67C /* FreeRTOSConfig.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = FreeRTOSConfig.h; sourceTree = "<group>"; };
|
||||
6589A983131DE24F006BD67C /* taskmonitor.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = taskmonitor.c; sourceTree = "<group>"; };
|
||||
6589A9DB131DEE76006BD67C /* pios_rtc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_rtc.c; sourceTree = "<group>"; };
|
||||
6589A9E2131DF1C7006BD67C /* pios_rtc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rtc.h; sourceTree = "<group>"; };
|
||||
65904E47146128A500FD9482 /* ins.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ins.c; sourceTree = "<group>"; };
|
||||
@ -2811,12 +2805,6 @@
|
||||
65B367FD121C2620003EAD18 /* systemstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = systemstats.xml; sourceTree = "<group>"; };
|
||||
65B367FE121C2620003EAD18 /* telemetrysettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = telemetrysettings.xml; sourceTree = "<group>"; };
|
||||
65B367FF121C2620003EAD18 /* src.pro */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = src.pro; sourceTree = "<group>"; };
|
||||
65B7E6AE120DF1E2000C1123 /* ahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs.c; path = ../../AHRS/ahrs.c; sourceTree = SOURCE_ROOT; };
|
||||
65B7E6B0120DF1E2000C1123 /* ahrs.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs.h; sourceTree = "<group>"; };
|
||||
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_fsm.h; sourceTree = "<group>"; };
|
||||
65B7E6B4120DF1E2000C1123 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_config.h; sourceTree = "<group>"; };
|
||||
65B7E6B6120DF1E2000C1123 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../AHRS/Makefile; sourceTree = SOURCE_ROOT; };
|
||||
65B7E6B7120DF1E2000C1123 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board.c; path = ../../AHRS/pios_board.c; sourceTree = SOURCE_ROOT; };
|
||||
65C35E5012EFB2F3004811C2 /* actuatorcommand.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatorcommand.xml; sourceTree = "<group>"; };
|
||||
65C35E5112EFB2F3004811C2 /* actuatordesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatordesired.xml; sourceTree = "<group>"; };
|
||||
65C35E5212EFB2F3004811C2 /* actuatorsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatorsettings.xml; sourceTree = "<group>"; };
|
||||
@ -2924,24 +2912,6 @@
|
||||
65E8C743139A6D0900E1F979 /* pios_crc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_crc.c; sourceTree = "<group>"; };
|
||||
65E8C745139A6D1A00E1F979 /* pios_crc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_crc.h; sourceTree = "<group>"; };
|
||||
65E8C788139AA2A800E1F979 /* accessorydesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = accessorydesired.xml; sourceTree = "<group>"; };
|
||||
65E8EF1F11EEA61E00BBF654 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../OpenPilot/Makefile; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF2011EEA61E00BBF654 /* Makefile.posix */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = Makefile.posix; path = ../../OpenPilot/Makefile.posix; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5C11EEA61E00BBF654 /* alarms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = alarms.c; path = ../../OpenPilot/System/alarms.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5E11EEA61E00BBF654 /* alarms.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = alarms.h; path = ../../OpenPilot/System/inc/alarms.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5F11EEA61E00BBF654 /* op_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = op_config.h; path = ../../OpenPilot/System/inc/op_config.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6011EEA61E00BBF654 /* openpilot.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = openpilot.h; path = ../../OpenPilot/System/inc/openpilot.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6111EEA61E00BBF654 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_board.h; path = ../../OpenPilot/System/inc/pios_board.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6211EEA61E00BBF654 /* pios_board_posix.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_board_posix.h; path = ../../OpenPilot/System/inc/pios_board_posix.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6311EEA61E00BBF654 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_config.h; path = ../../OpenPilot/System/inc/pios_config.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6411EEA61E00BBF654 /* pios_config_posix.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_config_posix.h; path = ../../OpenPilot/System/inc/pios_config_posix.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6511EEA61E00BBF654 /* openpilot.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = openpilot.c; path = ../../OpenPilot/System/openpilot.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6611EEA61E00BBF654 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board.c; path = ../../OpenPilot/System/pios_board.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6711EEA61E00BBF654 /* pios_board_posix.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board_posix.c; path = ../../OpenPilot/System/pios_board_posix.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6911EEA61E00BBF654 /* test_BMP085.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_BMP085.c; path = ../../OpenPilot/Tests/test_BMP085.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6A11EEA61E00BBF654 /* test_common.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_common.c; path = ../../OpenPilot/Tests/test_common.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6B11EEA61E00BBF654 /* test_cpuload.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_cpuload.c; path = ../../OpenPilot/Tests/test_cpuload.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6C11EEA61E00BBF654 /* test_i2c_PCF8570.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_i2c_PCF8570.c; path = ../../OpenPilot/Tests/test_i2c_PCF8570.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF6D11EEA61E00BBF654 /* test_uavobjects.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_uavobjects.c; path = ../../OpenPilot/Tests/test_uavobjects.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8F03211EFF25C00BBF654 /* pios_com.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_com.c; path = ../../PiOS/Common/pios_com.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8F03311EFF25C00BBF654 /* pios_hmc5843.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_hmc5843.c; path = ../../PiOS/Common/pios_hmc5843.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8F03411EFF25C00BBF654 /* pios_opahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_opahrs.c; path = ../../PiOS/Common/pios_opahrs.c; sourceTree = SOURCE_ROOT; };
|
||||
@ -3348,8 +3318,6 @@
|
||||
65FAB9091482072E000FF8B2 /* altitude.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = altitude.c; sourceTree = "<group>"; };
|
||||
65FAB90B1482072E000FF8B2 /* altitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = altitude.h; sourceTree = "<group>"; };
|
||||
65FBE14412E7C98100176B5A /* pios_servo_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_servo_priv.h; sourceTree = "<group>"; };
|
||||
65FC66AA123F30F100B04F74 /* ahrs_timer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs_timer.c; path = ../../AHRS/ahrs_timer.c; sourceTree = SOURCE_ROOT; };
|
||||
65FC66AB123F312A00B04F74 /* ahrs_timer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_timer.h; sourceTree = "<group>"; };
|
||||
65FF4BB513791C3300146BE4 /* ahrs_slave_test.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_slave_test.c; sourceTree = "<group>"; };
|
||||
65FF4BB613791C3300146BE4 /* ahrs_spi_program.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_program.c; sourceTree = "<group>"; };
|
||||
65FF4BB713791C3300146BE4 /* ahrs_spi_program_master.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_program_master.c; sourceTree = "<group>"; };
|
||||
@ -3802,9 +3770,7 @@
|
||||
65904E3F146128A500FD9482 /* Revolution */,
|
||||
65FF4BB313791C3300146BE4 /* Bootloaders */,
|
||||
65F93B9012EE09280047DB36 /* PipXtreme */,
|
||||
65B7E6AC120DF1CD000C1123 /* AHRS */,
|
||||
65E6DF7012E02E8E00058553 /* CopterControl */,
|
||||
65E8EF1E11EEA61E00BBF654 /* OpenPilot */,
|
||||
650D8E1F12DFE16400D05CC9 /* Modules */,
|
||||
657CEEB6121DBC63007A1FBE /* Libraries */,
|
||||
65E8F02F11EFF25C00BBF654 /* PiOS */,
|
||||
@ -8249,33 +8215,6 @@
|
||||
path = uavobjectdefinition;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
65B7E6AC120DF1CD000C1123 /* AHRS */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
6509C7E912CA57DC002E5DC2 /* insgps16state.c */,
|
||||
6502584212CA4D2600583CDF /* insgps13state.c */,
|
||||
65FC66AA123F30F100B04F74 /* ahrs_timer.c */,
|
||||
65B7E6AE120DF1E2000C1123 /* ahrs.c */,
|
||||
65B7E6AF120DF1E2000C1123 /* inc */,
|
||||
65B7E6B6120DF1E2000C1123 /* Makefile */,
|
||||
65B7E6B7120DF1E2000C1123 /* pios_board.c */,
|
||||
);
|
||||
name = AHRS;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
65B7E6AF120DF1E2000C1123 /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65FC66AB123F312A00B04F74 /* ahrs_timer.h */,
|
||||
6543304F121980300063F913 /* insgps.h */,
|
||||
65B7E6B0120DF1E2000C1123 /* ahrs.h */,
|
||||
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,
|
||||
65B7E6B4120DF1E2000C1123 /* pios_config.h */,
|
||||
);
|
||||
name = inc;
|
||||
path = ../../AHRS/inc;
|
||||
sourceTree = SOURCE_ROOT;
|
||||
};
|
||||
65C35E4E12EFB2F3004811C2 /* shared */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
@ -8451,62 +8390,6 @@
|
||||
path = inc;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
65E8EF1E11EEA61E00BBF654 /* OpenPilot */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
6536D4881307AB950042A298 /* UAVObjects.inc */,
|
||||
65E8EF1F11EEA61E00BBF654 /* Makefile */,
|
||||
65E8EF2011EEA61E00BBF654 /* Makefile.posix */,
|
||||
65E8EF5B11EEA61E00BBF654 /* System */,
|
||||
65E8EF6811EEA61E00BBF654 /* Tests */,
|
||||
);
|
||||
name = OpenPilot;
|
||||
path = ../../OpenPilot;
|
||||
sourceTree = SOURCE_ROOT;
|
||||
};
|
||||
65E8EF5B11EEA61E00BBF654 /* System */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65E8EF5C11EEA61E00BBF654 /* alarms.c */,
|
||||
65E8EF5D11EEA61E00BBF654 /* inc */,
|
||||
65E8EF6511EEA61E00BBF654 /* openpilot.c */,
|
||||
65E8EF6611EEA61E00BBF654 /* pios_board.c */,
|
||||
65E8EF6711EEA61E00BBF654 /* pios_board_posix.c */,
|
||||
6589A983131DE24F006BD67C /* taskmonitor.c */,
|
||||
);
|
||||
name = System;
|
||||
path = ../../OpenPilot/System;
|
||||
sourceTree = SOURCE_ROOT;
|
||||
};
|
||||
65E8EF5D11EEA61E00BBF654 /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
6589A972131DDE93006BD67C /* FreeRTOSConfig.h */,
|
||||
65E8EF5E11EEA61E00BBF654 /* alarms.h */,
|
||||
65E8EF5F11EEA61E00BBF654 /* op_config.h */,
|
||||
65E8EF6011EEA61E00BBF654 /* openpilot.h */,
|
||||
65E8EF6111EEA61E00BBF654 /* pios_board.h */,
|
||||
65E8EF6211EEA61E00BBF654 /* pios_board_posix.h */,
|
||||
65E8EF6311EEA61E00BBF654 /* pios_config.h */,
|
||||
65E8EF6411EEA61E00BBF654 /* pios_config_posix.h */,
|
||||
);
|
||||
name = inc;
|
||||
path = ../../OpenPilot/System/inc;
|
||||
sourceTree = SOURCE_ROOT;
|
||||
};
|
||||
65E8EF6811EEA61E00BBF654 /* Tests */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65E8EF6911EEA61E00BBF654 /* test_BMP085.c */,
|
||||
65E8EF6A11EEA61E00BBF654 /* test_common.c */,
|
||||
65E8EF6B11EEA61E00BBF654 /* test_cpuload.c */,
|
||||
65E8EF6C11EEA61E00BBF654 /* test_i2c_PCF8570.c */,
|
||||
65E8EF6D11EEA61E00BBF654 /* test_uavobjects.c */,
|
||||
);
|
||||
name = Tests;
|
||||
path = ../../OpenPilot/Tests;
|
||||
sourceTree = SOURCE_ROOT;
|
||||
};
|
||||
65E8F02F11EFF25C00BBF654 /* PiOS */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
|
Loading…
x
Reference in New Issue
Block a user