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

Remove references to deprecated targets

Browse Source
This commit is contained in:
James Cotton 2012-01-04 19:39:31 -06:00
parent cce351bb9a
commit d5eb38a065
36 changed files with 1 additions and 10685 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -432,7 +432,7 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bl_$(1), $$(BL_TARGETS)))
all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
endef
ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins revolution
ALL_BOARDS := coptercontrol pipxtreme ins revolution
# Friendly names of each board (used to find source tree)
openpilot_friendly := OpenPilot

422
flight/AHRS/Makefile
View File

@ -1,422 +0,0 @@
#####
# Project: OpenPilot AHRS
#
#
# Makefile for OpenPilot AHRS project
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := fw_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
else
REMOVE_CMD = rm
endif
FLASH_TOOL = OPENOCD
# Paths
AHRS = ./
AHRSINC = $(AHRS)/inc
PIOS = ../PiOS
PIOSINC = $(PIOS)/inc
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = ../Libraries/inc
PIOSSTM32F10X = $(PIOS)/STM32F10x
PIOSCOMMON = $(PIOS)/Common
PIOSBOARDS = $(PIOS)/Boards
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
STMLIBDIR = $(APPLIBDIR)
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
STMSPDSRCDIR = $(STMSPDDIR)/src
STMSPDINCDIR = $(STMSPDDIR)/inc
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
OPDIR = ../OpenPilot
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPSYSINC = $(OPDIR)/System/inc
BOOT = ../Bootloaders/AHRS
BOOTINC = $(BOOT)/inc
OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
## AHRS:
SRC = ahrs.c
SRC += pios_board.c
SRC += ahrs_timer.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
SRC += $(BOOT)/ahrs_spi_program_slave.c
SRC += $(BOOT)/ahrs_slave_test.c
SRC += $(BOOT)/ahrs_spi_program.c
SRC += insgps13state.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_adc.c
SRC += $(PIOSSTM32F10X)/pios_sys.c
SRC += $(PIOSSTM32F10X)/pios_led.c
SRC += $(PIOSSTM32F10X)/pios_delay.c
SRC += $(PIOSSTM32F10X)/pios_usart.c
SRC += $(PIOSSTM32F10X)/pios_irq.c
SRC += $(PIOSSTM32F10X)/pios_i2c.c
SRC += $(PIOSSTM32F10X)/pios_gpio.c
SRC += $(PIOSSTM32F10X)/pios_spi.c
SRC += $(PIOSSTM32F10X)/pios_exti.c
SRC += $(PIOSSTM32F10X)/pios_iap.c
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_hmc5843.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c
SRC += $(CMSISDIR)/system_stm32f10x.c
## Used parts of the STM-Library
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
SRC += $(STMSPDSRCDIR)/misc.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
# List Assembler source files here which must be assembled in ARM-Mode..
ASRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSSTM32F10X)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)
EXTRAINCDIRS += $(STMSPDINCDIR)
EXTRAINCDIRS += $(CMSISDIR)
EXTRAINCDIRS += $(AHRSINC)
EXTRAINCDIRS += $(OPUAVSYNTHDIR)
EXTRAINCDIRS += $(BOOTINC)
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
CFLAGS += -O0
CFLAGS += -DGENERAL_COV
else
CFLAGS += -Os
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
CDEFS = -DSTM32F10X_$(MODEL)
CDEFS += -DUSE_STDPERIPH_DRIVER
CDEFS += -DUSE_$(BOARD)
CDEFS += -DIN_AHRS
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
CFLAGS += -g$(DEBUGF)
CFLAGS += -ffast-math
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
CFLAGS += -Werror
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
# Set linker-script name depending on selected submodel name
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
# Define programs and commands.
REMOVE = $(REMOVE_CMD) -f
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
# Default target.
all: gccversion build
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin lss sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin lss sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
# Compile: create assembler files from C source files. ARM only
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
# Add jtag targets (program and wipe)
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
.PHONY: elf lss sym hex bin bino opfw
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
bino: $(OUTDIR)/$(TARGET).bin.o
opfw: $(OUTDIR)/$(TARGET).opfw
# Display sizes of sections.
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Install: install binary file with prefix/suffix into install directory
install: $(OUTDIR)/$(TARGET).opfw
ifneq ($(INSTALL_DIR),)
@echo $(MSG_INSTALLING) $(call toprel, $<)
$(V1) mkdir -p $(INSTALL_DIR)
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
else
$(error INSTALL_DIR must be specified for $@)
endif
# Target: clean project.
clean: clean_list
clean_list :
@echo $(MSG_CLEANING)
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
$(V1) $(REMOVE) $(ALLOBJ)
$(V1) $(REMOVE) $(LSTFILES)
$(V1) $(REMOVE) $(DEPFILES)
$(V1) $(REMOVE) $(SRC:.c=.s)
$(V1) $(REMOVE) $(SRCARM:.c=.s)
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
else
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all build clean clean_list install

1265
flight/AHRS/ahrs.c
View File

@ -1,1265 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_Main
* @brief Main function which does the hardware dependent stuff
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "ahrs.h"
#include <pios_board_info.h>
#include "pios.h"
#include "ahrs_timer.h"
#include "ahrs_spi_comm.h"
#include "insgps.h"
#include "CoordinateConversions.h"
#include <stdbool.h>
#include "fifo_buffer.h"
#define DEG_TO_RAD (M_PI / 180.0)
#define RAD_TO_DEG (180.0 / M_PI)
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */
#define INSGPS_MAGLEN 1000
#define INSGPS_MAGTOL 0.5 /* error in magnetic vector length to use */
#define GYRO_OOB(x) ((x > (1000 * DEG_TO_RAD)) || (x < (-1000 * DEG_TO_RAD)))
#define ACCEL_OOB(x) (((x > 12*9.81) || (x < -12*9.81)))
#define ISNAN(x) (x != x)
// down-sampled data index
#define ACCEL_RAW_X_IDX 2
#define ACCEL_RAW_Y_IDX 0
#define ACCEL_RAW_Z_IDX 4
#define GYRO_RAW_X_IDX 1
#define GYRO_RAW_Y_IDX 3
#define GYRO_RAW_Z_IDX 5
#define GYRO_TEMP_RAW_XY_IDX 6
#define GYRO_TEMP_RAW_Z_IDX 7
#define MAG_RAW_X_IDX 1
#define MAG_RAW_Y_IDX 0
#define MAG_RAW_Z_IDX 2
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_EKF
//#define PIP_DUMP_RAW
volatile int8_t ahrs_algorithm;
/* INS functions */
void ins_outdoor_update();
void ins_indoor_update();
void simple_update();
/* Data accessors */
void adc_callback(float *);
bool get_accel_gyro_data();
void process_mag_data();
void reset_values();
void calibrate_sensors(void);
/* Communication functions */
void send_calibration(void);
void send_attitude(void);
void send_velocity(void);
void send_position(void);
void homelocation_callback(AhrsObjHandle obj);
void altitude_callback(AhrsObjHandle obj);
void calibration_callback(AhrsObjHandle obj);
void gps_callback(AhrsObjHandle obj);
void settings_callback(AhrsObjHandle obj);
void affine_rotate(float scale[3][4], float rotation[3]);
void calibration(float result[3], float scale[3][4], float arg[3]);
/* Bootloader related functions and var*/
void firmwareiapobj_callback(AhrsObjHandle obj);
volatile uint8_t reset_count=0;
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
* @{
* Public data. Used by both EKF and the sender
*/
//! Contains the data from the mag sensor chip
struct mag_sensor mag_data;
//! Contains the data from the accelerometer
struct accel_sensor accel_data;
//! Contains the data from the gyro
struct gyro_sensor gyro_data;
//! Conains the current estimate of the attitude
struct attitude_solution attitude_data;
//! Contains data from the altitude sensor
struct altitude_sensor altitude_data;
//! Contains data from the GPS (via the SPI link)
struct gps_sensor gps_data;
//! The oversampling rate, ekf is 2k / this
uint8_t adc_oversampling = 15;
//! Offset correction of barometric alt, to match gps data
static float baro_offset = 0;
static float mag_len = 0;
typedef enum { AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING } states;
volatile int32_t ekf_too_slow;
volatile int32_t total_conversion_blocks;
//! Buffer to allow ADC to run a bit faster than EKF
t_fifo_buffer adc_fifo_buffer;
/**
* @}
*/
/* INS functions */
/**
* @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values.
*/
void ins_outdoor_update()
{
float gyro[3], accel[3], vel[3];
static uint32_t last_gps_time = 0;
uint16_t sensors;
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3];
send_attitude(); // get message out quickly
send_velocity();
send_position();
INSCovariancePrediction(1 / (float)EKF_RATE);
sensors = 0;
/*
* Detect if greater than certain time since last gps update and if so
* reset EKF to that position since probably drifted too far for safe
* update
*/
uint32_t this_gps_time = timer_count();
float gps_delay;
if (this_gps_time < last_gps_time)
gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
else
gps_delay = (this_gps_time - last_gps_time) / timer_rate();
last_gps_time = this_gps_time;
if (gps_data.updated)
{
vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
vel[2] = 0;
if(gps_delay > INSGPS_GPS_TIMEOUT)
INSPosVelReset(gps_data.NED,vel); // position stale, reset
else {
sensors |= HORIZ_SENSORS | POS_SENSORS;
}
/*
* When using gps need to make sure that barometer is brought into NED frame
* we should try and see if the altitude from the home location is good enough
* to use for the offset but for now starting with this conservative filter
*/
if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
baro_offset = gps_data.NED[2] + altitude_data.altitude;
} else {
/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
}
gps_data.updated = false;
} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
sensors |= VERT_SENSORS | HORIZ_SENSORS;
}
if(mag_data.updated) {
sensors |= MAG_SENSORS;
mag_data.updated = false;
}
if(altitude_data.updated) {
sensors |= BARO_SENSOR;
altitude_data.updated = false;
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);
}
/**
* @brief Update the EKF when in indoor mode
*/
void ins_indoor_update()
{
float gyro[3], accel[3], vel[3];
static uint32_t last_indoor_time = 0;
uint16_t sensors = 0;
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3];
send_attitude(); // get message out quickly
send_velocity();
send_position();
INSCovariancePrediction(1 / (float)EKF_RATE);
/* Indoors, update with zero position and velocity and high covariance */
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
uint32_t this_indoor_time = timer_count();
float indoor_delay;
/*
* Detect if greater than certain time since last gps update and if so
* reset EKF to that position since probably drifted too far for safe
* update
*/
if (this_indoor_time < last_indoor_time)
indoor_delay = ((0xFFFF - last_indoor_time) - this_indoor_time) / timer_rate();
else
indoor_delay = (this_indoor_time - last_indoor_time) / timer_rate();
last_indoor_time = this_indoor_time;
if(indoor_delay > INSGPS_GPS_TIMEOUT)
INSPosVelReset(vel,vel);
else
sensors = HORIZ_SENSORS | VERT_SENSORS;
if(mag_data.updated && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
sensors |= MAG_SENSORS;
mag_data.updated = false;
}
if(altitude_data.updated) {
sensors |= BARO_SENSOR;
altitude_data.updated = false;
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS);
}
/**
* @brief Initialize the EKF assuming stationary
*/
void ins_init_algorithm()
{
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
bool using_mags, using_gps;
INSGPSInit();
HomeLocationData home;
HomeLocationGet(&home);
accels[0]=accel_data.filtered.x;
accels[1]=accel_data.filtered.y;
accels[2]=accel_data.filtered.z;
using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0);
/* Block till a data update */
get_accel_gyro_data();
/* Ensure we get mag data in a timely manner */
uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec
while(using_mags && !mag_data.updated && fail_count--) {
get_accel_gyro_data();
AhrsPoll();
}
using_mags &= mag_data.updated;
if (using_mags) {
/* Spin waiting for mag data */
while(!mag_data.updated) {
get_accel_gyro_data();
AhrsPoll();
}
mag_data.updated = false;
RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
R2Quaternion(Rbe,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
else
INSSetState(zeros, zeros, q, zeros, zeros);
} else {
// assume yaw = 0
mag = VectorMagnitude(accels);
rpy[1] = asinf(-accels[0]/mag);
rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
rpy[2] = 0;
RPY2Quaternion(rpy,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
else
INSSetState(zeros, zeros, q, zeros, zeros);
}
INSResetP(Pdiag);
// TODO: include initial estimate of gyro bias?
}
/**
* @brief Simple update using just mag and accel. Yaw biased and big attitude changes.
*/
void simple_update() {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] = atan2((mag_data.raw.axis[MAG_RAW_Y_IDX]), (-1 * mag_data.raw.axis[MAG_RAW_X_IDX])) * RAD_TO_DEG;
rpy[1] = atan2(accel_data.filtered.x, accel_data.filtered.z) * RAD_TO_DEG;
rpy[0] = atan2(accel_data.filtered.y, accel_data.filtered.z) * RAD_TO_DEG;
RPY2Quaternion(rpy, q);
attitude_data.quaternion.q1 = q[0];
attitude_data.quaternion.q2 = q[1];
attitude_data.quaternion.q3 = q[2];
attitude_data.quaternion.q4 = q[3];
send_attitude();
}
/**
* @brief Output all the important inputs and states of the ekf through serial port
*/
#ifdef DUMP_EKF
extern float **P, *X; // covariance matrix and state vector
void print_ekf_binary()
{
uint16_t states = ins_get_num_states();
uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
// Dump raw buffer
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number (17:20)
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); // accel data (21:32)
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); // gyro data (33:44)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * states); // X (86:149)
for(uint8_t i = 0; i < states; i++)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &((*P)[i + i * states]), 4); // diag(P) (150:213)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (214:217)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4); // baro_offset (218:221)
}
#else
void print_ekf_binary() {}
#endif
/**
* @brief Debugging function to output all the ADC samples
*/
#if defined(DUMP_RAW)
void print_ahrs_raw()
{
int result;
static int previous_conversion = 0;
int16_t * valid_data_buffer;
uint8_t framing[16] =
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15 };
get_accel_gyro_data();
valid_data_buffer = PIOS_ADC_GetRawBuffer();
if (total_conversion_blocks != previous_conversion + 1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
// Dump raw buffer
result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
adc_oversampling *
PIOS_ADC_NUM_PINS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
#endif
#if defined(PIP_DUMP_RAW)
#define MAX_OVERSAMPLING PIOS_ADC_MAX_OVERSAMPLING
void print_ahrs_raw()
{
int16_t accel_x[MAX_OVERSAMPLING], accel_y[MAX_OVERSAMPLING], accel_z[MAX_OVERSAMPLING];
int16_t gyro_x[MAX_OVERSAMPLING], gyro_y[MAX_OVERSAMPLING], gyro_z[MAX_OVERSAMPLING];
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
int16_t mag[3];
int16_t mag_x, mag_y, mag_z;
#endif
static int previous_conversion = 0;
uint8_t framing[3] = {0xD2, 0x73, 0x00};
// wait for new raw samples
while (previous_conversion == total_conversion_blocks);
if ((previous_conversion + 1) != total_conversion_blocks)
PIOS_LED_On(LED1); // we are not keeping up
previous_conversion = total_conversion_blocks;
// fetch the buffer address for the new samples
int16_t *valid_data_buffer = PIOS_ADC_GetRawBuffer();
// fetch number of raw samples in the buffer (per channel)
int over_sampling = PIOS_ADC_GetOverSampling();
framing[2] = over_sampling;
// copy the raw samples into their own buffers
for (uint16_t i = 0, j = 0; i < over_sampling; i++, j += PIOS_ADC_NUM_CHANNELS)
{
accel_x[i] = valid_data_buffer[ACCEL_RAW_X_IDX + j];
accel_y[i] = valid_data_buffer[ACCEL_RAW_Y_IDX + j];
accel_z[i] = valid_data_buffer[ACCEL_RAW_Z_IDX + j];
gyro_x[i] = valid_data_buffer[GYRO_RAW_X_IDX + j];
gyro_y[i] = valid_data_buffer[GYRO_RAW_Y_IDX + j];
gyro_z[i] = valid_data_buffer[GYRO_RAW_Z_IDX + j];
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if (PIOS_HMC5843_NewDataAvailable())
{
PIOS_HMC5843_ReadMag(mag);
mag_x = mag[MAG_RAW_X_IDX];
mag_y = mag[MAG_RAW_Y_IDX];
mag_z = mag[MAG_RAW_Z_IDX];
}
#endif
// send the raw samples
int result = PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, framing, sizeof(framing));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_x, over_sampling * sizeof(accel_x[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_y, over_sampling * sizeof(accel_y[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)accel_z, over_sampling * sizeof(accel_z[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_x, over_sampling * sizeof(gyro_x[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_y, over_sampling * sizeof(gyro_y[0]));
result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0]));
if (result != 0)
PIOS_LED_On(LED1); // all data not sent
else
PIOS_LED_Off(LED1);
}
#endif
extern void PIOS_Board_Init(void);
/**
* @brief AHRS Main function
*/
int main()
{
gps_data.quality = -1;
uint32_t up_time_real = 0;
uint32_t up_time = 0;
uint32_t last_up_time = 0;
static int8_t last_ahrs_algorithm;
uint32_t last_counter_idle_start = 0;
uint32_t last_counter_idle_end = 0;
uint32_t idle_counts = 0;
uint32_t running_counts = 0;
uint32_t counter_val = 0;
ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE;
reset_values();
PIOS_Board_Init();
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
#endif
while(!AhrsLinkReady()) {
AhrsPoll();
}
/* we didn't connect the callbacks before because we have to wait
for all data to be up to date before doing anything*/
AHRSCalibrationConnectCallback(calibration_callback);
GPSPositionConnectCallback(gps_callback);
BaroAltitudeConnectCallback(altitude_callback);
AHRSSettingsConnectCallback(settings_callback);
HomeLocationConnectCallback(homelocation_callback);
FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
calibration_callback(AHRSCalibrationHandle()); //force an update
#if defined(DUMP_RAW) || defined(PIP_DUMP_RAW)
while (1) {
AhrsPoll();
print_ahrs_raw();
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while(1) {
AhrsPoll();
AhrsStatusData status;
AhrsStatusGet(&status);
status.CPULoad = ((float)running_counts /
(float)(idle_counts + running_counts)) * 100;
status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
status.RunningTimePerCyle = running_counts / (timer_rate() / 10000);
status.DroppedUpdates = ekf_too_slow;
up_time = timer_count();
if(up_time >= last_up_time) // normal condition
up_time_real += ((up_time - last_up_time) * 1000) / timer_rate();
else
up_time_real += ((0xFFFF - last_up_time + up_time) * 1000) / timer_rate();
last_up_time = up_time;
status.RunningTime = up_time_real;
AhrsStatusSet(&status);
// Alive signal
if (((total_conversion_blocks % 100) & 0xFFFE) == 0)
PIOS_LED_Toggle(LED1);
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
// This function blocks till data avilable
get_accel_gyro_data();
// Get any mag data available
process_mag_data();
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) ||
ACCEL_OOB(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
GYRO_OOB(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z)) {
// If any values are NaN or huge don't update
//TODO: add field to ahrs status to track number of these events
continue;
}
print_ekf_binary();
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
if (ahrs_algorithm != last_ahrs_algorithm)
ins_init_algorithm();
last_ahrs_algorithm = ahrs_algorithm;
switch(ahrs_algorithm) {
case AHRSSETTINGS_ALGORITHM_SIMPLE:
simple_update();
break;
case AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
ins_outdoor_update();
break;
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR:
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
ins_indoor_update();
break;
}
}
return 0;
}
/**
* @brief Get the accel and gyro data from whichever source when available
*
* This function will act as the HAL for the new INS sensors
*/
bool get_accel_gyro_data()
{
float accel[6];
float gyro[6];
uint16_t spin_count = 1;
while(fifoBuf_getUsed(&adc_fifo_buffer) < (sizeof(accel) + sizeof(gyro))) {
if(spin_count++ == 0)
AhrsPoll();
}
fifoBuf_getData(&adc_fifo_buffer, &accel[0], sizeof(float) * 3);
fifoBuf_getData(&adc_fifo_buffer, &gyro[0], sizeof(float) * 3);
fifoBuf_getData(&adc_fifo_buffer, &accel[3], sizeof(float) * 3);
fifoBuf_getData(&adc_fifo_buffer, &gyro[3], sizeof(float) * 3);
accel_data.filtered.x = (accel[0] + accel[3]) / 2;
accel_data.filtered.y = (accel[1] + accel[4]) / 2;
accel_data.filtered.z = (accel[2] + accel[5]) / 2;
gyro_data.filtered.x = (gyro[0] + gyro[3]) / 2;
gyro_data.filtered.y = (gyro[1] + gyro[4]) / 2;
gyro_data.filtered.z = (gyro[2] + gyro[5]) / 2;
return true;
}
/**
* @brief Perform calibration of a 3-axis field sensor using an affine transformation
* matrix.
*
* Computes result = scale * arg.
*
* @param result[out] The three-axis resultant field.
* @param scale[in] The 4x4 affine transformation matrix. The fourth row is implicitly
* [0 0 0 1]
* @param arg[in] The 3-axis input field. The 'w' component is implicitly 1.
*/
void calibration(float result[3], float scale[3][4], float arg[3])
{
for (int row = 0; row < 3; ++row) {
result[row] = 0.0f;
int col;
for (col = 0; col < 3; ++col) {
result[row] += scale[row][col] * arg[col];
}
// fourth column: arg has an implicit w value of 1.0f.
result[row] += scale[row][col];
}
}
/**
* @brief Scale an affine transformation matrix by a rotation, defined by a
* rotation vector. scale <- rotation * scale
*
* @param scale[in,out] The affine transformation matrix to be rotated
* @param rotation[in] The rotation vector defining the rotation
*/
void affine_rotate(float scale[3][4], float rotation[3])
{
// Rotate the scale factor matrix in-place
float rmatrix[3][3];
Rv2Rot(rotation, rmatrix);
float ret[3][4];
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
ret[row][col] = 0.0f;
for (int i = 0; i < 3; ++i) {
ret[row][col] += rmatrix[row][i] * scale[i][col];
}
}
}
// copy output to argument
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
scale[row][col] = ret[row][col];
}
}
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void adc_callback(float * downsampled_data)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
#if 0
// Get the Y data. Third byte in. Convert to m/s
float accel_filtered[3];
accel_filtered[1] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_filtered[1] += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_filtered[1] /= (float) fir_coeffs[adc_oversampling];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_filtered[0] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_filtered[0] += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_filtered[0] /= (float) fir_coeffs[adc_oversampling];
// Get the Z data. Third byte in. Convert to m/s
accel_filtered[2] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_filtered[2] += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
accel_filtered[2] /= (float) fir_coeffs[adc_oversampling];
float accel_scaled[3];
calibration(accel_scaled, accel_data.calibration.scale, accel_filtered);
accel_data.filtered.x = accel_scaled[0];
accel_data.filtered.y = accel_scaled[1];
accel_data.filtered.z = accel_scaled[2];
#else
float accel[3], gyro[3];
float raw_accel[3] = {
downsampled_data[ACCEL_RAW_X_IDX],
downsampled_data[ACCEL_RAW_Y_IDX],
-downsampled_data[ACCEL_RAW_Z_IDX]
};
// Accel data is (y,x,z) in first third and fifth byte. Convert to m/s^2
calibration(accel, accel_data.calibration.scale, raw_accel);
// Gyro data is (x,y,z) in second, fifth and seventh byte. Convert to rad/s
gyro[0] = ( ( downsampled_data[GYRO_RAW_X_IDX] + gyro_data.calibration.tempcompfactor[0] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
gyro[1] = ( ( downsampled_data[GYRO_RAW_Y_IDX] + gyro_data.calibration.tempcompfactor[1] * downsampled_data[GYRO_TEMP_RAW_XY_IDX] ) * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
gyro[2] = ( ( downsampled_data[GYRO_RAW_Z_IDX] + gyro_data.calibration.tempcompfactor[2] * downsampled_data[GYRO_TEMP_RAW_Z_IDX] ) * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
#endif
#if 0
static float gravity_tracking[3] = {0,0,0};
const float tau = 0.9999;
gravity_tracking[0] = tau * gravity_tracking[0] + (1-tau) * accel[0];
gravity_tracking[1] = tau * gravity_tracking[1] + (1-tau) * accel[1];
gravity_tracking[2] = tau * gravity_tracking[2] + (1-tau) * (accel[2] + 9.81);
if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
accel[0] -= gravity_tracking[0];
accel[1] -= gravity_tracking[1];
accel[2] -= gravity_tracking[2];
}
#endif
if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
} else {
ekf_too_slow++;
}
AttitudeRawData raw;
raw.gyrotemp[0] = downsampled_data[GYRO_TEMP_RAW_XY_IDX];
raw.gyrotemp[1] = downsampled_data[GYRO_TEMP_RAW_Z_IDX];
raw.gyros[0] = gyro[0] * RAD_TO_DEG;
raw.gyros[1] = gyro[1] * RAD_TO_DEG;
raw.gyros[2] = gyro[2] * RAD_TO_DEG;
raw.accels[0] = accel[0];
raw.accels[1] = accel[1];
raw.accels[2] = accel[2];
raw.magnetometers[0] = mag_data.scaled.axis[0];
raw.magnetometers[1] = mag_data.scaled.axis[1];
raw.magnetometers[2] = mag_data.scaled.axis[2];
if (settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE)
{
raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
raw.accels[0] -= Nav.accel_bias[0];
raw.accels[1] -= Nav.accel_bias[1];
raw.accels[2] -= Nav.accel_bias[2];
}
AttitudeRawSet(&raw);
total_conversion_blocks++;
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/**
* @brief Get the mag data from the I2C sensor and load into structure
* @return none
*
* This function also considers if the home location is set and has a valid
* magnetic field before updating the mag data to prevent data being used that
* cannot be interpreted. In addition the mag data is not used for the first
* five seconds to allow the filter to start to converge
*/
void process_mag_data()
{
// Get magnetic readings
// For now don't use mags until the magnetic field is set AND until 5 seconds
// after initialization otherwise it seems to have problems
// TODO: Follow up this initialization issue
HomeLocationData home;
HomeLocationGet(&home);
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
// Only use if magnetic length reasonable
float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
mag_data.updated = (home.Set == HOMELOCATION_SET_TRUE) &&
((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) &&
((mag_data.raw.axis[MAG_RAW_X_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Y_IDX] != 0) || (mag_data.raw.axis[MAG_RAW_Z_IDX] != 0)) &&
((Blen < mag_len * (1 + INSGPS_MAGTOL)) && (Blen > mag_len * (1 - INSGPS_MAGTOL)));
}
}
#else
void process_mag_data() { }
#endif
/**
* @brief Assumes board is not moving computes biases and variances of sensors
* @returns None
*
* All data is stored in global structures. This function should be called from OP when
* aircraft is in stable state and then the data stored to SD card.
*
* After this function the bias for each sensor will be the mean value. This doesn't make
* sense for the z accel so make sure 6 point calibration is also run and those values set
* after these read.
*/
#define NBIAS 100
#define NVAR 500
void calibrate_sensors()
{
int i,j;
float accel_bias[3] = {0, 0, 0};
float gyro_bias[3] = {0, 0, 0};
float mag_bias[3] = {0, 0, 0};
for (i = 0, j = 0; i < NBIAS; i++) {
get_accel_gyro_data();
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
accel_bias[0] += accel_data.filtered.x / NBIAS;
accel_bias[1] += accel_data.filtered.y / NBIAS;
accel_bias[2] += accel_data.filtered.z / NBIAS;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_bias[0] += mag_data.scaled.axis[0];
mag_bias[1] += mag_data.scaled.axis[1];
mag_bias[2] += mag_data.scaled.axis[2];
}
#endif
}
mag_bias[0] /= j;
mag_bias[1] /= j;
mag_bias[2] /= j;
gyro_data.calibration.variance[0] = 0;
gyro_data.calibration.variance[1] = 0;
gyro_data.calibration.variance[2] = 0;
mag_data.calibration.variance[0] = 0;
mag_data.calibration.variance[1] = 0;
mag_data.calibration.variance[2] = 0;
accel_data.calibration.variance[0] = 0;
accel_data.calibration.variance[1] = 0;
accel_data.calibration.variance[2] = 0;
for (i = 0, j = 0; j < NVAR; j++) {
get_accel_gyro_data();
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5843_NewDataAvailable()) {
j ++;
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[MAG_RAW_X_IDX] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[MAG_RAW_Y_IDX] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[MAG_RAW_Z_IDX] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
}
#endif
}
mag_data.calibration.variance[0] /= j;
mag_data.calibration.variance[1] /= j;
mag_data.calibration.variance[2] /= j;
gyro_data.calibration.bias[0] -= gyro_bias[0];
gyro_data.calibration.bias[1] -= gyro_bias[1];
gyro_data.calibration.bias[2] -= gyro_bias[2];
}
/**
* @brief Populate fields with initial values
*/
void reset_values()
{
accel_data.calibration.scale[0][1] = 0;
accel_data.calibration.scale[1][0] = 0;
accel_data.calibration.scale[0][2] = 0;
accel_data.calibration.scale[2][0] = 0;
accel_data.calibration.scale[1][2] = 0;
accel_data.calibration.scale[2][1] = 0;
accel_data.calibration.scale[0][0] = 0.0359;
accel_data.calibration.scale[1][1] = 0.0359;
accel_data.calibration.scale[2][2] = 0.0359;
accel_data.calibration.scale[0][3] = -73.5;
accel_data.calibration.scale[1][3] = -73.5;
accel_data.calibration.scale[2][3] = -73.5;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
mag_data.calibration.bias[0] = 0;
mag_data.calibration.bias[1] = 0;
mag_data.calibration.bias[2] = 0;
mag_data.calibration.variance[0] = 50;
mag_data.calibration.variance[1] = 50;
mag_data.calibration.variance[2] = 50;
}
void send_attitude(void)
{
AttitudeActualData attitude;
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
attitude.q1 = attitude_data.quaternion.q1;
attitude.q2 = attitude_data.quaternion.q2;
attitude.q3 = attitude_data.quaternion.q3;
attitude.q4 = attitude_data.quaternion.q4;
float rpy[3];
Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
attitude.Roll = rpy[0] + settings.RollBias;
attitude.Pitch = rpy[1] + settings.PitchBias;
attitude.Yaw = rpy[2] + settings.YawBias;
if(attitude.Yaw > 360)
attitude.Yaw -= 360;
AttitudeActualSet(&attitude);
}
void send_velocity(void)
{
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
// convert into cm
velocityActual.North = Nav.Vel[0] * 100;
velocityActual.East = Nav.Vel[1] * 100;
velocityActual.Down = Nav.Vel[2] * 100;
VelocityActualSet(&velocityActual);
}
void send_position(void)
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
// convert into cm
positionActual.North = Nav.Pos[0] * 100;
positionActual.East = Nav.Pos[1] * 100;
positionActual.Down = Nav.Pos[2] * 100;
PositionActualSet(&positionActual);
}
void send_calibration(void)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
for(int ct=0; ct<3; ct++)
{
cal.accel_var[ct] = accel_data.calibration.variance[ct];
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
cal.mag_var[ct] = mag_data.calibration.variance[ct];
}
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}
/**
* @brief AHRS calibration callback
*
* Called when the OP board sets the calibration
*/
void calibration_callback(AhrsObjHandle obj)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
accel_data.calibration.scale[0][1] = cal.accel_ortho[0];
accel_data.calibration.scale[1][0] = cal.accel_ortho[0];
accel_data.calibration.scale[0][2] = cal.accel_ortho[1];
accel_data.calibration.scale[2][0] = cal.accel_ortho[1];
accel_data.calibration.scale[1][2] = cal.accel_ortho[2];
accel_data.calibration.scale[2][1] = cal.accel_ortho[2];
#if 0
// TODO: Enable after v1.0 feature freeze.
float rotation[3] = { cal.accel_rotation[0],
cal.accel_rotation[1],
cal.accel_rotation[2],
};
affine_rotate(accel_data.calibration.scale, rotation);
#endif
for(int ct=0; ct<3; ct++)
{
accel_data.calibration.scale[ct][ct] = cal.accel_scale[ct];
accel_data.calibration.scale[ct][3] = cal.accel_bias[ct];
accel_data.calibration.variance[ct] = cal.accel_var[ct];
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
#if 1
gyro_data.calibration.tempcompfactor[ct] = cal.gyro_tempcompfactor[ct];
#endif
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct];
}
// Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to
// one in code
float mag_var[3] = {mag_data.calibration.variance[0] / INSGPS_MAGLEN / INSGPS_MAGLEN,
mag_data.calibration.variance[1] / INSGPS_MAGLEN / INSGPS_MAGLEN,
mag_data.calibration.variance[2] / INSGPS_MAGLEN / INSGPS_MAGLEN};
INSSetMagVar(mag_var);
INSSetAccelVar(accel_data.calibration.variance);
INSSetGyroVar(gyro_data.calibration.variance);
}
else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE) {
calibrate_sensors();
send_calibration();
}
INSSetPosVelVar(cal.pos_var, cal.vel_var);
}
void gps_callback(AhrsObjHandle obj)
{
GPSPositionData pos;
GPSPositionGet(&pos);
HomeLocationData home;
HomeLocationGet(&home);
// convert from cm back to meters
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
// put in local NED frame
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1; /* currently unused */
gps_data.updated = true;
// if poor don't use this update
if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
(pos.Satellites < INSGPS_GPS_MINSAT) ||
(pos.PDOP >= INSGPS_GPS_MINPDOP) ||
(home.Set == FALSE) ||
((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
{
gps_data.quality = 0;
gps_data.updated = false;
}
}
void altitude_callback(AhrsObjHandle obj)
{
BaroAltitudeData alt;
BaroAltitudeGet(&alt);
altitude_data.altitude = alt.Altitude;
altitude_data.updated = true;
}
void settings_callback(AhrsObjHandle obj)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
ahrs_algorithm = settings.Algorithm;
if(settings.Downsampling != adc_oversampling) {
adc_oversampling = settings.Downsampling;
PIOS_ADC_Config(adc_oversampling);
}
}
void homelocation_callback(AhrsObjHandle obj)
{
HomeLocationData data;
HomeLocationGet(&data);
mag_len = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2));
float Be[3] = {data.Be[0] / mag_len, data.Be[1] / mag_len, data.Be[2] / mag_len};
INSSetMagNorth(Be);
}
void firmwareiapobj_callback(AhrsObjHandle obj)
{
const struct pios_board_info * bdinfo = &pios_board_info_blob;
FirmwareIAPObjData firmwareIAPObj;
FirmwareIAPObjGet(&firmwareIAPObj);
if(firmwareIAPObj.ArmReset==0)
reset_count=0;
if(firmwareIAPObj.ArmReset==1)
{
if((firmwareIAPObj.BoardType==bdinfo->board_type) || (firmwareIAPObj.BoardType==0xFF))
{
++reset_count;
if(reset_count>2)
{
PIOS_IAP_SetRequest1();
PIOS_IAP_SetRequest2();
PIOS_SYS_Reset();
}
}
}
else if(firmwareIAPObj.BoardType==bdinfo->board_type && firmwareIAPObj.crc!=PIOS_BL_HELPER_CRC_Memory_Calc())
{
PIOS_BL_HELPER_FLASH_Read_Description(firmwareIAPObj.Description,bdinfo->desc_size);
firmwareIAPObj.crc=PIOS_BL_HELPER_CRC_Memory_Calc();
firmwareIAPObj.BoardRevision=bdinfo->board_rev;
FirmwareIAPObjSet(&firmwareIAPObj);
}
}
/**
* @}
*/

