mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Added EKF code. Most of this was bulk copy and paste from the AHRS, so a lot of verification needs to take place. Grabbing sensor data remains untested.
This commit is contained in:
parent
48a0881dc4
commit
876ca3044c
446
flight/Bootloaders/INS/Makefile
Normal file
446
flight/Bootloaders/INS/Makefile
Normal file
@ -0,0 +1,446 @@
|
||||
#####
|
||||
# Project: OpenPilot INS_BOOTLOADER
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot INS_BOOTLOADER project
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
TOP := $(realpath $(WHEREAMI)/../../../)
|
||||
include $(TOP)/make/firmware-defs.mk
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES for debugging
|
||||
DEBUG ?= NO
|
||||
OVERRIDE USE_BOOTLOADER = 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
|
||||
|
||||
# MCU name, submodel and board
|
||||
# - MCU used for compiler-option (-mcpu)
|
||||
# - MODEL used for linker-script name (-T) and passed as define
|
||||
# - BOARD just passed as define (optional)
|
||||
MCU = cortex-m3
|
||||
CHIP = STM32F103RET
|
||||
BOARD = STM3210E_INS
|
||||
MODEL = HD
|
||||
|
||||
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR = $(TOP)/build/bl_ins
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET = INS_BL
|
||||
|
||||
# Paths
|
||||
INS_BL = ./
|
||||
INS_BLINC = $(INS_BL)/inc
|
||||
PIOS = ../../PiOS
|
||||
PIOSINC = $(PIOS)/inc
|
||||
FLIGHTLIB = ../Libraries
|
||||
FLIGHTLIBINC = ../Libraries/inc
|
||||
PIOSSTM32F10X = $(PIOS)/STM32F10x
|
||||
PIOSCOMMON = $(PIOS)/Common
|
||||
PIOSBOARDS = $(PIOS)/Boards
|
||||
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
|
||||
STMLIBDIR = $(APPLIBDIR)
|
||||
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
|
||||
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
|
||||
STMSPDSRCDIR = $(STMSPDDIR)/src
|
||||
STMSPDINCDIR = $(STMSPDDIR)/inc
|
||||
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
|
||||
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
## BOOTLOADER:
|
||||
SRC = main.c
|
||||
SRC += pios_board.c
|
||||
SRC += bl_fsm.c
|
||||
|
||||
## 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_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spi.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
SRC += $(PIOSCOMMON)/pios_bl_helper.c
|
||||
SRC += $(PIOSCOMMON)/pios_iap.c
|
||||
|
||||
## CMSIS for STM32
|
||||
SRC += $(CMSISDIR)/core_cm3.c
|
||||
SRC += $(CMSISDIR)/system_stm32f10x.c
|
||||
|
||||
## Used parts of the STM-Library
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
|
||||
SRC += $(STMSPDSRCDIR)/misc.c
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
# will not be considered source files but generated files (assembler
|
||||
# output from the compiler), and will be deleted upon "make clean"!
|
||||
# Even though the DOS/Win* filesystem matches both .s and .S the same,
|
||||
# it will preserve the spelling of the filenames, and gcc itself does
|
||||
# care about how the name is spelled on its command-line.
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL).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 += $(INS_BLINC)
|
||||
|
||||
# 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 ($(USE_BOOTLOADER), YES)
|
||||
CDEFS += -DUSE_BOOTLOADER
|
||||
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 = -g$(DEBUGF)
|
||||
endif
|
||||
|
||||
CFLAGS += -O$(OPT)
|
||||
ifeq ($(DEBUG),NO)
|
||||
CFLAGS += -fdata-sections -ffunction-sections
|
||||
endif
|
||||
CFLAGS += -mcpu=$(MCU) -mthumb
|
||||
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
|
||||
ifeq ($(DEBUG),NO)
|
||||
LDFLAGS += -Wl,-static -Wl,-s
|
||||
endif
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
LDFLAGS +=-T$(LINKERSCRIPTPATH)/link_stm32f10x_$(MODEL).ld
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Options for OpenOCD flash-programming
|
||||
# see openocd.pdf/openocd.texi for further information
|
||||
#
|
||||
OOCD_LOADFILE+=$(OUTDIR)/$(TARGET).elf
|
||||
# if OpenOCD is in the $PATH just set OPENOCDEXE=openocd
|
||||
OOCD_EXE=openocd
|
||||
# debug level
|
||||
OOCD_CL=-d0
|
||||
# interface and board/target settings (using the OOCD target-library here)
|
||||
UNAME := $(shell uname)
|
||||
ifeq ($(UNAME), Darwin)
|
||||
OOCD_CL+=-f ../../Project/OpenOCD/floss-jtag.ahrs.osx.cfg -f ../../Project/OpenOCD/stm32.cfg
|
||||
else
|
||||
OOCD_CL+=-f ../../Project/OpenOCD/floss-jtag.ahrs.cfg -f ../../Project/OpenOCD/stm32.cfg
|
||||
endif
|
||||
# initialize
|
||||
OOCD_CL+=-c init
|
||||
# show the targets
|
||||
OOCD_CL+=-c targets
|
||||
# commands to prepare flash-write
|
||||
OOCD_CL+= -c "reset halt"
|
||||
# flash erase
|
||||
OOCD_CL+=-c "stm32x mass_erase 0"
|
||||
# flash-write
|
||||
OOCD_CL+=-c "flash write_image $(OOCD_LOADFILE)"
|
||||
# Verify
|
||||
OOCD_CL+=-c "verify_image $(OOCD_LOADFILE)"
|
||||
# reset target
|
||||
OOCD_CL+=-c "reset run"
|
||||
# terminate OOCD after programming
|
||||
OOCD_CL+=-c shutdown
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
# Define programs and commands.
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
###SHELL = sh
|
||||
###COPY = cp
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
# Default target.
|
||||
all: build
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# 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 "Programming with OPENOCD"
|
||||
$(OOCD_EXE) $(OOCD_CL)
|
||||
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))
|
||||
|
||||
.PHONY: elf lss sym hex bin
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
# Display sizes of sections.
|
||||
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
|
||||
.PHONY: size
|
||||
size: $(OUTDIR)/$(TARGET).elf_size
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Target: clean project.
|
||||
clean: clean_list
|
||||
|
||||
clean_list :
|
||||
@echo $(MSG_CLEANING)
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(V1) $(REMOVE) $(ALLOBJ)
|
||||
$(V1) $(REMOVE) $(LSTFILES)
|
||||
$(V1) $(REMOVE) $(DEPFILES)
|
||||
$(V1) $(REMOVE) $(SRC:.c=.s)
|
||||
$(V1) $(REMOVE) $(SRCARM:.c=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
|
||||
else
|
||||
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir -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 program
|
38
flight/Bootloaders/INS/ahrs_slave_test.c
Normal file
38
flight/Bootloaders/INS/ahrs_slave_test.c
Normal file
@ -0,0 +1,38 @@
|
||||
#include "ins_bl.h"
|
||||
#include "ahrs_spi_program.h"
|
||||
|
||||
uint8_t buf[256];
|
||||
|
||||
bool StartProgramming(void)
|
||||
{
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Started programming\r\n");
|
||||
return(true);
|
||||
}
|
||||
|
||||
bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size)
|
||||
{
|
||||
if(size > SPI_MAX_PROGRAM_DATA_SIZE)
|
||||
{
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX,"oversize: %d\r\n", size);
|
||||
return(false);
|
||||
}
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Wrote %d bytes to %d\r\n", size, offset);
|
||||
memcpy(buf,buffer,size);
|
||||
PIOS_LED_Toggle(LED1);
|
||||
return(true);
|
||||
}
|
||||
|
||||
bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size)
|
||||
{
|
||||
if(size > SPI_MAX_PROGRAM_DATA_SIZE)
|
||||
{
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX,"oversize: %d\r\n", size);
|
||||
return(false);
|
||||
}
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Read %d bytes from %d\r\n", size, offset);
|
||||
memcpy(buffer,buf,size);
|
||||
PIOS_LED_Toggle(LED1);
|
||||
return(true);
|
||||
}
|
||||
|
||||
|
89
flight/Bootloaders/INS/ahrs_spi_program.c
Normal file
89
flight/Bootloaders/INS/ahrs_spi_program.c
Normal file
@ -0,0 +1,89 @@
|
||||
#include <stdint.h>
|
||||
#include "ahrs_spi_program.h"
|
||||
|
||||
// Static CRC polynomial table
|
||||
static uint32_t crcTable[256] =
|
||||
{
|
||||
0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA,
|
||||
0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
|
||||
0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
|
||||
0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
|
||||
0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
|
||||
0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
|
||||
0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
|
||||
0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
|
||||
0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
|
||||
0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
|
||||
0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
|
||||
0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
|
||||
0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
|
||||
0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
|
||||
0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
|
||||
0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
|
||||
|
||||
0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
|
||||
0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
|
||||
0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
|
||||
0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
|
||||
0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
|
||||
0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
|
||||
0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
|
||||
0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
|
||||
0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
|
||||
0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
|
||||
0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
|
||||
0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
|
||||
0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
|
||||
0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
|
||||
0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
|
||||
0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
|
||||
|
||||
0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
|
||||
0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
|
||||
0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
|
||||
0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
|
||||
0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
|
||||
0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
|
||||
0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
|
||||
0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
|
||||
0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
|
||||
0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
|
||||
0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
|
||||
0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
|
||||
0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
|
||||
0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
|
||||
0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
|
||||
0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
|
||||
|
||||
0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
|
||||
0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
|
||||
0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
|
||||
0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
|
||||
0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
|
||||
0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
|
||||
0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
|
||||
0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
|
||||
0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
|
||||
0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
|
||||
0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
|
||||
0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
|
||||
0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
|
||||
0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
|
||||
0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
|
||||
0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D,
|
||||
};
|
||||
|
||||
/**generate CRC32 from a program packet
|
||||
this is slightly overkill but we want to be sure
|
||||
*/
|
||||
uint32_t GenerateCRC (AhrsProgramPacket * packet)
|
||||
{
|
||||
uint8_t * ptr = (uint8_t *)packet;
|
||||
int size = ((int)&packet->crc) - (int)packet;
|
||||
uint32_t crc = 0xFFFFFFFF;
|
||||
for(int ct=0; ct< size; ct++)
|
||||
{
|
||||
crc = ((crc) >> 8) ^ crcTable[(*ptr++) ^ ((crc) & 0x000000FF)];
|
||||
}
|
||||
return(~crc);
|
||||
}
|
137
flight/Bootloaders/INS/ahrs_spi_program_master.c
Normal file
137
flight/Bootloaders/INS/ahrs_spi_program_master.c
Normal file
@ -0,0 +1,137 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_spi_program_master.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief AHRS programming over SPI link - master(OpenPilot) end.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "ahrs_spi_program_master.h"
|
||||
#include "ahrs_spi_program.h"
|
||||
#include "pios_spi.h"
|
||||
|
||||
PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf, AhrsProgramPacket *rxBuf);
|
||||
|
||||
#define MAX_CONNECT_TRIES 500 //half a second
|
||||
|
||||
bool AhrsProgramConnect(uint32_t spi_id)
|
||||
{
|
||||
AhrsProgramPacket rxBuf;
|
||||
AhrsProgramPacket txBuf;
|
||||
memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
|
||||
memcpy(&txBuf,SPI_PROGRAM_REQUEST,SPI_PROGRAM_REQUEST_LENGTH);
|
||||
for(int ct = 0; ct < MAX_CONNECT_TRIES; ct++) {
|
||||
PIOS_SPI_RC_PinSet(spi_id, 0);
|
||||
uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) &txBuf,
|
||||
(uint8_t *) & rxBuf, SPI_PROGRAM_REQUEST_LENGTH +1, NULL);
|
||||
PIOS_SPI_RC_PinSet(spi_id, 1);
|
||||
if(res == 0 && memcmp(&rxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
|
||||
return (true);
|
||||
}
|
||||
|
||||
vTaskDelay(1 / portTICK_RATE_MS);
|
||||
}
|
||||
return (false);
|
||||
}
|
||||
|
||||
|
||||
PROGERR AhrsProgramWrite(uint32_t spi_id, uint32_t address, void * data, uint32_t size)
|
||||
{
|
||||
AhrsProgramPacket rxBuf;
|
||||
AhrsProgramPacket txBuf;
|
||||
memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
|
||||
memcpy(txBuf.data,data,size);
|
||||
txBuf.size = size;
|
||||
txBuf.type = PROGRAM_WRITE;
|
||||
txBuf.address = address;
|
||||
PROGERR ret = TransferPacket(spi_id, &txBuf, &rxBuf);
|
||||
if(ret != PROGRAM_ERR_OK) {
|
||||
return(ret);
|
||||
}
|
||||
return(PROGRAM_ERR_OK);
|
||||
}
|
||||
|
||||
PROGERR AhrsProgramRead(uint32_t spi_id, uint32_t address, void * data, uint32_t size)
|
||||
{
|
||||
AhrsProgramPacket rxBuf;
|
||||
AhrsProgramPacket txBuf;
|
||||
memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
|
||||
txBuf.size = size;
|
||||
txBuf.type = PROGRAM_READ;
|
||||
txBuf.address = address;
|
||||
PROGERR ret = TransferPacket(spi_id, &txBuf, &rxBuf);
|
||||
if(ret != PROGRAM_ERR_OK) {
|
||||
return(ret);
|
||||
}
|
||||
memcpy(data, rxBuf.data, size);
|
||||
return(PROGRAM_ERR_OK);
|
||||
}
|
||||
|
||||
PROGERR AhrsProgramReboot(uint32_t spi_id)
|
||||
{
|
||||
AhrsProgramPacket rxBuf;
|
||||
AhrsProgramPacket txBuf;
|
||||
memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
|
||||
txBuf.type = PROGRAM_REBOOT;
|
||||
memcpy(txBuf.data,REBOOT_CONFIRMATION,REBOOT_CONFIRMATION_LENGTH);
|
||||
PROGERR ret = TransferPacket(spi_id, &txBuf, &rxBuf);
|
||||
//If AHRS has rebooted we will get comms errors
|
||||
if(ret == PROGRAM_ERR_LINK) {
|
||||
return(PROGRAM_ERR_OK);
|
||||
}
|
||||
return(PROGRAM_ERR_FUNCTION);
|
||||
}
|
||||
|
||||
PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf, AhrsProgramPacket *rxBuf)
|
||||
{
|
||||
static uint32_t pktId = 0;
|
||||
pktId++;
|
||||
txBuf->packetId = pktId;
|
||||
txBuf->crc = GenerateCRC(txBuf);
|
||||
int ct = 0;
|
||||
for(; ct < MAX_CONNECT_TRIES; ct++) {
|
||||
PIOS_SPI_RC_PinSet(spi_id, 0);
|
||||
uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) txBuf,
|
||||
(uint8_t *) rxBuf, sizeof(AhrsProgramPacket), NULL);
|
||||
PIOS_SPI_RC_PinSet(spi_id, 1);
|
||||
if(res == 0) {
|
||||
if(rxBuf->type != PROGRAM_NULL &&
|
||||
rxBuf->crc == GenerateCRC(rxBuf) &&
|
||||
rxBuf->packetId == pktId) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
vTaskDelay(1 / portTICK_RATE_MS);
|
||||
}
|
||||
if(ct == MAX_CONNECT_TRIES) {
|
||||
return (PROGRAM_ERR_LINK);
|
||||
}
|
||||
if(rxBuf->type != PROGRAM_ACK) {
|
||||
return(PROGRAM_ERR_FUNCTION);
|
||||
}
|
||||
return(PROGRAM_ERR_OK);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
142
flight/Bootloaders/INS/ahrs_spi_program_slave.c
Normal file
142
flight/Bootloaders/INS/ahrs_spi_program_slave.c
Normal file
@ -0,0 +1,142 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_spi_program_slave.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief AHRS programming over SPI link - slave(AHRS) end.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "pios_opahrs_proto.h"
|
||||
#include "pios_spi.h"
|
||||
#include "STM3210E_INS.h"
|
||||
|
||||
#include "ins_bl.h"
|
||||
#include "ahrs_spi_program_slave.h"
|
||||
#include "ahrs_spi_program.h"
|
||||
|
||||
static AhrsProgramPacket txBuf;
|
||||
static AhrsProgramPacket rxBuf;
|
||||
static bool done = false;
|
||||
|
||||
static void ProcessPacket();
|
||||
|
||||
#define WAIT_IF_RECEIVING() while(!(GPIOB->IDR & GPIO_Pin_12)){}; //NSS must be high
|
||||
|
||||
//Number of crc failures to allow before giving up
|
||||
#define PROGRAM_PACKET_TRIES 4
|
||||
|
||||
void AhrsProgramReceive(uint32_t spi_id)
|
||||
{
|
||||
done = false;
|
||||
memset(&txBuf,0,sizeof(AhrsProgramPacket));
|
||||
//wait for a program request
|
||||
int count = PROGRAM_PACKET_TRIES;
|
||||
while(1) {
|
||||
WAIT_IF_RECEIVING();
|
||||
while((PIOS_SPI_Busy(spi_id) != 0)){};
|
||||
memset(&rxBuf,'a',sizeof(AhrsProgramPacket));
|
||||
int32_t res = PIOS_SPI_TransferBlock(spi_id, NULL, (uint8_t*) &rxBuf,
|
||||
SPI_PROGRAM_REQUEST_LENGTH + 1, NULL);
|
||||
|
||||
if(res == 0 &&
|
||||
memcmp(&rxBuf, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
|
||||
break;
|
||||
}
|
||||
if(count-- == 0) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if(!StartProgramming()) {
|
||||
//Couldn't erase FLASH. Nothing we can do.
|
||||
return;
|
||||
}
|
||||
|
||||
//send ack
|
||||
memcpy(&txBuf,SPI_PROGRAM_ACK,SPI_PROGRAM_REQUEST_LENGTH);
|
||||
WAIT_IF_RECEIVING();
|
||||
while(0 != PIOS_SPI_TransferBlock(spi_id,(uint8_t*) &txBuf, NULL,
|
||||
SPI_PROGRAM_REQUEST_LENGTH + 1, NULL)) {};
|
||||
|
||||
txBuf.type = PROGRAM_NULL;
|
||||
|
||||
while(!done) {
|
||||
WAIT_IF_RECEIVING();
|
||||
if(0 == PIOS_SPI_TransferBlock(spi_id,(uint8_t*) &txBuf,
|
||||
(uint8_t*) &rxBuf, sizeof(AhrsProgramPacket), NULL)) {
|
||||
|
||||
uint32_t crc = GenerateCRC(&rxBuf);
|
||||
if(crc != rxBuf.crc ||
|
||||
txBuf.packetId == rxBuf.packetId) {
|
||||
continue;
|
||||
}
|
||||
ProcessPacket();
|
||||
txBuf.packetId = rxBuf.packetId;
|
||||
txBuf.crc = GenerateCRC(&txBuf);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void ProcessPacket()
|
||||
{
|
||||
switch(rxBuf.type) {
|
||||
case PROGRAM_NULL:
|
||||
txBuf.type = PROGRAM_NULL;
|
||||
break;
|
||||
|
||||
case PROGRAM_WRITE:
|
||||
if(WriteData(rxBuf.address, rxBuf.data, rxBuf.size)) {
|
||||
txBuf.type = PROGRAM_ACK;
|
||||
txBuf.size = rxBuf.size;
|
||||
} else {
|
||||
txBuf.type = PROGRAM_ERR;
|
||||
}
|
||||
break;
|
||||
|
||||
case PROGRAM_READ:
|
||||
if(ReadData(rxBuf.address, txBuf.data, rxBuf.size)) {
|
||||
txBuf.type = PROGRAM_ACK;
|
||||
txBuf.size = rxBuf.size;
|
||||
} else {
|
||||
txBuf.type = PROGRAM_ERR;
|
||||
}
|
||||
break;
|
||||
|
||||
case PROGRAM_REBOOT:
|
||||
if(0 == memcmp(rxBuf.data,REBOOT_CONFIRMATION,REBOOT_CONFIRMATION_LENGTH)) {
|
||||
done = true;
|
||||
txBuf.type = PROGRAM_ACK;
|
||||
}else{
|
||||
txBuf.type = PROGRAM_ERR;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
txBuf.type = PROGRAM_ERR;
|
||||
}
|
||||
}
|
626
flight/Bootloaders/INS/bl_fsm.c
Normal file
626
flight/Bootloaders/INS/bl_fsm.c
Normal file
@ -0,0 +1,626 @@
|
||||
#include <stdint.h> /* uint*_t */
|
||||
#include <stddef.h> /* NULL */
|
||||
|
||||
#include "bl_fsm.h"
|
||||
|
||||
#include "pios_opahrs_proto.h"
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
struct lfsm_context {
|
||||
enum lfsm_state curr_state;
|
||||
enum opahrs_msg_link_state link_state;
|
||||
enum opahrs_msg_type user_payload_type;
|
||||
uint32_t user_payload_len;
|
||||
|
||||
uint32_t errors;
|
||||
|
||||
uint8_t * rx;
|
||||
uint8_t * tx;
|
||||
|
||||
uint8_t * link_rx;
|
||||
uint8_t * link_tx;
|
||||
|
||||
uint8_t * user_rx;
|
||||
uint8_t * user_tx;
|
||||
|
||||
struct lfsm_link_stats stats;
|
||||
};
|
||||
|
||||
static struct lfsm_context context = { 0 };
|
||||
|
||||
static void lfsm_update_link_tx (struct lfsm_context * context);
|
||||
static void lfsm_init_rx (struct lfsm_context * context);
|
||||
|
||||
static uint32_t PIOS_SPI_OP;
|
||||
void lfsm_attach(uint32_t spi_id)
|
||||
{
|
||||
PIOS_SPI_OP = spi_id;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Link Finite State Machine
|
||||
*
|
||||
*/
|
||||
|
||||
struct lfsm_transition {
|
||||
void (*entry_fn)(struct lfsm_context * context);
|
||||
enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS];
|
||||
};
|
||||
|
||||
static void go_faulted(struct lfsm_context * context);
|
||||
static void go_stopped(struct lfsm_context * context);
|
||||
static void go_stopping(struct lfsm_context * context);
|
||||
static void go_inactive(struct lfsm_context * context);
|
||||
static void go_user_busy(struct lfsm_context * context);
|
||||
static void go_user_busy_rx_pending(struct lfsm_context * context);
|
||||
static void go_user_busy_tx_pending(struct lfsm_context * context);
|
||||
static void go_user_busy_rxtx_pending(struct lfsm_context * context);
|
||||
static void go_user_rx_pending(struct lfsm_context * context);
|
||||
static void go_user_tx_pending(struct lfsm_context * context);
|
||||
static void go_user_rxtx_pending(struct lfsm_context * context);
|
||||
static void go_user_rx_active(struct lfsm_context * context);
|
||||
static void go_user_tx_active(struct lfsm_context * context);
|
||||
static void go_user_rxtx_active(struct lfsm_context * context);
|
||||
|
||||
const static struct lfsm_transition lfsm_transitions[LFSM_STATE_NUM_STATES] = {
|
||||
[LFSM_STATE_FAULTED] = {
|
||||
.entry_fn = go_faulted,
|
||||
},
|
||||
[LFSM_STATE_STOPPED] = {
|
||||
.entry_fn = go_stopped,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_INIT_LINK] = LFSM_STATE_INACTIVE,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_STOPPING] = {
|
||||
.entry_fn = go_stopping,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_STOPPED,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_STOPPED,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_INACTIVE] = {
|
||||
.entry_fn = go_inactive,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
|
||||
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
|
||||
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_BUSY] = {
|
||||
.entry_fn = go_user_busy,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
|
||||
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
|
||||
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
|
||||
[LFSM_EVENT_USER_DONE] = LFSM_STATE_INACTIVE,
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_BUSY_RX_PENDING] = {
|
||||
.entry_fn = go_user_busy_rx_pending,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
|
||||
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING,
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_BUSY_TX_PENDING] = {
|
||||
.entry_fn = go_user_busy_tx_pending,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
|
||||
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING,
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_BUSY_RXTX_PENDING] = {
|
||||
.entry_fn = go_user_busy_rxtx_pending,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING,
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_RX_PENDING] = {
|
||||
.entry_fn = go_user_rx_pending,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RX_ACTIVE,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_TX_PENDING] = {
|
||||
.entry_fn = go_user_tx_pending,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_TX_ACTIVE,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_TX_ACTIVE,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_RXTX_PENDING] = {
|
||||
.entry_fn = go_user_rxtx_pending,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_RX_ACTIVE] = {
|
||||
.entry_fn = go_user_rx_active,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_TX_ACTIVE] = {
|
||||
.entry_fn = go_user_tx_active,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
|
||||
},
|
||||
},
|
||||
[LFSM_STATE_USER_RXTX_ACTIVE] = {
|
||||
.entry_fn = go_user_rxtx_active,
|
||||
.next_state = {
|
||||
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
|
||||
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
|
||||
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
/*
|
||||
* FSM State Entry Functions
|
||||
*/
|
||||
|
||||
static void go_faulted(struct lfsm_context * context)
|
||||
{
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
static void go_stopped(struct lfsm_context * context)
|
||||
{
|
||||
#if 0
|
||||
PIOS_SPI_Stop(PIOS_SPI_OP);
|
||||
#endif
|
||||
}
|
||||
|
||||
static void go_stopping(struct lfsm_context * context)
|
||||
{
|
||||
context->link_tx = NULL;
|
||||
context->tx = NULL;
|
||||
}
|
||||
|
||||
static void go_inactive(struct lfsm_context * context)
|
||||
{
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->user_rx = NULL;
|
||||
context->user_tx = NULL;
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_busy(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_rx);
|
||||
|
||||
context->user_rx = NULL;
|
||||
context->user_tx = NULL;
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_busy_rx_pending(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_rx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_busy_tx_pending(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_tx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_busy_rxtx_pending(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_rx);
|
||||
PIOS_DEBUG_Assert(context->user_tx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_rx_pending(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_rx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_tx_pending(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_tx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_rxtx_pending(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_rx);
|
||||
PIOS_DEBUG_Assert(context->user_tx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
lfsm_update_link_tx(context);
|
||||
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->link_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_rx_active(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_rx);
|
||||
|
||||
context->rx = context->user_rx;
|
||||
context->tx = context->link_tx;
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
|
||||
|
||||
lfsm_update_link_tx(context);
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_tx_active(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_tx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
|
||||
context->rx = context->link_rx;
|
||||
context->tx = context->user_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
static void go_user_rxtx_active(struct lfsm_context * context)
|
||||
{
|
||||
/* Sanity checks */
|
||||
PIOS_DEBUG_Assert(context->user_rx);
|
||||
PIOS_DEBUG_Assert(context->user_tx);
|
||||
|
||||
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
|
||||
context->rx = context->user_rx;
|
||||
context->tx = context->user_tx;
|
||||
|
||||
lfsm_init_rx(context);
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Misc Helper Functions
|
||||
*
|
||||
*/
|
||||
|
||||
static void lfsm_update_link_tx_v0 (struct opahrs_msg_v0 * msg, enum opahrs_msg_link_state state, uint16_t errors)
|
||||
{
|
||||
opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
|
||||
|
||||
msg->payload.link.state = state;
|
||||
msg->payload.link.errors = errors;
|
||||
}
|
||||
|
||||
static void lfsm_update_link_tx_v1 (struct opahrs_msg_v1 * msg, enum opahrs_msg_link_state state, uint16_t errors)
|
||||
{
|
||||
opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
|
||||
|
||||
msg->payload.link.state = state;
|
||||
msg->payload.link.errors = errors;
|
||||
}
|
||||
|
||||
static void lfsm_update_link_tx (struct lfsm_context * context)
|
||||
{
|
||||
PIOS_DEBUG_Assert(context->link_tx);
|
||||
|
||||
switch (context->user_payload_type) {
|
||||
case OPAHRS_MSG_TYPE_USER_V0:
|
||||
lfsm_update_link_tx_v0 ((struct opahrs_msg_v0 *)context->link_tx, context->link_state, context->errors);
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_USER_V1:
|
||||
lfsm_update_link_tx_v1 ((struct opahrs_msg_v1 *)context->link_tx, context->link_state, context->errors);
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_LINK:
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
}
|
||||
|
||||
static void lfsm_init_rx (struct lfsm_context * context)
|
||||
{
|
||||
PIOS_DEBUG_Assert(context->rx);
|
||||
|
||||
switch (context->user_payload_type) {
|
||||
case OPAHRS_MSG_TYPE_USER_V0:
|
||||
opahrs_msg_v0_init_rx ((struct opahrs_msg_v0 *)context->rx);
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_USER_V1:
|
||||
opahrs_msg_v1_init_rx ((struct opahrs_msg_v1 *)context->rx);
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_LINK:
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* External API
|
||||
*
|
||||
*/
|
||||
|
||||
void lfsm_inject_event(enum lfsm_event event)
|
||||
{
|
||||
PIOS_IRQ_Disable();
|
||||
|
||||
/*
|
||||
* Move to the next state
|
||||
*
|
||||
* This is done prior to calling the new state's entry function to
|
||||
* guarantee that the entry function never depends on the previous
|
||||
* state. This way, it cannot ever know what the previous state was.
|
||||
*/
|
||||
context.curr_state = lfsm_transitions[context.curr_state].next_state[event];
|
||||
|
||||
/* Call the entry function (if any) for the next state. */
|
||||
if (lfsm_transitions[context.curr_state].entry_fn) {
|
||||
lfsm_transitions[context.curr_state].entry_fn(&context);
|
||||
}
|
||||
PIOS_IRQ_Enable();
|
||||
}
|
||||
|
||||
void lfsm_init(void)
|
||||
{
|
||||
context.curr_state = LFSM_STATE_STOPPED;
|
||||
go_stopped(&context);
|
||||
}
|
||||
|
||||
void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx)
|
||||
{
|
||||
PIOS_DEBUG_Assert(link_tx);
|
||||
|
||||
context.link_tx = (uint8_t *)link_tx;
|
||||
context.link_rx = (uint8_t *)link_rx;
|
||||
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0;
|
||||
context.user_payload_len = sizeof(*link_tx);
|
||||
|
||||
lfsm_update_link_tx_v0(link_tx, context.link_state, context.errors);
|
||||
|
||||
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
|
||||
}
|
||||
|
||||
void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx)
|
||||
{
|
||||
PIOS_DEBUG_Assert(link_tx);
|
||||
|
||||
context.link_tx = (uint8_t *)link_tx;
|
||||
context.link_rx = (uint8_t *)link_rx;
|
||||
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1;
|
||||
context.user_payload_len = sizeof(*link_tx);
|
||||
|
||||
lfsm_update_link_tx_v1(link_tx, context.link_state, context.errors);
|
||||
|
||||
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
|
||||
}
|
||||
|
||||
void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx)
|
||||
{
|
||||
PIOS_DEBUG_Assert(user_tx);
|
||||
|
||||
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
|
||||
context.user_tx = (uint8_t *)user_tx;
|
||||
|
||||
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
|
||||
}
|
||||
|
||||
void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx)
|
||||
{
|
||||
PIOS_DEBUG_Assert(user_rx);
|
||||
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
|
||||
|
||||
context.user_rx = (uint8_t *)user_rx;
|
||||
|
||||
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
|
||||
}
|
||||
|
||||
void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx)
|
||||
{
|
||||
PIOS_DEBUG_Assert(user_tx);
|
||||
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
|
||||
|
||||
context.user_tx = (uint8_t *)user_tx;
|
||||
|
||||
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
|
||||
}
|
||||
|
||||
void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx)
|
||||
{
|
||||
PIOS_DEBUG_Assert(user_rx);
|
||||
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
|
||||
|
||||
context.user_rx = (uint8_t *)user_rx;
|
||||
|
||||
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
|
||||
}
|
||||
|
||||
void lfsm_user_done (void)
|
||||
{
|
||||
lfsm_inject_event(LFSM_EVENT_USER_DONE);
|
||||
}
|
||||
|
||||
void lfsm_stop (void)
|
||||
{
|
||||
lfsm_inject_event(LFSM_EVENT_STOP);
|
||||
}
|
||||
|
||||
void lfsm_get_link_stats (struct lfsm_link_stats * stats)
|
||||
{
|
||||
PIOS_DEBUG_Assert(stats);
|
||||
|
||||
*stats = context.stats;
|
||||
}
|
||||
|
||||
enum lfsm_state lfsm_get_state (void)
|
||||
{
|
||||
return context.curr_state;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* ISR Callback
|
||||
*
|
||||
*/
|
||||
|
||||
void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val)
|
||||
{
|
||||
if (!crc_ok) {
|
||||
context.stats.rx_badcrc++;
|
||||
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!context.rx) {
|
||||
/* No way to know what we just received, assume invalid */
|
||||
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Recover the head and tail pointers from the message */
|
||||
struct opahrs_msg_link_head * head=0;
|
||||
struct opahrs_msg_link_tail * tail=0;
|
||||
|
||||
switch (context.user_payload_type) {
|
||||
case OPAHRS_MSG_TYPE_USER_V0:
|
||||
head = &((struct opahrs_msg_v0 *)context.rx)->head;
|
||||
tail = &((struct opahrs_msg_v0 *)context.rx)->tail;
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_USER_V1:
|
||||
head = &((struct opahrs_msg_v1 *)context.rx)->head;
|
||||
tail = &((struct opahrs_msg_v1 *)context.rx)->tail;
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_LINK:
|
||||
/* Should never be rx'ing before the link protocol version is known */
|
||||
PIOS_DEBUG_Assert(0);
|
||||
break;
|
||||
}
|
||||
|
||||
/* Check for bad magic */
|
||||
if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) ||
|
||||
(tail->magic != OPAHRS_MSG_MAGIC_TAIL)) {
|
||||
if (head->magic != OPAHRS_MSG_MAGIC_HEAD) {
|
||||
context.stats.rx_badmagic_head++;
|
||||
}
|
||||
if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) {
|
||||
context.stats.rx_badmagic_tail++;
|
||||
}
|
||||
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Good magic, find out what type of payload we've got */
|
||||
switch (head->type) {
|
||||
case OPAHRS_MSG_TYPE_LINK:
|
||||
context.stats.rx_link++;
|
||||
lfsm_inject_event (LFSM_EVENT_RX_LINK);
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_USER_V0:
|
||||
case OPAHRS_MSG_TYPE_USER_V1:
|
||||
if (head->type == context.user_payload_type) {
|
||||
context.stats.rx_user++;
|
||||
lfsm_inject_event (LFSM_EVENT_RX_USER);
|
||||
} else {
|
||||
/* Mismatched user payload type */
|
||||
context.stats.rx_badver++;
|
||||
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
/* Unidentifiable payload type */
|
||||
context.stats.rx_badtype++;
|
||||
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
|
||||
}
|
||||
}
|
59
flight/Bootloaders/INS/inc/ahrs_spi_program.h
Normal file
59
flight/Bootloaders/INS/inc/ahrs_spi_program.h
Normal file
@ -0,0 +1,59 @@
|
||||
#ifndef AHRS_SPI_PROGRAM_H
|
||||
#define AHRS_SPI_PROGRAM_H
|
||||
|
||||
/* Special packets to enter programming mode.
|
||||
Note: these must both be SPI_PROGRAM_REQUEST_LENGTH long.
|
||||
Pad with spaces if needed.
|
||||
*/
|
||||
#define SPI_PROGRAM_REQUEST "AHRS START PROGRAMMING "
|
||||
#define SPI_PROGRAM_ACK "AHRS PROGRAMMING STARTED"
|
||||
#define SPI_PROGRAM_REQUEST_LENGTH 24
|
||||
|
||||
/**Proposed programming protocol:
|
||||
In the master:
|
||||
1) Send a AhrsProgramPacket containing the relevant data.
|
||||
Note crc is a CRC32 as the CRC8 used in hardware can be fooled.
|
||||
2) Keep sending PROGRAM_NULL packets and wait for an ack.
|
||||
Time out if we waited too long.
|
||||
3) Compare ack packet with transmitted packet. The data
|
||||
should be the bitwise inverse of the data transmitted.
|
||||
packetId should correspond to the transmitted packet.
|
||||
4) repeat for next packet until finished
|
||||
5) Repeat using verify packets
|
||||
Returned data should be exactly as read from memory
|
||||
|
||||
In the slave:
|
||||
1) Wait for an AhrsProgramPacket
|
||||
2) Check CRC then write to memory
|
||||
3) Bitwise invert data and add it to the return packet
|
||||
4) Copy packetId from received packet
|
||||
5) Transmit packet.
|
||||
6) repeat until we receive a read packet
|
||||
7) read memory as requested until we receive a reboot packet
|
||||
Reboot packets had better have some sort of magic number in the data,
|
||||
just to be absolutely sure
|
||||
|
||||
*/
|
||||
|
||||
typedef enum { PROGRAM_NULL, PROGRAM_WRITE, PROGRAM_READ, PROGRAM_ACK, PROGRAM_REBOOT, PROGRAM_ERR} ProgramType;
|
||||
#define SPI_MAX_PROGRAM_DATA_SIZE (14 * 4) //USB comms uses 14x 32 bit words
|
||||
|
||||
#define REBOOT_CONFIRMATION "AHRS REBOOT"
|
||||
#define REBOOT_CONFIRMATION_LENGTH 11
|
||||
|
||||
/** Proposed program packet defintion
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
ProgramType type;
|
||||
uint32_t packetId; //Transmission packet ID
|
||||
uint32_t address; //base address to place data
|
||||
uint32_t size; //Size of data (0 to SPI_MAX_PROGRAM_DATA_SIZE)
|
||||
uint8_t data[SPI_MAX_PROGRAM_DATA_SIZE];
|
||||
uint32_t crc; //CRC32 - hardware CRC8 can be fooled
|
||||
uint8_t dummy; //for some reason comms trashes the last byte sent
|
||||
} AhrsProgramPacket;
|
||||
|
||||
uint32_t GenerateCRC (AhrsProgramPacket * packet);
|
||||
|
||||
#endif
|
63
flight/Bootloaders/INS/inc/ahrs_spi_program_master.h
Normal file
63
flight/Bootloaders/INS/inc/ahrs_spi_program_master.h
Normal file
@ -0,0 +1,63 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_spi_program_master.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief AHRS programming over SPI link - master(OpenPilot) end.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef AHRS_PROGRAM_MASTER_H
|
||||
#define AHRS_PROGRAM_MASTER_H
|
||||
|
||||
typedef enum {PROGRAM_ERR_OK, //OK
|
||||
PROGRAM_ERR_LINK, //comms error
|
||||
PROGRAM_ERR_FUNCTION, //function failed
|
||||
} PROGERR;
|
||||
|
||||
|
||||
/** Connect to AHRS and request programming mode
|
||||
* returns: false if failed.
|
||||
*/
|
||||
bool AhrsProgramConnect(uint32_t spi_id);
|
||||
|
||||
/** Write data to AHRS
|
||||
* size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE
|
||||
* returns: error status
|
||||
*/
|
||||
|
||||
PROGERR AhrsProgramWrite(uint32_t spi_id, uint32_t address, void * data, uint32_t size);
|
||||
|
||||
/** Read data from AHRS
|
||||
* size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE
|
||||
* returns: error status
|
||||
*/
|
||||
|
||||
PROGERR AhrsProgramRead(uint32_t spi_id, uint32_t address, void * data, uint32_t size);
|
||||
|
||||
/** reboot AHRS
|
||||
* returns: error status
|
||||
*/
|
||||
|
||||
PROGERR AhrsProgramReboot(uint32_t spi_id);
|
||||
|
||||
//TODO: Implement programming protocol
|
||||
|
||||
#endif //AHRS_PROGRAM_MASTER_H
|
35
flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h
Normal file
35
flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h
Normal file
@ -0,0 +1,35 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_spi_program_slave.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief AHRS programming over SPI link - slave(AHRS) end.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef AHRS_SPI_PROGRAM_SLAVE_H
|
||||
#define AHRS_SPI_PROGRAM_SLAVE_H
|
||||
|
||||
/** Check if OpenPilot is trying to program AHRS
|
||||
* If so, it will program the FLASH then return
|
||||
* If not it just returns.
|
||||
*/
|
||||
void AhrsProgramReceive(uint32_t spi_id);
|
||||
#endif //AHRS_PROGRAM_SLAVE_H
|
94
flight/Bootloaders/INS/inc/bl_fsm.h
Normal file
94
flight/Bootloaders/INS/inc/bl_fsm.h
Normal file
@ -0,0 +1,94 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_fsm.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BL_FSM_H
|
||||
#define BL_FSM_H
|
||||
|
||||
#include "pios_opahrs_proto.h"
|
||||
|
||||
enum lfsm_state {
|
||||
LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */
|
||||
LFSM_STATE_STOPPED,
|
||||
LFSM_STATE_STOPPING,
|
||||
LFSM_STATE_INACTIVE,
|
||||
LFSM_STATE_USER_BUSY,
|
||||
LFSM_STATE_USER_BUSY_RX_PENDING,
|
||||
LFSM_STATE_USER_BUSY_TX_PENDING,
|
||||
LFSM_STATE_USER_BUSY_RXTX_PENDING,
|
||||
LFSM_STATE_USER_RX_PENDING,
|
||||
LFSM_STATE_USER_TX_PENDING,
|
||||
LFSM_STATE_USER_RXTX_PENDING,
|
||||
LFSM_STATE_USER_RX_ACTIVE,
|
||||
LFSM_STATE_USER_TX_ACTIVE,
|
||||
LFSM_STATE_USER_RXTX_ACTIVE,
|
||||
|
||||
LFSM_STATE_NUM_STATES /* Must be last */
|
||||
};
|
||||
|
||||
enum lfsm_event {
|
||||
LFSM_EVENT_INIT_LINK,
|
||||
LFSM_EVENT_STOP,
|
||||
LFSM_EVENT_USER_SET_RX,
|
||||
LFSM_EVENT_USER_SET_TX,
|
||||
LFSM_EVENT_USER_DONE,
|
||||
LFSM_EVENT_RX_LINK,
|
||||
LFSM_EVENT_RX_USER,
|
||||
LFSM_EVENT_RX_UNKNOWN,
|
||||
|
||||
LFSM_EVENT_NUM_EVENTS /* Must be last */
|
||||
};
|
||||
|
||||
struct lfsm_link_stats {
|
||||
uint32_t rx_badcrc;
|
||||
uint32_t rx_badmagic_head;
|
||||
uint32_t rx_badmagic_tail;
|
||||
uint32_t rx_link;
|
||||
uint32_t rx_user;
|
||||
uint32_t tx_user;
|
||||
uint32_t rx_badtype;
|
||||
uint32_t rx_badver;
|
||||
};
|
||||
|
||||
extern void lfsm_attach(uint32_t spi_id);
|
||||
extern void lfsm_init(void);
|
||||
extern void lfsm_inject_event(enum lfsm_event event);
|
||||
|
||||
extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val);
|
||||
|
||||
extern void lfsm_get_link_stats (struct lfsm_link_stats * stats);
|
||||
extern enum lfsm_state lfsm_get_state (void);
|
||||
|
||||
extern void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx);
|
||||
extern void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx);
|
||||
extern void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx);
|
||||
|
||||
extern void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx);
|
||||
extern void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx);
|
||||
extern void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx);
|
||||
|
||||
extern void lfsm_user_done (void);
|
||||
|
||||
#endif /* BL_FSM_H */
|
53
flight/Bootloaders/INS/inc/ins_bl.h
Normal file
53
flight/Bootloaders/INS/inc/ins_bl.h
Normal file
@ -0,0 +1,53 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_bl.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Main AHRS_BL header.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef INS_BL_H
|
||||
#define INS_BL_H
|
||||
|
||||
|
||||
/* PIOS Includes */
|
||||
#include <pios.h>
|
||||
|
||||
/** Start programming
|
||||
returns: true if FLASH erased and ready to program
|
||||
*/
|
||||
bool StartProgramming(void);
|
||||
|
||||
/** Write a block to FLASH
|
||||
buffer contains the data to be written
|
||||
returns: true if FLASH programmed correctly
|
||||
*/
|
||||
bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size);
|
||||
|
||||
/** Read a block from FLASH
|
||||
returns: true if FLASH read correctly.
|
||||
Buffer is set to the read data
|
||||
*/
|
||||
bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size);
|
||||
|
||||
|
||||
|
||||
#endif /* AHRS_BL_H */
|
40
flight/Bootloaders/INS/inc/pios_config.h
Normal file
40
flight/Bootloaders/INS/inc/pios_config.h
Normal file
@ -0,0 +1,40 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief PiOS configuration header.
|
||||
* - Central compile time config for the project.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PIOS_CONFIG_H
|
||||
#define PIOS_CONFIG_H
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_LED
|
||||
#define PIOS_INCLUDE_SPI
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
277
flight/Bootloaders/INS/main.c
Normal file
277
flight/Bootloaders/INS/main.c
Normal file
@ -0,0 +1,277 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS BOOTLOADER
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_BOOTLOADER_Main
|
||||
* @brief Main function which does the hardware dependent stuff
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file main.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "ins_bl.h"
|
||||
#include "pios_opahrs_proto.h"
|
||||
#include "bl_fsm.h" /* lfsm_state */
|
||||
#include "stm32f10x_flash.h"
|
||||
|
||||
extern void PIOS_Board_Init(void);
|
||||
|
||||
#define NSS_HOLD_STATE ((GPIOB->IDR & GPIO_Pin_12) ? 0 : 1)
|
||||
enum bootloader_status boot_status;
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
typedef void
|
||||
(*pFunction)(void);
|
||||
pFunction Jump_To_Application;
|
||||
uint32_t JumpAddress;
|
||||
/* Function Prototypes */
|
||||
void
|
||||
process_spi_request(void);
|
||||
void
|
||||
jump_to_app();
|
||||
uint8_t jumpFW = FALSE;
|
||||
uint32_t Fw_crc;
|
||||
/**
|
||||
* @brief Bootloader Main function
|
||||
*/
|
||||
int main() {
|
||||
|
||||
uint8_t GO_dfu = false;
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
/* Enable Prefetch Buffer */
|
||||
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
|
||||
|
||||
/* Flash 2 wait state */
|
||||
FLASH_SetLatency(FLASH_Latency_2);
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
for (uint32_t t = 0; t < 10000000; ++t) {
|
||||
if (NSS_HOLD_STATE == 1)
|
||||
GO_dfu = TRUE;
|
||||
else {
|
||||
GO_dfu = FALSE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//while(TRUE)
|
||||
// {
|
||||
// PIOS_LED_Toggle(LED1);
|
||||
// PIOS_DELAY_WaitmS(1000);
|
||||
// }
|
||||
//GO_dfu = TRUE;
|
||||
PIOS_IAP_Init();
|
||||
GO_dfu = GO_dfu | PIOS_IAP_CheckRequest();// OR with app boot request
|
||||
if (GO_dfu == FALSE) {
|
||||
jump_to_app();
|
||||
}
|
||||
if(PIOS_IAP_CheckRequest())
|
||||
{
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
PIOS_IAP_ClearRequest();
|
||||
}
|
||||
PIOS_Board_Init();
|
||||
boot_status = idle;
|
||||
Fw_crc = crc_memory_calc();
|
||||
PIOS_LED_On(LED1);
|
||||
while (1) {
|
||||
process_spi_request();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct opahrs_msg_v0 link_tx_v0;
|
||||
static struct opahrs_msg_v0 link_rx_v0;
|
||||
static struct opahrs_msg_v0 user_tx_v0;
|
||||
static struct opahrs_msg_v0 user_rx_v0;
|
||||
void process_spi_request(void) {
|
||||
bool msg_to_process = FALSE;
|
||||
|
||||
PIOS_IRQ_Disable();
|
||||
/* Figure out if we're in an interesting stable state */
|
||||
switch (lfsm_get_state()) {
|
||||
case LFSM_STATE_USER_BUSY:
|
||||
msg_to_process = TRUE;
|
||||
break;
|
||||
case LFSM_STATE_INACTIVE:
|
||||
/* Queue up a receive buffer */
|
||||
lfsm_user_set_rx_v0(&user_rx_v0);
|
||||
lfsm_user_done();
|
||||
break;
|
||||
case LFSM_STATE_STOPPED:
|
||||
/* Get things going */
|
||||
lfsm_set_link_proto_v0(&link_tx_v0, &link_rx_v0);
|
||||
break;
|
||||
default:
|
||||
/* Not a stable state */
|
||||
break;
|
||||
}
|
||||
PIOS_IRQ_Enable();
|
||||
|
||||
if (!msg_to_process) {
|
||||
/* Nothing to do */
|
||||
//PIOS_COM_SendFormattedString(PIOS_COM_AUX, ".");
|
||||
return;
|
||||
}
|
||||
|
||||
if (user_rx_v0.tail.magic != OPAHRS_MSG_MAGIC_TAIL) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (user_rx_v0.payload.user.t) {
|
||||
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_VERIFY:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
Fw_crc = crc_memory_calc();
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
boot_status = idle;
|
||||
PIOS_LED_Off(LED1);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_RESET:
|
||||
PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.reset.reset_delay_in_ms);
|
||||
PIOS_SYS_Reset();
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_VERSIONS:
|
||||
//PIOS_LED_On(LED1);
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS);
|
||||
user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION;
|
||||
user_tx_v0.payload.user.v.rsp.versions.hw_version = (BOARD_TYPE << 8) | BOARD_REVISION;
|
||||
user_tx_v0.payload.user.v.rsp.versions.fw_crc = Fw_crc;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_MEM_MAP:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP);
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.density = HW_TYPE;
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE
|
||||
| (BOARD_WRITABLA << 1));
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory
|
||||
= SIZE_OF_CODE;
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.size_of_description
|
||||
= SIZE_OF_DESCRIPTION;
|
||||
user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code
|
||||
= START_OF_USER_CODE;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_SERIAL:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_SERIAL);
|
||||
PIOS_SYS_SerialNumberGet(
|
||||
(char *) &(user_tx_v0.payload.user.v.rsp.serial.serial_bcd));
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_STATUS:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_DATA:
|
||||
PIOS_LED_On(LED1);
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
if (!(user_rx_v0.payload.user.v.req.fwup_data.adress
|
||||
< START_OF_USER_CODE)) {
|
||||
for (uint8_t x = 0; x
|
||||
< user_rx_v0.payload.user.v.req.fwup_data.size; ++x) {
|
||||
if (FLASH_ProgramWord(
|
||||
(user_rx_v0.payload.user.v.req.fwup_data.adress
|
||||
+ ((uint32_t)(x * 4))),
|
||||
user_rx_v0.payload.user.v.req.fwup_data.data[x])
|
||||
!= FLASH_COMPLETE) {
|
||||
boot_status = write_error;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
boot_status = outside_dev_capabilities;
|
||||
}
|
||||
PIOS_LED_Off(LED1);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWDN_DATA:
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWDN_DATA);
|
||||
uint32_t adr=user_rx_v0.payload.user.v.req.fwdn_data.adress;
|
||||
for(uint8_t x=0;x<4;++x)
|
||||
{
|
||||
user_tx_v0.payload.user.v.rsp.fw_dn.data[x]=*FLASH_If_Read(adr+x);
|
||||
}
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_FWUP_START:
|
||||
FLASH_Unlock();
|
||||
opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS);
|
||||
user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status;
|
||||
lfsm_user_set_tx_v0(&user_tx_v0);
|
||||
PIOS_LED_On(LED1);
|
||||
if (FLASH_Start() == TRUE) {
|
||||
boot_status = started;
|
||||
PIOS_LED_Off(LED1);
|
||||
} else {
|
||||
boot_status = start_failed;
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
case OPAHRS_MSG_V0_REQ_BOOT:
|
||||
PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.boot.boot_delay_in_ms);
|
||||
FLASH_Lock();
|
||||
jump_to_app();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/* Finished processing the received message, requeue it */
|
||||
lfsm_user_set_rx_v0(&user_rx_v0);
|
||||
lfsm_user_done();
|
||||
return;
|
||||
}
|
||||
void jump_to_app() {
|
||||
//while(TRUE)
|
||||
//{
|
||||
// PIOS_LED_Toggle(LED1);
|
||||
// PIOS_DELAY_WaitmS(1000);
|
||||
//}
|
||||
PIOS_LED_On(LED1);
|
||||
if (((*(__IO uint32_t*) START_OF_USER_CODE) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
|
||||
FLASH_Lock();
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
|
||||
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
|
||||
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
|
||||
//_SetCNTR(0); // clear interrupt mask
|
||||
//_SetISTR(0); // clear all requests
|
||||
|
||||
JumpAddress = *(__IO uint32_t*) (START_OF_USER_CODE + 4);
|
||||
Jump_To_Application = (pFunction) JumpAddress;
|
||||
/* Initialize user application's Stack Pointer */
|
||||
__set_MSP(*(__IO uint32_t*) START_OF_USER_CODE);
|
||||
Jump_To_Application();
|
||||
} else {
|
||||
boot_status = jump_failed;
|
||||
return;
|
||||
}
|
||||
}
|
150
flight/Bootloaders/INS/pios_board.c
Normal file
150
flight/Bootloaders/INS/pios_board.c
Normal file
@ -0,0 +1,150 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @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 = {
|
||||
.handler = PIOS_SPI_op_irq_handler,
|
||||
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA1_Channel4,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA1_Channel5,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralDST,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
},
|
||||
.ssel = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_12,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_14,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
uint32_t pios_spi_op_id;
|
||||
void PIOS_SPI_op_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_SPI_IRQ_Handler(pios_spi_op_id);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_SPI */
|
||||
|
||||
#include "bl_fsm.h" /* lfsm_* */
|
||||
|
||||
void PIOS_Board_Init()
|
||||
{
|
||||
/* Set up the SPI interface to the OP board */
|
||||
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
lfsm_attach(pios_spi_op_id);
|
||||
lfsm_init();
|
||||
}
|
@ -94,15 +94,15 @@ OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
|
||||
## INS:
|
||||
SRC = ins.c
|
||||
SRC += pios_board.c
|
||||
#SRC += ins_timer.c
|
||||
#SRC += insgps13state.c
|
||||
SRC += ahrs_timer.c
|
||||
SRC += insgps13state.c
|
||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
#SRC += $(FLIGHTLIB)/ins_spi_comm.c
|
||||
#SRC += $(FLIGHTLIB)/ins_comm_objects.c
|
||||
#SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||
#SRC += $(BOOT)/ins_spi_program_slave.c
|
||||
#SRC += $(BOOT)/ins_slave_test.c
|
||||
#SRC += $(BOOT)/ins_spi_program.c
|
||||
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
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
SRC += $(PIOSSTM32F10X)/pios_sys.c
|
||||
@ -230,7 +230,7 @@ DEBUGF = dwarf-2
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
CDEFS += -DUSE_$(BOARD)
|
||||
CDEFS += -DIN_INS
|
||||
CDEFS += -DIN_AHRS
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
CDEFS += -DUSE_BOOTLOADER
|
||||
endif
|
||||
@ -280,7 +280,7 @@ CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
#CFLAGS += -Werror
|
||||
CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
61
flight/INS/ahrs_timer.c
Normal file
61
flight/INS/ahrs_timer.c
Normal file
@ -0,0 +1,61 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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;
|
||||
}
|
45
flight/INS/inc/ahrs_timer.h
Normal file
45
flight/INS/inc/ahrs_timer.h
Normal file
@ -0,0 +1,45 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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
|
94
flight/INS/inc/ins_fsm.h
Normal file
94
flight/INS/inc/ins_fsm.h
Normal file
@ -0,0 +1,94 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @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 */
|
93
flight/INS/inc/insgps.h
Normal file
93
flight/INS/inc/insgps.h
Normal file
@ -0,0 +1,93 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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_ */
|
@ -45,6 +45,7 @@
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_USART
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_COM_AUX
|
||||
#define PIOS_INCLUDE_GPS
|
||||
#define PIOS_INCLUDE_HMC5883
|
||||
#define PIOS_INCLUDE_BMP085
|
||||
|
910
flight/INS/ins.c
910
flight/INS/ins.c
@ -36,10 +36,63 @@
|
||||
/* OpenPilot Includes */
|
||||
#include "ins.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
|
||||
|
||||
volatile int8_t ahrs_algorithm;
|
||||
|
||||
/* 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 calibration_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*/
|
||||
static uint32_t iap_calc_crc(void);
|
||||
static void read_description(uint8_t *);
|
||||
void firmwareiapobj_callback(AhrsObjHandle obj);
|
||||
volatile uint8_t reset_count=0;
|
||||
|
||||
/**
|
||||
* @addtogroup INS_Global_Data INS Global Data
|
||||
@ -66,9 +119,9 @@ struct altitude_sensor altitude_data;
|
||||
struct gps_sensor gps_data;
|
||||
|
||||
//! Offset correction of barometric alt, to match gps data
|
||||
//static float baro_offset = 0;
|
||||
static float baro_offset = 0;
|
||||
|
||||
//static float mag_len = 0;
|
||||
static float mag_len = 0;
|
||||
|
||||
typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states;
|
||||
volatile int32_t ekf_too_slow;
|
||||
@ -79,93 +132,637 @@ volatile int32_t total_conversion_blocks;
|
||||
*/
|
||||
|
||||
/* INS functions */
|
||||
void blink(int led, int times)
|
||||
/**
|
||||
* @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values.
|
||||
*/
|
||||
void ins_outdoor_update()
|
||||
{
|
||||
for(int i=0; i<times; i++)
|
||||
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)
|
||||
{
|
||||
PIOS_LED_Toggle(led);
|
||||
PIOS_DELAY_WaitmS(250);
|
||||
PIOS_LED_Toggle(led);
|
||||
PIOS_DELAY_WaitmS(250);
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
void test_accel()
|
||||
/**
|
||||
* @brief Update the EKF when in indoor mode
|
||||
*/
|
||||
void ins_indoor_update()
|
||||
{
|
||||
if(PIOS_BMA180_Test())
|
||||
blink(LED1, 1);
|
||||
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
|
||||
blink(LED2, 1);
|
||||
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);
|
||||
}
|
||||
|
||||
#if defined (PIOS_INCLUDE_HMC5883)
|
||||
void test_mag()
|
||||
/**
|
||||
* @brief Initialize the EKF assuming stationary
|
||||
*/
|
||||
void ins_init_algorithm()
|
||||
{
|
||||
if(PIOS_HMC5883_Test())
|
||||
blink(LED1, 2);
|
||||
else
|
||||
blink(LED2, 2);
|
||||
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
|
||||
|
||||
#if defined (PIOS_INCLUDE_BMP085)
|
||||
void test_pressure()
|
||||
{
|
||||
if(PIOS_BMP085_Test())
|
||||
blink(LED1, 3);
|
||||
else
|
||||
blink(LED2, 3);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_IMU3000)
|
||||
void test_imu()
|
||||
{
|
||||
if(PIOS_IMU3000_Test())
|
||||
blink(LED1, 4);
|
||||
else
|
||||
blink(LED2, 4);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
extern void PIOS_Board_Init(void);
|
||||
struct pios_bma180_data bma180_data;
|
||||
|
||||
/**
|
||||
* @brief INS Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
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();
|
||||
|
||||
while (1)
|
||||
{
|
||||
test_accel();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
|
||||
#if defined (PIOS_INCLUDE_HMC5883)
|
||||
test_mag();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#if defined(PIOS_INCLUDE_HMC5883) && defined(PIOS_INCLUDE_I2C)
|
||||
// Get 3 ID bytes
|
||||
strcpy((char *)mag_data.id, "ZZZ");
|
||||
PIOS_HMC5883_ReadID(mag_data.id);
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_BMP085)
|
||||
test_pressure();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_IMU3000)
|
||||
test_imu();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
PIOS_DELAY_WaitmS(3000);
|
||||
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);
|
||||
AHRSSettingsConnectCallback(settings_callback);
|
||||
HomeLocationConnectCallback(homelocation_callback);
|
||||
FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
|
||||
|
||||
calibration_callback(AHRSCalibrationHandle()); //force an update
|
||||
|
||||
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()
|
||||
{
|
||||
// struct pios_bma180_data accel;
|
||||
// struct pios_imu3000_data gyro;
|
||||
|
||||
//PIOS_BMA180_Read(&accel);
|
||||
//PIOS_IMU3000_ReadGyros(&gyro);
|
||||
|
||||
accel_data.raw.x=0;
|
||||
accel_data.raw.y=0;
|
||||
accel_data.raw.z=-9;
|
||||
gyro_data.raw.x=0;
|
||||
gyro_data.raw.y=0;
|
||||
gyro_data.raw.z=0;
|
||||
accel_data.filtered.x = 0;
|
||||
accel_data.filtered.y = 0;
|
||||
accel_data.filtered.z = -9.8;
|
||||
gyro_data.filtered.x = 0;
|
||||
gyro_data.filtered.y = 0;
|
||||
gyro_data.filtered.z = 0;
|
||||
/*
|
||||
PIOS_BMA180_Read(&accel[1]);
|
||||
accel_data.raw.x=accel[1].x;
|
||||
accel_data.raw.y=accel[1].y;
|
||||
accel_data.raw.z=accel[1].z;
|
||||
PIOS_IMU3000_Read(&gyro[1]);
|
||||
gyro_data.raw.x=gyro[1].x;
|
||||
gyro_data.raw.y=gyro[1].y;
|
||||
gyro_data.raw.z=gyro[1].z;
|
||||
|
||||
accel_data.filtered.x = ((float)(accel[0].x + accel[1].x)) / 2;
|
||||
accel_data.filtered.y = ((float)(accel[0].y + accel[1].y)) / 2;
|
||||
accel_data.filtered.z = ((float)(accel[0].z + accel[1].z)) / 2;
|
||||
gyro_data.filtered.x = ((float)(gyro[0].x + gyro[1].x)) / 2;
|
||||
gyro_data.filtered.y = ((float)(gyro[0].y + gyro[1].y)) / 2;
|
||||
gyro_data.filtered.z = ((float)(gyro[0].z + gyro[1].z)) / 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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5883) && 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_HMC5883_NewDataAvailable()) {
|
||||
PIOS_HMC5883_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_HMC5883) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5883_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5883_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_HMC5883) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5883_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5883_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];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
@ -211,6 +808,201 @@ void reset_values()
|
||||
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 INS 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 settings_callback(AhrsObjHandle obj)
|
||||
{
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
ahrs_algorithm = settings.Algorithm;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
FirmwareIAPObjData firmwareIAPObj;
|
||||
FirmwareIAPObjGet(&firmwareIAPObj);
|
||||
if(firmwareIAPObj.ArmReset==0)
|
||||
reset_count=0;
|
||||
if(firmwareIAPObj.ArmReset==1)
|
||||
{
|
||||
|
||||
if((firmwareIAPObj.BoardType==BOARD_TYPE) || (firmwareIAPObj.BoardType==0xFF))
|
||||
{
|
||||
|
||||
++reset_count;
|
||||
if(reset_count>2)
|
||||
{
|
||||
PIOS_IAP_SetRequest1();
|
||||
PIOS_IAP_SetRequest2();
|
||||
PIOS_SYS_Reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(firmwareIAPObj.BoardType==BOARD_TYPE && firmwareIAPObj.crc!=iap_calc_crc())
|
||||
{
|
||||
read_description(firmwareIAPObj.Description);
|
||||
firmwareIAPObj.crc=iap_calc_crc();
|
||||
firmwareIAPObj.BoardRevision=BOARD_REVISION;
|
||||
FirmwareIAPObjSet(&firmwareIAPObj);
|
||||
}
|
||||
}
|
||||
|
||||
static uint32_t iap_calc_crc(void)
|
||||
{
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
|
||||
CRC_ResetDR();
|
||||
CRC_CalcBlockCRC((uint32_t *) START_OF_USER_CODE, (SIZE_OF_CODE) >> 2);
|
||||
return CRC_GetCRC();
|
||||
}
|
||||
|
||||
static uint8_t *FLASH_If_Read(uint32_t SectorAddress)
|
||||
{
|
||||
return (uint8_t *) (SectorAddress);
|
||||
}
|
||||
|
||||
static void read_description(uint8_t * array)
|
||||
{
|
||||
uint8_t x = 0;
|
||||
for (uint32_t i = START_OF_USER_CODE + SIZE_OF_CODE; i < START_OF_USER_CODE + SIZE_OF_CODE + SIZE_OF_DESCRIPTION; ++i) {
|
||||
array[x] = *FLASH_If_Read(i);
|
||||
++x;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
|
1644
flight/INS/insgps13state.c
Normal file
1644
flight/INS/insgps13state.c
Normal file
@ -0,0 +1,1644 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -41,10 +41,10 @@
|
||||
* NOTE: Leave this declared as const data so that it ends up in the
|
||||
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
|
||||
*/
|
||||
void PIOS_SPI_op_mag_irq_handler(void);
|
||||
void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler")));
|
||||
void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler")));
|
||||
static const struct pios_spi_cfg pios_spi_op_mag_cfg = {
|
||||
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,
|
||||
@ -61,7 +61,7 @@ static const struct pios_spi_cfg pios_spi_op_mag_cfg = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
|
||||
.irq = {
|
||||
.handler = PIOS_SPI_op_mag_irq_handler,
|
||||
.handler = PIOS_SPI_op_irq_handler,
|
||||
.flags =
|
||||
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
|
||||
DMA1_FLAG_GL4),
|
||||
@ -144,11 +144,11 @@ static const struct pios_spi_cfg pios_spi_op_mag_cfg = {
|
||||
},
|
||||
};
|
||||
|
||||
uint32_t pios_spi_op_mag_id;
|
||||
void PIOS_SPI_op_mag_irq_handler(void)
|
||||
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_mag_id);
|
||||
PIOS_SPI_IRQ_Handler(pios_spi_op_id);
|
||||
}
|
||||
|
||||
/* SPI1 Interface
|
||||
@ -318,53 +318,52 @@ void PIOS_USART_gps_irq_handler(void)
|
||||
|
||||
#endif /* PIOS_INCLUDE_GPS */
|
||||
|
||||
#ifdef PIOS_COM_AUX
|
||||
#ifdef PIOS_INCLUDE_COM_AUX
|
||||
/*
|
||||
* AUX USART
|
||||
*/
|
||||
void PIOS_USART_aux_irq_handler(void);
|
||||
void USART4_IRQHandler()
|
||||
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
|
||||
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler")));
|
||||
const struct pios_usart_cfg pios_usart_aux_cfg = {
|
||||
.regs = USART4,
|
||||
.init = {
|
||||
#if defined (PIOS_USART_BAUDRATE)
|
||||
.USART_BaudRate = PIOS_USART_BAUDRATE,
|
||||
#else
|
||||
.USART_BaudRate = 57600,
|
||||
#endif
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl =
|
||||
USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.handler = PIOS_USART_aux_irq_handler,
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
.regs = USART3,
|
||||
.remap = GPIO_PartialRemap_USART3,
|
||||
.init = {
|
||||
#if defined (PIOS_COM_AUX_BAUDRATE)
|
||||
.USART_BaudRate = PIOS_COM_AUX_BAUDRATE,
|
||||
#else
|
||||
.USART_BaudRate = 57600,
|
||||
#endif
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.handler = PIOS_USART_aux_irq_handler,
|
||||
.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,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static uint32_t pios_usart_aux_id;
|
||||
@ -373,7 +372,7 @@ void PIOS_USART_aux_irq_handler(void)
|
||||
PIOS_USART_IRQ_Handler(pios_usart_aux_id);
|
||||
}
|
||||
|
||||
#endif /* PIOS_COM_AUX */
|
||||
#endif /* PIOS_INCLUDE_COM_AUX */
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
@ -554,6 +553,15 @@ void PIOS_Board_Init(void) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_GPS */
|
||||
|
||||
#if defined(PIOS_INCLUDE_COM_AUX)
|
||||
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)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_COM_AUX */
|
||||
#endif /* PIOS_INCLUDE_COM */
|
||||
|
||||
#if defined (PIOS_INCLUDE_I2C)
|
||||
@ -583,15 +591,15 @@ void PIOS_Board_Init(void) {
|
||||
|
||||
PIOS_BMA180_Attach(pios_spi_accel_id);
|
||||
|
||||
// #include "ahrs_spi_comm.h"
|
||||
// InsInitComms();
|
||||
//
|
||||
// /* Set up the SPI interface to the OP board */
|
||||
// if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
|
||||
// PIOS_DEBUG_Assert(0);
|
||||
// }
|
||||
//
|
||||
// InsConnect(pios_spi_op_id);
|
||||
|
||||
/* Set up the SPI interface to the OP board */
|
||||
#include "ahrs_spi_comm.h"
|
||||
AhrsInitComms();
|
||||
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
AhrsConnect(pios_spi_op_id);
|
||||
#endif /* PIOS_INCLUDE_SPI */
|
||||
}
|
||||
|
||||
|
219
flight/INS/test.c
Normal file
219
flight/INS/test.c
Normal file
@ -0,0 +1,219 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup INS INS
|
||||
|
||||
* @brief The INS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup INS_Main
|
||||
* @brief Main function which does the hardware dependent stuff
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ins.c
|
||||
* @author David "Buzz" Carlson (buzz@chebuzz.com)
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief INSGPS Test Program
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "ins.h"
|
||||
#include "pios.h"
|
||||
#include "ahrs_spi_comm.h"
|
||||
#include <stdbool.h>
|
||||
#include "fifo_buffer.h"
|
||||
|
||||
void reset_values();
|
||||
|
||||
/**
|
||||
* @addtogroup INS_Global_Data INS Global Data
|
||||
* @{
|
||||
* Public data. Used by both EKF and the sender
|
||||
*/
|
||||
|
||||
//! Contains the data from the mag sensor chip
|
||||
struct mag_sensor mag_data;
|
||||
|
||||
//! Contains the data from the accelerometer
|
||||
struct accel_sensor accel_data;
|
||||
|
||||
//! Contains the data from the gyro
|
||||
struct gyro_sensor gyro_data;
|
||||
|
||||
//! Conains the current estimate of the attitude
|
||||
struct attitude_solution attitude_data;
|
||||
|
||||
//! Contains data from the altitude sensor
|
||||
struct altitude_sensor altitude_data;
|
||||
|
||||
//! Contains data from the GPS (via the SPI link)
|
||||
struct gps_sensor gps_data;
|
||||
|
||||
//! Offset correction of barometric alt, to match gps data
|
||||
//static float baro_offset = 0;
|
||||
|
||||
//static float mag_len = 0;
|
||||
|
||||
typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states;
|
||||
volatile int32_t ekf_too_slow;
|
||||
volatile int32_t total_conversion_blocks;
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* INS functions */
|
||||
void blink(int led, int times)
|
||||
{
|
||||
for(int i=0; i<times; i++)
|
||||
{
|
||||
PIOS_LED_Toggle(led);
|
||||
PIOS_DELAY_WaitmS(250);
|
||||
PIOS_LED_Toggle(led);
|
||||
PIOS_DELAY_WaitmS(250);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void test_accel()
|
||||
{
|
||||
if(PIOS_BMA180_Test())
|
||||
blink(LED1, 1);
|
||||
else
|
||||
blink(LED2, 1);
|
||||
}
|
||||
|
||||
#if defined (PIOS_INCLUDE_HMC5883)
|
||||
void test_mag()
|
||||
{
|
||||
if(PIOS_HMC5883_Test())
|
||||
blink(LED1, 2);
|
||||
else
|
||||
blink(LED2, 2);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_BMP085)
|
||||
void test_pressure()
|
||||
{
|
||||
if(PIOS_BMP085_Test())
|
||||
blink(LED1, 3);
|
||||
else
|
||||
blink(LED2, 3);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_IMU3000)
|
||||
void test_imu()
|
||||
{
|
||||
if(PIOS_IMU3000_Test())
|
||||
blink(LED1, 4);
|
||||
else
|
||||
blink(LED2, 4);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
extern void PIOS_Board_Init(void);
|
||||
struct pios_bma180_data bma180_data;
|
||||
|
||||
/**
|
||||
* @brief INS Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
reset_values();
|
||||
|
||||
PIOS_Board_Init();
|
||||
|
||||
while (1)
|
||||
{
|
||||
test_accel();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
|
||||
#if defined (PIOS_INCLUDE_HMC5883)
|
||||
test_mag();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_BMP085)
|
||||
test_pressure();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
|
||||
#if defined (PIOS_INCLUDE_IMU3000)
|
||||
test_imu();
|
||||
PIOS_DELAY_WaitmS(1000);
|
||||
#endif
|
||||
PIOS_DELAY_WaitmS(3000);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Populate fields with initial values
|
||||
*/
|
||||
void reset_values()
|
||||
{
|
||||
accel_data.calibration.scale[0][1] = 0;
|
||||
accel_data.calibration.scale[1][0] = 0;
|
||||
accel_data.calibration.scale[0][2] = 0;
|
||||
accel_data.calibration.scale[2][0] = 0;
|
||||
accel_data.calibration.scale[1][2] = 0;
|
||||
accel_data.calibration.scale[2][1] = 0;
|
||||
|
||||
accel_data.calibration.scale[0][0] = 0.0359;
|
||||
accel_data.calibration.scale[1][1] = 0.0359;
|
||||
accel_data.calibration.scale[2][2] = 0.0359;
|
||||
accel_data.calibration.scale[0][3] = -73.5;
|
||||
accel_data.calibration.scale[1][3] = -73.5;
|
||||
accel_data.calibration.scale[2][3] = -73.5;
|
||||
|
||||
accel_data.calibration.variance[0] = 1e-4;
|
||||
accel_data.calibration.variance[1] = 1e-4;
|
||||
accel_data.calibration.variance[2] = 1e-4;
|
||||
|
||||
gyro_data.calibration.scale[0] = -0.014;
|
||||
gyro_data.calibration.scale[1] = 0.014;
|
||||
gyro_data.calibration.scale[2] = -0.014;
|
||||
gyro_data.calibration.bias[0] = -24;
|
||||
gyro_data.calibration.bias[1] = -24;
|
||||
gyro_data.calibration.bias[2] = -24;
|
||||
gyro_data.calibration.variance[0] = 1;
|
||||
gyro_data.calibration.variance[1] = 1;
|
||||
gyro_data.calibration.variance[2] = 1;
|
||||
mag_data.calibration.scale[0] = 1;
|
||||
mag_data.calibration.scale[1] = 1;
|
||||
mag_data.calibration.scale[2] = 1;
|
||||
mag_data.calibration.bias[0] = 0;
|
||||
mag_data.calibration.bias[1] = 0;
|
||||
mag_data.calibration.bias[2] = 0;
|
||||
mag_data.calibration.variance[0] = 50;
|
||||
mag_data.calibration.variance[1] = 50;
|
||||
mag_data.calibration.variance[2] = 50;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
@ -33,7 +33,7 @@
|
||||
#include "pios_spi.h"
|
||||
#include "pios_irq.h"
|
||||
#include "ahrs_spi_program_slave.h"
|
||||
#include "STM32103CB_AHRS.h"
|
||||
//#include "STM32103CB_AHRS.h"
|
||||
#endif
|
||||
|
||||
/*transmit and receive packet magic numbers.
|
||||
|
@ -91,6 +91,8 @@ TIM8 | | | |
|
||||
#define BOARD_READABLE TRUE
|
||||
#define BOARD_WRITABLA TRUE
|
||||
#define MAX_DEL_RETRYS 3
|
||||
#define EKF_RATE 75
|
||||
|
||||
|
||||
//------------------------
|
||||
// PIOS_LED
|
||||
@ -158,12 +160,9 @@ extern uint32_t pios_i2c_gyro_adapter_id;
|
||||
extern uint32_t pios_com_gps_id;
|
||||
#define PIOS_COM_GPS (pios_com_gps_id)
|
||||
|
||||
#ifdef PIOS_ENABLE_AUX_UART
|
||||
#define PIOS_COM_AUX_BAUDRATE 57600
|
||||
extern uint32_t pios_com_aux_id;
|
||||
#define PIOS_COM_AUX (pios_com_aux_id)
|
||||
#define PIOS_COM_DEBUG PIOS_COM_AUX
|
||||
#endif
|
||||
|
||||
//-------------------------
|
||||
// Delay Timer
|
||||
|
@ -130,10 +130,11 @@ static void PIOS_IMU3000_Config(PIOS_IMU3000_ConfigTypeDef * IMU3000_Config_Stru
|
||||
/**
|
||||
* @brief Read current X, Z, Y values (in that order)
|
||||
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
|
||||
* \return none
|
||||
* \returns The number of samples remaining in the fifo
|
||||
*/
|
||||
void PIOS_IMU3000_ReadGyros(int16_t out[3])
|
||||
uint8_t PIOS_IMU3000_ReadGyros(struct pios_imu3000_data * data)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -220,7 +221,7 @@ static bool PIOS_IMU3000_Write(uint8_t address, uint8_t buffer)
|
||||
uint8_t PIOS_IMU3000_Test(void)
|
||||
{
|
||||
/* Verify that ID matches (IMU3000 ID is 0x69) */
|
||||
return (0x69 ^ PIOS_IMU3000_ReadID() );
|
||||
return ( !(0x69 ^ PIOS_IMU3000_ReadID()) );
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -42,32 +42,33 @@ void EXTI15_10_IRQHandler(void)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
|
||||
#endif
|
||||
#endif /* PIOS_INCLUDE_FREERTOS */
|
||||
|
||||
#if defined(PIOS_INCLUDE_BMP085)
|
||||
if (EXTI_GetITStatus(PIOS_BMP085_EOC_EXTI_LINE) != RESET) {
|
||||
/* Read the ADC Value */
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
xSemaphoreGiveFromISR(PIOS_BMP085_EOC, &xHigherPriorityTaskWoken);
|
||||
#else
|
||||
#else
|
||||
PIOS_BMP085_EOC=1;
|
||||
#endif
|
||||
#endif /* PIOS_INCLUDE_FREERTOS */
|
||||
|
||||
/* Clear the EXTI line pending bit */
|
||||
EXTI_ClearITPendingBit(PIOS_BMP085_EOC_EXTI_LINE);
|
||||
}
|
||||
#endif
|
||||
#endif /* PIOS_INCLUDE_BMP085 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
/* Yield From ISR if needed */
|
||||
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
||||
#endif
|
||||
#endif /* PIOS_INCLUDE_FREERTOS */
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle external lines 9 to 5 interrupt requests
|
||||
*/
|
||||
extern void PIOS_HMC5843_IRQHandler(void);
|
||||
extern void PIOS_HMC5883_IRQHandler(void);
|
||||
void EXTI9_5_IRQHandler(void)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_HMC5843)
|
||||
@ -75,7 +76,12 @@ void EXTI9_5_IRQHandler(void)
|
||||
PIOS_HMC5843_IRQHandler();
|
||||
EXTI_ClearITPendingBit(PIOS_HMC5843_DRDY_EXTI_LINE);
|
||||
}
|
||||
#endif
|
||||
#elif defined(PIOS_INCLUDE_HMC5883)
|
||||
if (EXTI_GetITStatus(PIOS_HMC5883_DRDY_EXTI_LINE) != RESET) {
|
||||
PIOS_HMC5883_IRQHandler();
|
||||
EXTI_ClearITPendingBit(PIOS_HMC5883_DRDY_EXTI_LINE);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_HMC5843 */
|
||||
}
|
||||
|
||||
/**
|
||||
@ -89,8 +95,20 @@ void EXTI4_IRQHandler(void)
|
||||
EXTI_ClearITPendingBit(PIOS_USB_DETECT_EXTI_LINE);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
#endif /* PIOS_INCLUDE_USB */
|
||||
|
||||
extern void PIOS_IMU3000_IRQHandler();
|
||||
void EXTI1_IRQHandler(void)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_IMU3000)
|
||||
if (EXTI_GetITStatus(PIOS_IMU3000_INT_EXTI_LINE) != RESET)
|
||||
{
|
||||
PIOS_IMU3000_IRQHandler();
|
||||
EXTI_ClearITPendingBit(PIOS_IMU3000_INT_EXTI_LINE);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_IMU3000 */
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_EXTI */
|
||||
|
||||
/**
|
||||
* @}
|
||||
|
@ -110,11 +110,16 @@
|
||||
#define PIOS_IMU3000_PWRMGMT_PLL_Z_CLK 0X03
|
||||
#define PIOS_IMU3000_PWRMGMT_STOP_CLK 0X07
|
||||
|
||||
struct pios_imu3000_data {
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
};
|
||||
|
||||
/* Public Functions */
|
||||
extern void PIOS_IMU3000_Init(void);
|
||||
extern bool PIOS_IMU3000_NewDataAvailable(void);
|
||||
extern void PIOS_IMU3000_ReadGyros(int16_t out[3]);
|
||||
extern uint8_t PIOS_IMU3000_ReadGyros(struct pios_imu3000_data * data);
|
||||
extern uint8_t PIOS_IMU3000_ReadID();
|
||||
extern uint8_t PIOS_IMU3000_Test();
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user