rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

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.

Browse Source
This commit is contained in:
David Buzz Carlson 2011-04-20 03:37:50 -07:00
parent 48a0881dc4
commit 876ca3044c
29 changed files with 5375 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

446
flight/Bootloaders/INS/Makefile Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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();
}

20
flight/INS/Makefile
View File

@ -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
View 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
View 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
View 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
View 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_ */

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

@ -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
View File

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

128
flight/INS/pios_board.c
View File

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

2
flight/Libraries/ahrs_spi_comm.c
View File

@ -33,7 +33,7 @@
#include "pios_spi.h"
#include "pios_irq.h"
#include "ahrs_spi_program_slave.h"
#include "STM32103CB_AHRS.h"
//#include "STM32103CB_AHRS.h"
#endif
/*transmit and receive packet magic numbers.

5
flight/PiOS/Boards/STM3210E_INS.h
View File

@ -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

7
flight/PiOS/Common/pios_imu3000.c
View File

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

36
flight/PiOS/STM32F10x/pios_exti.c
View File

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

7
flight/PiOS/inc/pios_imu3000.h
View File

@ -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();