diff --git a/flight/Bootloaders/INS/Makefile b/flight/Bootloaders/INS/Makefile new file mode 100644 index 000000000..195680176 --- /dev/null +++ b/flight/Bootloaders/INS/Makefile @@ -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 diff --git a/flight/Bootloaders/INS/ahrs_slave_test.c b/flight/Bootloaders/INS/ahrs_slave_test.c new file mode 100644 index 000000000..9c2759c82 --- /dev/null +++ b/flight/Bootloaders/INS/ahrs_slave_test.c @@ -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); +} + + diff --git a/flight/Bootloaders/INS/ahrs_spi_program.c b/flight/Bootloaders/INS/ahrs_spi_program.c new file mode 100644 index 000000000..22d48b1a8 --- /dev/null +++ b/flight/Bootloaders/INS/ahrs_spi_program.c @@ -0,0 +1,89 @@ +#include +#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); +} diff --git a/flight/Bootloaders/INS/ahrs_spi_program_master.c b/flight/Bootloaders/INS/ahrs_spi_program_master.c new file mode 100644 index 000000000..561c4435a --- /dev/null +++ b/flight/Bootloaders/INS/ahrs_spi_program_master.c @@ -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); +} + + + + diff --git a/flight/Bootloaders/INS/ahrs_spi_program_slave.c b/flight/Bootloaders/INS/ahrs_spi_program_slave.c new file mode 100644 index 000000000..a7d011448 --- /dev/null +++ b/flight/Bootloaders/INS/ahrs_spi_program_slave.c @@ -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 +#include + +#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; + } +} diff --git a/flight/Bootloaders/INS/bl_fsm.c b/flight/Bootloaders/INS/bl_fsm.c new file mode 100644 index 000000000..48126037e --- /dev/null +++ b/flight/Bootloaders/INS/bl_fsm.c @@ -0,0 +1,626 @@ +#include /* uint*_t */ +#include /* 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); + } +} diff --git a/flight/Bootloaders/INS/inc/ahrs_spi_program.h b/flight/Bootloaders/INS/inc/ahrs_spi_program.h new file mode 100644 index 000000000..dcb285416 --- /dev/null +++ b/flight/Bootloaders/INS/inc/ahrs_spi_program.h @@ -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 diff --git a/flight/Bootloaders/INS/inc/ahrs_spi_program_master.h b/flight/Bootloaders/INS/inc/ahrs_spi_program_master.h new file mode 100644 index 000000000..2e93b805c --- /dev/null +++ b/flight/Bootloaders/INS/inc/ahrs_spi_program_master.h @@ -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 diff --git a/flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h b/flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h new file mode 100644 index 000000000..d13b44334 --- /dev/null +++ b/flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h @@ -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 diff --git a/flight/Bootloaders/INS/inc/bl_fsm.h b/flight/Bootloaders/INS/inc/bl_fsm.h new file mode 100644 index 000000000..7ab3369e7 --- /dev/null +++ b/flight/Bootloaders/INS/inc/bl_fsm.h @@ -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 */ diff --git a/flight/Bootloaders/INS/inc/ins_bl.h b/flight/Bootloaders/INS/inc/ins_bl.h new file mode 100644 index 000000000..d96ba3bf6 --- /dev/null +++ b/flight/Bootloaders/INS/inc/ins_bl.h @@ -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 + +/** 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 */ diff --git a/flight/Bootloaders/INS/inc/pios_config.h b/flight/Bootloaders/INS/inc/pios_config.h new file mode 100644 index 000000000..601701c1e --- /dev/null +++ b/flight/Bootloaders/INS/inc/pios_config.h @@ -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 */ diff --git a/flight/Bootloaders/INS/main.c b/flight/Bootloaders/INS/main.c new file mode 100644 index 000000000..46d78a631 --- /dev/null +++ b/flight/Bootloaders/INS/main.c @@ -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; + } +} diff --git a/flight/Bootloaders/INS/pios_board.c b/flight/Bootloaders/INS/pios_board.c new file mode 100644 index 000000000..0b1cd6fd0 --- /dev/null +++ b/flight/Bootloaders/INS/pios_board.c @@ -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 + +#if defined(PIOS_INCLUDE_SPI) + +#include + + +/* 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(); +} diff --git a/flight/INS/Makefile b/flight/INS/Makefile index 8e0423a45..d255930ec 100644 --- a/flight/INS/Makefile +++ b/flight/INS/Makefile @@ -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 diff --git a/flight/INS/ahrs_timer.c b/flight/INS/ahrs_timer.c new file mode 100644 index 000000000..d30a6c9f6 --- /dev/null +++ b/flight/INS/ahrs_timer.c @@ -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; +} diff --git a/flight/INS/inc/ahrs_timer.h b/flight/INS/inc/ahrs_timer.h new file mode 100644 index 000000000..aaddf5769 --- /dev/null +++ b/flight/INS/inc/ahrs_timer.h @@ -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 + +#define TIMER_RATE (8e6 / 128) + +void timer_start(); +uint32_t timer_count(); +uint32_t timer_rate(); + +#endif diff --git a/flight/INS/inc/ins_fsm.h b/flight/INS/inc/ins_fsm.h new file mode 100644 index 000000000..dd9d24b21 --- /dev/null +++ b/flight/INS/inc/ins_fsm.h @@ -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 */ diff --git a/flight/INS/inc/insgps.h b/flight/INS/inc/insgps.h new file mode 100644 index 000000000..bf4faae4b --- /dev/null +++ b/flight/INS/inc/insgps.h @@ -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_ */ diff --git a/flight/INS/inc/pios_config.h b/flight/INS/inc/pios_config.h index 41e7b4f44..ebe29398b 100644 --- a/flight/INS/inc/pios_config.h +++ b/flight/INS/inc/pios_config.h @@ -45,6 +45,7 @@ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_USART #define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_AUX #define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_HMC5883 #define PIOS_INCLUDE_BMP085 diff --git a/flight/INS/ins.c b/flight/INS/ins.c index 92224a239..f54f59539 100644 --- a/flight/INS/ins.c +++ b/flight/INS/ins.c @@ -36,10 +36,63 @@ /* OpenPilot Includes */ #include "ins.h" #include "pios.h" +#include "ahrs_timer.h" +#include "ahrs_spi_comm.h" +#include "insgps.h" +#include "CoordinateConversions.h" #include #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 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; + } +} /** * @} diff --git a/flight/INS/insgps13state.c b/flight/INS/insgps13state.c new file mode 100644 index 000000000..24ffab1db --- /dev/null +++ b/flight/INS/insgps13state.c @@ -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 +#include + +// 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 +#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