From d5eb38a065c089f7e76190d4e65d2bc7769d3f11 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 4 Jan 2012 19:39:31 -0600 Subject: [PATCH] Remove references to deprecated targets --- Makefile | 2 +- flight/AHRS/Makefile | 422 ----- flight/AHRS/ahrs.c | 1265 ------------- flight/AHRS/ahrs_timer.c | 61 - flight/AHRS/inc/ahrs.h | 115 -- flight/AHRS/inc/ahrs_fsm.h | 94 - flight/AHRS/inc/ahrs_timer.h | 45 - flight/AHRS/inc/insgps.h | 93 - flight/AHRS/inc/pios_config.h | 45 - flight/AHRS/insgps13state.c | 1644 ----------------- flight/AHRS/insgps16state.c | 866 --------- flight/AHRS/pios_board.c | 473 ----- flight/AHRS/test.c | 37 - flight/OpenPilot/Makefile | 590 ------ flight/OpenPilot/Makefile.posix | 665 ------- flight/OpenPilot/Makefile.win32 | 637 ------- flight/OpenPilot/System/alarms.c | 210 --- flight/OpenPilot/System/inc/FreeRTOSConfig.h | 99 - flight/OpenPilot/System/inc/alarms.h | 50 - flight/OpenPilot/System/inc/op_config.h | 39 - flight/OpenPilot/System/inc/openpilot.h | 53 - .../OpenPilot/System/inc/pios_board_posix.h | 91 - flight/OpenPilot/System/inc/pios_config.h | 102 - .../OpenPilot/System/inc/pios_config_posix.h | 78 - flight/OpenPilot/System/inc/taskmonitor.h | 43 - flight/OpenPilot/System/openpilot.c | 339 ---- flight/OpenPilot/System/pios_board.c | 1306 ------------- flight/OpenPilot/System/pios_board_posix.c | 197 -- flight/OpenPilot/System/taskmonitor.c | 150 -- flight/OpenPilot/Tests/test_BMP085.c | 100 - flight/OpenPilot/Tests/test_common.c | 37 - flight/OpenPilot/Tests/test_cpuload.c | 89 - flight/OpenPilot/Tests/test_i2c_PCF8570.c | 282 --- flight/OpenPilot/Tests/test_uavobjects.c | 174 -- flight/OpenPilot/UAVObjects.inc | 76 - .../OpenPilotOSX.xcodeproj/project.pbxproj | 117 -- 36 files changed, 1 insertion(+), 10685 deletions(-) delete mode 100644 flight/AHRS/Makefile delete mode 100644 flight/AHRS/ahrs.c delete mode 100644 flight/AHRS/ahrs_timer.c delete mode 100644 flight/AHRS/inc/ahrs.h delete mode 100644 flight/AHRS/inc/ahrs_fsm.h delete mode 100644 flight/AHRS/inc/ahrs_timer.h delete mode 100644 flight/AHRS/inc/insgps.h delete mode 100644 flight/AHRS/inc/pios_config.h delete mode 100644 flight/AHRS/insgps13state.c delete mode 100644 flight/AHRS/insgps16state.c delete mode 100644 flight/AHRS/pios_board.c delete mode 100644 flight/AHRS/test.c delete mode 100644 flight/OpenPilot/Makefile delete mode 100644 flight/OpenPilot/Makefile.posix delete mode 100644 flight/OpenPilot/Makefile.win32 delete mode 100644 flight/OpenPilot/System/alarms.c delete mode 100644 flight/OpenPilot/System/inc/FreeRTOSConfig.h delete mode 100644 flight/OpenPilot/System/inc/alarms.h delete mode 100644 flight/OpenPilot/System/inc/op_config.h delete mode 100644 flight/OpenPilot/System/inc/openpilot.h delete mode 100644 flight/OpenPilot/System/inc/pios_board_posix.h delete mode 100644 flight/OpenPilot/System/inc/pios_config.h delete mode 100644 flight/OpenPilot/System/inc/pios_config_posix.h delete mode 100644 flight/OpenPilot/System/inc/taskmonitor.h delete mode 100644 flight/OpenPilot/System/openpilot.c delete mode 100644 flight/OpenPilot/System/pios_board.c delete mode 100644 flight/OpenPilot/System/pios_board_posix.c delete mode 100644 flight/OpenPilot/System/taskmonitor.c delete mode 100644 flight/OpenPilot/Tests/test_BMP085.c delete mode 100644 flight/OpenPilot/Tests/test_common.c delete mode 100644 flight/OpenPilot/Tests/test_cpuload.c delete mode 100644 flight/OpenPilot/Tests/test_i2c_PCF8570.c delete mode 100644 flight/OpenPilot/Tests/test_uavobjects.c delete mode 100644 flight/OpenPilot/UAVObjects.inc diff --git a/Makefile b/Makefile index 4fb3ca5ef..aaffbb1fc 100644 --- a/Makefile +++ b/Makefile @@ -432,7 +432,7 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bl_$(1), $$(BL_TARGETS))) all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS))) endef -ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins revolution +ALL_BOARDS := coptercontrol pipxtreme ins revolution # Friendly names of each board (used to find source tree) openpilot_friendly := OpenPilot diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile deleted file mode 100644 index 2d11cf2a9..000000000 --- a/flight/AHRS/Makefile +++ /dev/null @@ -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 diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c deleted file mode 100644 index 830245572..000000000 --- a/flight/AHRS/ahrs.c +++ /dev/null @@ -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 -#include "pios.h" -#include "ahrs_timer.h" -#include "ahrs_spi_comm.h" -#include "insgps.h" -#include "CoordinateConversions.h" -#include -#include "fifo_buffer.h" - -#define DEG_TO_RAD (M_PI / 180.0) -#define RAD_TO_DEG (180.0 / M_PI) - -#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */ -#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */ -#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */ -#define INSGPS_MAGLEN 1000 -#define INSGPS_MAGTOL 0.5 /* error in magnetic vector length to use */ - -#define GYRO_OOB(x) ((x > (1000 * DEG_TO_RAD)) || (x < (-1000 * DEG_TO_RAD))) -#define ACCEL_OOB(x) (((x > 12*9.81) || (x < -12*9.81))) -#define ISNAN(x) (x != x) -// down-sampled data index -#define ACCEL_RAW_X_IDX 2 -#define ACCEL_RAW_Y_IDX 0 -#define ACCEL_RAW_Z_IDX 4 -#define GYRO_RAW_X_IDX 1 -#define GYRO_RAW_Y_IDX 3 -#define GYRO_RAW_Z_IDX 5 -#define GYRO_TEMP_RAW_XY_IDX 6 -#define GYRO_TEMP_RAW_Z_IDX 7 -#define MAG_RAW_X_IDX 1 -#define MAG_RAW_Y_IDX 0 -#define MAG_RAW_Z_IDX 2 - -// 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); - } -} - -/** - * @} - */ - diff --git a/flight/AHRS/ahrs_timer.c b/flight/AHRS/ahrs_timer.c deleted file mode 100644 index d30a6c9f6..000000000 --- a/flight/AHRS/ahrs_timer.c +++ /dev/null @@ -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; -} diff --git a/flight/AHRS/inc/ahrs.h b/flight/AHRS/inc/ahrs.h deleted file mode 100644 index 99c0656e6..000000000 --- a/flight/AHRS/inc/ahrs.h +++ /dev/null @@ -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 - -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 */ diff --git a/flight/AHRS/inc/ahrs_fsm.h b/flight/AHRS/inc/ahrs_fsm.h deleted file mode 100644 index dd9d24b21..000000000 --- a/flight/AHRS/inc/ahrs_fsm.h +++ /dev/null @@ -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 */ diff --git a/flight/AHRS/inc/ahrs_timer.h b/flight/AHRS/inc/ahrs_timer.h deleted file mode 100644 index aaddf5769..000000000 --- a/flight/AHRS/inc/ahrs_timer.h +++ /dev/null @@ -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 - -#define TIMER_RATE (8e6 / 128) - -void timer_start(); -uint32_t timer_count(); -uint32_t timer_rate(); - -#endif diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h deleted file mode 100644 index bf4faae4b..000000000 --- a/flight/AHRS/inc/insgps.h +++ /dev/null @@ -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_ */ diff --git a/flight/AHRS/inc/pios_config.h b/flight/AHRS/inc/pios_config.h deleted file mode 100644 index 64f3559a0..000000000 --- a/flight/AHRS/inc/pios_config.h +++ /dev/null @@ -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 */ diff --git a/flight/AHRS/insgps13state.c b/flight/AHRS/insgps13state.c deleted file mode 100644 index 24ffab1db..000000000 --- a/flight/AHRS/insgps13state.c +++ /dev/null @@ -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 -#include - -// constants/macros/typdefs -#define NUMX 13 // number of states, X is the state vector -#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector -#define NUMV 10 // number of measurements, v is the measurement noise vector -#define NUMU 6 // number of deterministic inputs, U is the input vector - -#if defined(GENERAL_COV) -// This might trick people so I have a note here. There is a slower but bigger version of the -// code here but won't fit when debugging disabled (requires -Os) -#define COVARIANCE_PREDICTION_GENERAL -#endif - -// Private functions -void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], - float Q[NUMW], float dT, float P[NUMX][NUMX]); -void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], - uint16_t SensorsUsed); -void RungeKutta(float X[NUMX], float U[NUMU], float dT); -void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); -void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], - float G[NUMX][NUMW]); -void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); -void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); - -// Private variables -float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices - // global to init to zero and maintain zero elements -float Be[3]; // local magnetic unit vector in NED frame -float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector -float Q[NUMW], R[NUMV]; // input noise and measurement noise variances -float K[NUMX][NUMV]; // feedback gain matrix - -// ************* Exposed Functions **************** -// ************************************************* - -uint16_t ins_get_num_states() -{ - return NUMX; -} - -void INSGPSInit() //pretty much just a place holder for now -{ - Be[0] = 1; - Be[1] = 0; - Be[2] = 0; // local magnetic unit vector - - for (int i = 0; i < NUMX; i++) { - for (int j = 0; j < NUMX; j++) { - P[i][j] = 0; // zero all terms - } - } - - P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2) - P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2 - P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance - P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2 - - X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m) - X[6] = 1; - X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s) - X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s) - - Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2 - Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2 - Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2 - - R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2) - R[2] = 0.036; // High freq GPS vertical position noise variance (m^2) - R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2 - R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2 - R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance - R[9] = .05; // High freq altimeter noise variance (m^2) -} - -void INSResetP(float PDiag[NUMX]) -{ - uint8_t i,j; - - // if PDiag[i] nonzero then clear row and column and set diagonal element - for (i=0;i -#include - -// 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 - -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* OP Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). - */ -void PIOS_SPI_op_irq_handler(void); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler"))); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler"))); -static const struct pios_spi_cfg pios_spi_op_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - }, - .use_crc = TRUE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .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 - -/* - * 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 - -#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 - -/* - * 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 -} - diff --git a/flight/AHRS/test.c b/flight/AHRS/test.c deleted file mode 100644 index 6ad7707ec..000000000 --- a/flight/AHRS/test.c +++ /dev/null @@ -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; -} diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile deleted file mode 100644 index 6c947fd48..000000000 --- a/flight/OpenPilot/Makefile +++ /dev/null @@ -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 diff --git a/flight/OpenPilot/Makefile.posix b/flight/OpenPilot/Makefile.posix deleted file mode 100644 index 4a947ae8d..000000000 --- a/flight/OpenPilot/Makefile.posix +++ /dev/null @@ -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 - diff --git a/flight/OpenPilot/Makefile.win32 b/flight/OpenPilot/Makefile.win32 deleted file mode 100644 index 261e4abfb..000000000 --- a/flight/OpenPilot/Makefile.win32 +++ /dev/null @@ -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 - diff --git a/flight/OpenPilot/System/alarms.c b/flight/OpenPilot/System/alarms.c deleted file mode 100644 index e61c7c1ea..000000000 --- a/flight/OpenPilot/System/alarms.c +++ /dev/null @@ -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; -} -/** - * @} - * @} - */ - diff --git a/flight/OpenPilot/System/inc/FreeRTOSConfig.h b/flight/OpenPilot/System/inc/FreeRTOSConfig.h deleted file mode 100644 index 4b37f7e44..000000000 --- a/flight/OpenPilot/System/inc/FreeRTOSConfig.h +++ /dev/null @@ -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 */ diff --git a/flight/OpenPilot/System/inc/alarms.h b/flight/OpenPilot/System/inc/alarms.h deleted file mode 100644 index 9fb047dca..000000000 --- a/flight/OpenPilot/System/inc/alarms.h +++ /dev/null @@ -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 - -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/System/inc/op_config.h b/flight/OpenPilot/System/inc/op_config.h deleted file mode 100644 index 97910f392..000000000 --- a/flight/OpenPilot/System/inc/op_config.h +++ /dev/null @@ -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 */ -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/System/inc/openpilot.h b/flight/OpenPilot/System/inc/openpilot.h deleted file mode 100644 index 59ae76fd4..000000000 --- a/flight/OpenPilot/System/inc/openpilot.h +++ /dev/null @@ -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 - -/* 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 */ -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/System/inc/pios_board_posix.h b/flight/OpenPilot/System/inc/pios_board_posix.h deleted file mode 100644 index fe6f6ba51..000000000 --- a/flight/OpenPilot/System/inc/pios_board_posix.h +++ /dev/null @@ -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 */ diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h deleted file mode 100644 index c5e694b1c..000000000 --- a/flight/OpenPilot/System/inc/pios_config.h +++ /dev/null @@ -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 */ -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/System/inc/pios_config_posix.h b/flight/OpenPilot/System/inc/pios_config_posix.h deleted file mode 100644 index 1148a497c..000000000 --- a/flight/OpenPilot/System/inc/pios_config_posix.h +++ /dev/null @@ -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 */ diff --git a/flight/OpenPilot/System/inc/taskmonitor.h b/flight/OpenPilot/System/inc/taskmonitor.h deleted file mode 100644 index 112598f1b..000000000 --- a/flight/OpenPilot/System/inc/taskmonitor.h +++ /dev/null @@ -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 - -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c deleted file mode 100644 index c90f5fd81..000000000 --- a/flight/OpenPilot/System/openpilot.c +++ /dev/null @@ -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
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler)
-* 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 -/** - * @} - * @} - */ - diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c deleted file mode 100644 index 03a75a30c..000000000 --- a/flight/OpenPilot/System/pios_board.c +++ /dev/null @@ -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 -#include -#include -#include -#include - -//#define I2C_DEBUG_PIN 0 -//#define USART_GPS_DEBUG_PIN 1 - -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* 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 - -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 - -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 -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 -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 -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 - -/* - * 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(); -} - -/** - * @} - */ diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c deleted file mode 100644 index bb1f7d5b2..000000000 --- a/flight/OpenPilot/System/pios_board_posix.c +++ /dev/null @@ -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 -#include -#include -#include -#include - -#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(); - -} - -/** - * @} - */ diff --git a/flight/OpenPilot/System/taskmonitor.c b/flight/OpenPilot/System/taskmonitor.c deleted file mode 100644 index dcd08945c..000000000 --- a/flight/OpenPilot/System/taskmonitor.c +++ /dev/null @@ -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 -} diff --git a/flight/OpenPilot/Tests/test_BMP085.c b/flight/OpenPilot/Tests/test_BMP085.c deleted file mode 100644 index 6bee6a1e6..000000000 --- a/flight/OpenPilot/Tests/test_BMP085.c +++ /dev/null @@ -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 */ - - -} diff --git a/flight/OpenPilot/Tests/test_common.c b/flight/OpenPilot/Tests/test_common.c deleted file mode 100644 index 6f5046a61..000000000 --- a/flight/OpenPilot/Tests/test_common.c +++ /dev/null @@ -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"); -} - - diff --git a/flight/OpenPilot/Tests/test_cpuload.c b/flight/OpenPilot/Tests/test_cpuload.c deleted file mode 100644 index a85f5f698..000000000 --- a/flight/OpenPilot/Tests/test_cpuload.c +++ /dev/null @@ -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; - } -} diff --git a/flight/OpenPilot/Tests/test_i2c_PCF8570.c b/flight/OpenPilot/Tests/test_i2c_PCF8570.c deleted file mode 100644 index 855f6e78c..000000000 --- a/flight/OpenPilot/Tests/test_i2c_PCF8570.c +++ /dev/null @@ -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); -} diff --git a/flight/OpenPilot/Tests/test_uavobjects.c b/flight/OpenPilot/Tests/test_uavobjects.c deleted file mode 100644 index fffbc3f25..000000000 --- a/flight/OpenPilot/Tests/test_uavobjects.c +++ /dev/null @@ -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 */ -} diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc deleted file mode 100644 index c989e948e..000000000 --- a/flight/OpenPilot/UAVObjects.inc +++ /dev/null @@ -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) ) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 0ee33ef94..6ab18a190 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -8,9 +8,7 @@ /* Begin PBXFileReference section */ 65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = ""; }; - 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 = ""; }; - 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 = ""; }; 650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = ""; }; 650D8E2512DFE16400D05CC9 /* ahrs_comms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_comms.c; sourceTree = ""; }; @@ -85,9 +83,7 @@ 6534B55A14763566003DF47C /* pios_flash_m25p16.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flash_m25p16.c; sourceTree = ""; }; 6534B55B1476D3A8003DF47C /* pios_ms5611.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_ms5611.h; sourceTree = ""; }; 6536D47B1307962C0042A298 /* stabilizationdesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = stabilizationdesired.xml; sourceTree = ""; }; - 6536D4881307AB950042A298 /* UAVObjects.inc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.pascal; path = UAVObjects.inc; sourceTree = ""; }; 65408AA812BB1648004DACC5 /* i2cstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = i2cstats.xml; sourceTree = ""; }; - 6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = ""; }; 6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = ""; }; 6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = ""; }; 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; @@ -116,8 +112,6 @@ 657CF024121F49CD007A1FBE /* WMMInternal.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = WMMInternal.h; sourceTree = ""; }; 657FF86A12EA8BFB00801617 /* pios_pwm_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_pwm_priv.h; sourceTree = ""; }; 6581785414A65B9B0007885F /* pios_bl_helper.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_bl_helper.c; sourceTree = ""; }; - 6589A972131DDE93006BD67C /* FreeRTOSConfig.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = FreeRTOSConfig.h; sourceTree = ""; }; - 6589A983131DE24F006BD67C /* taskmonitor.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = taskmonitor.c; sourceTree = ""; }; 6589A9DB131DEE76006BD67C /* pios_rtc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_rtc.c; sourceTree = ""; }; 6589A9E2131DF1C7006BD67C /* pios_rtc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rtc.h; sourceTree = ""; }; 65904E47146128A500FD9482 /* ins.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ins.c; sourceTree = ""; }; @@ -2811,12 +2805,6 @@ 65B367FD121C2620003EAD18 /* systemstats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = systemstats.xml; sourceTree = ""; }; 65B367FE121C2620003EAD18 /* telemetrysettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = telemetrysettings.xml; sourceTree = ""; }; 65B367FF121C2620003EAD18 /* src.pro */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = src.pro; sourceTree = ""; }; - 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 = ""; }; - 65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_fsm.h; sourceTree = ""; }; - 65B7E6B4120DF1E2000C1123 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_config.h; sourceTree = ""; }; - 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 = ""; }; 65C35E5112EFB2F3004811C2 /* actuatordesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatordesired.xml; sourceTree = ""; }; 65C35E5212EFB2F3004811C2 /* actuatorsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatorsettings.xml; sourceTree = ""; }; @@ -2924,24 +2912,6 @@ 65E8C743139A6D0900E1F979 /* pios_crc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_crc.c; sourceTree = ""; }; 65E8C745139A6D1A00E1F979 /* pios_crc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_crc.h; sourceTree = ""; }; 65E8C788139AA2A800E1F979 /* accessorydesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = accessorydesired.xml; sourceTree = ""; }; - 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 = ""; }; 65FAB90B1482072E000FF8B2 /* altitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = altitude.h; sourceTree = ""; }; 65FBE14412E7C98100176B5A /* pios_servo_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_servo_priv.h; sourceTree = ""; }; - 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 = ""; }; 65FF4BB513791C3300146BE4 /* ahrs_slave_test.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_slave_test.c; sourceTree = ""; }; 65FF4BB613791C3300146BE4 /* ahrs_spi_program.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_program.c; sourceTree = ""; }; 65FF4BB713791C3300146BE4 /* ahrs_spi_program_master.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_program_master.c; sourceTree = ""; }; @@ -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 = ""; }; - 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 = ""; - }; - 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 = ""; }; - 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 = (