61
flight/AHRS/ahrs_timer.c
View File

@ -1,61 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_TIMER AHRS Timer
* @brief Sets up a simple timer that can be polled to estimate idle time
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "ahrs_timer.h"
void timer_start()
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,
ENABLE);
PWR_BackupAccessCmd(ENABLE);
RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128);
RCC_RTCCLKCmd(ENABLE);
RTC_WaitForLastTask();
RTC_WaitForSynchro();
RTC_WaitForLastTask();
RTC_SetPrescaler(0); // counting at 8e6 / 128
RTC_WaitForLastTask();
RTC_SetCounter(0);
RTC_WaitForLastTask();
}
uint32_t timer_count()
{
return RTC_GetCounter();
}
uint32_t timer_rate()
{
return TIMER_RATE;
}

115
flight/AHRS/inc/ahrs.h
View File

@ -1,115 +0,0 @@
/**
******************************************************************************
*
* @file ahrs.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main AHRS header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_H
#define AHRS_H
/* PIOS Includes */
#include <pios.h>
struct mag_sensor {
uint8_t id[4];
uint8_t updated;
struct {
int16_t axis[3];
} raw;
struct {
float axis[3];
} scaled;
struct {
float bias[3];
float scale[3];
float variance[3];
} calibration;
};
//! Contains the data from the accelerometer
struct accel_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float scale[3][4];
float variance[3];
} calibration;
};
//! Contains the data from the gyro
struct gyro_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
float bias[3];
float scale[3];
float variance[3];
float tempcompfactor[3];
} calibration;
struct {
uint16_t xy;
uint16_t z;
} temp;
};
//! Conains the current estimate of the attitude
struct attitude_solution {
struct {
float q1;
float q2;
float q3;
float q4;
} quaternion;
};
//! Contains data from the altitude sensor
struct altitude_sensor {
float altitude;
bool updated;
};
//! Contains data from the GPS (via the SPI link)
struct gps_sensor {
float NED[3];
float heading;
float groundspeed;
float quality;
bool updated;
};
#endif /* AHRS_H */

94
flight/AHRS/inc/ahrs_fsm.h
View File

@ -1,94 +0,0 @@
/**
******************************************************************************
*
* @file ahrs_fsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_FSM_H
#define AHRS_FSM_H
#include "pios_opahrs_proto.h"
enum lfsm_state {
LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */
LFSM_STATE_STOPPED,
LFSM_STATE_STOPPING,
LFSM_STATE_INACTIVE,
LFSM_STATE_USER_BUSY,
LFSM_STATE_USER_BUSY_RX_PENDING,
LFSM_STATE_USER_BUSY_TX_PENDING,
LFSM_STATE_USER_BUSY_RXTX_PENDING,
LFSM_STATE_USER_RX_PENDING,
LFSM_STATE_USER_TX_PENDING,
LFSM_STATE_USER_RXTX_PENDING,
LFSM_STATE_USER_RX_ACTIVE,
LFSM_STATE_USER_TX_ACTIVE,
LFSM_STATE_USER_RXTX_ACTIVE,
LFSM_STATE_NUM_STATES /* Must be last */
};
enum lfsm_event {
LFSM_EVENT_INIT_LINK,
LFSM_EVENT_STOP,
LFSM_EVENT_USER_SET_RX,
LFSM_EVENT_USER_SET_TX,
LFSM_EVENT_USER_DONE,
LFSM_EVENT_RX_LINK,
LFSM_EVENT_RX_USER,
LFSM_EVENT_RX_UNKNOWN,
LFSM_EVENT_NUM_EVENTS /* Must be last */
};
struct lfsm_link_stats {
uint32_t rx_badcrc;
uint32_t rx_badmagic_head;
uint32_t rx_badmagic_tail;
uint32_t rx_link;
uint32_t rx_user;
uint32_t tx_user;
uint32_t rx_badtype;
uint32_t rx_badver;
};
extern void lfsm_init(void);
extern void lfsm_inject_event(enum lfsm_event event);
extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val);
extern void lfsm_get_link_stats(struct lfsm_link_stats *stats);
extern enum lfsm_state lfsm_get_state(void);
extern void lfsm_set_link_proto_v0(struct opahrs_msg_v0 *link_tx,
struct opahrs_msg_v0 *link_rx);
extern void lfsm_user_set_rx_v0(struct opahrs_msg_v0 *user_rx);
extern void lfsm_user_set_tx_v0(struct opahrs_msg_v0 *user_tx);
extern void lfsm_set_link_proto_v1(struct opahrs_msg_v1 *link_tx,
struct opahrs_msg_v1 *link_rx);
extern void lfsm_user_set_rx_v1(struct opahrs_msg_v1 *user_rx);
extern void lfsm_user_set_tx_v1(struct opahrs_msg_v1 *user_tx);
extern void lfsm_user_done(void);
#endif /* AHRS_FSM_H */

45
flight/AHRS/inc/ahrs_timer.h
View File

@ -1,45 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS Control
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_TIMER
* @brief Sets up a simple timer that can be polled to estimate idle time
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_TIMER
#define AHRS_TIMER
#include <pios.h>
#define TIMER_RATE (8e6 / 128)
void timer_start();
uint32_t timer_count();
uint32_t timer_rate();
#endif

93
flight/AHRS/inc/insgps.h
View File

@ -1,93 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @file insgps.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the INSGPS exposed functionality.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef INSGPS_H_
#define INSGPS_H_
#include "stdint.h"
/**
* @addtogroup Constants
* @{
*/
#define POS_SENSORS 0x007
#define HORIZ_SENSORS 0x018
#define VERT_SENSORS 0x020
#define MAG_SENSORS 0x1C0
#define BARO_SENSOR 0x200
#define FULL_SENSORS 0x3FF
/**
* @}
*/
// Exposed Function Prototypes
void INSGPSInit();
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
void INSSetPosVelVar(float PosVar, float VelVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]);
void INSPosVelReset(float pos[3], float vel[3]);
void MagCorrection(float mag_data[3]);
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt);
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
void VelBaroCorrection(float Vel[3], float BaroAlt);
uint16_t ins_get_num_states();
// Nav structure containing current solution
struct NavStruct {
float Pos[3]; // Position in meters and relative to a local NED frame
float Vel[3]; // Velocity in meters and in NED
float q[4]; // unit quaternion rotation relative to NED
float gyro_bias[3];
float accel_bias[3];
} Nav;
/**
* @}
* @}
*/
#endif /* EKF_H_ */

45
flight/AHRS/inc/pios_config.h
View File

@ -1,45 +0,0 @@
/**
******************************************************************************
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* - Central compile time config for the project.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_HMC5843
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_BL_HELPER
#endif /* PIOS_CONFIG_H */

1644
flight/AHRS/insgps13state.c
View File

@ -1,1644 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @file insgps.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief An INS/GPS algorithm implemented with an EKF.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "insgps.h"
#include <math.h>
#include <stdint.h>
// constants/macros/typdefs
#define NUMX 13 // number of states, X is the state vector
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector
#if defined(GENERAL_COV)
// This might trick people so I have a note here. There is a slower but bigger version of the
// code here but won't fit when debugging disabled (requires -Os)
#define COVARIANCE_PREDICTION_GENERAL
#endif
// Private functions
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW]);
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
// global to init to zero and maintain zero elements
float Be[3]; // local magnetic unit vector in NED frame
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
float K[NUMX][NUMV]; // feedback gain matrix
// ************* Exposed Functions ****************
// *************************************************
uint16_t ins_get_num_states()
{
return NUMX;
}
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1;
Be[1] = 0;
Be[2] = 0; // local magnetic unit vector
for (int i = 0; i < NUMX; i++) {
for (int j = 0; j < NUMX; j++) {
P[i][j] = 0; // zero all terms
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
X[6] = 1;
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
R[9] = .05; // High freq altimeter noise variance (m^2)
}
void INSResetP(float PDiag[NUMX])
{
uint8_t i,j;
// if PDiag[i] nonzero then clear row and column and set diagonal element
for (i=0;i<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0;
P[i][i]=PDiag[i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
{
/* Note: accel_bias not used in 13 state INS */
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
X[6] = q[0];
X[7] = q[1];
X[8] = q[2];
X[9] = q[3];
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSPosVelReset(float pos[3], float vel[3])
{
for (int i = 0; i < 6; i++) {
for(int j = i; j < NUMX; j++) {
P[i][j] = 0; // zero the first 6 rows and columns
P[j][i] = 0;
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar, float VelVar)
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = VelVar;
R[4] = VelVar;
// R[5] = PosVar; // Don't change vertical velocity, not measured
}
void INSSetGyroBias(float gyro_bias[3])
{
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSSetAccelVar(float accel_var[3])
{
Q[3] = accel_var[0];
Q[4] = accel_var[1];
Q[5] = accel_var[2];
}
void INSSetGyroVar(float gyro_var[3])
{
Q[0] = gyro_var[0];
Q[1] = gyro_var[1];
Q[2] = gyro_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2];
}
void INSSetMagNorth(float B[3])
{
Be[0] = B[0];
Be[1] = B[1];
Be[2] = B[2];
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
float qmag;
// rate gyro inputs in units of rad/s
U[0] = gyro_data[0];
U[1] = gyro_data[1];
U[2] = gyro_data[2];
// accelerometer inputs in units of m/s
U[3] = accel_data[0];
U[4] = accel_data[1];
U[5] = accel_data[2];
// EKF prediction step
LinearizeFG(X, U, F, G);
RungeKutta(X, U, dT);
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
//CovariancePrediction(F,G,Q,dT,P);
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
}
void INSCovariancePrediction(float dT)
{
CovariancePrediction(F, G, Q, dT, P);
}
float zeros[3] = { 0, 0, 0 };
void MagCorrection(float mag_data[3])
{
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
}
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, zeros, Vel, BaroAlt,
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
BARO_SENSOR);
}
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(zeros, Pos, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt)
{
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
}
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
{
INSCorrection(mag_data, Pos, Vel, zeros[0],
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
}
void VelBaroCorrection(float Vel[3], float BaroAlt)
{
INSCorrection(zeros, zeros, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed)
{
float Z[10], Y[10];
float Bmag, qmag;
// GPS Position in meters and in local NED frame
Z[0] = Pos[0];
Z[1] = Pos[1];
Z[2] = Pos[2];
// GPS Velocity in meters and in local NED frame
Z[3] = Vel[0];
Z[4] = Vel[1];
Z[5] = Vel[2];
// magnetometer data in any units (use unit vector) and in body frame
Bmag =
sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] / Bmag;
Z[7] = mag_data[1] / Bmag;
Z[8] = mag_data[2] / Bmag;
// barometric altimeter in meters and in local NED frame
Z[9] = BaroAlt;
// EKF correction step
LinearizeH(X, Be, H);
MeasurementEq(X, Be, Y);
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
}
// ************* CovariancePrediction *************
// Does the prediction step of the Kalman filter for the covariance matrix
// Output, Pnew, overwrites P, the input covariance
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
// Q is the discrete time covariance of process noise
// Q is vector of the diagonal for a square matrix with
// dimensions equal to the number of disturbance noise variables
// The General Method is very inefficient,not taking advantage of the sparse F and G
// The first Method is very specific to this implementation
// ************************************************
#ifdef COVARIANCE_PREDICTION_GENERAL
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float Dummy[NUMX][NUMX], dTsq;
uint8_t i, j, k;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
dTsq = dT * dT;
for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
for (j = 0; j < NUMX; j++) {
Dummy[i][j] = P[i][j] / dT;
for (k = 0; k < NUMX; k++)
Dummy[i][j] += F[i][k] * P[k][j];
}
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
P[i][j] = Dummy[i][j] / dT;
for (k = 0; k < NUMX; k++)
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
for (k = 0; k < NUMW; k++)
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
}
}
#else
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float D[NUMX][NUMX], T, Tsq;
uint8_t i, j;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
T = dT;
Tsq = dT * dT;
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
for (j = i; j < NUMX; j++)
D[i][j] = P[i][j];
// Brute force calculation of the elements of P
P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0];
P[0][1] = P[1][0] =
D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1];
P[0][2] = P[2][0] =
D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2];
P[0][3] = P[3][0] =
(F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] +
F[3][9] * D[3][9]) * Tsq + (D[3][3] + F[3][6] * D[0][6] +
F[3][7] * D[0][7] +
F[3][8] * D[0][8] +
F[3][9] * D[0][9]) * T + D[0][3];
P[0][4] = P[4][0] =
(F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] +
F[4][9] * D[3][9]) * Tsq + (D[3][4] + F[4][6] * D[0][6] +
F[4][7] * D[0][7] +
F[4][8] * D[0][8] +
F[4][9] * D[0][9]) * T + D[0][4];
P[0][5] = P[5][0] =
(F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] +
F[5][9] * D[3][9]) * Tsq + (D[3][5] + F[5][6] * D[0][6] +
F[5][7] * D[0][7] +
F[5][8] * D[0][8] +
F[5][9] * D[0][9]) * T + D[0][5];
P[0][6] = P[6][0] =
(F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] +
F[6][10] * D[3][10] + F[6][11] * D[3][11] +
F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] +
F[6][8] * D[0][8] +
F[6][9] * D[0][9] +
F[6][10] * D[0][10] +
F[6][11] * D[0][11] +
F[6][12] * D[0][12]) * T +
D[0][6];
P[0][7] = P[7][0] =
(F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] +
F[7][10] * D[3][10] + F[7][11] * D[3][11] +
F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] +
F[7][8] * D[0][8] +
F[7][9] * D[0][9] +
F[7][10] * D[0][10] +
F[7][11] * D[0][11] +
F[7][12] * D[0][12]) * T +
D[0][7];
P[0][8] = P[8][0] =
(F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] +
F[8][10] * D[3][10] + F[8][11] * D[3][11] +
F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] +
F[8][7] * D[0][7] +
F[8][9] * D[0][9] +
F[8][10] * D[0][10] +
F[8][11] * D[0][11] +
F[8][12] * D[0][12]) * T +
D[0][8];
P[0][9] = P[9][0] =
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
F[9][10] * D[3][10] + F[9][11] * D[3][11] +
F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] +
F[9][7] * D[0][7] +
F[9][8] * D[0][8] +
F[9][10] * D[0][10] +
F[9][11] * D[0][11] +
F[9][12] * D[0][12]) * T +
D[0][9];
P[0][10] = P[10][0] = D[3][10] * T + D[0][10];
P[0][11] = P[11][0] = D[3][11] * T + D[0][11];
P[0][12] = P[12][0] = D[3][12] * T + D[0][12];
P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1];
P[1][2] = P[2][1] =
D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2];
P[1][3] = P[3][1] =
(F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] +
F[3][9] * D[4][9]) * Tsq + (D[3][4] + F[3][6] * D[1][6] +
F[3][7] * D[1][7] +
F[3][8] * D[1][8] +
F[3][9] * D[1][9]) * T + D[1][3];
P[1][4] = P[4][1] =
(F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] +
F[4][9] * D[4][9]) * Tsq + (D[4][4] + F[4][6] * D[1][6] +
F[4][7] * D[1][7] +
F[4][8] * D[1][8] +
F[4][9] * D[1][9]) * T + D[1][4];
P[1][5] = P[5][1] =
(F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] +
F[5][9] * D[4][9]) * Tsq + (D[4][5] + F[5][6] * D[1][6] +
F[5][7] * D[1][7] +
F[5][8] * D[1][8] +
F[5][9] * D[1][9]) * T + D[1][5];
P[1][6] = P[6][1] =
(F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] +
F[6][10] * D[4][10] + F[6][11] * D[4][11] +
F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] +
F[6][8] * D[1][8] +
F[6][9] * D[1][9] +
F[6][10] * D[1][10] +
F[6][11] * D[1][11] +
F[6][12] * D[1][12]) * T +
D[1][6];
P[1][7] = P[7][1] =
(F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] +
F[7][10] * D[4][10] + F[7][11] * D[4][11] +
F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] +
F[7][8] * D[1][8] +
F[7][9] * D[1][9] +
F[7][10] * D[1][10] +
F[7][11] * D[1][11] +
F[7][12] * D[1][12]) * T +
D[1][7];
P[1][8] = P[8][1] =
(F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] +
F[8][10] * D[4][10] + F[8][11] * D[4][11] +
F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] +
F[8][7] * D[1][7] +
F[8][9] * D[1][9] +
F[8][10] * D[1][10] +
F[8][11] * D[1][11] +
F[8][12] * D[1][12]) * T +
D[1][8];
P[1][9] = P[9][1] =
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
F[9][10] * D[4][10] + F[9][11] * D[4][11] +
F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] +
F[9][7] * D[1][7] +
F[9][8] * D[1][8] +
F[9][10] * D[1][10] +
F[9][11] * D[1][11] +
F[9][12] * D[1][12]) * T +
D[1][9];
P[1][10] = P[10][1] = D[4][10] * T + D[1][10];
P[1][11] = P[11][1] = D[4][11] * T + D[1][11];
P[1][12] = P[12][1] = D[4][12] * T + D[1][12];
P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2];
P[2][3] = P[3][2] =
(F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] +
F[3][9] * D[5][9]) * Tsq + (D[3][5] + F[3][6] * D[2][6] +
F[3][7] * D[2][7] +
F[3][8] * D[2][8] +
F[3][9] * D[2][9]) * T + D[2][3];
P[2][4] = P[4][2] =
(F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] +
F[4][9] * D[5][9]) * Tsq + (D[4][5] + F[4][6] * D[2][6] +
F[4][7] * D[2][7] +
F[4][8] * D[2][8] +
F[4][9] * D[2][9]) * T + D[2][4];
P[2][5] = P[5][2] =
(F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] +
F[5][9] * D[5][9]) * Tsq + (D[5][5] + F[5][6] * D[2][6] +
F[5][7] * D[2][7] +
F[5][8] * D[2][8] +
F[5][9] * D[2][9]) * T + D[2][5];
P[2][6] = P[6][2] =
(F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] +
F[6][10] * D[5][10] + F[6][11] * D[5][11] +
F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] +
F[6][8] * D[2][8] +
F[6][9] * D[2][9] +
F[6][10] * D[2][10] +
F[6][11] * D[2][11] +
F[6][12] * D[2][12]) * T +
D[2][6];
P[2][7] = P[7][2] =
(F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] +
F[7][10] * D[5][10] + F[7][11] * D[5][11] +
F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] +
F[7][8] * D[2][8] +
F[7][9] * D[2][9] +
F[7][10] * D[2][10] +
F[7][11] * D[2][11] +
F[7][12] * D[2][12]) * T +
D[2][7];
P[2][8] = P[8][2] =
(F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] +
F[8][10] * D[5][10] + F[8][11] * D[5][11] +
F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] +
F[8][7] * D[2][7] +
F[8][9] * D[2][9] +
F[8][10] * D[2][10] +
F[8][11] * D[2][11] +
F[8][12] * D[2][12]) * T +
D[2][8];
P[2][9] = P[9][2] =
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
F[9][10] * D[5][10] + F[9][11] * D[5][11] +
F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] +
F[9][7] * D[2][7] +
F[9][8] * D[2][8] +
F[9][10] * D[2][10] +
F[9][11] * D[2][11] +
F[9][12] * D[2][12]) * T +
D[2][9];
P[2][10] = P[10][2] = D[5][10] * T + D[2][10];
P[2][11] = P[11][2] = D[5][11] * T + D[2][11];
P[2][12] = P[12][2] = D[5][12] * T + D[2][12];
P[3][3] =
(Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] +
Q[5] * G[3][5] * G[3][5] + F[3][9] * (F[3][9] * D[9][9] +
F[3][6] * D[6][9] +
F[3][7] * D[7][9] +
F[3][8] * D[8][9]) +
F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] +
2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9]) * T + D[3][3];
P[3][4] = P[4][3] =
(F[4][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[4][6] * (F[3][6] * D[6][6] +
F[3][7] * D[6][7] +
F[3][8] * D[6][8] +
F[3][9] * D[6][9]) +
F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] +
G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] +
F[4][6] * D[3][6] +
F[3][7] * D[4][7] +
F[4][7] * D[3][7] +
F[3][8] * D[4][8] +
F[4][8] * D[3][8] +
F[3][9] * D[4][9] +
F[4][9] * D[3][9]) * T +
D[3][4];
P[3][5] = P[5][3] =
(F[5][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[5][6] * (F[3][6] * D[6][6] +
F[3][7] * D[6][7] +
F[3][8] * D[6][8] +
F[3][9] * D[6][9]) +
F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] +
G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] +
F[5][6] * D[3][6] +
F[3][7] * D[5][7] +
F[5][7] * D[3][7] +
F[3][8] * D[5][8] +
F[5][8] * D[3][8] +
F[3][9] * D[5][9] +
F[5][9] * D[3][9]) * T +
D[3][5];
P[3][6] = P[6][3] =
(F[6][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[6][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[6][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[6][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] +
F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] +
F[6][9] * D[3][9] + F[6][10] * D[3][10] +
F[6][11] * D[3][11] + F[6][12] * D[3][12]) * T + D[3][6];
P[3][7] = P[7][3] =
(F[7][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[7][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[7][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[7][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] +
F[7][9] * D[3][9] + F[7][10] * D[3][10] +
F[7][11] * D[3][11] + F[7][12] * D[3][12]) * T + D[3][7];
P[3][8] = P[8][3] =
(F[8][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[8][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[8][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[8][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9])) * Tsq +
(F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] +
F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] +
F[8][9] * D[3][9] + F[8][10] * D[3][10] +
F[8][11] * D[3][11] + F[8][12] * D[3][12]) * T + D[3][8];
P[3][9] = P[9][3] =
(F[9][10] *
(F[3][9] * D[9][10] + F[3][6] * D[6][10] +
F[3][7] * D[7][10] + F[3][8] * D[8][10]) +
F[9][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[9][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
F[3][9] * D[9][9] + F[9][10] * D[3][10] +
F[9][11] * D[3][11] + F[9][12] * D[3][12] +
F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) * T + D[3][9];
P[3][10] = P[10][3] =
(F[3][9] * D[9][10] + F[3][6] * D[6][10] + F[3][7] * D[7][10] +
F[3][8] * D[8][10]) * T + D[3][10];
P[3][11] = P[11][3] =
(F[3][9] * D[9][11] + F[3][6] * D[6][11] + F[3][7] * D[7][11] +
F[3][8] * D[8][11]) * T + D[3][11];
P[3][12] = P[12][3] =
(F[3][9] * D[9][12] + F[3][6] * D[6][12] + F[3][7] * D[7][12] +
F[3][8] * D[8][12]) * T + D[3][12];
P[4][4] =
(Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] +
Q[5] * G[4][5] * G[4][5] + F[4][9] * (F[4][9] * D[9][9] +
F[4][6] * D[6][9] +
F[4][7] * D[7][9] +
F[4][8] * D[8][9]) +
F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] +
2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9]) * T + D[4][4];
P[4][5] = P[5][4] =
(F[5][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[5][6] * (F[4][6] * D[6][6] +
F[4][7] * D[6][7] +
F[4][8] * D[6][8] +
F[4][9] * D[6][9]) +
F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9]) +
G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] +
G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] +
F[5][6] * D[4][6] +
F[4][7] * D[5][7] +
F[5][7] * D[4][7] +
F[4][8] * D[5][8] +
F[5][8] * D[4][8] +
F[4][9] * D[5][9] +
F[5][9] * D[4][9]) * T +
D[4][5];
P[4][6] = P[6][4] =
(F[6][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[6][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[6][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[6][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] +
F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] +
F[6][9] * D[4][9] + F[6][10] * D[4][10] +
F[6][11] * D[4][11] + F[6][12] * D[4][12]) * T + D[4][6];
P[4][7] = P[7][4] =
(F[7][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[7][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[7][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[7][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] +
F[7][9] * D[4][9] + F[7][10] * D[4][10] +
F[7][11] * D[4][11] + F[7][12] * D[4][12]) * T + D[4][7];
P[4][8] = P[8][4] =
(F[8][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[8][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[8][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[8][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9])) * Tsq +
(F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] +
F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] +
F[8][9] * D[4][9] + F[8][10] * D[4][10] +
F[8][11] * D[4][11] + F[8][12] * D[4][12]) * T + D[4][8];
P[4][9] = P[9][4] =
(F[9][10] *
(F[4][9] * D[9][10] + F[4][6] * D[6][10] +
F[4][7] * D[7][10] + F[4][8] * D[8][10]) +
F[9][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[9][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
F[4][9] * D[9][9] + F[9][10] * D[4][10] +
F[9][11] * D[4][11] + F[9][12] * D[4][12] +
F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) * T + D[4][9];
P[4][10] = P[10][4] =
(F[4][9] * D[9][10] + F[4][6] * D[6][10] + F[4][7] * D[7][10] +
F[4][8] * D[8][10]) * T + D[4][10];
P[4][11] = P[11][4] =
(F[4][9] * D[9][11] + F[4][6] * D[6][11] + F[4][7] * D[7][11] +
F[4][8] * D[8][11]) * T + D[4][11];
P[4][12] = P[12][4] =
(F[4][9] * D[9][12] + F[4][6] * D[6][12] + F[4][7] * D[7][12] +
F[4][8] * D[8][12]) * T + D[4][12];
P[5][5] =
(Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] +
Q[5] * G[5][5] * G[5][5] + F[5][9] * (F[5][9] * D[9][9] +
F[5][6] * D[6][9] +
F[5][7] * D[7][9] +
F[5][8] * D[8][9]) +
F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] +
2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9]) * T + D[5][5];
P[5][6] = P[6][5] =
(F[6][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[6][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[6][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[6][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] +
F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] +
F[6][9] * D[5][9] + F[6][10] * D[5][10] +
F[6][11] * D[5][11] + F[6][12] * D[5][12]) * T + D[5][6];
P[5][7] = P[7][5] =
(F[7][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[7][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[7][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[7][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] +
F[7][9] * D[5][9] + F[7][10] * D[5][10] +
F[7][11] * D[5][11] + F[7][12] * D[5][12]) * T + D[5][7];
P[5][8] = P[8][5] =
(F[8][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[8][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[8][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[8][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9])) * Tsq +
(F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] +
F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] +
F[8][9] * D[5][9] + F[8][10] * D[5][10] +
F[8][11] * D[5][11] + F[8][12] * D[5][12]) * T + D[5][8];
P[5][9] = P[9][5] =
(F[9][10] *
(F[5][9] * D[9][10] + F[5][6] * D[6][10] +
F[5][7] * D[7][10] + F[5][8] * D[8][10]) +
F[9][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[9][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
F[5][9] * D[9][9] + F[9][10] * D[5][10] +
F[9][11] * D[5][11] + F[9][12] * D[5][12] +
F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) * T + D[5][9];
P[5][10] = P[10][5] =
(F[5][9] * D[9][10] + F[5][6] * D[6][10] + F[5][7] * D[7][10] +
F[5][8] * D[8][10]) * T + D[5][10];
P[5][11] = P[11][5] =
(F[5][9] * D[9][11] + F[5][6] * D[6][11] + F[5][7] * D[7][11] +
F[5][8] * D[8][11]) * T + D[5][11];
P[5][12] = P[12][5] =
(F[5][9] * D[9][12] + F[5][6] * D[6][12] + F[5][7] * D[7][12] +
F[5][8] * D[8][12]) * T + D[5][12];
P[6][6] =
(Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] +
Q[2] * G[6][2] * G[6][2] + F[6][9] * (F[6][9] * D[9][9] +
F[6][10] * D[9][10] +
F[6][11] * D[9][11] +
F[6][12] * D[9][12] +
F[6][7] * D[7][9] +
F[6][8] * D[8][9]) +
F[6][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[6][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[6][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12])) * Tsq +
(2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] +
2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] +
2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T +
D[6][6];
P[6][7] = P[7][6] =
(F[7][9] *
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
F[7][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[7][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[7][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] +
G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] +
F[6][7] * D[7][7] +
F[6][8] * D[7][8] +
F[7][8] * D[6][8] +
F[6][9] * D[7][9] +
F[7][9] * D[6][9] +
F[6][10] * D[7][10] +
F[7][10] * D[6][10] +
F[6][11] * D[7][11] +
F[7][11] * D[6][11] +
F[6][12] * D[7][12] +
F[7][12] * D[6][12]) * T +
D[6][7];
P[6][8] = P[8][6] =
(F[8][9] *
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
F[8][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[8][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[8][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] +
G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] +
F[8][6] * D[6][6] +
F[8][7] * D[6][7] +
F[6][8] * D[8][8] +
F[6][9] * D[8][9] +
F[8][9] * D[6][9] +
F[6][10] * D[8][10] +
F[8][10] * D[6][10] +
F[6][11] * D[8][11] +
F[8][11] * D[6][11] +
F[6][12] * D[8][12] +
F[8][12] * D[6][12]) * T +
D[6][8];
P[6][9] = P[9][6] =
(F[9][10] *
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[9][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[9][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
G[9][0] * G[6][0] * Q[0] + G[9][1] * G[6][1] * Q[1] +
G[9][2] * G[6][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] +
F[9][7] * D[6][7] +
F[9][8] * D[6][8] +
F[6][9] * D[9][9] +
F[9][10] * D[6][10] +
F[6][10] * D[9][10] +
F[9][11] * D[6][11] +
F[6][11] * D[9][11] +
F[9][12] * D[6][12] +
F[6][12] * D[9][12] +
F[6][7] * D[7][9] +
F[6][8] * D[8][9]) * T +
D[6][9];
P[6][10] = P[10][6] =
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) * T + D[6][10];
P[6][11] = P[11][6] =
(F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) * T + D[6][11];
P[6][12] = P[12][6] =
(F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) * T + D[6][12];
P[7][7] =
(Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] +
Q[2] * G[7][2] * G[7][2] + F[7][9] * (F[7][9] * D[9][9] +
F[7][10] * D[9][10] +
F[7][11] * D[9][11] +
F[7][12] * D[9][12] +
F[7][6] * D[6][9] +
F[7][8] * D[8][9]) +
F[7][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[7][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[7][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
F[7][11] * D[8][11] + F[7][12] * D[8][12])) * Tsq +
(2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] +
2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] +
2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T +
D[7][7];
P[7][8] = P[8][7] =
(F[8][9] *
(F[7][9] * D[9][9] + F[7][10] * D[9][10] +
F[7][11] * D[9][11] + F[7][12] * D[9][12] +
F[7][6] * D[6][9] + F[7][8] * D[8][9]) +
F[8][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[8][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[8][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] +
G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] +
F[8][6] * D[6][7] +
F[8][7] * D[7][7] +
F[7][8] * D[8][8] +
F[7][9] * D[8][9] +
F[8][9] * D[7][9] +
F[7][10] * D[8][10] +
F[8][10] * D[7][10] +
F[7][11] * D[8][11] +
F[8][11] * D[7][11] +
F[7][12] * D[8][12] +
F[8][12] * D[7][12]) * T +
D[7][8];
P[7][9] = P[9][7] =
(F[9][10] *
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[9][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[9][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
F[7][11] * D[8][11] + F[7][12] * D[8][12]) +
G[9][0] * G[7][0] * Q[0] + G[9][1] * G[7][1] * Q[1] +
G[9][2] * G[7][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] +
F[9][7] * D[7][7] +
F[9][8] * D[7][8] +
F[7][9] * D[9][9] +
F[9][10] * D[7][10] +
F[7][10] * D[9][10] +
F[9][11] * D[7][11] +
F[7][11] * D[9][11] +
F[9][12] * D[7][12] +
F[7][12] * D[9][12] +
F[7][6] * D[6][9] +
F[7][8] * D[8][9]) * T +
D[7][9];
P[7][10] = P[10][7] =
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) * T + D[7][10];
P[7][11] = P[11][7] =
(F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) * T + D[7][11];
P[7][12] = P[12][7] =
(F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) * T + D[7][12];
P[8][8] =
(Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] +
Q[2] * G[8][2] * G[8][2] + F[8][9] * (F[8][9] * D[9][9] +
F[8][10] * D[9][10] +
F[8][11] * D[9][11] +
F[8][12] * D[9][12] +
F[8][6] * D[6][9] +
F[8][7] * D[7][9]) +
F[8][10] * (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
F[8][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
F[8][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
F[8][11] * D[7][11] + F[8][12] * D[7][12])) * Tsq +
(2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] +
2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] +
2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T +
D[8][8];
P[8][9] = P[9][8] =
(F[9][10] *
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
F[9][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
F[9][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
F[8][11] * D[7][11] + F[8][12] * D[7][12]) +
F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] +
F[8][9] * D[8][9] + F[8][10] * D[8][10] +
F[8][11] * D[8][11] + F[8][12] * D[8][12]) +
G[9][0] * G[8][0] * Q[0] + G[9][1] * G[8][1] * Q[1] +
G[9][2] * G[8][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] +
F[9][7] * D[7][8] +
F[9][8] * D[8][8] +
F[8][9] * D[9][9] +
F[9][10] * D[8][10] +
F[8][10] * D[9][10] +
F[9][11] * D[8][11] +
F[8][11] * D[9][11] +
F[9][12] * D[8][12] +
F[8][12] * D[9][12] +
F[8][6] * D[6][9] +
F[8][7] * D[7][9]) * T +
D[8][9];
P[8][10] = P[10][8] =
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) * T + D[8][10];
P[8][11] = P[11][8] =
(F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) * T + D[8][11];
P[8][12] = P[12][8] =
(F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) * T + D[8][12];
P[9][9] =
(Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] +
Q[2] * G[9][2] * G[9][2] + F[9][10] * (F[9][10] * D[10][10] +
F[9][11] * D[10][11] +
F[9][12] * D[10][12] +
F[9][6] * D[6][10] +
F[9][7] * D[7][10] +
F[9][8] * D[8][10]) +
F[9][11] * (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
F[9][7] * D[7][11] + F[9][8] * D[8][11]) +
F[9][12] * (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
F[9][7] * D[7][12] + F[9][8] * D[8][12]) +
F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] +
F[9][8] * D[6][8] + F[9][10] * D[6][10] +
F[9][11] * D[6][11] + F[9][12] * D[6][12]) +
F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] +
F[9][8] * D[7][8] + F[9][10] * D[7][10] +
F[9][11] * D[7][11] + F[9][12] * D[7][12]) +
F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] +
F[9][8] * D[8][8] + F[9][10] * D[8][10] +
F[9][11] * D[8][11] + F[9][12] * D[8][12])) * Tsq +
(2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] +
2 * F[9][12] * D[9][12] + 2 * F[9][6] * D[6][9] +
2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9]) * T + D[9][9];
P[9][10] = P[10][9] =
(F[9][10] * D[10][10] + F[9][11] * D[10][11] +
F[9][12] * D[10][12] + F[9][6] * D[6][10] +
F[9][7] * D[7][10] + F[9][8] * D[8][10]) * T + D[9][10];
P[9][11] = P[11][9] =
(F[9][10] * D[10][11] + F[9][11] * D[11][11] +
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
F[9][7] * D[7][11] + F[9][8] * D[8][11]) * T + D[9][11];
P[9][12] = P[12][9] =
(F[9][10] * D[10][12] + F[9][11] * D[11][12] +
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
F[9][7] * D[7][12] + F[9][8] * D[8][12]) * T + D[9][12];
P[10][10] = Q[6] * Tsq + D[10][10];
P[10][11] = P[11][10] = D[10][11];
P[10][12] = P[12][10] = D[10][12];
P[11][11] = Q[7] * Tsq + D[11][11];
P[11][12] = P[12][11] = D[11][12];
P[12][12] = Q[8] * Tsq + D[12][12];
}
#endif
// ************* SerialUpdate *******************
// Does the update step of the Kalman filter for the covariance and estimate
// Outputs are Xnew & Pnew, and are written over P and X
// Z is actual measurement, Y is predicted measurement
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
// where K=P*H'*inv[H*P*H'+R]
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
// i.e. the measurment noises are uncorrelated.
// It therefore uses a serial update that requires no matrix inversion by
// processing the measurements one at a time.
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
// The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update.
// ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed)
{
float HP[NUMX], HPHR, Error;
uint8_t i, j, k, m;
for (m = 0; m < NUMV; m++) {
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0;
for (k = 0; k < NUMX; k++)
HP[j] += H[m][k] * P[k][j];
}
HPHR = R[m]; // Find HPHR = H*P*H' + R
for (k = 0; k < NUMX; k++)
HPHR += HP[k] * H[m][k];
for (k = 0; k < NUMX; k++)
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++)
P[i][j] = P[j][i] =
P[i][j] - K[i][m] * HP[j];
}
Error = Z[m] - Y[m];
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
X[i] = X[i] + K[i][m] * Error;
}
}
}
// ************* RungeKutta **********************
// Does a 4th order Runge Kutta numerical integration step
// Output, Xnew, is written over X
// NOTE the algorithm assumes time invariant state equations and
// constant inputs over integration step
// ************************************************
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{
float dT2 =
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i = 0; i < NUMX; i++)
Xlast[i] = X[i]; // make a working copy
StateEq(X, U, K1); // k1 = f(x,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K1[i];
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K2[i];
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT * K3[i];
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i = 0; i < NUMX; i++)
X[i] =
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
K4[i]) / 6;
}
// ************* Model Specific Stuff ***************************
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
//
// State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
// Deterministic Inputs = [AngularVel Accel]
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
//
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
// Inputs to Measurement = [EarthFrameMagField]
//
// Notes: Pos and Vel in earth frame
// AngularVel and Accel in body frame
// MagFields are unit vectors
// Xdot is output of StateEq()
// F and G are outputs of LinearizeFG(), all elements not set should be zero
// y is output of OutputEq()
// H is output of LinearizeH(), all elements not set should be zero
// ************************************************
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
Xdot[0] = X[3];
Xdot[1] = X[4];
Xdot[2] = X[5];
// Vdot = Reb*a
Xdot[3] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
q0 * q3) *
ay + 2 * (q1 * q3 + q0 * q2) * az;
Xdot[4] =
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
q3 * q3) * ay + 2 * (q2 * q3 -
q0 * q1) *
az;
Xdot[5] =
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
// best guess is that bias stays constant
Xdot[10] = Xdot[11] = Xdot[12] = 0;
}
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
F[0][3] = F[1][4] = F[2][5] = 1;
// dVdot/dq
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
// dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
// F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
// F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
// F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dq
F[6][6] = 0;
F[6][7] = -wx / 2;
F[6][8] = -wy / 2;
F[6][9] = -wz / 2;
F[7][6] = wx / 2;
F[7][7] = 0;
F[7][8] = wz / 2;
F[7][9] = -wy / 2;
F[8][6] = wy / 2;
F[8][7] = -wz / 2;
F[8][8] = 0;
F[8][9] = wx / 2;
F[9][6] = wz / 2;
F[9][7] = wy / 2;
F[9][8] = -wx / 2;
F[9][9] = 0;
// dqdot/dwbias
F[6][10] = q1 / 2;
F[6][11] = q2 / 2;
F[6][12] = q3 / 2;
F[7][10] = -q0 / 2;
F[7][11] = q3 / 2;
F[7][12] = -q2 / 2;
F[8][10] = -q3 / 2;
F[8][11] = -q0 / 2;
F[8][12] = q1 / 2;
F[9][10] = q2 / 2;
F[9][11] = -q1 / 2;
F[9][12] = -q0 / 2;
// dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
G[3][4] = 2 * (-q1 * q2 + q0 * q3);
G[3][5] = -2 * (q1 * q3 + q0 * q2);
G[4][3] = -2 * (q1 * q2 + q0 * q3);
G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
G[4][5] = 2 * (-q2 * q3 + q0 * q1);
G[5][3] = 2 * (-q1 * q3 + q0 * q2);
G[5][4] = -2 * (q2 * q3 + q0 * q1);
G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
// dqdot/dnw
G[6][0] = q1 / 2;
G[6][1] = q2 / 2;
G[6][2] = q3 / 2;
G[7][0] = -q0 / 2;
G[7][1] = q3 / 2;
G[7][2] = -q2 / 2;
G[8][0] = -q3 / 2;
G[8][1] = -q0 / 2;
G[8][2] = q1 / 2;
G[9][0] = q2 / 2;
G[9][1] = -q1 / 2;
G[9][2] = -q0 / 2;
// dwbias = random walk noise
G[10][6] = G[11][7] = G[12][8] = 1;
// dabias = random walk noise
// G[13][9]=G[14][10]=G[15][11]=1; // NO BIAS STATES ON ACCELS
}
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// first six outputs are P and V
Y[0] = X[0];
Y[1] = X[1];
Y[2] = X[2];
Y[3] = X[3];
Y[4] = X[4];
Y[5] = X[5];
// Bb=Rbe*Be
Y[6] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
q0 * q2) * Be[2];
Y[7] =
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
q2 * q2 - q3 * q3) * Be[1] +
2 * (q2 * q3 + q0 * q1) * Be[2];
Y[8] =
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
q0 * q1) * Be[1] +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
// Alt = -Pz
Y[9] = -X[2];
}
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// dP/dP=I;
H[0][0] = H[1][1] = H[2][2] = 1;
// dV/dV=I;
H[3][3] = H[4][4] = H[5][5] = 1;
// dBb/dq
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
// dAlt/dPz = -1
H[9][2] = -1;
}
/**
* @}
* @}
*/

866
flight/AHRS/insgps16state.c
View File

@ -1,866 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @file insgps.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief An INS/GPS algorithm implemented with an EKF.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "insgps.h"
#include <math.h>
#include <stdint.h>
// constants/macros/typdefs
#define NUMX 16 // number of states, X is the state vector
#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector
#if defined(GENERAL_COV)
// This might trick people so I have a note here. There is a slower but bigger version of the
// code here but won't fit when debugging disabled (requires -Os)
#define COVARIANCE_PREDICTION_GENERAL
#endif
// Private functions
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW]);
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
// global to init to zero and maintain zero elements
float Be[3]; // local magnetic unit vector in NED frame
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
float K[NUMX][NUMV]; // feedback gain matrix
// ************* Exposed Functions ****************
// *************************************************
uint16_t ins_get_num_states()
{
return NUMX;
}
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1;
Be[1] = 0;
Be[2] = 0; // local magnetic unit vector
for (int i = 0; i < NUMX; i++) {
for (int j = 0; j < NUMX; j++) {
P[i][j] = 0; // zero all terms
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
X[6] = 1;
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
Q[9] = Q[10] = Q[11] = 2e-20; // accel bias random walk variance (m/s^3)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
R[9] = .05; // High freq altimeter noise variance (m^2)
}
void INSResetP(float PDiag[NUMX])
{
uint8_t i,j;
// if PDiag[i] nonzero then clear row and column and set diagonal element
for (i=0;i<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0;
P[i][i]=PDiag[i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
{
Nav.Pos[0] = X[0] = pos[0];
Nav.Pos[1] = X[1] = pos[1];
Nav.Pos[2] = X[2] = pos[2];
Nav.Vel[0] = X[3] = vel[0];
Nav.Vel[1] = X[4] = vel[1];
Nav.Vel[2] = X[5] = vel[2];
Nav.q[0] = X[6] = q[0];
Nav.q[1] = X[7] = q[1];
Nav.q[2] = X[8] = q[2];
Nav.q[3] = X[9] = q[3];
Nav.gyro_bias[0] = X[10] = gyro_bias[0];
Nav.gyro_bias[1] = X[11] = gyro_bias[1];
Nav.gyro_bias[2] = X[12] = gyro_bias[2];
Nav.accel_bias[0] = X[13] = accel_bias[0];
Nav.accel_bias[1] = X[14] = accel_bias[1];
Nav.accel_bias[2] = X[15] = accel_bias[2];
}
void INSPosVelReset(float pos[3], float vel[3])
{
for (int i = 0; i < 6; i++) {
for(int j = i; j < NUMX; j++) {
P[i][j] = 0; // zero the first 6 rows and columns
P[j][i] = 0;
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar, float VelVar)
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = VelVar;
R[4] = VelVar;
// R[5] = PosVar; // Don't change vertical velocity, not measured
}
void INSSetGyroBias(float gyro_bias[3])
{
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSSetAccelVar(float accel_var[3])
{
Q[3] = accel_var[0];
Q[4] = accel_var[1];
Q[5] = accel_var[2];
}
void INSSetGyroVar(float gyro_var[3])
{
Q[0] = gyro_var[0];
Q[1] = gyro_var[1];
Q[2] = gyro_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2];
}
void INSSetMagNorth(float B[3])
{
Be[0] = B[0];
Be[1] = B[1];
Be[2] = B[2];
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
float qmag;
// rate gyro inputs in units of rad/s
U[0] = gyro_data[0];
U[1] = gyro_data[1];
U[2] = gyro_data[2];
// accelerometer inputs in units of m/s
U[3] = accel_data[0];
U[4] = accel_data[1];
U[5] = accel_data[2];
// EKF prediction step
LinearizeFG(X, U, F, G);
RungeKutta(X, U, dT);
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
//CovariancePrediction(F,G,Q,dT,P);
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
}
void INSCovariancePrediction(float dT)
{
CovariancePrediction(F, G, Q, dT, P);
}
float zeros[3] = { 0, 0, 0 };
void MagCorrection(float mag_data[3])
{
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
}
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, zeros, Vel, BaroAlt,
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
BARO_SENSOR);
}
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(zeros, Pos, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt)
{
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
}
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
{
INSCorrection(mag_data, Pos, Vel, zeros[0],
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
}
void VelBaroCorrection(float Vel[3], float BaroAlt)
{
INSCorrection(zeros, zeros, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed)
{
float Z[10], Y[10];
float Bmag, qmag;
// GPS Position in meters and in local NED frame
Z[0] = Pos[0];
Z[1] = Pos[1];
Z[2] = Pos[2];
// GPS Velocity in meters and in local NED frame
Z[3] = Vel[0];
Z[4] = Vel[1];
Z[5] = Vel[2];
// magnetometer data in any units (use unit vector) and in body frame
Bmag =
sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] / Bmag;
Z[7] = mag_data[1] / Bmag;
Z[8] = mag_data[2] / Bmag;
// barometric altimeter in meters and in local NED frame
Z[9] = BaroAlt;
// EKF correction step
LinearizeH(X, Be, H);
MeasurementEq(X, Be, Y);
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
X[9] /= qmag;
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
Nav.q[0] = X[6];
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
Nav.accel_bias[0] = X[13];
Nav.accel_bias[1] = X[14];
Nav.accel_bias[2] = X[15];
}
// ************* CovariancePrediction *************
// Does the prediction step of the Kalman filter for the covariance matrix
// Output, Pnew, overwrites P, the input covariance
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
// Q is the discrete time covariance of process noise
// Q is vector of the diagonal for a square matrix with
// dimensions equal to the number of disturbance noise variables
// The General Method is very inefficient,not taking advantage of the sparse F and G
// The first Method is very specific to this implementation
// ************************************************
#ifdef COVARIANCE_PREDICTION_GENERAL
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float Dummy[NUMX][NUMX], dTsq;
uint8_t i, j, k;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
dTsq = dT * dT;
for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
for (j = 0; j < NUMX; j++) {
Dummy[i][j] = P[i][j] / dT;
for (k = 0; k < NUMX; k++)
Dummy[i][j] += F[i][k] * P[k][j];
}
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
P[i][j] = Dummy[i][j] / dT;
for (k = 0; k < NUMX; k++)
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
for (k = 0; k < NUMW; k++)
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
}
}
#else
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float D[NUMX][NUMX], T, Tsq;
uint8_t i, j;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
T = dT;
Tsq = dT * dT;
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
for (j = i; j < NUMX; j++)
D[i][j] = P[i][j];
// Brute force calculation of the elements of P
P[0][0] = D[3][3]*Tsq + (2*D[0][3])*T + D[0][0];
P[0][1] = P[1][0] = D[3][4]*Tsq + (D[0][4] + D[1][3])*T + D[0][1];
P[0][2] = P[2][0] = D[3][5]*Tsq + (D[0][5] + D[2][3])*T + D[0][2];
P[0][3] = P[3][0] = (F[3][6]*D[3][6] + F[3][7]*D[3][7] + F[3][8]*D[3][8] + F[3][9]*D[3][9] + F[3][13]*D[3][13] + F[3][14]*D[3][14] + F[3][15]*D[3][15])*Tsq + (D[3][3] + F[3][6]*D[0][6] + F[3][7]*D[0][7] + F[3][8]*D[0][8] + F[3][9]*D[0][9] + F[3][13]*D[0][13] + F[3][14]*D[0][14] + F[3][15]*D[0][15])*T + D[0][3];
P[0][4] = P[4][0] = (F[4][6]*D[3][6] + F[4][7]*D[3][7] + F[4][8]*D[3][8] + F[4][9]*D[3][9] + F[4][13]*D[3][13] + F[4][14]*D[3][14] + F[4][15]*D[3][15])*Tsq + (D[3][4] + F[4][6]*D[0][6] + F[4][7]*D[0][7] + F[4][8]*D[0][8] + F[4][9]*D[0][9] + F[4][13]*D[0][13] + F[4][14]*D[0][14] + F[4][15]*D[0][15])*T + D[0][4];
P[0][5] = P[5][0] = (F[5][6]*D[3][6] + F[5][7]*D[3][7] + F[5][8]*D[3][8] + F[5][9]*D[3][9] + F[5][13]*D[3][13] + F[5][14]*D[3][14] + F[5][15]*D[3][15])*Tsq + (D[3][5] + F[5][6]*D[0][6] + F[5][7]*D[0][7] + F[5][8]*D[0][8] + F[5][9]*D[0][9] + F[5][13]*D[0][13] + F[5][14]*D[0][14] + F[5][15]*D[0][15])*T + D[0][5];
P[0][6] = P[6][0] = (F[6][7]*D[3][7] + F[6][8]*D[3][8] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*Tsq + (D[3][6] + F[6][7]*D[0][7] + F[6][8]*D[0][8] + F[6][9]*D[0][9] + F[6][10]*D[0][10] + F[6][11]*D[0][11] + F[6][12]*D[0][12])*T + D[0][6];
P[0][7] = P[7][0] = (F[7][6]*D[3][6] + F[7][8]*D[3][8] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*Tsq + (D[3][7] + F[7][6]*D[0][6] + F[7][8]*D[0][8] + F[7][9]*D[0][9] + F[7][10]*D[0][10] + F[7][11]*D[0][11] + F[7][12]*D[0][12])*T + D[0][7];
P[0][8] = P[8][0] = (F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*Tsq + (D[3][8] + F[8][6]*D[0][6] + F[8][7]*D[0][7] + F[8][9]*D[0][9] + F[8][10]*D[0][10] + F[8][11]*D[0][11] + F[8][12]*D[0][12])*T + D[0][8];
P[0][9] = P[9][0] = (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12])*Tsq + (D[3][9] + F[9][6]*D[0][6] + F[9][7]*D[0][7] + F[9][8]*D[0][8] + F[9][10]*D[0][10] + F[9][11]*D[0][11] + F[9][12]*D[0][12])*T + D[0][9];
P[0][10] = P[10][0] = D[3][10]*T + D[0][10];
P[0][11] = P[11][0] = D[3][11]*T + D[0][11];
P[0][12] = P[12][0] = D[3][12]*T + D[0][12];
P[0][13] = P[13][0] = D[3][13]*T + D[0][13];
P[0][14] = P[14][0] = D[3][14]*T + D[0][14];
P[0][15] = P[15][0] = D[3][15]*T + D[0][15];
P[1][1] = D[4][4]*Tsq + (2*D[1][4])*T + D[1][1];
P[1][2] = P[2][1] = D[4][5]*Tsq + (D[1][5] + D[2][4])*T + D[1][2];
P[1][3] = P[3][1] = (F[3][6]*D[4][6] + F[3][7]*D[4][7] + F[3][8]*D[4][8] + F[3][9]*D[4][9] + F[3][13]*D[4][13] + F[3][14]*D[4][14] + F[3][15]*D[4][15])*Tsq + (D[3][4] + F[3][6]*D[1][6] + F[3][7]*D[1][7] + F[3][8]*D[1][8] + F[3][9]*D[1][9] + F[3][13]*D[1][13] + F[3][14]*D[1][14] + F[3][15]*D[1][15])*T + D[1][3];
P[1][4] = P[4][1] = (F[4][6]*D[4][6] + F[4][7]*D[4][7] + F[4][8]*D[4][8] + F[4][9]*D[4][9] + F[4][13]*D[4][13] + F[4][14]*D[4][14] + F[4][15]*D[4][15])*Tsq + (D[4][4] + F[4][6]*D[1][6] + F[4][7]*D[1][7] + F[4][8]*D[1][8] + F[4][9]*D[1][9] + F[4][13]*D[1][13] + F[4][14]*D[1][14] + F[4][15]*D[1][15])*T + D[1][4];
P[1][5] = P[5][1] = (F[5][6]*D[4][6] + F[5][7]*D[4][7] + F[5][8]*D[4][8] + F[5][9]*D[4][9] + F[5][13]*D[4][13] + F[5][14]*D[4][14] + F[5][15]*D[4][15])*Tsq + (D[4][5] + F[5][6]*D[1][6] + F[5][7]*D[1][7] + F[5][8]*D[1][8] + F[5][9]*D[1][9] + F[5][13]*D[1][13] + F[5][14]*D[1][14] + F[5][15]*D[1][15])*T + D[1][5];
P[1][6] = P[6][1] = (F[6][7]*D[4][7] + F[6][8]*D[4][8] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*Tsq + (D[4][6] + F[6][7]*D[1][7] + F[6][8]*D[1][8] + F[6][9]*D[1][9] + F[6][10]*D[1][10] + F[6][11]*D[1][11] + F[6][12]*D[1][12])*T + D[1][6];
P[1][7] = P[7][1] = (F[7][6]*D[4][6] + F[7][8]*D[4][8] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*Tsq + (D[4][7] + F[7][6]*D[1][6] + F[7][8]*D[1][8] + F[7][9]*D[1][9] + F[7][10]*D[1][10] + F[7][11]*D[1][11] + F[7][12]*D[1][12])*T + D[1][7];
P[1][8] = P[8][1] = (F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*Tsq + (D[4][8] + F[8][6]*D[1][6] + F[8][7]*D[1][7] + F[8][9]*D[1][9] + F[8][10]*D[1][10] + F[8][11]*D[1][11] + F[8][12]*D[1][12])*T + D[1][8];
P[1][9] = P[9][1] = (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12])*Tsq + (D[4][9] + F[9][6]*D[1][6] + F[9][7]*D[1][7] + F[9][8]*D[1][8] + F[9][10]*D[1][10] + F[9][11]*D[1][11] + F[9][12]*D[1][12])*T + D[1][9];
P[1][10] = P[10][1] = D[4][10]*T + D[1][10];
P[1][11] = P[11][1] = D[4][11]*T + D[1][11];
P[1][12] = P[12][1] = D[4][12]*T + D[1][12];
P[1][13] = P[13][1] = D[4][13]*T + D[1][13];
P[1][14] = P[14][1] = D[4][14]*T + D[1][14];
P[1][15] = P[15][1] = D[4][15]*T + D[1][15];
P[2][2] = D[5][5]*Tsq + (2*D[2][5])*T + D[2][2];
P[2][3] = P[3][2] = (F[3][6]*D[5][6] + F[3][7]*D[5][7] + F[3][8]*D[5][8] + F[3][9]*D[5][9] + F[3][13]*D[5][13] + F[3][14]*D[5][14] + F[3][15]*D[5][15])*Tsq + (D[3][5] + F[3][6]*D[2][6] + F[3][7]*D[2][7] + F[3][8]*D[2][8] + F[3][9]*D[2][9] + F[3][13]*D[2][13] + F[3][14]*D[2][14] + F[3][15]*D[2][15])*T + D[2][3];
P[2][4] = P[4][2] = (F[4][6]*D[5][6] + F[4][7]*D[5][7] + F[4][8]*D[5][8] + F[4][9]*D[5][9] + F[4][13]*D[5][13] + F[4][14]*D[5][14] + F[4][15]*D[5][15])*Tsq + (D[4][5] + F[4][6]*D[2][6] + F[4][7]*D[2][7] + F[4][8]*D[2][8] + F[4][9]*D[2][9] + F[4][13]*D[2][13] + F[4][14]*D[2][14] + F[4][15]*D[2][15])*T + D[2][4];
P[2][5] = P[5][2] = (F[5][6]*D[5][6] + F[5][7]*D[5][7] + F[5][8]*D[5][8] + F[5][9]*D[5][9] + F[5][13]*D[5][13] + F[5][14]*D[5][14] + F[5][15]*D[5][15])*Tsq + (D[5][5] + F[5][6]*D[2][6] + F[5][7]*D[2][7] + F[5][8]*D[2][8] + F[5][9]*D[2][9] + F[5][13]*D[2][13] + F[5][14]*D[2][14] + F[5][15]*D[2][15])*T + D[2][5];
P[2][6] = P[6][2] = (F[6][7]*D[5][7] + F[6][8]*D[5][8] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*Tsq + (D[5][6] + F[6][7]*D[2][7] + F[6][8]*D[2][8] + F[6][9]*D[2][9] + F[6][10]*D[2][10] + F[6][11]*D[2][11] + F[6][12]*D[2][12])*T + D[2][6];
P[2][7] = P[7][2] = (F[7][6]*D[5][6] + F[7][8]*D[5][8] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*Tsq + (D[5][7] + F[7][6]*D[2][6] + F[7][8]*D[2][8] + F[7][9]*D[2][9] + F[7][10]*D[2][10] + F[7][11]*D[2][11] + F[7][12]*D[2][12])*T + D[2][7];
P[2][8] = P[8][2] = (F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*Tsq + (D[5][8] + F[8][6]*D[2][6] + F[8][7]*D[2][7] + F[8][9]*D[2][9] + F[8][10]*D[2][10] + F[8][11]*D[2][11] + F[8][12]*D[2][12])*T + D[2][8];
P[2][9] = P[9][2] = (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12])*Tsq + (D[5][9] + F[9][6]*D[2][6] + F[9][7]*D[2][7] + F[9][8]*D[2][8] + F[9][10]*D[2][10] + F[9][11]*D[2][11] + F[9][12]*D[2][12])*T + D[2][9];
P[2][10] = P[10][2] = D[5][10]*T + D[2][10];
P[2][11] = P[11][2] = D[5][11]*T + D[2][11];
P[2][12] = P[12][2] = D[5][12]*T + D[2][12];
P[2][13] = P[13][2] = D[5][13]*T + D[2][13];
P[2][14] = P[14][2] = D[5][14]*T + D[2][14];
P[2][15] = P[15][2] = D[5][15]*T + D[2][15];
P[3][3] = (Q[3]*G[3][3]*G[3][3] + Q[4]*G[3][4]*G[3][4] + Q[5]*G[3][5]*G[3][5] + F[3][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[3][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[3][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[3][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[3][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[3][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[3][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (2*F[3][6]*D[3][6] + 2*F[3][7]*D[3][7] + 2*F[3][8]*D[3][8] + 2*F[3][9]*D[3][9] + 2*F[3][13]*D[3][13] + 2*F[3][14]*D[3][14] + 2*F[3][15]*D[3][15])*T + D[3][3];
P[3][4] = P[4][3] = (F[4][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[4][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[4][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[4][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[4][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[4][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[4][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[4][3]*Q[3] + G[3][4]*G[4][4]*Q[4] + G[3][5]*G[4][5]*Q[5])*Tsq + (F[3][6]*D[4][6] + F[4][6]*D[3][6] + F[3][7]*D[4][7] + F[4][7]*D[3][7] + F[3][8]*D[4][8] + F[4][8]*D[3][8] + F[3][9]*D[4][9] + F[4][9]*D[3][9] + F[3][13]*D[4][13] + F[4][13]*D[3][13] + F[3][14]*D[4][14] + F[4][14]*D[3][14] + F[3][15]*D[4][15] + F[4][15]*D[3][15])*T + D[3][4];
P[3][5] = P[5][3] = (F[5][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[5][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[5][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[5][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[5][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[5][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[5][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[5][3]*Q[3] + G[3][4]*G[5][4]*Q[4] + G[3][5]*G[5][5]*Q[5])*Tsq + (F[3][6]*D[5][6] + F[5][6]*D[3][6] + F[3][7]*D[5][7] + F[5][7]*D[3][7] + F[3][8]*D[5][8] + F[5][8]*D[3][8] + F[3][9]*D[5][9] + F[5][9]*D[3][9] + F[3][13]*D[5][13] + F[5][13]*D[3][13] + F[3][14]*D[5][14] + F[5][14]*D[3][14] + F[3][15]*D[5][15] + F[5][15]*D[3][15])*T + D[3][5];
P[3][6] = P[6][3] = (F[6][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[6][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[6][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[6][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[6][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[6][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[6][7]*D[3][7] + F[3][8]*D[6][8] + F[6][8]*D[3][8] + F[3][9]*D[6][9] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15])*T + D[3][6];
P[3][7] = P[7][3] = (F[7][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[7][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[7][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[7][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[7][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[7][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][7] + F[7][6]*D[3][6] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[7][8]*D[3][8] + F[3][9]*D[7][9] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15])*T + D[3][7];
P[3][8] = P[8][3] = (F[8][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[8][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[8][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[8][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[8][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[8][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]))*Tsq + (F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15])*T + D[3][8];
P[3][9] = P[9][3] = (F[9][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[9][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[9][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[9][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[9][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[9][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[3][9]*D[9][9] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9])*T + D[3][9];
P[3][10] = P[10][3] = (F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10])*T + D[3][10];
P[3][11] = P[11][3] = (F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11])*T + D[3][11];
P[3][12] = P[12][3] = (F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12])*T + D[3][12];
P[3][13] = P[13][3] = (F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13])*T + D[3][13];
P[3][14] = P[14][3] = (F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14])*T + D[3][14];
P[3][15] = P[15][3] = (F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15])*T + D[3][15];
P[4][4] = (Q[3]*G[4][3]*G[4][3] + Q[4]*G[4][4]*G[4][4] + Q[5]*G[4][5]*G[4][5] + F[4][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[4][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[4][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[4][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[4][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[4][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[4][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (2*F[4][6]*D[4][6] + 2*F[4][7]*D[4][7] + 2*F[4][8]*D[4][8] + 2*F[4][9]*D[4][9] + 2*F[4][13]*D[4][13] + 2*F[4][14]*D[4][14] + 2*F[4][15]*D[4][15])*T + D[4][4];
P[4][5] = P[5][4] = (F[5][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[5][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[5][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[5][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[5][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[5][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[5][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + G[4][3]*G[5][3]*Q[3] + G[4][4]*G[5][4]*Q[4] + G[4][5]*G[5][5]*Q[5])*Tsq + (F[4][6]*D[5][6] + F[5][6]*D[4][6] + F[4][7]*D[5][7] + F[5][7]*D[4][7] + F[4][8]*D[5][8] + F[5][8]*D[4][8] + F[4][9]*D[5][9] + F[5][9]*D[4][9] + F[4][13]*D[5][13] + F[5][13]*D[4][13] + F[4][14]*D[5][14] + F[5][14]*D[4][14] + F[4][15]*D[5][15] + F[5][15]*D[4][15])*T + D[4][5];
P[4][6] = P[6][4] = (F[6][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[6][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[6][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[6][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[6][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[6][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[6][7]*D[4][7] + F[4][8]*D[6][8] + F[6][8]*D[4][8] + F[4][9]*D[6][9] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15])*T + D[4][6];
P[4][7] = P[7][4] = (F[7][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[7][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[7][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[7][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[7][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[7][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][7] + F[7][6]*D[4][6] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[7][8]*D[4][8] + F[4][9]*D[7][9] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15])*T + D[4][7];
P[4][8] = P[8][4] = (F[8][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[8][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[8][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[8][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[8][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[8][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]))*Tsq + (F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15])*T + D[4][8];
P[4][9] = P[9][4] = (F[9][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[9][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[9][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[9][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[9][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[9][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[4][9]*D[9][9] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9])*T + D[4][9];
P[4][10] = P[10][4] = (F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10])*T + D[4][10];
P[4][11] = P[11][4] = (F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11])*T + D[4][11];
P[4][12] = P[12][4] = (F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12])*T + D[4][12];
P[4][13] = P[13][4] = (F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13])*T + D[4][13];
P[4][14] = P[14][4] = (F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14])*T + D[4][14];
P[4][15] = P[15][4] = (F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15])*T + D[4][15];
P[5][5] = (Q[3]*G[5][3]*G[5][3] + Q[4]*G[5][4]*G[5][4] + Q[5]*G[5][5]*G[5][5] + F[5][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[5][13]*(F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13]) + F[5][14]*(F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14]) + F[5][15]*(F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15]) + F[5][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[5][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[5][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (2*F[5][6]*D[5][6] + 2*F[5][7]*D[5][7] + 2*F[5][8]*D[5][8] + 2*F[5][9]*D[5][9] + 2*F[5][13]*D[5][13] + 2*F[5][14]*D[5][14] + 2*F[5][15]*D[5][15])*T + D[5][5];
P[5][6] = P[6][5] = (F[6][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[6][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[6][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[6][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[6][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[6][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[6][7]*D[5][7] + F[5][8]*D[6][8] + F[6][8]*D[5][8] + F[5][9]*D[6][9] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15])*T + D[5][6];
P[5][7] = P[7][5] = (F[7][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[7][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[7][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[7][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[7][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[7][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][7] + F[7][6]*D[5][6] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[7][8]*D[5][8] + F[5][9]*D[7][9] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15])*T + D[5][7];
P[5][8] = P[8][5] = (F[8][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[8][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[8][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[8][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[8][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[8][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]))*Tsq + (F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15])*T + D[5][8];
P[5][9] = P[9][5] = (F[9][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[9][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[9][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[9][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[9][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[9][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[5][9]*D[9][9] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9])*T + D[5][9];
P[5][10] = P[10][5] = (F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10])*T + D[5][10];
P[5][11] = P[11][5] = (F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11])*T + D[5][11];
P[5][12] = P[12][5] = (F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12])*T + D[5][12];
P[5][13] = P[13][5] = (F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13])*T + D[5][13];
P[5][14] = P[14][5] = (F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14])*T + D[5][14];
P[5][15] = P[15][5] = (F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15])*T + D[5][15];
P[6][6] = (Q[0]*G[6][0]*G[6][0] + Q[1]*G[6][1]*G[6][1] + Q[2]*G[6][2]*G[6][2] + F[6][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[6][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[6][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[6][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[6][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[6][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]))*Tsq + (2*F[6][7]*D[6][7] + 2*F[6][8]*D[6][8] + 2*F[6][9]*D[6][9] + 2*F[6][10]*D[6][10] + 2*F[6][11]*D[6][11] + 2*F[6][12]*D[6][12])*T + D[6][6];
P[6][7] = P[7][6] = (F[7][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[7][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[7][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[7][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[7][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[7][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[6][0]*G[7][0]*Q[0] + G[6][1]*G[7][1]*Q[1] + G[6][2]*G[7][2]*Q[2])*Tsq + (F[7][6]*D[6][6] + F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[7][8]*D[6][8] + F[6][9]*D[7][9] + F[7][9]*D[6][9] + F[6][10]*D[7][10] + F[7][10]*D[6][10] + F[6][11]*D[7][11] + F[7][11]*D[6][11] + F[6][12]*D[7][12] + F[7][12]*D[6][12])*T + D[6][7];
P[6][8] = P[8][6] = (F[8][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[8][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[8][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[8][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[8][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[8][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + G[6][0]*G[8][0]*Q[0] + G[6][1]*G[8][1]*Q[1] + G[6][2]*G[8][2]*Q[2])*Tsq + (F[6][7]*D[7][8] + F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[8][9]*D[6][9] + F[6][10]*D[8][10] + F[8][10]*D[6][10] + F[6][11]*D[8][11] + F[8][11]*D[6][11] + F[6][12]*D[8][12] + F[8][12]*D[6][12])*T + D[6][8];
P[6][9] = P[9][6] = (F[9][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[9][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[9][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[9][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[9][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[9][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[9][0]*G[6][0]*Q[0] + G[9][1]*G[6][1]*Q[1] + G[9][2]*G[6][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][9]*D[9][9] + F[9][10]*D[6][10] + F[6][10]*D[9][10] + F[9][11]*D[6][11] + F[6][11]*D[9][11] + F[9][12]*D[6][12] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9])*T + D[6][9];
P[6][10] = P[10][6] = (F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10])*T + D[6][10];
P[6][11] = P[11][6] = (F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11])*T + D[6][11];
P[6][12] = P[12][6] = (F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12])*T + D[6][12];
P[6][13] = P[13][6] = (F[6][9]*D[9][13] + F[6][10]*D[10][13] + F[6][11]*D[11][13] + F[6][12]*D[12][13] + F[6][7]*D[7][13] + F[6][8]*D[8][13])*T + D[6][13];
P[6][14] = P[14][6] = (F[6][9]*D[9][14] + F[6][10]*D[10][14] + F[6][11]*D[11][14] + F[6][12]*D[12][14] + F[6][7]*D[7][14] + F[6][8]*D[8][14])*T + D[6][14];
P[6][15] = P[15][6] = (F[6][9]*D[9][15] + F[6][10]*D[10][15] + F[6][11]*D[11][15] + F[6][12]*D[12][15] + F[6][7]*D[7][15] + F[6][8]*D[8][15])*T + D[6][15];
P[7][7] = (Q[0]*G[7][0]*G[7][0] + Q[1]*G[7][1]*G[7][1] + Q[2]*G[7][2]*G[7][2] + F[7][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[7][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[7][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[7][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[7][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[7][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]))*Tsq + (2*F[7][6]*D[6][7] + 2*F[7][8]*D[7][8] + 2*F[7][9]*D[7][9] + 2*F[7][10]*D[7][10] + 2*F[7][11]*D[7][11] + 2*F[7][12]*D[7][12])*T + D[7][7];
P[7][8] = P[8][7] = (F[8][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[8][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[8][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[8][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[8][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[8][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + G[7][0]*G[8][0]*Q[0] + G[7][1]*G[8][1]*Q[1] + G[7][2]*G[8][2]*Q[2])*Tsq + (F[7][6]*D[6][8] + F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[8][9]*D[7][9] + F[7][10]*D[8][10] + F[8][10]*D[7][10] + F[7][11]*D[8][11] + F[8][11]*D[7][11] + F[7][12]*D[8][12] + F[8][12]*D[7][12])*T + D[7][8];
P[7][9] = P[9][7] = (F[9][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[9][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[9][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[9][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[9][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[9][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + G[9][0]*G[7][0]*Q[0] + G[9][1]*G[7][1]*Q[1] + G[9][2]*G[7][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][9]*D[9][9] + F[9][10]*D[7][10] + F[7][10]*D[9][10] + F[9][11]*D[7][11] + F[7][11]*D[9][11] + F[9][12]*D[7][12] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9])*T + D[7][9];
P[7][10] = P[10][7] = (F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10])*T + D[7][10];
P[7][11] = P[11][7] = (F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11])*T + D[7][11];
P[7][12] = P[12][7] = (F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12])*T + D[7][12];
P[7][13] = P[13][7] = (F[7][9]*D[9][13] + F[7][10]*D[10][13] + F[7][11]*D[11][13] + F[7][12]*D[12][13] + F[7][6]*D[6][13] + F[7][8]*D[8][13])*T + D[7][13];
P[7][14] = P[14][7] = (F[7][9]*D[9][14] + F[7][10]*D[10][14] + F[7][11]*D[11][14] + F[7][12]*D[12][14] + F[7][6]*D[6][14] + F[7][8]*D[8][14])*T + D[7][14];
P[7][15] = P[15][7] = (F[7][9]*D[9][15] + F[7][10]*D[10][15] + F[7][11]*D[11][15] + F[7][12]*D[12][15] + F[7][6]*D[6][15] + F[7][8]*D[8][15])*T + D[7][15];
P[8][8] = (Q[0]*G[8][0]*G[8][0] + Q[1]*G[8][1]*G[8][1] + Q[2]*G[8][2]*G[8][2] + F[8][9]*(F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[8][11]*D[9][11] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9]) + F[8][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[8][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[8][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[8][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[8][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]))*Tsq + (2*F[8][6]*D[6][8] + 2*F[8][7]*D[7][8] + 2*F[8][9]*D[8][9] + 2*F[8][10]*D[8][10] + 2*F[8][11]*D[8][11] + 2*F[8][12]*D[8][12])*T + D[8][8];
P[8][9] = P[9][8] = (F[9][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[9][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[9][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[9][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[9][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[9][8]*(F[8][6]*D[6][8] + F[8][7]*D[7][8] + F[8][9]*D[8][9] + F[8][10]*D[8][10] + F[8][11]*D[8][11] + F[8][12]*D[8][12]) + G[9][0]*G[8][0]*Q[0] + G[9][1]*G[8][1]*Q[1] + G[9][2]*G[8][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][9]*D[9][9] + F[9][10]*D[8][10] + F[8][10]*D[9][10] + F[9][11]*D[8][11] + F[8][11]*D[9][11] + F[9][12]*D[8][12] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9])*T + D[8][9];
P[8][10] = P[10][8] = (F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10])*T + D[8][10];
P[8][11] = P[11][8] = (F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11])*T + D[8][11];
P[8][12] = P[12][8] = (F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12])*T + D[8][12];
P[8][13] = P[13][8] = (F[8][9]*D[9][13] + F[8][10]*D[10][13] + F[8][11]*D[11][13] + F[8][12]*D[12][13] + F[8][6]*D[6][13] + F[8][7]*D[7][13])*T + D[8][13];
P[8][14] = P[14][8] = (F[8][9]*D[9][14] + F[8][10]*D[10][14] + F[8][11]*D[11][14] + F[8][12]*D[12][14] + F[8][6]*D[6][14] + F[8][7]*D[7][14])*T + D[8][14];
P[8][15] = P[15][8] = (F[8][9]*D[9][15] + F[8][10]*D[10][15] + F[8][11]*D[11][15] + F[8][12]*D[12][15] + F[8][6]*D[6][15] + F[8][7]*D[7][15])*T + D[8][15];
P[9][9] = (Q[0]*G[9][0]*G[9][0] + Q[1]*G[9][1]*G[9][1] + Q[2]*G[9][2]*G[9][2] + F[9][10]*(F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10]) + F[9][11]*(F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11]) + F[9][12]*(F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12]) + F[9][6]*(F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[9][10]*D[6][10] + F[9][11]*D[6][11] + F[9][12]*D[6][12]) + F[9][7]*(F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[9][10]*D[7][10] + F[9][11]*D[7][11] + F[9][12]*D[7][12]) + F[9][8]*(F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[9][10]*D[8][10] + F[9][11]*D[8][11] + F[9][12]*D[8][12]))*Tsq + (2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12] + 2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9])*T + D[9][9];
P[9][10] = P[10][9] = (F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10])*T + D[9][10];
P[9][11] = P[11][9] = (F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11])*T + D[9][11];
P[9][12] = P[12][9] = (F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12])*T + D[9][12];
P[9][13] = P[13][9] = (F[9][10]*D[10][13] + F[9][11]*D[11][13] + F[9][12]*D[12][13] + F[9][6]*D[6][13] + F[9][7]*D[7][13] + F[9][8]*D[8][13])*T + D[9][13];
P[9][14] = P[14][9] = (F[9][10]*D[10][14] + F[9][11]*D[11][14] + F[9][12]*D[12][14] + F[9][6]*D[6][14] + F[9][7]*D[7][14] + F[9][8]*D[8][14])*T + D[9][14];
P[9][15] = P[15][9] = (F[9][10]*D[10][15] + F[9][11]*D[11][15] + F[9][12]*D[12][15] + F[9][6]*D[6][15] + F[9][7]*D[7][15] + F[9][8]*D[8][15])*T + D[9][15];
P[10][10] = Q[6]*Tsq + D[10][10];
P[10][11] = P[11][10] = D[10][11];
P[10][12] = P[12][10] = D[10][12];
P[10][13] = P[13][10] = D[10][13];
P[10][14] = P[14][10] = D[10][14];
P[10][15] = P[15][10] = D[10][15];
P[11][11] = Q[7]*Tsq + D[11][11];
P[11][12] = P[12][11] = D[11][12];
P[11][13] = P[13][11] = D[11][13];
P[11][14] = P[14][11] = D[11][14];
P[11][15] = P[15][11] = D[11][15];
P[12][12] = Q[8]*Tsq + D[12][12];
P[12][13] = P[13][12] = D[12][13];
P[12][14] = P[14][12] = D[12][14];
P[12][15] = P[15][12] = D[12][15];
P[13][13] = Q[9]*Tsq + D[13][13];
P[13][14] = P[14][13] = D[13][14];
P[13][15] = P[15][13] = D[13][15];
P[14][14] = Q[10]*Tsq + D[14][14];
P[14][15] = P[15][14] = D[14][15];
P[15][15] = Q[11]*Tsq + D[14][15];
}
#endif
// ************* SerialUpdate *******************
// Does the update step of the Kalman filter for the covariance and estimate
// Outputs are Xnew & Pnew, and are written over P and X
// Z is actual measurement, Y is predicted measurement
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
// where K=P*H'*inv[H*P*H'+R]
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
// i.e. the measurment noises are uncorrelated.
// It therefore uses a serial update that requires no matrix inversion by
// processing the measurements one at a time.
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
// The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update.
// ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed)
{
float HP[NUMX], HPHR, Error;
uint8_t i, j, k, m;
for (m = 0; m < NUMV; m++) {
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0;
for (k = 0; k < NUMX; k++)
HP[j] += H[m][k] * P[k][j];
}
HPHR = R[m]; // Find HPHR = H*P*H' + R
for (k = 0; k < NUMX; k++)
HPHR += HP[k] * H[m][k];
for (k = 0; k < NUMX; k++)
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++)
P[i][j] = P[j][i] =
P[i][j] - K[i][m] * HP[j];
}
Error = Z[m] - Y[m];
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
X[i] = X[i] + K[i][m] * Error;
}
}
}
// ************* RungeKutta **********************
// Does a 4th order Runge Kutta numerical integration step
// Output, Xnew, is written over X
// NOTE the algorithm assumes time invariant state equations and
// constant inputs over integration step
// ************************************************
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{
float dT2 =
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i = 0; i < NUMX; i++)
Xlast[i] = X[i]; // make a working copy
StateEq(X, U, K1); // k1 = f(x,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K1[i];
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K2[i];
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT * K3[i];
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i = 0; i < NUMX; i++)
X[i] =
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
K4[i]) / 6;
}
// ************* Model Specific Stuff ***************************
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
//
// State Variables = [Pos Vel Quaternion GyroBias AccelBias]
// Deterministic Inputs = [AngularVel Accel]
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise]
//
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
// Inputs to Measurement = [EarthFrameMagField]
//
// Notes: Pos and Vel in earth frame
// AngularVel and Accel in body frame
// MagFields are unit vectors
// Xdot is output of StateEq()
// F and G are outputs of LinearizeFG(), all elements not set should be zero
// y is output of OutputEq()
// H is output of LinearizeH(), all elements not set should be zero
// ************************************************
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
ax=U[3]-X[13];
ay=U[4]-X[14];
az=U[5]-X[15]; // subtract the biases on accels
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
Xdot[0] = X[3];
Xdot[1] = X[4];
Xdot[2] = X[5];
// Vdot = Reb*a
Xdot[3] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
q0 * q3) *
ay + 2 * (q1 * q3 + q0 * q2) * az;
Xdot[4] =
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
q3 * q3) * ay + 2 * (q2 * q3 -
q0 * q1) *
az;
Xdot[5] =
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
// best guess is that bias stays constant
Xdot[10] = Xdot[11] = Xdot[12] = 0;
Xdot[13] = Xdot[14] = Xdot[15] = 0;
}
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
F[0][3] = F[1][4] = F[2][5] = 1;
// dVdot/dq
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
// dVdot/dabias & dVdot/dna
F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dq
F[6][6] = 0;
F[6][7] = -wx / 2;
F[6][8] = -wy / 2;
F[6][9] = -wz / 2;
F[7][6] = wx / 2;
F[7][7] = 0;
F[7][8] = wz / 2;
F[7][9] = -wy / 2;
F[8][6] = wy / 2;
F[8][7] = -wz / 2;
F[8][8] = 0;
F[8][9] = wx / 2;
F[9][6] = wz / 2;
F[9][7] = wy / 2;
F[9][8] = -wx / 2;
F[9][9] = 0;
// dqdot/dwbias
F[6][10] = q1 / 2;
F[6][11] = q2 / 2;
F[6][12] = q3 / 2;
F[7][10] = -q0 / 2;
F[7][11] = q3 / 2;
F[7][12] = -q2 / 2;
F[8][10] = -q3 / 2;
F[8][11] = -q0 / 2;
F[8][12] = q1 / 2;
F[9][10] = q2 / 2;
F[9][11] = -q1 / 2;
F[9][12] = -q0 / 2;
// dVdot/dna - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
//G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3); G[3][5]=-2*(q1*q3+q0*q2);
//G[4][3]=-2*(q1*q2+q0*q3); G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; G[4][5]=2*(-q2*q3+q0*q1);
//G[5][3]=2*(-q1*q3+q0*q2); G[5][4]=-2*(q2*q3+q0*q1); G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dnw
G[6][0] = q1 / 2;
G[6][1] = q2 / 2;
G[6][2] = q3 / 2;
G[7][0] = -q0 / 2;
G[7][1] = q3 / 2;
G[7][2] = -q2 / 2;
G[8][0] = -q3 / 2;
G[8][1] = -q0 / 2;
G[8][2] = q1 / 2;
G[9][0] = q2 / 2;
G[9][1] = -q1 / 2;
G[9][2] = -q0 / 2;
// dwbias = random walk noise
G[10][6] = G[11][7] = G[12][8] = 1;
// dabias = random walk noise
G[13][9] = G[14][10] = G[15][11] = 1;
}
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// first six outputs are P and V
Y[0] = X[0];
Y[1] = X[1];
Y[2] = X[2];
Y[3] = X[3];
Y[4] = X[4];
Y[5] = X[5];
// Bb=Rbe*Be
Y[6] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
q0 * q2) * Be[2];
Y[7] =
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
q2 * q2 - q3 * q3) * Be[1] +
2 * (q2 * q3 + q0 * q1) * Be[2];
Y[8] =
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
q0 * q1) * Be[1] +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
// Alt = -Pz
Y[9] = -X[2];
}
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// dP/dP=I;
H[0][0] = H[1][1] = H[2][2] = 1;
// dV/dV=I;
H[3][3] = H[4][4] = H[5][5] = 1;
// dBb/dq
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
// dAlt/dPz = -1
H[9][2] = -1;
}
/**
* @}
* @}
*/

473
flight/AHRS/pios_board.c
View File

@ -1,473 +0,0 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the AHRS board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* OP Interface
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_op_irq_handler(void);
void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler")));
static const struct pios_spi_cfg pios_spi_op_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
uint32_t pios_spi_op_id;
void PIOS_SPI_op_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_op_id);
}
#endif /* PIOS_INCLUDE_SPI */
/*
* ADC system
*/
#include "pios_adc_priv.h"
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one
static const struct pios_adc_cfg pios_adc_cfg = {
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel1,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR,
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
}
},
.half_flag = DMA1_IT_HT1,
.full_flag = DMA1_IT_TC1,
};
struct pios_adc_dev pios_adc_devs[] = {
{
.cfg = &pios_adc_cfg,
.callback_function = NULL,
},
};
uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
void PIOS_ADC_handler() {
PIOS_ADC_DMA_Handler();
}
#if defined(PIOS_INCLUDE_USART)
#include <pios_usart_priv.h>
/*
* AUX USART
*/
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 230400,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl =
USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>
#define PIOS_COM_AUX_TX_BUF_LEN 192
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>
/*
* I2C Adapters
*/
void PIOS_I2C_main_adapter_ev_irq_handler(void);
void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C1_EV_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 200000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_main_adapter_id;
void PIOS_I2C_main_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id);
}
void PIOS_I2C_main_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id);
}
#endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_ENABLE_DEBUG_PINS)
static const struct stm32_gpio pios_debug_pins[] = {
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
};
#endif /* PIOS_ENABLE_DEBUG_PINS */
#include "pios_bmp085.h"
static const struct pios_bmp085_cfg pios_bmp085_cfg = {
.drdy = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_OD,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.eoc_exti = {
// .pin_source = GPIO_PinSource2,
// .port_source = GPIO_PortSourceGPIOC,
.init = {
.EXTI_Line = EXTI_Line2, // matches above GPIO pin
.EXTI_Mode = EXTI_Mode_Interrupt,
.EXTI_Trigger = EXTI_Trigger_Rising,
.EXTI_LineCmd = ENABLE,
},
},
.eoc_irq = {
.init = {
.NVIC_IRQChannel = EXTI15_10_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.xclr = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL,
},
},
.oversampling = 3,
};
extern const struct pios_com_driver pios_usart_com_driver;
uint32_t pios_com_aux_id;
uint8_t adc_fifo_buf[sizeof(float) * 6 * 4] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement
/**
* PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
PIOS_LED_On(LED1);
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
#if !defined(PIOS_ENABLE_DEBUG_PINS)
#if defined(PIOS_INCLUDE_COM)
{
uint32_t pios_usart_aux_id;
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
NULL, 0,
pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
PIOS_DEBUG_Assert(0);
}
}
#endif /* PIOS_INCLUDE_COM */
#endif
/* IAP System Setup */
PIOS_IAP_Init();
/* ADC system */
PIOS_ADC_Init();
extern uint8_t adc_oversampling;
PIOS_ADC_Config(adc_oversampling);
extern void adc_callback(float *);
PIOS_ADC_SetCallback(adc_callback);
/* ADC buffer */
extern t_fifo_buffer adc_fifo_buffer;
fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf));
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0);
SET_ACCEL_6G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
PIOS_HMC5843_Init();
#endif
PIOS_BMP085_Init(&pios_bmp085_cfg);
#if defined(PIOS_INCLUDE_SPI)
#include "ahrs_spi_comm.h"
AhrsInitComms();
/* Set up the SPI interface to the OP board */
if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
PIOS_DEBUG_Assert(0);
}
AhrsConnect(pios_spi_op_id);
#endif
}

37
flight/AHRS/test.c
View File

@ -1,37 +0,0 @@
#include "inc/insgps.h"
#include "stdio.h"
#include "math.h"
extern struct NavStruct Nav;
extern float X[13];
int main()
{
float gyro[3] = { 2.47, -0.25, 7.71 }, accel[3] = {
-1.02, 0.70, -10.11}, dT = 0.04, mags[3] = {
-50, -180, -376};
float Pos[3] = { 0, 0, 0 }, Vel[3] = {
0, 0, 0}, BaroAlt = 2.66, Speed = 4.4, Heading = 0;
float yaw;
int i, j;
INSGPSInit();
for (i = 0; i < 10000000; i++) {
INSPrediction(gyro, accel, dT);
//MagCorrection(mags);
FullCorrection(mags, Pos, Vel, BaroAlt);
yaw =
atan2((float)2 *
(Nav.q[0] * Nav.q[3] + Nav.q[1] * Nav.q[2]),
(float)(1 -
2 * (Nav.q[2] * Nav.q[2] +
Nav.q[3] * Nav.q[3]))) * 180 / M_PI;
printf("%0.3f ", yaw);
for (j = 0; j < 13; j++)
printf("%f ", X[j]);
printf("\r\n");
}
return 0;
}

590
flight/OpenPilot/Makefile
View File

@ -1,590 +0,0 @@
#####
# Project: OpenPilot
#
#
# Makefile for OpenPilot project build PiOS and the AP.
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
TOP := $(realpath $(WHEREAMI)/../../)
include $(TOP)/make/firmware-defs.mk
include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
# Target file name (without extension).
TARGET := fw_$(BOARD_NAME)
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES to compile for debugging
DEBUG ?= YES
# Include objects that are just nice information to show
DIAGNOSTICS ?= YES
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
# Remove command is different for Code Sourcery on Windows
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
else
REMOVE_CMD = rm
endif
FLASH_TOOL = OPENOCD
# List of modules to include
OPTMODULES = CameraStab GPS
MODULES = Actuator ManualControl Altitude AHRSComms Stabilization Guidance FirmwareIAP
PYMODULES = FlightPlan
#MODULES += Osd/OsdEtStd
#MODULES += Guidance
MODULES += Telemetry
# Paths
OPSYSTEM = ./System
OPSYSTEMINC = $(OPSYSTEM)/inc
OPUAVTALK = ../UAVTalk
OPUAVTALKINC = $(OPUAVTALK)/inc
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPTESTS = ./Tests
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = $(FLIGHTLIB)/inc
PIOS = ../PiOS
PIOSINC = $(PIOS)/inc
PIOSSTM32F10X = $(PIOS)/STM32F10x
PIOSCOMMON = $(PIOS)/Common
PIOSBOARDS = $(PIOS)/Boards
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
STMLIBDIR = $(APPLIBDIR)
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
STMSPDSRCDIR = $(STMSPDDIR)/src
STMSPDINCDIR = $(STMSPDDIR)/inc
STMUSBSRCDIR = $(STMUSBDIR)/src
STMUSBINCDIR = $(STMUSBDIR)/inc
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
DOSFSDIR = $(APPLIBDIR)/dosfs
MSDDIR = $(APPLIBDIR)/msd
RTOSDIR = $(APPLIBDIR)/FreeRTOS
RTOSSRCDIR = $(RTOSDIR)/Source
RTOSINCDIR = $(RTOSSRCDIR)/include
DOXYGENDIR = ../Doc/Doxygen
AHRSBOOTLOADER = ../Bootloaders/AHRS/
AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
PYMITE = $(FLIGHTLIB)/PyMite
PYMITELIB = $(PYMITE)/lib
PYMITEPLAT = $(PYMITE)/platform/openpilot
PYMITETOOLS = $(PYMITE)/tools
PYMITEVM = $(PYMITE)/vm
PYMITEINC = $(PYMITEVM)
PYMITEINC += $(PYMITEPLAT)
PYMITEINC += $(OUTDIR)
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
ifndef TESTAPP
## PyMite files and modules
SRC += $(OUTDIR)/pmlib_img.c
SRC += $(OUTDIR)/pmlib_nat.c
SRC += $(OUTDIR)/pmlibusr_img.c
SRC += $(OUTDIR)/pmlibusr_nat.c
PYSRC += $(wildcard ${PYMITEVM}/*.c)
PYSRC += $(wildcard ${PYMITEPLAT}/*.c)
PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += $(PYSRC)
## MODULES
SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/openpilot.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPSYSTEM)/taskmonitor.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
else
## TESTCODE
SRC += $(OPTESTS)/test_common.c
SRC += $(OPTESTS)/$(TESTAPP).c
endif
## UAVOBJECTS
ifndef TESTAPP
#include $(UAVOBJSYNTHDIR)/Makefile.inc
include ./UAVObjects.inc
SRC += $(UAVOBJSRC)
endif
## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_sys.c
SRC += $(PIOSSTM32F10X)/pios_led.c
SRC += $(PIOSSTM32F10X)/pios_delay.c
SRC += $(PIOSSTM32F10X)/pios_usart.c
SRC += $(PIOSSTM32F10X)/pios_irq.c
SRC += $(PIOSSTM32F10X)/pios_adc.c
SRC += $(PIOSSTM32F10X)/pios_servo.c
SRC += $(PIOSSTM32F10X)/pios_i2c.c
SRC += $(PIOSSTM32F10X)/pios_spi.c
SRC += $(PIOSSTM32F10X)/pios_ppm.c
SRC += $(PIOSSTM32F10X)/pios_pwm.c
SRC += $(PIOSSTM32F10X)/pios_dsm.c
SRC += $(PIOSSTM32F10X)/pios_sbus.c
SRC += $(PIOSSTM32F10X)/pios_tim.c
SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.c
SRC += $(PIOSSTM32F10X)/pios_exti.c
SRC += $(PIOSSTM32F10X)/pios_rtc.c
SRC += $(PIOSSTM32F10X)/pios_wdg.c
SRC += $(PIOSSTM32F10X)/pios_iap.c
# PIOS USB related files (seperated to make code maintenance more easy)
SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_desc.c
#SRC += $(PIOSSTM32F10X)/pios_usb_hid_endp.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c
SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c
#SRC += $(PIOSSTM32F10X)/pios_bmp085.c
SRC += $(PIOSSTM32F10X)/pios_bl_helper.c
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_crc.c
SRC += $(PIOSCOMMON)/pios_sdcard.c
SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c
SRC += $(CMSISDIR)/system_stm32f10x.c
## Used parts of the STM-Library
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
SRC += $(STMSPDSRCDIR)/stm32f10x_iwdg.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
SRC += $(STMSPDSRCDIR)/misc.c
## STM32 USB Library
SRC += $(STMUSBSRCDIR)/usb_core.c
SRC += $(STMUSBSRCDIR)/usb_init.c
SRC += $(STMUSBSRCDIR)/usb_int.c
SRC += $(STMUSBSRCDIR)/usb_mem.c
SRC += $(STMUSBSRCDIR)/usb_regs.c
SRC += $(STMUSBSRCDIR)/usb_sil.c
## RTOS
SRC += $(RTOSSRCDIR)/list.c
SRC += $(RTOSSRCDIR)/queue.c
SRC += $(RTOSSRCDIR)/tasks.c
## RTOS Portable
SRC += $(RTOSSRCDIR)/portable/GCC/ARM_CM3/port.c
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_2.c
## Dosfs file system
SRC += $(DOSFSDIR)/dosfs.c
SRC += $(DOSFSDIR)/dfs_sdcard.c
## AHRS boot loader comms
SRC += $(AHRSBOOTLOADER)/ahrs_spi_program_master.c
SRC += $(AHRSBOOTLOADER)/ahrs_spi_program.c
## Mass Storage Device
#SRC += $(MSDDIR)/msd.c
#SRC += $(MSDDIR)/msd_bot.c
#SRC += $(MSDDIR)/msd_desc.c
#SRC += $(MSDDIR)/msd_memory.c
#SRC += $(MSDDIR)/msd_scsi.c
#SRC += $(MSDDIR)/msd_scsi_data.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)$(MODEL_SUFFIX).S
# List Assembler source files here which must be assembled in ARM-Mode..
ASRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS = $(OPSYSTEM)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(OPUAVTALK)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSSTM32F10X)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)
EXTRAINCDIRS += $(STMSPDINCDIR)
EXTRAINCDIRS += $(STMUSBINCDIR)
EXTRAINCDIRS += $(CMSISDIR)
EXTRAINCDIRS += $(DOSFSDIR)
EXTRAINCDIRS += $(MSDDIR)
EXTRAINCDIRS += $(RTOSINCDIR)
EXTRAINCDIRS += $(APPLIBDIR)
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
EXTRAINCDIRS += $(AHRSBOOTLOADERINC)
EXTRAINCDIRS += $(PYMITEINC)
EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES} ${PYMODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
OPT = 0
else
OPT = s
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
CDEFS = -DSTM32F10X_$(MODEL)
CDEFS += -DUSE_STDPERIPH_DRIVER
CDEFS += -DUSE_$(BOARD)
ifeq ($(ENABLE_DEBUG_PINS), YES)
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
endif
ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART
endif
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS = -DDEBUG
endif
ifeq ($(DIAGNOSTICS),YES)
CFLAGS = -DDIAGNOSTICS
endif
CFLAGS += -g$(DEBUGF)
CFLAGS += -O$(OPT)
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
CFLAGS += -Werror
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mcpu=$(MCU) -mthumb -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
# Set linker-script name depending on selected submodel name
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
# Define programs and commands.
REMOVE = $(REMOVE_CMD) -f
PYTHON = python
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
# Default target.
all: gccversion build
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin lss sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin lss sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Generate intermediate code
# Generate code for PyMite
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py)
@echo $(MSG_PYMITEINIT) $(call toprel, $@)
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
# Compile: create assembler files from C source files. ARM only
$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
# Add jtag targets (program and wipe)
$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
.PHONY: elf lss sym hex bin bino opfw
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
bino: $(OUTDIR)/$(TARGET).bin.o
opfw: $(OUTDIR)/$(TARGET).opfw
# Display sizes of sections.
$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Install: install binary file with prefix/suffix into install directory
install: $(OUTDIR)/$(TARGET).opfw
ifneq ($(INSTALL_DIR),)
@echo $(MSG_INSTALLING) $(call toprel, $<)
$(V1) mkdir -p $(INSTALL_DIR)
$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
else
$(error INSTALL_DIR must be specified for $@)
endif
# Target: clean project.
clean: clean_list
clean_list :
@echo $(MSG_CLEANING)
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
$(V1) $(REMOVE) $(wildcard $(OUTDIR)/*.c)
$(V1) $(REMOVE) $(wildcard $(OUTDIR)/*.h)
$(V1) $(REMOVE) $(ALLOBJ)
$(V1) $(REMOVE) $(LSTFILES)
$(V1) $(REMOVE) $(DEPFILES)
$(V1) $(REMOVE) $(SRC:.c=.s)
$(V1) $(REMOVE) $(SRCARM:.c=.s)
$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
else
$(shell mkdir -p $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir -p $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all build clean clean_list install

665
flight/OpenPilot/Makefile.posix
View File

@ -1,665 +0,0 @@
#####
# Project: OpenPilot
#
#
# Makefile for OpenPilot project build PiOS and the AP.
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
# Set developer code and compile options
# Set to YES to compile for debugging
DEBUG ?= YES
# Include objects that are just nice information to show
DIAGNOSTICS ?= YES
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
#
USE_BOOTLOADER ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= NO
# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
TCHAIN_PREFIX ?= ""
# Remove command is different for Code Sourcery on Windows
REMOVE_CMD ?= rm
FLASH_TOOL = OPENOCD
# YES enables -mthumb option to flags for source-files listed
# in SRC and CPPSRC
USE_THUMB_MODE = YES
# List of modules to include
OPTMODULES = CameraStab GPS
MODULES = Telemetry Actuator Stabilization Guidance ManualControl
#MODULES = Telemetry ManualControl Actuator Attitude Stabilization
#MODULES = Telemetry Example
#MODULES = Telemetry MK/MKSerial
PYMODULES = FlightPlan
#MODULES += Osd/OsdEtStd
# MCU name, submodel and board
# - MCU used for compiler-option (-mtune)
# - MODEL used for linker-script name (-T) and passed as define
# - BOARD just passed as define (optional)
MCU = i686
#CHIP = STM32F103RET
#BOARD = STM3210E_OP
MODEL = HD
ifeq ($(USE_BOOTLOADER), YES)
BOOT_MODEL = $(MODEL)_BL
else
BOOT_MODEL = $(MODEL)_NB
endif
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR = ../../build/sitl_posix
# Target file name (without extension).
TARGET = OpenPilot
# Paths
OPSYSTEM = ./System
OPSYSTEMINC = $(OPSYSTEM)/inc
OPUAVTALK = ../UAVTalk
OPUAVTALKINC = $(OPUAVTALK)/inc
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPTESTS = ./Tests
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = $(FLIGHTLIB)/inc
PIOS = ../PiOS.posix
PIOSINC = $(PIOS)/inc
PIOSPOSIX = $(PIOS)/posix
APPLIBDIR = $(PIOSPOSIX)/Libraries
RTOSDIR = $(APPLIBDIR)/FreeRTOS
RTOSSRCDIR = $(RTOSDIR)/Source
RTOSINCDIR = $(RTOSSRCDIR)/include
DOXYGENDIR = ../Doc/Doxygen
AHRSBOOTLOADER = ../Bootloaders/AHRS/
AHRSBOOTLOADERINC = $(AHRSBOOTLOADER)/inc
PYMITE = $(FLIGHTLIB)/PyMite
PYMITELIB = $(PYMITE)/lib
PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
PYMITETOOLS = $(PYMITE)/tools
PYMITEVM = $(PYMITE)/vm
PYMITEINC = $(PYMITEVM)
PYMITEINC += $(PYMITEPLAT)
PYMITEINC += $(OUTDIR)
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
UAVOBJPYTHONSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/python
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
MODNAMES = $(notdir ${MODULES})
MODNAMES += $(notdir ${OPTMODULES})
ifndef TESTAPP
## PyMite files
SRC += $(OUTDIR)/pmlib_img.c
SRC += $(OUTDIR)/pmlib_nat.c
SRC += $(OUTDIR)/pmlibusr_img.c
SRC += $(OUTDIR)/pmlibusr_nat.c
PYSRC += $(wildcard ${PYMITEVM}/*.c)
PYSRC += $(wildcard ${PYMITEPLAT}/*.c)
PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += $(PYSRC)
## MODULES
SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += ${OUTDIR}/InitMods.c
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/openpilot.c
SRC += $(OPSYSTEM)/pios_board_posix.c
SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPSYSTEM)/taskmonitor.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
else
## TESTCODE
SRC += $(OPTESTS)/test_common.c
SRC += $(OPTESTS)/$(TESTAPP).c
endif
## UAVOBJECTS
ifndef TESTAPP
#include $(UAVOBJSYNTHDIR)/Makefile.inc
include ./UAVObjects.inc
SRC += $(UAVOBJSRC)
CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
endif
## PIOS Hardware (posix)
SRC += $(PIOSPOSIX)/pios_crc.c
SRC += $(PIOSPOSIX)/pios_sys.c
SRC += $(PIOSPOSIX)/pios_led.c
SRC += $(PIOSPOSIX)/pios_irq.c
SRC += $(PIOSPOSIX)/pios_delay.c
SRC += $(PIOSPOSIX)/pios_sdcard.c
SRC += $(PIOSPOSIX)/pios_udp.c
SRC += $(PIOSPOSIX)/pios_com.c
SRC += $(PIOSPOSIX)/pios_servo.c
SRC += $(PIOSPOSIX)/pios_wdg.c
SRC += $(PIOSPOSIX)/pios_debug.c
SRC += $(PIOSPOSIX)/pios_rcvr.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
## RTOS and RTOS Portable
SRC += $(RTOSSRCDIR)/list.c
SRC += $(RTOSSRCDIR)/queue.c
UNAME := $(shell uname)
ifeq ($(UNAME), Linux)
SRC += $(RTOSSRCDIR)/tasks_linux.c
SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port_linux.c
else
SRC += $(RTOSSRCDIR)/tasks_posix.c
SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port_posix.c
endif
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS = $(OPSYSTEM)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(OPUAVTALK)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSPOSIX)
EXTRAINCDIRS += $(RTOSINCDIR)
EXTRAINCDIRS += $(APPLIBDIR)
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/Posix
EXTRAINCDIRS += $(PYMITEINC)
EXTRAINCDIRS += ${foreach MOD, ${PYMODULES} ${OPTMODULES} ${MODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS =
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
OPT = 0
else
OPT = s
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
#DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
ifeq ($(ENABLE_DEBUG_PINS), YES)
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
endif
ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART
endif
ifeq ($(USE_BOOTLOADER), YES)
CDEFS += -DUSE_BOOTLOADER
endif
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS = -g$(DEBUGF) -DDEBUG
endif
ifeq ($(DIAGNOSTICS),YES)
CFLAGS = -DDIAGNOSTICS
endif
CFLAGS += $(CFLAGS_UAVOBJECTS)
CFLAGS += -DARCH_POSIX
CFLAGS += -O$(OPT)
CFLAGS += -mtune=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
CFLAGS += -Werror
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mtune=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS += -lpthread
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
# Define programs and commands.
CC = $(TCHAIN_PREFIX)gcc
CPP = $(TCHAIN_PREFIX)g++
AR = $(TCHAIN_PREFIX)ar
OBJCOPY = $(TCHAIN_PREFIX)objcopy
OBJDUMP = $(TCHAIN_PREFIX)objdump
SIZE = $(TCHAIN_PREFIX)size
NM = $(TCHAIN_PREFIX)nm
REMOVE = $(REMOVE_CMD) -f
PYTHON = python
###SHELL = sh
###COPY = cp
# Define Messages
# English
MSG_ERRORS_NONE = Errors: none
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
MSG_END = ${quote}-------- end --------${quote}
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
MSG_LINKING = ${quote}**** Linking :${quote}
MSG_COMPILING = ${quote}**** Compiling C :${quote}
MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
MSG_CLEANING = ${quote}Cleaning project:${quote}
MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
# Default target.
#all: begin gccversion sizebefore build sizeafter finished end
#all: begin gencode gccversion build sizeafter finished end
all: elf
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin lss sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin lss sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Test if quotes are needed for the echo-command
result = ${shell echo "test"}
ifeq (${result}, test)
quote = '
else
quote =
endif
# Generate code for module initialization
${OUTDIR}/InitMods.c: Makefile.posix
@echo ${MSG_MODINIT}
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Start(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void StartModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Start();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
# Generate code for PyMite
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py)
@echo $(MSG_PYMITEINIT) $(call toprel, $@)
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
# Eye candy.
begin:
## @echo
@echo $(MSG_BEGIN)
finished:
## @echo $(MSG_ERRORS_NONE)
end:
@echo $(MSG_END)
## @echo
# Display sizes of sections.
ELFSIZE = $(SIZE) -A $(OUTDIR)/$(TARGET).elf
##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
sizebefore:
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
sizeafter:
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
@echo $(MSG_SIZE_AFTER)
$(ELFSIZE)
# Display compiler version information.
gccversion :
@$(CC) --version
# @echo $(ALLOBJ)
# Program the device.
ifeq ($(USE_BOOTLOADER), YES)
# Program the device with OP Upload Tool".
program: $(OUTDIR)/$(TARGET).bin
@echo ${quote}Programming with OP Upload Tool${quote}
../../ground/src/experimental/upload-build-desktop/debug/OPUploadTool -d 0 -p $(OUTDIR)/$(TARGET).bin
else
ifeq ($(FLASH_TOOL),OPENOCD)
# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
program: $(OUTDIR)/$(TARGET).elf
@echo ${quote}Programming with OPENOCD${quote}
$(OOCD_EXE) $(OOCD_CL)
endif
endif
# Create final output file (.hex) from ELF output file.
%.hex: %.elf
## @echo
@echo $(MSG_LOAD_FILE) $@
$(OBJCOPY) -O ihex $< $@
# Create final output file (.bin) from ELF output file.
%.bin: %.elf
## @echo
@echo $(MSG_LOAD_FILE) $@
$(OBJCOPY) -O binary $< $@
# Create extended listing file/disassambly from ELF output file.
# using objdump testing: option -C
%.lss: %.elf
## @echo
@echo $(MSG_EXTENDED_LISTING) $@
$(OBJDUMP) -h -S -C -r $< > $@
# $(OBJDUMP) -x -S $< > $@
# Create a symbol table from ELF output file.
%.sym: %.elf
## @echo
@echo $(MSG_SYMBOL_TABLE) $@
$(NM) -n $< > $@
# Link: create ELF output file from object files.
.SECONDARY : $(TARGET).elf
.PRECIOUS : $(ALLOBJ)
%.elf: $(ALLOBJ)
@echo $(MSG_LINKING) $@
# use $(CC) for C-only projects or $(CPP) for C++-projects:
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# Assemble: create object files from assembler source files.
define ASSEMBLE_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_ASSEMBLING) $$< to $$@
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
endef
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
define ASSEMBLE_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
$(CC) -c $$(ASFLAGS) $$< -o $$@
endef
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
define COMPILE_C_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILING) $$< to $$@
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
endef
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
define COMPILE_C_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILING_ARM) $$< to $$@
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
endef
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
define COMPILE_CPP_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILINGCPP) $$< to $$@
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
endef
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
define COMPILE_CPP_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
endef
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(SRC:.c=.s) : %.s : %.c
@echo $(MSG_ASMFROMC) $< to $@
$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
# Compile: create assembler files from C source files. ARM only
$(SRCARM:.c=.s) : %.s : %.c
@echo $(MSG_ASMFROMC_ARM) $< to $@
$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Target: clean project.
clean: begin clean_list finished end
clean_list :
## @echo
@echo $(MSG_CLEANING)
$(REMOVE) $(OUTDIR)/$(TARGET).map
$(REMOVE) $(OUTDIR)/$(TARGET).elf
$(REMOVE) $(OUTDIR)/$(TARGET).hex
$(REMOVE) $(OUTDIR)/$(TARGET).bin
$(REMOVE) $(OUTDIR)/$(TARGET).sym
$(REMOVE) $(OUTDIR)/$(TARGET).lss
$(REMOVE) $(wildcard $(OUTDIR)/*.c)
$(REMOVE) $(wildcard $(OUTDIR)/*.h)
$(REMOVE) $(ALLOBJ)
$(REMOVE) $(LSTFILES)
$(REMOVE) $(DEPFILES)
$(REMOVE) $(SRC:.c=.s)
$(REMOVE) $(SRCARM:.c=.s)
$(REMOVE) $(CPPSRC:.cpp=.s)
$(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(OUTDIR) 2>NUL)
else
$(shell mkdir $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(OUTDIR)\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all begin finish end sizebefore sizeafter gccversion \
build elf hex bin lss sym clean clean_list program

637
flight/OpenPilot/Makefile.win32
View File

@ -1,637 +0,0 @@
#####
# Project: OpenPilot
#
#
# Makefile for OpenPilot project build PiOS and the AP.
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
# Set developer code and compile options
# Set to YES to compile for debugging
DEBUG ?= YES
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
#
USE_BOOTLOADER ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= NO
# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
TCHAIN_PREFIX ?= mingw32-
# Remove command is different for Code Sourcery on Windows
REMOVE_CMD ?= rm
FLASH_TOOL = OPENOCD
# YES enables -mthumb option to flags for source-files listed
# in SRC and CPPSRC
USE_THUMB_MODE = YES
# List of modules to include
MODULES = Telemetry Actuator Stabilization Guidance ManualControl FlightPlan
#MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization
#MODULES = Telemetry Example
#MODULES = Telemetry MK/MKSerial
#MODULES += Osd/OsdEtStd
# MCU name, submodel and board
# - MCU used for compiler-option (-mtune)
# - MODEL used for linker-script name (-T) and passed as define
# - BOARD just passed as define (optional)
MCU = i686
#CHIP = STM32F103RET
#BOARD = STM3210E_OP
ifeq ($(USE_BOOTLOADER), YES)
MODEL = HD_BL
else
MODEL = HD_NB
endif
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
OUTDIR = ../../build/sitl_win32
# Target file name (without extension).
TARGET = OpenPilot
# Paths
OPSYSTEM = ./System
OPSYSTEMINC = $(OPSYSTEM)/inc
OPUAVTALK = ../UAVTalk
OPUAVTALKINC = $(OPUAVTALK)/inc
OPUAVOBJ = ../UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPTESTS = ./Tests
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = ../Libraries/inc
PIOS = ../PiOS.win32
PIOSINC = $(PIOS)/inc
PIOSWIN32 = $(PIOS)/win32
APPLIBDIR = $(PIOSWIN32)/Libraries
RTOSDIR = $(APPLIBDIR)/FreeRTOS
RTOSSRCDIR = $(RTOSDIR)/Source
RTOSINCDIR = $(RTOSSRCDIR)/include
DOXYGENDIR = ../Doc/Doxygen
PYMITE = $(FLIGHTLIB)/PyMite
PYMITELIB = $(PYMITE)/lib
PYMITEPLAT = $(PYMITE)/platform/openpilot_sitl
PYMITETOOLS = $(PYMITE)/tools
PYMITEVM = $(PYMITE)/vm
PYMITEINC = $(PYMITEVM)
PYMITEINC += $(PYMITEPLAT)
PYMITEINC += $(OUTDIR)
FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib
FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans
UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
MODNAMES = $(notdir ${MODULES})
ifndef TESTAPP
## PyMite files
SRC += $(OUTDIR)/pmlib_img.c
SRC += $(OUTDIR)/pmlib_nat.c
SRC += $(OUTDIR)/pmlibusr_img.c
SRC += $(OUTDIR)/pmlibusr_nat.c
SRC += $(wildcard ${PYMITEVM}/*.c)
SRC += $(wildcard ${PYMITEPLAT}/*.c)
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += ${OUTDIR}/InitMods.c
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/openpilot.c
SRC += $(OPSYSTEM)/pios_board_posix.c
SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPSYSTEM)/taskmonitor.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
else
## TESTCODE
SRC += $(OPTESTS)/test_common.c
SRC += $(OPTESTS)/$(TESTAPP).c
endif
## UAVOBJECTS
ifndef TESTAPP
#include $(UAVOBJSYNTHDIR)/Makefile.inc
include ./UAVObjects.inc
SRC += $(UAVOBJSRC)
CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
endif
## PIOS Hardware (win32)
SRC += $(PIOSWIN32)/pios_sys.c
SRC += $(PIOSWIN32)/pios_led.c
SRC += $(PIOSWIN32)/pios_delay.c
SRC += $(PIOSWIN32)/pios_sdcard.c
SRC += $(PIOSWIN32)/pios_udp.c
SRC += $(PIOSWIN32)/pios_com.c
SRC += $(PIOSWIN32)/pios_servo.c
SRC += $(PIOSWIN32)/pios_wdg.c
SRC += $(PIOSWIN32)/pios_crc.c
#
## RTOS
SRC += $(RTOSSRCDIR)/list.c
SRC += $(RTOSSRCDIR)/queue.c
SRC += $(RTOSSRCDIR)/tasks.c
#
## RTOS Portable
SRC += $(RTOSSRCDIR)/portable/GCC/Win32/port.c
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
# use file-extension c for "c-only"-files
## just for testing, timer.c could be compiled in thumb-mode too
SRCARM =
# List C++ source files here.
# use file-extension .cpp for C++-files (not .C)
CPPSRC =
# List C++ source files here which must be compiled in ARM-Mode.
# use file-extension .cpp for C++-files (not .C)
#CPPSRCARM = $(TARGET).cpp
CPPSRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS = $(PIOSINC)
EXTRAINCDIRS += $(OPSYSTEM)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(OPUAVTALK)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSWIN32)
EXTRAINCDIRS += $(MININIDIR)
EXTRAINCDIRS += $(RTOSINCDIR)
EXTRAINCDIRS += $(APPLIBDIR)
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/Win32
EXTRAINCDIRS += $(FLIGHTLIB)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PYMITEINC)
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
EXTRAINCDIRS += ${foreach MOD, ${MODULES}, ${OPMODULEDIR}/${MOD}/inc} ${OPMODULEDIR}/System/inc
# List any extra directories to look for library files here.
# Also add directories where the linker should search for
# includes from linker-script to the list
# Each directory must be seperated by a space.
EXTRA_LIBDIRS =
# Extra Libraries
# Each library-name must be seperated by a space.
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
# EXTRA_LIBS = xyz abc efsl
# for newlib-lpc (file: libnewlibc-lpc.a):
# EXTRA_LIBS = newlib-lpc
EXTRA_LIBS = WS2_32 Winmm
# Path to Linker-Scripts
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
OPT = 0
else
OPT = s
endif
# Output format. (can be ihex or binary or both)
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
# ihex to create a load-image in Intel hex format
#LOADFORMAT = ihex
#LOADFORMAT = binary
LOADFORMAT = both
# Debugging format.
#DEBUGF = dwarf-2
# Place project-specific -D (define) and/or
# -U options for C here.
ifeq ($(ENABLE_DEBUG_PINS), YES)
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
endif
ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART
endif
ifeq ($(USE_BOOTLOADER), YES)
CDEFS += -DUSE_BOOTLOADER
endif
# Compiler flag to set the C Standard level.
# c89 - "ANSI" C
# gnu89 - c89 plus GCC extensions
# c99 - ISO C99 standard (not yet fully implemented)
# gnu99 - c99 plus GCC extensions
CSTANDARD = -std=gnu99
#-----
# Compiler flags.
# -g*: generate debugging information
# -O*: optimization level
# -f...: tuning, see GCC manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -adhlns...: create assembler listing
#
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
ifeq ($(DEBUG),YES)
CFLAGS = -g
endif
CFLAGS += $(CFLAGS_UAVOBJECTS)
CFLAGS += -DARCH_WIN32
CFLAGS += -O$(OPT)
CFLAGS += -mtune=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices
endif
CFLAGS += -Wall
CFLAGS += -Werror
# Compiler flags to generate dependency files:
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
# flags only for C
#CONLYFLAGS += -Wnested-externs
CONLYFLAGS += $(CSTANDARD)
# Assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlns: create listing
ASFLAGS = -mtune=$(MCU) -I. -x assembler-with-cpp
ASFLAGS += $(ADEFS)
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
MATH_LIB = -lm
# Linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
#LDFLAGS += -lpthread
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
#LDFLAGS += -lc
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lgcc
# Define programs and commands.
CC = $(TCHAIN_PREFIX)gcc
CPP = $(TCHAIN_PREFIX)g++
AR = ar
OBJCOPY = objcopy
OBJDUMP = objdump
SIZE = size
NM = nm
REMOVE = $(REMOVE_CMD) -f
PYTHON = python
###SHELL = sh
###COPY = cp
# Define Messages
# English
MSG_ERRORS_NONE = Errors: none
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
MSG_END = ${quote}-------- end --------${quote}
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
MSG_LINKING = ${quote}**** Linking :${quote}
MSG_COMPILING = ${quote}**** Compiling C :${quote}
MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
MSG_CLEANING = ${quote}Cleaning project:${quote}
MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
MSG_PYMITEINIT = ${quote}**** Generating PyMite intermediate code${quote}
# List of all source files.
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
# List of all source files without directory and file-extension.
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
# Define all object files.
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
# Define all listing files (used for make clean).
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
# Define all depedency-files (used for make clean).
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
elf: $(OUTDIR)/$(TARGET).elf
lss: $(OUTDIR)/$(TARGET).lss
sym: $(OUTDIR)/$(TARGET).sym
hex: $(OUTDIR)/$(TARGET).hex
bin: $(OUTDIR)/$(TARGET).bin
exe: $(OUTDIR)/$(TARGET).exe
# Default target.
#all: begin gccversion sizebefore build sizeafter finished end
#all: begin gccversion build sizeafter finished end
#all: elf
all: gencode exe
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
else
ifeq ($(LOADFORMAT),binary)
build: elf bin lss sym
else
ifeq ($(LOADFORMAT),both)
build: elf hex bin lss sym
else
$(error "$(MSG_FORMATERROR) $(FORMAT)")
endif
endif
endif
# Test if quotes are needed for the echo-command
result = ${shell echo "test"}
ifeq (${result}, test)
quote = '
else
quote =
endif
# Generate intermediate code
gencode: ${OUTDIR}/InitMods.c $(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h
${OUTDIR}/InitMods.c: Makefile.win32
@echo ${MSG_MODINIT}
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
#Generate code for PyMite
$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h $(OPMODULEDIR)/FlightPlan/flightplan.c: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py)
@echo ${MSG_PYMITEINIT}
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
# Eye candy.
begin:
## @echo
@echo $(MSG_BEGIN)
finished:
## @echo $(MSG_ERRORS_NONE)
end:
@echo $(MSG_END)
## @echo
# Display sizes of sections.
ELFSIZE = $(SIZE) -A $(OUTDIR)/$(TARGET).elf
##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
sizebefore:
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
sizeafter:
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
@echo $(MSG_SIZE_AFTER)
$(ELFSIZE)
# Display compiler version information.
gccversion :
@$(CC) --version
# @echo $(ALLOBJ)
# Program the device.
ifeq ($(FLASH_TOOL),OPENOCD)
# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
program: $(OUTDIR)/$(TARGET).elf
@echo ${quote}Programming with OPENOCD${quote}
$(OOCD_EXE) $(OOCD_CL)
endif
# Create final output file (.hex) from ELF output file.
%.hex: %.elf
## @echo
@echo $(MSG_LOAD_FILE) $@
$(OBJCOPY) -O ihex $< $@
# Create final output file (.bin) from ELF output file.
%.bin: %.elf
## @echo
@echo $(MSG_LOAD_FILE) $@
$(OBJCOPY) -O binary $< $@
# Create extended listing file/disassambly from ELF output file.
# using objdump testing: option -C
%.lss: %.elf
## @echo
@echo $(MSG_EXTENDED_LISTING) $@
$(OBJDUMP) -h -S -C -r $< > $@
# $(OBJDUMP) -x -S $< > $@
# Create a symbol table from ELF output file.
%.sym: %.elf
## @echo
@echo $(MSG_SYMBOL_TABLE) $@
$(NM) -n $< > $@
# Link: create ELF output file from object files.
.SECONDARY : $(TARGET).elf
.PRECIOUS : $(ALLOBJ)
%.elf: $(ALLOBJ)
@echo $(MSG_LINKING) $@
# use $(CC) for C-only projects or $(CPP) for C++-projects:
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# Link: create EXE output file from object files.
.SECONDARY : $(TARGET).exe
.PRECIOUS : $(ALLOBJ)
%.exe: $(ALLOBJ)
@echo $(MSG_LINKING) $@
# use $(CC) for C-only projects or $(CPP) for C++-projects:
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
# Assemble: create object files from assembler source files.
define ASSEMBLE_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_ASSEMBLING) $$< to $$@
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
endef
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
# Assemble: create object files from assembler source files. ARM-only
define ASSEMBLE_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
$(CC) -c $$(ASFLAGS) $$< -o $$@
endef
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
# Compile: create object files from C source files.
define COMPILE_C_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILING) $$< to $$@
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
endef
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
# Compile: create object files from C source files. ARM-only
define COMPILE_C_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILING_ARM) $$< to $$@
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
endef
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
define COMPILE_CPP_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILINGCPP) $$< to $$@
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
endef
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
define COMPILE_CPP_ARM_TEMPLATE
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
## @echo
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
endef
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
# Compile: create assembler files from C source files. ARM/Thumb
$(SRC:.c=.s) : %.s : %.c
@echo $(MSG_ASMFROMC) $< to $@
$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
# Compile: create assembler files from C source files. ARM only
$(SRCARM:.c=.s) : %.s : %.c
@echo $(MSG_ASMFROMC_ARM) $< to $@
$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
# Generate Doxygen documents
docs:
doxygen $(DOXYGENDIR)/doxygen.cfg
# Target: clean project.
clean: begin clean_list finished end
clean_list :
## @echo
@echo $(MSG_CLEANING)
$(REMOVE) $(OUTDIR)/$(TARGET).map
$(REMOVE) $(OUTDIR)/$(TARGET).elf
$(REMOVE) $(OUTDIR)/$(TARGET).hex
$(REMOVE) $(OUTDIR)/$(TARGET).bin
$(REMOVE) $(OUTDIR)/$(TARGET).sym
$(REMOVE) $(OUTDIR)/$(TARGET).lss
$(REMOVE) $(OUTDIR)/$(TARGET).exe
$(REMOVE) $(ALLOBJ)
$(REMOVE) $(LSTFILES)
$(REMOVE) $(DEPFILES)
$(REMOVE) $(SRC:.c=.s)
$(REMOVE) $(SRCARM:.c=.s)
$(REMOVE) $(CPPSRC:.cpp=.s)
$(REMOVE) $(CPPSRCARM:.cpp=.s)
# Create output files directory
# all known MS Windows OS define the ComSpec environment variable
ifdef ComSpec
$(shell md $(OUTDIR) 2>NUL)
else
$(shell mkdir $(OUTDIR) 2>/dev/null)
endif
# Include the dependency files.
ifdef ComSpec
-include $(shell md $(OUTDIR)\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
else
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
endif
# Listing of phony targets.
.PHONY : all begin gencode finish end sizebefore sizeafter gccversion \
build exe elf hex bin lss sym clean clean_list program

210
flight/OpenPilot/System/alarms.c
View File

@ -1,210 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @brief OpenPilot System libraries are available to all OP modules.
* @{
* @file alarms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Library for setting and clearing system alarms
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "alarms.h"
// Private constants
// Private types
// Private variables
static xSemaphoreHandle lock;
// Private functions
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
/**
* Initialize the alarms library
*/
int32_t AlarmsInitialize(void)
{
SystemAlarmsInitialize();
lock = xSemaphoreCreateRecursiveMutex();
return 0;
}
/**
* Set an alarm
* @param alarm The system alarm to be modified
* @param severity The alarm severity
* @return 0 if success, -1 if an error
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
{
return -1;
}
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if ( alarms.Alarm[alarm] != severity )
{
alarms.Alarm[alarm] = severity;
SystemAlarmsSet(&alarms);
}
// Release lock
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* Get an alarm
* @param alarm The system alarm to be read
* @return Alarm severity
*/
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
{
SystemAlarmsData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
{
return 0;
}
// Read alarm
SystemAlarmsGet(&alarms);
return alarms.Alarm[alarm];
}
/**
* Set an alarm to it's default value
* @param alarm The system alarm to be modified
* @return 0 if success, -1 if an error
*/
int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm)
{
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT);
}
/**
* Default all alarms
*/
void AlarmsDefaultAll()
{
uint32_t n;
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
AlarmsDefault(n);
}
}
/**
* Clear an alarm
* @param alarm The system alarm to be modified
* @return 0 if success, -1 if an error
*/
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm)
{
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK);
}
/**
* Clear all alarms
*/
void AlarmsClearAll()
{
uint32_t n;
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
AlarmsClear(n);
}
}
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasWarnings()
{
return hasSeverity(SYSTEMALARMS_ALARM_WARNING);
}
/**
* Check if there are any alarms with error or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasErrors()
{
return hasSeverity(SYSTEMALARMS_ALARM_ERROR);
};
/**
* Check if there are any alarms with critical or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasCritical()
{
return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL);
};
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
uint32_t n;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsGet(&alarms);
// Go through alarms and check if any are of the given severity or higher
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
{
if ( alarms.Alarm[n] >= severity)
{
xSemaphoreGiveRecursive(lock);
return 1;
}
}
// If this point is reached then no alarms found
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* @}
* @}
*/

99
flight/OpenPilot/System/inc/FreeRTOSConfig.h
View File

@ -1,99 +0,0 @@
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
*
* See http://www.freertos.org/a00110.html.
*----------------------------------------------------------*/
/**
* @addtogroup PIOS PIOS
* @{
* @addtogroup FreeRTOS FreeRTOS
* @{
*/
/* Notes: We use 5 task priorities */
#define configUSE_PREEMPTION 1
#define configUSE_IDLE_HOOK 1
#define configUSE_TICK_HOOK 0
#define configUSE_MALLOC_FAILED_HOOK 1
#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 )
#define configTICK_RATE_HZ ( ( portTickType ) 1000 )
#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 )
#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 )
#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 41 * 1024 ) )
#define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 0
#define configUSE_16_BIT_TICKS 0
#define configIDLE_SHOULD_YIELD 0
#define configUSE_MUTEXES 1
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 0
#define configUSE_ALTERNATIVE_API 0
//#define configCHECK_FOR_STACK_OVERFLOW 2
#define configQUEUE_REGISTRY_SIZE 10
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 0
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255
(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */
#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */
/* This is the value being used as per the ST library which permits 16
priority values, 0 to 15. This must correspond to the
configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest
NVIC value of 255. */
#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15
/* Enable run time stats collection */
#if defined(DIAGNOSTICS)
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
do {\
(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\
(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
} while(0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
#else
#define configCHECK_FOR_STACK_OVERFLOW 1
#endif
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
#define CHECK_IRQ_STACK
#endif
/**
* @}
*/
#endif /* FREERTOS_CONFIG_H */

50
flight/OpenPilot/System/inc/alarms.h
View File

@ -1,50 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
* @file alarms.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the alarm library
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ALARMS_H
#define ALARMS_H
#include "systemalarms.h"
#define SYSTEMALARMS_ALARM_DEFAULT SYSTEMALARMS_ALARM_UNINITIALISED
int32_t AlarmsInitialize(void);
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity);
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm);
int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm);
void AlarmsDefaultAll();
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm);
void AlarmsClearAll();
int32_t AlarmsHasWarnings();
int32_t AlarmsHasErrors();
int32_t AlarmsHasCritical();
#endif // ALARMS_H
/**
* @}
* @}
*/

39
flight/OpenPilot/System/inc/op_config.h
View File

@ -1,39 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file op_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief OpenPilot configuration header.
* Compile time config for OpenPilot Application
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef OP_CONFIG_H
#define OP_CONFIG_H
#endif /* OP_CONFIG_H */
/**
* @}
* @}
*/

53
flight/OpenPilot/System/inc/openpilot.h
View File

@ -1,53 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @file openpilot.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main OpenPilot header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef OPENPILOT_H
#define OPENPILOT_H
/* PIOS Includes */
#include <pios.h>
/* OpenPilot Libraries */
#include "op_config.h"
#include "utlist.h"
#include "uavobjectmanager.h"
#include "eventdispatcher.h"
#include "alarms.h"
#include "taskmonitor.h"
#include "uavtalk.h"
/* Global Functions */
void OpenPilotInit(void);
#endif /* OPENPILOT_H */
/**
* @}
* @}
*/

91
flight/OpenPilot/System/inc/pios_board_posix.h
View File

@ -1,91 +0,0 @@
/**
******************************************************************************
*
* @file pios_board.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board hardware for the OpenPilot Version 1.1 hardware.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_BOARD_H
#define PIOS_BOARD_H
//------------------------
// PIOS_LED
//------------------------
//#define PIOS_LED_LED1_GPIO_PORT GPIOC
//#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12
//#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC
//#define PIOS_LED_LED2_GPIO_PORT GPIOC
//#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13
//#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC
#define PIOS_LED_NUM 2
//#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT }
//#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN }
//#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK }
//-------------------------
// COM
//
// See also pios_board_posix.c
//-------------------------
//#define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_COM_BUFFER_SIZE 1024
#define PIOS_COM_MAX_DEVS 256
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_aux_id;
extern uint32_t pios_com_spectrum_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_GPS (pios_com_gps_id)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG (PIOS_COM_AUX
#endif
/**
* glue macros for file IO
* STM32 uses DOSFS for file IO
*/
#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL
#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL
#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length
#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file)
#define PIOS_FCLOSE(file) fclose(file)
#define PIOS_FUNLINK(file) unlink((char*)filename)
#endif /* PIOS_BOARD_H */

102
flight/OpenPilot/System/inc/pios_config.h
View File

@ -1,102 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_DSM
//#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_PWM
#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
##define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_BMP085
//#define PIOS_INCLUDE_HCSR04
#define PIOS_INCLUDE_OPAHRS
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_I2C_ESC
#define PIOS_INCLUDE_BL_HELPER
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
/* Enable a priority queue in telemetry */
#define PIOS_TELEM_PRIORITY_QUEUE
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 4000
#define HEAP_LIMIT_CRITICAL 1000
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
/* Stabilization options */
#define PIOS_QUATERNION_STABILIZATION
/* GPS options */
#define PIOS_GPS_SETS_HOMELOCATION
/* PIOS Initcall infrastructure */
#define PIOS_INCLUDE_INITCALL
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/

78
flight/OpenPilot/System/inc/pios_config_posix.h
View File

@ -1,78 +0,0 @@
/**
******************************************************************************
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_CONFIG_POSIX_H
#define PIOS_CONFIG_POSIX_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_UDP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_RCVR
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_RCVR_MAX_DEVS 3
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
#define TELEM_QUEUE_SIZE 20
#define PIOS_TELEM_STACK_SIZE 2048
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 4000
#define HEAP_LIMIT_CRITICAL 1000
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
/* Stabilization options */
#define PIOS_QUATERNION_STABILIZATION
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 4000
#define HEAP_LIMIT_CRITICAL 1000
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
/* GPS options */
#define PIOS_GPS_SETS_HOMELOCATION
#endif /* PIOS_CONFIG_POSIX_H */

43
flight/OpenPilot/System/inc/taskmonitor.h
View File

@ -1,43 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
* @file taskmonitor.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the task monitoring library
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef TASKMONITOR_H
#define TASKMONITOR_H
#include "taskinfo.h"
int32_t TaskMonitorInitialize(void);
int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle);
int32_t TaskMonitorRemove(TaskInfoRunningElem task);
void TaskMonitorUpdateAll(void);
#endif // TASKMONITOR_H
/**
* @}
* @}
*/

339
flight/OpenPilot/System/openpilot.c
View File

@ -1,339 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @brief These files are the core system files of OpenPilot.
* They are the ground layer just above PiOS. In practice, OpenPilot actually starts
* in the main() function of openpilot.c
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @brief This is where the OP firmware starts. Those files also define the compile-time
* options of the firmware.
* @{
* @file openpilot.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up and runs main OpenPilot tasks.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "openpilot.h"
#include "uavobjectsinit.h"
#include "systemmod.h"
/* Task Priorities */
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
/* Global Variables */
/* Local Variables */
#define INCLUDE_TEST_TASKS 0
#if INCLUDE_TEST_TASKS
static uint8_t sdcard_available;
#endif
FILEINFO File;
char Buffer[1024];
uint32_t Cache;
/* Function Prototypes */
#if INCLUDE_TEST_TASKS
static void TaskTick(void *pvParameters);
static void TaskTesting(void *pvParameters);
static void TaskHIDTest(void *pvParameters);
static void TaskServos(void *pvParameters);
static void TaskSDCard(void *pvParameters);
#endif
int32_t CONSOLE_Parse(uint8_t port, char c);
void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void Stack_Change(void);
static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change")));
/**
* OpenPilot Main function:
*
* Initialize PiOS<BR>
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
* If something goes wrong, blink LED1 and LED2 every 100ms
*
*/
int main()
{
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Architecture dependant Hardware and
* core subsystem initialisation
* (see pios_board.c for your arch)
* */
PIOS_Board_Init();
/* Initialize modules */
MODULE_INITIALISE_ALL
#if INCLUDE_TEST_TASKS
/* Create test tasks */
xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
#endif
/* swap the stack to use the IRQ stack (does nothing in sim mode) */
Stack_Change_Weak();
/* Start the FreeRTOS scheduler which should never returns.*/
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some indication to user that something bad just happened */
PIOS_LED_Off(LED1); \
for(;;) { \
PIOS_LED_Toggle(LED1); \
PIOS_DELAY_WaitmS(100); \
};
return 0;
}
#if INCLUDE_TEST_TASKS
static void TaskTesting(void *pvParameters)
{
portTickType xDelay = 250 / portTICK_RATE_MS;
portTickType xTimeout = 10 / portTICK_RATE_MS;
//PIOS_BMP085_Init();
for(;;)
{
/* This blocks the task until the BMP085 EOC */
/*
PIOS_BMP085_StartADC(TemperatureConv);
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
PIOS_BMP085_ReadADC();
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetTemperature());
PIOS_BMP085_StartADC(PressureConv);
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
PIOS_BMP085_ReadADC();
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetPressure());
*/
#if defined(PIOS_INCLUDE_DSM)
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_DSM_Get(0), PIOS_DSM_Get(1), PIOS_DSM_Get(2), PIOS_DSM_Get(3), PIOS_DSM_Get(4), PIOS_DSM_Get(5), PIOS_DSM_Get(6), PIOS_DSM_Get(7));
#endif
#if defined(PIOS_INCLUDE_SBUS)
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SBus_Get(0), PIOS_SBus_Get(1), PIOS_SBus_Get(2), PIOS_SBus_Get(3), PIOS_SBus_Get(4), PIOS_SBus_Get(5), PIOS_SBus_Get(6), PIOS_SBus_Get(7));
#endif
#if defined(PIOS_INCLUDE_PWM)
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PWM_Get(0), PIOS_PWM_Get(1), PIOS_PWM_Get(2), PIOS_PWM_Get(3), PIOS_PWM_Get(4), PIOS_PWM_Get(5), PIOS_PWM_Get(6), PIOS_PWM_Get(7));
#endif
#if defined(PIOS_INCLUDE_PPM)
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PPM_Get(0), PIOS_PPM_Get(1), PIOS_PPM_Get(2), PIOS_PPM_Get(3), PIOS_PPM_Get(4), PIOS_PPM_Get(5), PIOS_PPM_Get(6), PIOS_PPM_Get(7));
#endif
/* This blocks the task until there is something on the buffer */
/*xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
int32_t len = PIOS_COM_ReceiveBufferUsed(COM_USART1);
for(int32_t i = 0; i < len; i++) {
PIOS_COM_SendFormattedString(COM_DEBUG_USART, ">%c\r", PIOS_COM_ReceiveBuffer(COM_USART1));
}*/
//int32_t state = PIOS_USB_CableConnected();
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "State: %d\r", state);
//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x57, (uint8_t *)50, 1);
/* Test ADC pins */
//temp = ((1.43 - ((Vsense / 4096) * 3.3)) / 4.3) + 25;
//uint32_t vsense = PIOS_ADC_PinGet(0);
//uint32_t Temp = (1.42 - vsense * 3.3 / 4096) * 1000 / 4.35 + 25;
//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "Temp: %d, CS_I: %d, CS_V: %d, 5v: %d\r", PIOS_ADC_PinGet(0), PIOS_ADC_PinGet(1), PIOS_ADC_PinGet(2), PIOS_ADC_PinGet(3));
//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "AUX1: %d, AUX2: %d, AUX3: %d\r", PIOS_ADC_PinGet(4), PIOS_ADC_PinGet(5), PIOS_ADC_PinGet(6));
vTaskDelay(xDelay);
}
}
#endif
#if INCLUDE_TEST_TASKS
static void TaskHIDTest(void *pvParameters)
{
uint8_t byte;
uint8_t line_buffer[128];
uint16_t line_ix = 0;
for(;;)
{
/* HID Loopback Test */
#if 0
if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) != 0) {
byte = PIOS_COM_ReceiveBuffer(COM_USB_HID);
if(byte == '\r' || byte == '\n' || byte == 0) {
PIOS_COM_SendFormattedString(COM_USB_HID, "RX: %s\r", line_buffer);
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
line_ix = 0;
} else if(line_ix < sizeof(line_buffer)) {
line_buffer[line_ix++] = byte;
line_buffer[line_ix] = 0;
}
}
#endif
/* HID Loopback Test */
if(PIOS_COM_ReceiveBufferUsed(COM_USART2) != 0) {
byte = PIOS_COM_ReceiveBuffer(COM_USART2);
#if 0
if(byte == '\r' || byte == '\n' || byte == 0) {
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
line_ix = 0;
} else if(line_ix < sizeof(line_buffer)) {
line_buffer[line_ix++] = byte;
line_buffer[line_ix] = 0;
}
#endif
PIOS_COM_SendChar(COM_DEBUG_USART, (char)byte);
}
}
}
#endif
#if INCLUDE_TEST_TASKS
static void TaskServos(void *pvParameters)
{
/* For testing servo outputs */
portTickType xDelay;
/* Used to test servos, cycles all servos from one side to the other */
for(;;) {
/*xDelay = 250 / portTICK_RATE_MS;
PIOS_Servo_Set(0, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(1, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(2, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(3, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(4, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(5, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(6, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(7, 2000);
vTaskDelay(xDelay);
PIOS_Servo_Set(7, 1000);
vTaskDelay(xDelay);
PIOS_Servo_Set(6, 1000);
vTaskDelay(xDelay);
PIOS_Servo_Set(5, 1000);
vTaskDelay(xDelay);
PIOS_Servo_Set(4, 1000);
vTaskDelay(xDelay);
PIOS_Servo_Set(3, 1000);
vTaskDelay(xDelay);
PIOS_Servo_Set(2, 1000);
vTaskDelay(xDelay);
PIOS_Servo_Set(1, 1000);
vTaskDelay(xDelay);
PIOS_Servo_Set(0, 1000);
vTaskDelay(xDelay);*/
xDelay = 1 / portTICK_RATE_MS;
for(int i = 1000; i < 2000; i++) {
PIOS_Servo_Set(0, i);
PIOS_Servo_Set(1, i);
PIOS_Servo_Set(2, i);
PIOS_Servo_Set(3, i);
PIOS_Servo_Set(4, i);
PIOS_Servo_Set(5, i);
PIOS_Servo_Set(6, i);
PIOS_Servo_Set(7, i);
vTaskDelay(xDelay);
}
for(int i = 2000; i > 1000; i--) {
PIOS_Servo_Set(0, i);
PIOS_Servo_Set(1, i);
PIOS_Servo_Set(2, i);
PIOS_Servo_Set(3, i);
PIOS_Servo_Set(4, i);
PIOS_Servo_Set(5, i);
PIOS_Servo_Set(6, i);
PIOS_Servo_Set(7, i);
vTaskDelay(xDelay);
}
}
}
#endif
#if INCLUDE_TEST_TASKS
static void TaskSDCard(void *pvParameters)
{
uint16_t second_delay_ctr = 0;
portTickType xLastExecutionTime;
/* Initialise the xLastExecutionTime variable on task entry */
xLastExecutionTime = xTaskGetTickCount();
for(;;) {
vTaskDelayUntil(&xLastExecutionTime, 1 / portTICK_RATE_MS);
/* Each second: */
/* Check if SD card is available */
/* High-speed access if SD card was previously available */
if(++second_delay_ctr >= 1000) {
second_delay_ctr = 0;
uint8_t prev_sdcard_available = sdcard_available;
sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available);
if(sdcard_available && !prev_sdcard_available) {
/* SD Card has been connected! */
/* Switch to mass storage device */
MSD_Init(0);
} else if(!sdcard_available && prev_sdcard_available) {
/* Re-init USB for HID */
PIOS_USB_Init(1);
/* SD Card disconnected! */
}
}
/* Each millisecond: */
/* Handle USB access if device is available */
if(sdcard_available) {
MSD_Periodic_mS();
}
}
}
#endif
/**
* @}
* @}
*/

1306
flight/OpenPilot/System/pios_board.c
View File

@ -1,1306 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the OpenPilot board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
#include <openpilot.h>
#include <uavobjectsinit.h>
#include <hwsettings.h>
#include <manualcontrolsettings.h>
//#define I2C_DEBUG_PIN 0
//#define USART_GPS_DEBUG_PIN 1
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* MicroSD Interface
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_sdcard_irq_handler(void);
void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
static const struct pios_spi_cfg pios_spi_sdcard_cfg = {
.regs = SPI1,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, /* Maximum divider (ie. slowest clock rate) */
},
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel2,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel3,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.sclk = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.miso = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.mosi = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
/* AHRS Interface
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_ahrs_irq_handler(void);
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
static const struct pios_spi_cfg pios_spi_ahrs_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
static uint32_t pios_spi_sdcard_id;
void PIOS_SPI_sdcard_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id);
}
uint32_t pios_spi_ahrs_id;
void PIOS_SPI_ahrs_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_ahrs_id);
}
#endif /* PIOS_INCLUDE_SPI */
/*
* ADC system
*/
#include "pios_adc_priv.h"
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one
static const struct pios_adc_cfg pios_adc_cfg = {
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel1,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR,
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_Low,
.DMA_M2M = DMA_M2M_Disable,
},
}
},
.half_flag = DMA1_IT_HT1,
.full_flag = DMA1_IT_TC1,
};
struct pios_adc_dev pios_adc_devs[] = {
{
.cfg = &pios_adc_cfg,
.callback_function = NULL,
},
};
uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
void PIOS_ADC_handler() {
PIOS_ADC_DMA_Handler();
}
#include "pios_tim_priv.h"
static const TIM_TimeBaseInitTypeDef tim_4_8_time_base = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
.TIM_RepetitionCounter = 0x0000,
};
static const struct pios_tim_clock_cfg tim_4_cfg = {
.timer = TIM4,
.time_base_init = &tim_4_8_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_8_cfg = {
.timer = TIM8,
.time_base_init = &tim_4_8_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM8_CC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const TIM_TimeBaseInitTypeDef tim_1_3_5_time_base = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = 0xFFFF,
.TIM_RepetitionCounter = 0x0000,
};
static const struct pios_tim_clock_cfg tim_1_cfg = {
.timer = TIM1,
.time_base_init = &tim_1_3_5_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM1_CC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_3_cfg = {
.timer = TIM3,
.time_base_init = &tim_1_3_5_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_5_cfg = {
.timer = TIM5,
.time_base_init = &tim_1_3_5_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM5_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#if defined(PIOS_INCLUDE_USART)
#include "pios_usart_priv.h"
/*
* Telemetry USART
*/
static const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = USART2,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_3,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
/*
* GPS USART
*/
static const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART3,
.remap = GPIO_PartialRemap_USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#ifdef PIOS_COM_AUX
/*
* AUX USART
*/
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.remap = GPIO_Remap_USART1,
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif
#if defined(PIOS_INCLUDE_RTC)
/*
* Realtime Clock (RTC)
*/
#include <pios_rtc_priv.h>
void PIOS_RTC_IRQ_Handler (void);
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
.clksrc = RCC_RTCCLKSource_HSE_Div128,
.prescaler = 100,
.irq = {
.init = {
.NVIC_IRQChannel = RTC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_RTC_IRQ_Handler (void)
{
PIOS_RTC_irq_handler ();
}
#endif
#if defined(PIOS_INCLUDE_DSM)
/*
* Spektrum/JR DSM USART
*/
#include <pios_dsm_priv.h>
static const struct pios_usart_cfg pios_usart_dsm_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static const struct pios_dsm_cfg pios_dsm_cfg = {
.bind = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
};
#endif /* PIOS_COM_DSM */
#if defined(PIOS_INCLUDE_SBUS)
#error PIOS_INCLUDE_SBUS not implemented
#endif /* PIOS_INCLUDE_SBUS */
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include "pios_com_priv.h"
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
#define PIOS_COM_GPS_RX_BUF_LEN 96
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
#endif /* PIOS_INCLUDE_COM */
/**
* Pios servo configuration structures
*/
#include <pios_servo_priv.h>
static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
{
.timer = TIM4,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM4,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM4,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM4,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM8,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
};
const struct pios_servo_cfg pios_servo_cfg = {
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
.channels = pios_tim_servoport_all_pins,
.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
};
/*
* PWM Inputs
*/
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
#include <pios_pwm_priv.h>
static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
{
.timer = TIM1,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM1,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM5,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM1,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap_TIM3,
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap_TIM3,
},
};
const struct pios_pwm_cfg pios_pwm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = pios_tim_rcvrport_all_channels,
.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
};
#endif
/*
* PPM Input
*/
#if defined(PIOS_INCLUDE_PPM)
#include <pios_ppm_priv.h>
static const struct pios_ppm_cfg pios_ppm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
.TIM_Channel = TIM_Channel_2,
},
/* Use only the first channel for ppm */
.channels = &pios_tim_rcvrport_all_channels[0],
.num_channels = 1,
};
#endif //PPM
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>
/*
* I2C Adapters
*/
void PIOS_I2C_main_adapter_ev_irq_handler(void);
void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C2,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 400000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_main_adapter_id;
void PIOS_I2C_main_adapter_ev_irq_handler(void)
{
#ifdef I2C_DEBUG_PIN
PIOS_DEBUG_PinHigh(I2C_DEBUG_PIN);
#endif
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id);
#ifdef I2C_DEBUG_PIN
PIOS_DEBUG_PinLow(I2C_DEBUG_PIN);
#endif
}
void PIOS_I2C_main_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id);
}
#endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_ENABLE_DEBUG_PINS)
static const struct stm32_gpio pios_debug_pins[] = {
#define PIOS_DEBUG_PIN_SERVO_1 0
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
#define PIOS_DEBUG_PIN_SERVO_2 1
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
#define PIOS_DEBUG_PIN_SERVO_3 2
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
#define PIOS_DEBUG_PIN_SERVO_4 3
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
#define PIOS_DEBUG_PIN_SERVO_5 4
{
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
#define PIOS_DEBUG_PIN_SERVO_6 5
{
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
#define PIOS_DEBUG_PIN_SERVO_7 6
{
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
#define PIOS_DEBUG_PIN_SERVO_8 7
{
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
};
#endif /* PIOS_ENABLE_DEBUG_PINS */
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
/* One slot per selectable receiver group.
* eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
* NOTE: No slot in this map for NONE.
*/
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#endif /* PIOS_INCLUDE_RCVR */
#if defined(PIOS_INCLUDE_USB_HID)
#include "pios_usb_hid_priv.h"
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#endif /* PIOS_INCLUDE_USB_HID */
extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_aux_id;
uint32_t pios_com_dsm_id;
#include "ahrs_spi_comm.h"
/**
* PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Remap AFIO pin */
//GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
#ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS
PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */
/* Delay system */
PIOS_DELAY_Init();
#if defined(PIOS_INCLUDE_SPI)
/* Set up the SPI interface to the SD card */
if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) {
PIOS_Assert(0);
}
/* Enable and mount the SDCard */
PIOS_SDCARD_Init(pios_spi_sdcard_id);
PIOS_SDCARD_MountFS(0);
#endif /* PIOS_INCLUDE_SPI */
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
HwSettingsInitialize();
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */
PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
/* Set up pulse timers */
PIOS_TIM_InitClock(&tim_1_cfg);
PIOS_TIM_InitClock(&tim_3_cfg);
PIOS_TIM_InitClock(&tim_5_cfg);
PIOS_TIM_InitClock(&tim_4_cfg);
PIOS_TIM_InitClock(&tim_8_cfg);
/* Prepare the AHRS Comms upper layer protocol */
AhrsInitComms();
/* Set up the SPI interface to the AHRS */
if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
PIOS_Assert(0);
}
/* Bind the AHRS comms layer to the AHRS SPI link */
AhrsConnect(pios_spi_ahrs_id);
/* Configure the main IO port */
uint8_t hwsettings_op_mainport;
HwSettingsOP_MainPortGet(&hwsettings_op_mainport);
switch (hwsettings_op_mainport) {
case HWSETTINGS_OP_MAINPORT_DISABLED:
break;
case HWSETTINGS_OP_MAINPORT_TELEMETRY:
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
break;
}
/* Configure the flexi port */
uint8_t hwsettings_op_flexiport;
HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport);
switch (hwsettings_op_flexiport) {
case HWSETTINGS_OP_FLEXIPORT_DISABLED:
break;
case HWSETTINGS_OP_FLEXIPORT_GPS:
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
break;
}
#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
PIOS_Servo_Init(&pios_servo_cfg);
#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */
PIOS_ADC_Init();
PIOS_GPIO_Init();
/* Configure the rcvr port */
uint8_t hwsettings_rcvrport;
HwSettingsOP_RcvrPortGet(&hwsettings_rcvrport);
switch (hwsettings_rcvrport) {
case HWSETTINGS_OP_RCVRPORT_DISABLED:
break;
case HWSETTINGS_OP_RCVRPORT_DEBUG:
/* Not supported yet */
break;
case HWSETTINGS_OP_RCVRPORT_DSM2:
case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
#if defined(PIOS_INCLUDE_DSM)
{
enum pios_dsm_proto proto;
switch (hwsettings_rcvrport) {
case HWSETTINGS_OP_RCVRPORT_DSM2:
proto = PIOS_DSM_PROTO_DSM2;
break;
case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
proto = PIOS_DSM_PROTO_DSMX10BIT;
break;
case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
proto = PIOS_DSM_PROTO_DSMX11BIT;
break;
default:
PIOS_Assert(0);
break;
}
uint32_t pios_usart_dsm_id;
if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_dsm_id;
if (PIOS_DSM_Init(&pios_dsm_id,
&pios_dsm_cfg,
&pios_usart_com_driver,
pios_usart_dsm_id,
proto, 0)) {
PIOS_Assert(0);
}
uint32_t pios_dsm_rcvr_id;
if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id;
}
#endif
break;
case HWSETTINGS_OP_RCVRPORT_PWM:
#if defined(PIOS_INCLUDE_PWM)
{
uint32_t pios_pwm_id;
PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg);
uint32_t pios_pwm_rcvr_id;
if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
}
#endif /* PIOS_INCLUDE_PWM */
break;
case HWSETTINGS_OP_RCVRPORT_PPM:
#if defined(PIOS_INCLUDE_PPM)
{
uint32_t pios_ppm_id;
PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
uint32_t pios_ppm_rcvr_id;
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
}
#endif /* PIOS_INCLUDE_PPM */
break;
}
#if defined(PIOS_INCLUDE_USB_HID)
uint32_t pios_usb_hid_id;
PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
#if defined(PIOS_INCLUDE_COM)
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
#endif /* PIOS_INCLUDE_USB_HID */
#if defined(PIOS_INCLUDE_I2C)
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_I2C */
PIOS_IAP_Init();
PIOS_WDG_Init();
}
/**
* @}
*/

197
flight/OpenPilot/System/pios_board_posix.c
View File

@ -1,197 +0,0 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the OpenPilot board.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios.h>
#include <pios_udp_priv.h>
#include <pios_com_priv.h>
#include <openpilot.h>
#include <uavobjectsinit.h>
#include "hwsettings.h"
#include "attituderaw.h"
#include "attitudeactual.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "manualcontrolsettings.h"
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
/* One slot per selectable receiver group.
* eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
* NOTE: No slot in this map for NONE.
*/
uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
#endif /* PIOS_INCLUDE_RCVR */
void Stack_Change() {
}
void Stack_Change_Weak() {
}
const struct pios_udp_cfg pios_udp_telem_cfg = {
.ip = "0.0.0.0",
.port = 9000,
};
const struct pios_udp_cfg pios_udp_gps_cfg = {
.ip = "0.0.0.0",
.port = 9001,
};
const struct pios_udp_cfg pios_udp_debug_cfg = {
.ip = "0.0.0.0",
.port = 9002,
};
#ifdef PIOS_COM_AUX
/*
* AUX USART
*/
const struct pios_udp_cfg pios_udp_aux_cfg = {
.ip = "0.0.0.0",
.port = 9003,
};
#endif
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
#define PIOS_COM_GPS_RX_BUF_LEN 96
/*
* Board specific number of devices.
*/
/*
struct pios_udp_dev pios_udp_devs[] = {
#define PIOS_UDP_TELEM 0
{
.cfg = &pios_udp0_cfg,
},
#define PIOS_UDP_GPS 1
{
.cfg = &pios_udp1_cfg,
},
#define PIOS_UDP_LOCAL 2
{
.cfg = &pios_udp2_cfg,
},
#ifdef PIOS_COM_AUX
#define PIOS_UDP_AUX 3
{
.cfg = &pios_udp3_cfg,
},
#endif
};
uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
*/
/*
* COM devices
*/
/*
* Board specific number of devices.
*/
extern const struct pios_com_driver pios_serial_com_driver;
extern const struct pios_com_driver pios_udp_com_driver;
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_aux_id;
uint32_t pios_com_spectrum_id;
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_udp_telem_rf_id;
if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_udp_gps_id;
if (PIOS_UDP_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
#endif
// Initialize these here as posix has no AHRSComms
AttitudeRawInitialize();
AttitudeActualInitialize();
VelocityActualInitialize();
PositionActualInitialize();
HwSettingsInitialize();
}
/**
* @}
*/

150
flight/OpenPilot/System/taskmonitor.c
View File

@ -1,150 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
* @file taskmonitor.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Task monitoring library
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
//#include "taskmonitor.h"
// Private constants
// Private types
// Private variables
static xSemaphoreHandle lock;
static xTaskHandle handles[TASKINFO_RUNNING_NUMELEM];
static uint32_t lastMonitorTime;
// Private functions
/**
* Initialize library
*/
int32_t TaskMonitorInitialize(void)
{
lock = xSemaphoreCreateRecursiveMutex();
memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
lastMonitorTime = 0;
#if defined(DIAGNOSTICS)
lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
#endif
return 0;
}
/**
* Register a task handle with the library
*/
int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
{
if (task < TASKINFO_RUNNING_NUMELEM)
{
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
handles[task] = handle;
xSemaphoreGiveRecursive(lock);
return 0;
}
else
{
return -1;
}
}
/**
* Remove a task handle from the library
*/
int32_t TaskMonitorRemove(TaskInfoRunningElem task)
{
if (task < TASKINFO_RUNNING_NUMELEM)
{
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
handles[task] = 0;
xSemaphoreGiveRecursive(lock);
return 0;
}
else
{
return -1;
}
}
/**
* Update the status of all tasks
*/
void TaskMonitorUpdateAll(void)
{
#if defined(DIAGNOSTICS)
TaskInfoData data;
int n;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
#if ( configGENERATE_RUN_TIME_STATS == 1 )
uint32_t currentTime;
uint32_t deltaTime;
/*
* Calculate the amount of elapsed run time between the last time we
* measured and now. Scale so that we can convert task run times
* directly to percentages.
*/
currentTime = portGET_RUN_TIME_COUNTER_VALUE();
deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */
lastMonitorTime = currentTime;
#endif
// Update all task information
for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)
{
if (handles[n] != 0)
{
data.Running[n] = TASKINFO_RUNNING_TRUE;
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
data.StackRemaining[n] = 10000;
#else
data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;
#if ( configGENERATE_RUN_TIME_STATS == 1 )
/* Generate run time stats */
data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;
#endif
#endif
}
else
{
data.Running[n] = TASKINFO_RUNNING_FALSE;
data.StackRemaining[n] = 0;
data.RunningTime[n] = 0;
}
}
// Update object
TaskInfoSet(&data);
// Done
xSemaphoreGiveRecursive(lock);
#endif
}

100
flight/OpenPilot/Tests/test_BMP085.c
View File

@ -1,100 +0,0 @@
/**
******************************************************************************
*
* @file openpilot.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up and runs main OpenPilot tasks.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "openpilot.h"
/* Task Priorities */
/* Global Variables */
/* Local Variables */
/* Function Prototypes */
static void TaskTesting(void *pvParameters);
/**
* Test Main function
*/
int main()
{
// Init
PIOS_SYS_Init();
PIOS_DELAY_Init();
PIOS_COM_Init();
PIOS_ADC_Init();
PIOS_I2C_Init();
xTaskCreate(TaskTesting, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
// Start the FreeRTOS scheduler
vTaskStartScheduler();
// Should not get here
PIOS_DEBUG_Assert(0);
return 0;
}
static void TaskTesting(void *pvParameters)
{
portTickType xDelay = 500 / portTICK_RATE_MS;
portTickType xTimeout = 10 / portTICK_RATE_MS;
PIOS_BMP085_Init();
for(;;)
{
PIOS_LED_Toggle(LED2);
int16_t temp;
int32_t pressure;
PIOS_BMP085_StartADC(TemperatureConv);
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
PIOS_BMP085_ReadADC();
PIOS_BMP085_StartADC(PressureConv);
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
PIOS_BMP085_ReadADC();
temp = PIOS_BMP085_GetTemperature();
pressure = PIOS_BMP085_GetPressure();
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_TELEM_RF, "%3u,%4u\r", temp, pressure);
vTaskDelay(xDelay);
}
}
/**
* Idle hook function
*/
void vApplicationIdleHook(void)
{
/* Called when the scheduler has no tasks to run */
}

37
flight/OpenPilot/Tests/test_common.c
View File

@ -1,37 +0,0 @@
/**
******************************************************************************
*
* @file test_common.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up ans runs main OpenPilot tasks.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* OpenPilot Includes */
#include "openpilot.h"
/**
* Called by the RTOS when a stack overflow is detected.
*/
void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed portCHAR *pcTaskName )
{
PIOS_DEBUG_Panic("STACK OVERFLOW");
}

89
flight/OpenPilot/Tests/test_cpuload.c
View File

@ -1,89 +0,0 @@
/**
******************************************************************************
*
* @file test_cpuload.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Calibration for the CPU load calculation
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* This file is used to calculate the number of counts we get when
* the CPU is fully idle (no load). This is used by systemmod.c to
* calculate the CPU load. This calibration should be run whenever
* changes are made in the FreeRTOS configuration or on the compiler
* optimisation parameters.
*/
#include "openpilot.h"
// Local constants
#define BENCHMARK_DURATION_MS 10000
// Local functions
static void testTask(void *pvParameters);
// Variables
static uint32_t idleCounter = 0;
static uint32_t idleCounterClear = 0;
int main()
{
PIOS_SYS_Init();
// Create test task
xTaskCreate(testTask, (signed portCHAR *)"Test", 1000 , NULL, 1, NULL);
// Start the FreeRTOS scheduler
vTaskStartScheduler();
return 0;
}
static void testTask(void *pvParameters)
{
uint32_t countsPerSecond = 0;
while (1)
{
// Delay until enough required test duration
vTaskDelay( BENCHMARK_DURATION_MS / portTICK_RATE_MS );
// Calculate counts per second, set breakpoint here
countsPerSecond = idleCounter / (BENCHMARK_DURATION_MS/1000);
// Reset and start again - do not clear idleCounter directly!
// SET BREAKPOINT HERE and read the countsPerSecond variable
// this should be used to update IDLE_COUNTS_PER_SEC_AT_NO_LOAD in systemmod.c
idleCounterClear = 1;
}
}
void vApplicationIdleHook(void)
{
/* Called when the scheduler has no tasks to run */
if (idleCounterClear == 0)
{
++idleCounter;
}
else
{
idleCounter = 0;
idleCounterClear = 0;
}
}

282
flight/OpenPilot/Tests/test_i2c_PCF8570.c
View File

@ -1,282 +0,0 @@
/**
******************************************************************************
*
* @file openpilot.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up ans runs main OpenPilot tasks.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/*
* I2C Test: communicate with external PCF8570 ram chip
* For this test to function, PCF8570 chips need to be attached to the I2C port
*
* Blinking Blue LED: No errors detected, test continues
* Blinking Red LED: Error detected, test stopped
*
*/
/* OpenPilot Includes */
#include "openpilot.h"
//#define USE_DEBUG_PINS
#define DEVICE_1_ADDRESS 0x50
#define DEVICE_2_ADDRESS 0x51
#ifdef USE_DEBUG_PINS
#define DEBUG_PIN_TASK1_WAIT 0
#define DEBUG_PIN_TASK1_LOCKED 1
#define DEBUG_PIN_TASK2_WAIT 2
#define DEBUG_PIN_TASK2_LOCKED 3
#define DEBUG_PIN_TRANSFER 4
#define DEBUG_PIN_IDLE 6
#define DEBUG_PIN_ERROR 7
#define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
#define DebugPinLow(x) PIOS_DEBUG_PinLow(x)
#else
#define DebugPinHigh(x)
#define DebugPinLow(x)
#endif
#define MAX_LOCK_WAIT 2 // Time in ms that a thread can normally block I2C
/* Task Priorities */
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
/* Global Variables */
/* Local Variables */
/* Function Prototypes */
static void Task1(void *pvParameters);
static void Task2(void *pvParameters);
/**
* OpenPilot Main function
*/
int main()
{
// Init
PIOS_SYS_Init();
PIOS_DEBUG_Init();
PIOS_DELAY_Init();
PIOS_COM_Init();
PIOS_USB_Init(0);
PIOS_I2C_Init();
// Both leds off
PIOS_LED_Off(LED1);
PIOS_LED_Off(LED2);
// Create task
xTaskCreate(Task1, (signed portCHAR *)"Task1", 1024 , NULL, 1, NULL);
xTaskCreate(Task2, (signed portCHAR *)"Task2", 1024 , NULL, 2, NULL);
// Start the FreeRTOS scheduler
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* If we do get here, it will most likely be because we ran out of heap space. */
return 0;
}
static void OnError(void)
{
PIOS_LED_Off(LED1);
while(1)
{
DebugPinHigh(DEBUG_PIN_ERROR);
PIOS_LED_Toggle(LED2);
vTaskDelay(50 / portTICK_RATE_MS);
DebugPinLow(DEBUG_PIN_ERROR);
}
}
//
// This is a low priority task that will continuously access one I2C device
// Frequently it will release the I2C device so that others can also use I2C
//
static void Task1(void *pvParameters)
{
int i = 0;
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
{
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20\xB0\xB1\xB2", 4) != 0)
OnError();
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
for(;;)
{
i++;
if (i==100)
{
PIOS_LED_Toggle(LED1);
i = 0;
}
DebugPinHigh(DEBUG_PIN_TASK1_WAIT);
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
{
uint8_t buf[20];
DebugPinLow(DEBUG_PIN_TASK1_WAIT);
DebugPinHigh(DEBUG_PIN_TASK1_LOCKED);
// Write A0 A1 A2 at address 0x10
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10\xA0\xA1\xA2", 4) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
// Read 3 bytes at address 0x20 and check
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20", 1) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
if (memcmp(buf, "\xB0\xB1\xB2",3) != 0)
OnError();
// Read 3 bytes at address 0x10 and check
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
if (memcmp(buf, "\xA0\xA1\xA2",3) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TASK1_LOCKED);
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
}
}
// This is a high priority task that will periodically perform some actions on the second I2C device
// Most of the time it will have to wait for the other task task to release I2C
static void Task2(void *pvParameters)
{
portTickType xLastExecutionTime;
xLastExecutionTime = xTaskGetTickCount();
uint32_t count = 0;
for(;;)
{
uint8_t buf[20];
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
{
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
// Write value of count to address 0x10
buf[0] = 0x10; // The address
memcpy(&buf[1], &count, 4); // The data to write
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_2_ADDRESS<<1, buf, 5) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
vTaskDelay(2 / portTICK_RATE_MS);
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
if (PIOS_I2C_LockDevice(1 / portTICK_RATE_MS))
{
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
// Read at address 0x10 and check
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_2_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_2_ADDRESS<<1, buf, 4) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinHigh(DEBUG_PIN_TRANSFER);
if (memcmp(buf, &count, 4) != 0)
OnError();
DebugPinLow(DEBUG_PIN_TRANSFER);
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
PIOS_I2C_UnlockDevice();
}
else
{
OnError();
}
vTaskDelay(5 / portTICK_RATE_MS);
count++;
}
}
/**
* Idle hook function
*/
void vApplicationIdleHook(void)
{
/* Called when the scheduler has no tasks to run */
DebugPinHigh(DEBUG_PIN_IDLE);
DebugPinLow(DEBUG_PIN_IDLE);
}

174
flight/OpenPilot/Tests/test_uavobjects.c
View File

@ -1,174 +0,0 @@
/**
******************************************************************************
*
* @file test_uavobjects.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Tests for the UAVObject libraries
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "exampleobject1.h"
#include "uavobjectsinit.h"
// Local functions
static void testTask(void *pvParameters);
static void eventCallback(UAVObjEvent* ev);
static void eventCallbackPeriodic(UAVObjEvent* ev);
// Variables
int testDelay = 0;
int main()
{
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* SPI Init */
PIOS_SPI_Init();
/* Enables the SDCard */
PIOS_SDCARD_Init();
PIOS_SDCARD_MountFS(0);
// Turn on both LEDs
PIOS_LED_On(LED1);
PIOS_LED_On(LED2);
// Create test task
xTaskCreate(testTask, (signed portCHAR *)"ObjTest", 1000 , NULL, 1, NULL);
// Start the FreeRTOS scheduler
vTaskStartScheduler();
return 0;
}
static void testTask(void *pvParameters)
{
ExampleObject1Data data;
// Initialize object and manager
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
// Set data
ExampleObject1Get(&data);
data.Field1 = 1;
data.Field2 = 2;
ExampleObject1Set(&data);
// Get data
memset(&data, 0, sizeof(data));
ExampleObject1Get(&data);
// Benchmark time is takes to get and set data (both operations)
int tickStart = xTaskGetTickCount();
for (int n = 0; n < 10000; ++n)
{
ExampleObject1Set(&data);
ExampleObject1Get(&data);
}
int delay = xTaskGetTickCount() - tickStart;
// Create a new instance and get/set its data
uint16_t instId = ExampleObject1CreateInstance();
ExampleObject1InstSet(instId, &data);
memset(&data, 0, sizeof(data));
ExampleObject1InstGet(instId, &data);
// Test pack/unpack
uint8_t buffer[EXAMPLEOBJECT1_NUMBYTES];
memset(buffer, 0, EXAMPLEOBJECT1_NUMBYTES);
UAVObjPack(ExampleObject1Handle(), 0, buffer);
memset(&data, 0, sizeof(data));
ExampleObject1Set(&data);
UAVObjUnpack(ExampleObject1Handle(), 0, buffer);
ExampleObject1Get(&data);
// Test object saving/loading to SD card
UAVObjSave(ExampleObject1Handle(), 0);
memset(&data, 0, sizeof(data));
ExampleObject1Set(&data);
UAVObjLoad(ExampleObject1Handle(), 0);
ExampleObject1Get(&data);
// Retrieve object handle by ID
UAVObjHandle handle = UAVObjGetByID(EXAMPLEOBJECT1_OBJID);
const char* name = UAVObjGetName(handle);
// Get/Set the metadata
UAVObjMetadata mdata;
ExampleObject1GetMetadata(&mdata);
mdata.gcsTelemetryAcked = 0;
ExampleObject1SetMetadata(&mdata);
memset(&mdata, 0, sizeof(mdata));
ExampleObject1GetMetadata(&mdata);
// Test callbacks
ExampleObject1ConnectCallback(eventCallback);
ExampleObject1Set(&data);
// Test queue events
xQueueHandle queue;
queue = xQueueCreate(10, sizeof(UAVObjEvent));
ExampleObject1ConnectQueue(queue);
// Test periodic events
UAVObjEvent ev;
ev.event = 0;
ev.instId = 0;
ev.obj = ExampleObject1Handle();
EventPeriodicCallbackCreate(&ev, eventCallbackPeriodic, 500);
EventPeriodicQueueCreate(&ev, queue, 250);
// Done testing
while (1)
{
if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
{
name = UAVObjGetName(ev.obj);
PIOS_LED_Toggle(LED2);
}
}
}
static void eventCallback(UAVObjEvent* ev)
{
const char* name = UAVObjGetName(ev->obj);
}
static void eventCallbackPeriodic(UAVObjEvent* ev)
{
static int lastUpdate;
testDelay = xTaskGetTickCount() - lastUpdate;
lastUpdate = xTaskGetTickCount();
const char* name = UAVObjGetName(ev->obj);
PIOS_LED_Toggle(LED1);
//ExampleObject1Updated();
}
void vApplicationIdleHook(void)
{
/* Called when the scheduler has no tasks to run */
}

76
flight/OpenPilot/UAVObjects.inc
View File

@ -1,76 +0,0 @@
#####
# Project: OpenPilot
#
# Makefile for OpenPilot UAVObject code
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += inssettings
UAVOBJSRCFILENAMES += insstatus
UAVOBJSRCFILENAMES += attitudeactual
UAVOBJSRCFILENAMES += attituderaw
UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate
UAVOBJSRCFILENAMES += flightplancontrol
UAVOBJSRCFILENAMES += flightplansettings
UAVOBJSRCFILENAMES += flightplanstatus
UAVOBJSRCFILENAMES += flighttelemetrystats
UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += guidancesettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel
UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += positiondesired
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += velocityactual
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

117
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -8,9 +8,7 @@
/* Begin PBXFileReference section */
65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = "<group>"; };
6502584212CA4D2600583CDF /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps13state.c; path = ../../AHRS/insgps13state.c; sourceTree = SOURCE_ROOT; };
65078B09136FCEE600536549 /* flightstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightstatus.xml; sourceTree = "<group>"; };
6509C7E912CA57DC002E5DC2 /* insgps16state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps16state.c; path = ../../AHRS/insgps16state.c; sourceTree = SOURCE_ROOT; };
650D8E2112DFE16400D05CC9 /* actuator.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = actuator.c; sourceTree = "<group>"; };
650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = "<group>"; };
650D8E2512DFE16400D05CC9 /* ahrs_comms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_comms.c; sourceTree = "<group>"; };
@ -85,9 +83,7 @@
6534B55A14763566003DF47C /* pios_flash_m25p16.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flash_m25p16.c; sourceTree = "<group>"; };
6534B55B1476D3A8003DF47C /* pios_ms5611.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_ms5611.h; sourceTree = "<group>"; };
6536D47B1307962C0042A298 /* stabilizationdesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = stabilizationdesired.xml; sourceTree = "<group>"; };
6536D4881307AB950042A298 /* UAVObjects.inc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.pascal; path = UAVObjects.inc; sourceTree = "<group>"; };
65408AA812BB1648004DACC5 /* i2cstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = i2cstats.xml; sourceTree = "<group>"; };
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = "<group>"; };
6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = "<group>"; };
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
@ -116,8 +112,6 @@
657CF024121F49CD007A1FBE /* WMMInternal.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = WMMInternal.h; sourceTree = "<group>"; };
657FF86A12EA8BFB00801617 /* pios_pwm_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_pwm_priv.h; sourceTree = "<group>"; };
6581785414A65B9B0007885F /* pios_bl_helper.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_bl_helper.c; sourceTree = "<group>"; };
6589A972131DDE93006BD67C /* FreeRTOSConfig.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = FreeRTOSConfig.h; sourceTree = "<group>"; };
6589A983131DE24F006BD67C /* taskmonitor.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = taskmonitor.c; sourceTree = "<group>"; };
6589A9DB131DEE76006BD67C /* pios_rtc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_rtc.c; sourceTree = "<group>"; };
6589A9E2131DF1C7006BD67C /* pios_rtc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rtc.h; sourceTree = "<group>"; };
65904E47146128A500FD9482 /* ins.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ins.c; sourceTree = "<group>"; };
@ -2811,12 +2805,6 @@
65B367FD121C2620003EAD18 /* systemstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = systemstats.xml; sourceTree = "<group>"; };
65B367FE121C2620003EAD18 /* telemetrysettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = telemetrysettings.xml; sourceTree = "<group>"; };
65B367FF121C2620003EAD18 /* src.pro */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = src.pro; sourceTree = "<group>"; };
65B7E6AE120DF1E2000C1123 /* ahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs.c; path = ../../AHRS/ahrs.c; sourceTree = SOURCE_ROOT; };
65B7E6B0120DF1E2000C1123 /* ahrs.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs.h; sourceTree = "<group>"; };
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_fsm.h; sourceTree = "<group>"; };
65B7E6B4120DF1E2000C1123 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_config.h; sourceTree = "<group>"; };
65B7E6B6120DF1E2000C1123 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../AHRS/Makefile; sourceTree = SOURCE_ROOT; };
65B7E6B7120DF1E2000C1123 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board.c; path = ../../AHRS/pios_board.c; sourceTree = SOURCE_ROOT; };
65C35E5012EFB2F3004811C2 /* actuatorcommand.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatorcommand.xml; sourceTree = "<group>"; };
65C35E5112EFB2F3004811C2 /* actuatordesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatordesired.xml; sourceTree = "<group>"; };
65C35E5212EFB2F3004811C2 /* actuatorsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatorsettings.xml; sourceTree = "<group>"; };
@ -2924,24 +2912,6 @@
65E8C743139A6D0900E1F979 /* pios_crc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_crc.c; sourceTree = "<group>"; };
65E8C745139A6D1A00E1F979 /* pios_crc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_crc.h; sourceTree = "<group>"; };
65E8C788139AA2A800E1F979 /* accessorydesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = accessorydesired.xml; sourceTree = "<group>"; };
65E8EF1F11EEA61E00BBF654 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../OpenPilot/Makefile; sourceTree = SOURCE_ROOT; };
65E8EF2011EEA61E00BBF654 /* Makefile.posix */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = Makefile.posix; path = ../../OpenPilot/Makefile.posix; sourceTree = SOURCE_ROOT; };
65E8EF5C11EEA61E00BBF654 /* alarms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = alarms.c; path = ../../OpenPilot/System/alarms.c; sourceTree = SOURCE_ROOT; };
65E8EF5E11EEA61E00BBF654 /* alarms.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = alarms.h; path = ../../OpenPilot/System/inc/alarms.h; sourceTree = SOURCE_ROOT; };
65E8EF5F11EEA61E00BBF654 /* op_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = op_config.h; path = ../../OpenPilot/System/inc/op_config.h; sourceTree = SOURCE_ROOT; };
65E8EF6011EEA61E00BBF654 /* openpilot.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = openpilot.h; path = ../../OpenPilot/System/inc/openpilot.h; sourceTree = SOURCE_ROOT; };
65E8EF6111EEA61E00BBF654 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_board.h; path = ../../OpenPilot/System/inc/pios_board.h; sourceTree = SOURCE_ROOT; };
65E8EF6211EEA61E00BBF654 /* pios_board_posix.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_board_posix.h; path = ../../OpenPilot/System/inc/pios_board_posix.h; sourceTree = SOURCE_ROOT; };
65E8EF6311EEA61E00BBF654 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_config.h; path = ../../OpenPilot/System/inc/pios_config.h; sourceTree = SOURCE_ROOT; };
65E8EF6411EEA61E00BBF654 /* pios_config_posix.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_config_posix.h; path = ../../OpenPilot/System/inc/pios_config_posix.h; sourceTree = SOURCE_ROOT; };
65E8EF6511EEA61E00BBF654 /* openpilot.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = openpilot.c; path = ../../OpenPilot/System/openpilot.c; sourceTree = SOURCE_ROOT; };
65E8EF6611EEA61E00BBF654 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board.c; path = ../../OpenPilot/System/pios_board.c; sourceTree = SOURCE_ROOT; };
65E8EF6711EEA61E00BBF654 /* pios_board_posix.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board_posix.c; path = ../../OpenPilot/System/pios_board_posix.c; sourceTree = SOURCE_ROOT; };
65E8EF6911EEA61E00BBF654 /* test_BMP085.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_BMP085.c; path = ../../OpenPilot/Tests/test_BMP085.c; sourceTree = SOURCE_ROOT; };
65E8EF6A11EEA61E00BBF654 /* test_common.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_common.c; path = ../../OpenPilot/Tests/test_common.c; sourceTree = SOURCE_ROOT; };
65E8EF6B11EEA61E00BBF654 /* test_cpuload.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_cpuload.c; path = ../../OpenPilot/Tests/test_cpuload.c; sourceTree = SOURCE_ROOT; };
65E8EF6C11EEA61E00BBF654 /* test_i2c_PCF8570.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_i2c_PCF8570.c; path = ../../OpenPilot/Tests/test_i2c_PCF8570.c; sourceTree = SOURCE_ROOT; };
65E8EF6D11EEA61E00BBF654 /* test_uavobjects.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = test_uavobjects.c; path = ../../OpenPilot/Tests/test_uavobjects.c; sourceTree = SOURCE_ROOT; };
65E8F03211EFF25C00BBF654 /* pios_com.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_com.c; path = ../../PiOS/Common/pios_com.c; sourceTree = SOURCE_ROOT; };
65E8F03311EFF25C00BBF654 /* pios_hmc5843.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_hmc5843.c; path = ../../PiOS/Common/pios_hmc5843.c; sourceTree = SOURCE_ROOT; };
65E8F03411EFF25C00BBF654 /* pios_opahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_opahrs.c; path = ../../PiOS/Common/pios_opahrs.c; sourceTree = SOURCE_ROOT; };
@ -3348,8 +3318,6 @@
65FAB9091482072E000FF8B2 /* altitude.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = altitude.c; sourceTree = "<group>"; };
65FAB90B1482072E000FF8B2 /* altitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = altitude.h; sourceTree = "<group>"; };
65FBE14412E7C98100176B5A /* pios_servo_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_servo_priv.h; sourceTree = "<group>"; };
65FC66AA123F30F100B04F74 /* ahrs_timer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs_timer.c; path = ../../AHRS/ahrs_timer.c; sourceTree = SOURCE_ROOT; };
65FC66AB123F312A00B04F74 /* ahrs_timer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_timer.h; sourceTree = "<group>"; };
65FF4BB513791C3300146BE4 /* ahrs_slave_test.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_slave_test.c; sourceTree = "<group>"; };
65FF4BB613791C3300146BE4 /* ahrs_spi_program.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_program.c; sourceTree = "<group>"; };
65FF4BB713791C3300146BE4 /* ahrs_spi_program_master.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_program_master.c; sourceTree = "<group>"; };
@ -3802,9 +3770,7 @@
65904E3F146128A500FD9482 /* Revolution */,
65FF4BB313791C3300146BE4 /* Bootloaders */,
65F93B9012EE09280047DB36 /* PipXtreme */,
65B7E6AC120DF1CD000C1123 /* AHRS */,
65E6DF7012E02E8E00058553 /* CopterControl */,
65E8EF1E11EEA61E00BBF654 /* OpenPilot */,
650D8E1F12DFE16400D05CC9 /* Modules */,
657CEEB6121DBC63007A1FBE /* Libraries */,
65E8F02F11EFF25C00BBF654 /* PiOS */,
@ -8249,33 +8215,6 @@
path = uavobjectdefinition;
sourceTree = "<group>";
};
65B7E6AC120DF1CD000C1123 /* AHRS */ = {
isa = PBXGroup;
children = (
6509C7E912CA57DC002E5DC2 /* insgps16state.c */,
6502584212CA4D2600583CDF /* insgps13state.c */,
65FC66AA123F30F100B04F74 /* ahrs_timer.c */,
65B7E6AE120DF1E2000C1123 /* ahrs.c */,
65B7E6AF120DF1E2000C1123 /* inc */,
65B7E6B6120DF1E2000C1123 /* Makefile */,
65B7E6B7120DF1E2000C1123 /* pios_board.c */,
);
name = AHRS;
sourceTree = "<group>";
};
65B7E6AF120DF1E2000C1123 /* inc */ = {
isa = PBXGroup;
children = (
65FC66AB123F312A00B04F74 /* ahrs_timer.h */,
6543304F121980300063F913 /* insgps.h */,
65B7E6B0120DF1E2000C1123 /* ahrs.h */,
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,
65B7E6B4120DF1E2000C1123 /* pios_config.h */,
);
name = inc;
path = ../../AHRS/inc;
sourceTree = SOURCE_ROOT;
};
65C35E4E12EFB2F3004811C2 /* shared */ = {
isa = PBXGroup;
children = (
@ -8451,62 +8390,6 @@
path = inc;
sourceTree = "<group>";
};
65E8EF1E11EEA61E00BBF654 /* OpenPilot */ = {
isa = PBXGroup;
children = (
6536D4881307AB950042A298 /* UAVObjects.inc */,
65E8EF1F11EEA61E00BBF654 /* Makefile */,
65E8EF2011EEA61E00BBF654 /* Makefile.posix */,
65E8EF5B11EEA61E00BBF654 /* System */,
65E8EF6811EEA61E00BBF654 /* Tests */,
);
name = OpenPilot;
path = ../../OpenPilot;
sourceTree = SOURCE_ROOT;
};
65E8EF5B11EEA61E00BBF654 /* System */ = {
isa = PBXGroup;
children = (
65E8EF5C11EEA61E00BBF654 /* alarms.c */,
65E8EF5D11EEA61E00BBF654 /* inc */,
65E8EF6511EEA61E00BBF654 /* openpilot.c */,
65E8EF6611EEA61E00BBF654 /* pios_board.c */,
65E8EF6711EEA61E00BBF654 /* pios_board_posix.c */,
6589A983131DE24F006BD67C /* taskmonitor.c */,
);
name = System;
path = ../../OpenPilot/System;
sourceTree = SOURCE_ROOT;
};
65E8EF5D11EEA61E00BBF654 /* inc */ = {
isa = PBXGroup;
children = (
6589A972131DDE93006BD67C /* FreeRTOSConfig.h */,
65E8EF5E11EEA61E00BBF654 /* alarms.h */,
65E8EF5F11EEA61E00BBF654 /* op_config.h */,
65E8EF6011EEA61E00BBF654 /* openpilot.h */,
65E8EF6111EEA61E00BBF654 /* pios_board.h */,
65E8EF6211EEA61E00BBF654 /* pios_board_posix.h */,
65E8EF6311EEA61E00BBF654 /* pios_config.h */,
65E8EF6411EEA61E00BBF654 /* pios_config_posix.h */,
);
name = inc;
path = ../../OpenPilot/System/inc;
sourceTree = SOURCE_ROOT;
};
65E8EF6811EEA61E00BBF654 /* Tests */ = {
isa = PBXGroup;
children = (
65E8EF6911EEA61E00BBF654 /* test_BMP085.c */,
65E8EF6A11EEA61E00BBF654 /* test_common.c */,
65E8EF6B11EEA61E00BBF654 /* test_cpuload.c */,
65E8EF6C11EEA61E00BBF654 /* test_i2c_PCF8570.c */,
65E8EF6D11EEA61E00BBF654 /* test_uavobjects.c */,
);
name = Tests;
path = ../../OpenPilot/Tests;
sourceTree = SOURCE_ROOT;
};
65E8F02F11EFF25C00BBF654 /* PiOS */ = {
isa = PBXGroup;
children = (