diff --git a/.gitignore b/.gitignore index 8555dcf4f30..670fbbb1ca5 100644 --- a/.gitignore +++ b/.gitignore @@ -143,6 +143,7 @@ # /sw/tools/ /sw/tools/fp_parser.ml /sw/tools/wiki_gen/wiki_gen +/sw/tools/mergelogs # /sw/ground_segment/misc /sw/ground_segment/misc/davis2ivy diff --git a/AUTHORS b/AUTHORS deleted file mode 100644 index 450f24c1b83..00000000000 --- a/AUTHORS +++ /dev/null @@ -1,36 +0,0 @@ -aibara = Allen Ibara -andy_hb = Andreas Kleinert -antonk = Anton Kochevar -bvandepo = Vandeportaele Bertrand -cracmike = Michel Gorraz -dewagter = Christophe De Wagter -disciple = Louis Dugrain -esden = Piotr Esden-Tempski -flixr = Felix Ruess -fuchsto = Tobias Fuchs -gany = Ganymed Stanek -gautier = Gautier Hattenberger -ghorn = Greg Horn -gov = Gustavo Oliveira Violato -gph = Pierre-Selim Huard -hecto = Pascal Brisset -jpdumont = jpdumont -lamestllama = Eric Parsonage -marcuswolschon = Marcus Wolschon -markgriffin = Mark Griffin -mayrit = Murat Bronz -mdieblich = Martin Dieblich -milesj = Miles Johnson -mmm = Martin Mueller -nzjrs = John Stowers -olri = Oliver Riesener -osam = OSAM-UAV Team -paulcox = Paul Cox -peddie = Matthew Peddie -pixhawkteam = Pixhawk Team -poine = Antoine Drouin -rad = rad -rbdavison = Bernard Davison -rkrash = Roman Krashanitsa -sergio812 = Serge Le Huitouze -vassilis = Vassilis V. diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 00000000000..63b0d3d0482 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,110 @@ +Paparazzi 4.1 - development branch +================================== + +- STM libs completely replaced by libopencm3 +- Input2ivy uses SDL for joysticks (cross-platform, works on OSX as well now) + [#220] (https://github.com/paparazzi/paparazzi/pull/220) +- Option to change text papget color using a combobox + [#194] (https://github.com/paparazzi/paparazzi/pull/194) + +Paparazzi 4.0 +============= + +Changes since old_master tag + +General +------- + +- Settings for the telemetry modes are automatically generated from the XML file + [#118] (https://github.com/paparazzi/paparazzi/pull/118) +- Documentation node for modules + [#182] (https://github.com/paparazzi/paparazzi/pull/182) +- Automatic conversion of units in airframe file, settings and messages. + See http://paparazzi.enac.fr/wiki/Units +- Fix rc_settings: this enables you to change some settings in flight + directly from the RC, is useful if you are alone or don't have a GCS. +- Prefer compiler found in PATH over /opt/paparazzi/arm-multilib + [#231] (https://github.com/paparazzi/paparazzi/issues/231) +- Usability improvements for calibration scripts and + added 3D view of magnetometer data with fitted ellipsoid + +New hardware support +-------------------- + +- Support for new autopilot boards + - [Umarim] (http://paparazzi.enac.fr/wiki/Umarim_v10) + - [Umarim Lite] (http://paparazzi.enac.fr/wiki/Umarim_Lite_v2) + - [NavGo] (http://paparazzi.enac.fr/wiki/NavGo_v3) + - [Lisa/M 2.0] (http://paparazzi.enac.fr/wiki/Lisa/M_v20) +- IMU Aspirin 2.1 support +- BlackMagicProbe JTAG support + +Airborne +-------- + +- All control gains are now positive + [#127] (https://github.com/paparazzi/paparazzi/pull/127) +- RC input follows sign conventions + [#124] (https://github.com/paparazzi/paparazzi/issues/124) +- A modification of the transport layer (pprz and xbee) + in order to allow to select the device at the message level. +- New modules: + - xtend_rssi + [#88] (https://github.com/paparazzi/paparazzi/pull/88) + - open_log + [#82] (https://github.com/paparazzi/paparazzi/pull/82) +- Subsystem for new ahrs estimation algorithms: float_cmpl_rmat +- Improvements for AHRS int_cmpl_quat and float_cmpl_rmat + - Correction of centrifugal acceleration + - Proper handling of BODY_TO_IMU rotations +- All status LEDs configurable (with sensible defaults for the boards): + SYS_TIME_LED, AHRS_ALIGNER_LED, BARO_LED, GPS_LED, RADIO_CONTROL_LED +- Possibility to use two 2-way switches for the mode instead of one 3-way switch +- GPS NMEA parser usable for basic position and fix + [#120] (https://github.com/paparazzi/paparazzi/issues/120) + +Rotorcraft firmware specific +---------------------------- + +- Stabilization/supervision commands with standard PPRZ range + [#169] (https://github.com/paparazzi/paparazzi/pull/169) +- Additional motor arming options + [#174] (https://github.com/paparazzi/paparazzi/pull/174) +- Replaced INV_M with NOMINAL_HOVER_THROTTLE (in %) + To use a fixed value instead of the adaptive vertical filter + [#177] (https://github.com/paparazzi/paparazzi/pull/177) +- Some fixes when changing vertical guidance modes +- Same behaviour (gains) for AP_MODE_HOVER and NAV when holding position + [#82] (https://github.com/paparazzi/paparazzi/pull/82) + +Fixedwing firmware specific +--------------------------- + +- Using a gyro (with IR sensors) is done via imu subsystem now as well + +Simulator +--------- + +- JSBSim interface updated for new FGAccelerations class +- FlightGear interface defaults to version 2.6, define FG_2_4 for 2.4 +- NPS simulator [#205] (https://github.com/paparazzi/paparazzi/pull/205) + - has it's own nps target (instead of sim) + - fdm type renamed from nps to jsbsim + - waypoint altitude fixed + - Improved ground interaction for JSBSim, can now initialize on ground + [#222] (https://github.com/paparazzi/paparazzi/pull/222) + - Radio control via joystick now uses SDL (so works on OSX as well) + [#232] (https://github.com/paparazzi/paparazzi/pull/232) + + +STM32 architecture +------------------ + +- Luftboot USB bootloader +- Updated ADC defines for lisa/m + You should now be able to use ADC_1, ADC_2, ADC_3 for the ADCs on the ANALOG1 + [#159] (https://github.com/paparazzi/paparazzi/issues/159) +- Enable second spektrum receiver via + `````` +- Enable new I2C driver via + `````` diff --git a/Makefile b/Makefile index ebb93d68b5e..ce48b6d1270 100644 --- a/Makefile +++ b/Makefile @@ -66,21 +66,6 @@ OCAML=$(shell which ocaml) OCAMLRUN=$(shell which ocamlrun) BUILD_DATETIME:=$(shell date +%Y%m%d-%H%M%S) -# try to find the paparazzi multilib toolchain -TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib ~/sat -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) -ifneq ($(TOOLCHAIN),) -TOOLCHAIN_DIR=$(shell dirname $(TOOLCHAIN)) -#found the compiler from the paparazzi multilib package -ARMGCC=$(TOOLCHAIN_DIR)/bin/arm-none-eabi-gcc -else -#try picking up the arm-none-eabi compiler from the path, otherwise use arm-elf -HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc) -ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),) -ARMGCC=$(shell which arm-elf-gcc) -else -ARMGCC=$(HAVE_ARM_NONE_EABI_GCC) -endif -endif all: conf commands static @@ -127,7 +112,7 @@ multimon: static_h: $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(DL_PROTOCOL2_H) usb_lib: - @[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x "$(ARMGCC)" && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing" + @[ -d sw/airborne/arch/lpc21/lpcusb ] && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE)) || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing" ext: $(MAKE) -C$(EXT) all diff --git a/conf/Makefile.arm-common b/conf/Makefile.arm-common new file mode 100644 index 00000000000..b1c9b7045d3 --- /dev/null +++ b/conf/Makefile.arm-common @@ -0,0 +1,82 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2012 Felix Ruess +# +# This file is part of paparazzi. +# +# paparazzi 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 2, or (at your option) +# any later version. +# +# paparazzi 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 paparazzi; see the file COPYING. If not, see +# . +# + +# +# This is the common Makefile for finding the arm compiler and OpenOcd +# + +# +# try to pick up the compiler from the path +# +CC = $(shell which arm-none-eabi-gcc) +LD = $(shell which arm-none-eabi-gcc) +AR = $(shell which arm-none-eabi-ar) +CP = $(shell which arm-none-eabi-objcopy) +DMP = $(shell which arm-none-eabi-objdump) +NM = $(shell which arm-none-eabi-nm) +SIZE = $(shell which arm-none-eabi-size) +GDB = $(shell which arm-none-eabi-gdb) +TOOLCHAIN_DIR=$(shell dirname `which arm-none-eabi-gcc`) +GCC_LIB_DIR=$(TOOLCHAIN_DIR)/../arm-none-eabi/lib + +# +# if not found in path, try the paparazzi toolchain in /opt +# +ifeq ($(CC),) +TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) +ifneq ($(TOOLCHAIN),) +TOOLCHAIN_DIR=$(shell dirname $(TOOLCHAIN)) +GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin +GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib + +# Define programs and commands. +GCC_BIN_PREFIX=$(GCC_BIN_DIR)/arm-none-eabi +CC = $(GCC_BIN_PREFIX)-gcc +LD = $(GCC_BIN_PREFIX)-gcc +AR = $(GCC_BIN_PREFIX)-ar +CP = $(GCC_BIN_PREFIX)-objcopy +DMP = $(GCC_BIN_PREFIX)-objdump +NM = $(GCC_BIN_PREFIX)-nm +SIZE = $(GCC_BIN_PREFIX)-size +GDB = $(GCC_BIN_PREFIX)-gdb +else +# toolchain not found... +endif +endif + +MULTILIB = $(shell if $(CC) --print-multi-lib | grep thumb2 > /dev/null ; then echo "yes"; else echo "no"; fi) + +# some general commands +RM = rm + + + +# +# Find OpenOCD +# +# first try in the path +OOCD = $(shell which openocd) +#if OpenOCD could not be found in the path, try the toolchain dir (for backwards compatibility) +ifeq ($(OOCD),) +ifneq ($(TOOLCHAIN),) +OOCD = $(shell if test -e $(TOOLCHAIN_DIR)/bin/openocd ; then echo $(TOOLCHAIN_DIR)/bin/openocd ; else echo "Warning: OpenOCD not found"; fi) +endif +endif diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21 index 015d3cce932..109e8f2e98f 100644 --- a/conf/Makefile.lpc21 +++ b/conf/Makefile.lpc21 @@ -31,67 +31,27 @@ # only define here or elsewhere? SRC_ARCH = arch/lpc21 +# Launch with "make Q=''" to get full command display +Q=@ -# Programs location -# try to find the paparazzi multilib toolchain -TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib ~/sat -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) # -# found the new paparazzi toolchain, use it +# find compiler toolchain # -ifneq ($(TOOLCHAIN),) -TOOLCHAIN_DIR=$(shell dirname $(TOOLCHAIN)) -GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin -GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib -GCC_BIN_PREFIX=$(GCC_BIN_DIR)/arm-none-eabi -CC = $(GCC_BIN_PREFIX)-gcc -LD = $(GCC_BIN_PREFIX)-gcc -OBJCOPY = $(GCC_BIN_PREFIX)-objcopy -OBJDUMP = $(GCC_BIN_PREFIX)-objdump -NM = $(GCC_BIN_PREFIX)-nm -SIZE = $(GCC_BIN_PREFIX)-size +include $(PAPARAZZI_SRC)/conf/Makefile.arm-common # -# If we can't find the toolchain (in /opt/paparazzi/arm-multilib or ~/sat) then try picking up the compilers from the path +# if the new arm-none-eabi multilib compiler was not found try the old arm-elf one # -else - -# see if we can find the new arm-none-eabi, otherwise use the older arm-elf -HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc) -ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),) -GCC_PREFIX = arm-elf -else -GCC_PREFIX = arm-none-eabi -GCC_LIB_DIR=$(shell dirname `which arm-none-eabi-gcc`)/../arm-none-eabi/lib -endif - -CC = $(shell which $(GCC_PREFIX)-gcc) -LD = $(shell which $(GCC_PREFIX)-gcc) -OBJCOPY = $(shell which $(GCC_PREFIX)-objcopy) -OBJDUMP = $(shell which $(GCC_PREFIX)-objdump) -NM = $(shell which $(GCC_PREFIX)-nm) -SIZE = $(shell which $(GCC_PREFIX)-size) +ifeq ($(CC),) +CC = $(shell which arm-elf-gcc) +LD = $(shell which arm-elf-gcc) +CP = $(shell which arm-elf-objcopy) +DMP = $(shell which arm-elf-objdump) +NM = $(shell which arm-elf-nm) +SIZE = $(shell which arm-elf-size) endif -#first try to find OpenOCD in the path -OOCD = $(shell which openocd) -#if OpenOCD could not be found in the path, try the toolchain dir -ifeq ($(OOCD),) -ifneq ($(TOOLCHAIN),) -OOCD = $(shell if test -e $(TOOLCHAIN_DIR)/bin/openocd ; then echo $(TOOLCHAIN_DIR)/bin/openocd ; else echo "Warning: OpenOCD not found"; fi) -endif -endif - -# Define some other programs and commands. -SHELL = sh -REMOVE = rm -f -COPY = cp - -MULTILIB = $(shell if $(CC) --print-multi-lib | grep thumb2 > /dev/null ; then echo "yes"; else echo "no"; fi) - - -# Launch with "make Q=''" to get full command display -Q=@ # MCU name and submodel MCU = arm7tdmi @@ -214,8 +174,8 @@ all: printcommands printmultilib sizebefore build sizeafter printcommands: @echo "Using CC = $(CC)" @echo "Using LD = $(LD)" - @echo "Using CP = $(OBJCOPY)" - @echo "Using DMP = $(OBJDUMP)" + @echo "Using CP = $(CP)" + @echo "Using DMP = $(DMP)" @echo "Using NM = $(NM)" @echo "Using SIZE = $(SIZE)" @echo "GCC version:" @@ -272,14 +232,14 @@ endif # TODO: handling the .eeprom-section should be redundant %.hex: %.elf @echo OBJC $@ - $(Q)$(OBJCOPY) -O $(FORMAT) $< $@ + $(Q)$(CP) -O $(FORMAT) $< $@ # Create extended listing file from ELF output file. # testing: option -C %.lss: %.elf @echo OBJD $@ - $(Q)$(OBJDUMP) -h -S -C $< > $@ + $(Q)$(DMP) -h -S -C $< > $@ # Create a symbol table from ELF output file. @@ -330,26 +290,26 @@ clean: clean_list clean_list : @echo - $(REMOVE) $(OBJDIR)/$(TARGET).hex - $(REMOVE) $(OBJDIR)/$(TARGET).obj - $(REMOVE) $(OBJDIR)/$(TARGET).elf - $(REMOVE) $(OBJDIR)/$(TARGET).map - $(REMOVE) $(OBJDIR)/$(TARGET).obj - $(REMOVE) $(OBJDIR)/$(TARGET).a90 - $(REMOVE) $(OBJDIR)/$(TARGET).sym - $(REMOVE) $(OBJDIR)/$(TARGET).lnk - $(REMOVE) $(OBJDIR)/$(TARGET).lss - $(REMOVE) $(COBJ) - $(REMOVE) $(AOBJ) - $(REMOVE) $(COBJARM) - $(REMOVE) $(AOBJARM) - $(REMOVE) $(LST) - $(REMOVE) $(SRC:.c=.s) - $(REMOVE) $(SRC:.c=.d) - $(REMOVE) $(SRCARM:.c=.s) - $(REMOVE) $(SRCARM:.c=.d) - $(REMOVE) .dep/* - $(REMOVE) *~ + $(RM) $(OBJDIR)/$(TARGET).hex + $(RM) $(OBJDIR)/$(TARGET).obj + $(RM) $(OBJDIR)/$(TARGET).elf + $(RM) $(OBJDIR)/$(TARGET).map + $(RM) $(OBJDIR)/$(TARGET).obj + $(RM) $(OBJDIR)/$(TARGET).a90 + $(RM) $(OBJDIR)/$(TARGET).sym + $(RM) $(OBJDIR)/$(TARGET).lnk + $(RM) $(OBJDIR)/$(TARGET).lss + $(RM) $(COBJ) + $(RM) $(AOBJ) + $(RM) $(COBJARM) + $(RM) $(AOBJARM) + $(RM) $(LST) + $(RM) $(SRC:.c=.s) + $(RM) $(SRC:.c=.d) + $(RM) $(SRCARM:.c=.s) + $(RM) $(SRCARM:.c=.d) + $(RM) .dep/* + $(RM) *~ # Listing of phony targets. diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index 0a8cf7dfd29..485766ca4e8 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -31,57 +31,24 @@ SRC_ARCH = arch/stm32 # Call with "make Q=''" to get full command display Q=@ + +# +# find compiler toolchain +# +include $(PAPARAZZI_SRC)/conf/Makefile.arm-common + + MCU = cortex-m3 #DEBUG = dwarf-2 OPT = s #OPT = 2 #OPT = 0 -# Programs location -TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib ~/sat /opt/paparazzi/stm32 -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) -ifneq ($(TOOLCHAIN),) -TOOLCHAIN_DIR=$(shell dirname $(TOOLCHAIN)) -GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin -GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib - -# Define programs and commands. -GCC_BIN_PREFIX=$(GCC_BIN_DIR)/arm-none-eabi -CC = $(GCC_BIN_PREFIX)-gcc -LD = $(GCC_BIN_PREFIX)-gcc -CP = $(GCC_BIN_PREFIX)-objcopy -DMP = $(GCC_BIN_PREFIX)-objdump -NM = $(GCC_BIN_PREFIX)-nm -SIZE = $(GCC_BIN_PREFIX)-size -GDB = $(GCC_BIN_PREFIX)-gdb -RM = rm - -# If we can't find the toolchain then try picking up the compilers from the path -else -CC = $(shell which arm-none-eabi-gcc) -LD = $(shell which arm-none-eabi-gcc) -CP = $(shell which arm-none-eabi-objcopy) -DMP = $(shell which arm-none-eabi-objdump) -NM = $(shell which arm-none-eabi-nm) -SIZE = $(shell which arm-none-eabi-size) -GDB = $(shell which arm-none-eabi-gdb) -RM = rm -TOOLCHAIN_DIR=$(shell dirname `which arm-none-eabi-gcc`) -GCC_LIB_DIR=$(TOOLCHAIN_DIR)/../arm-none-eabi/lib -endif ifndef $(PYTHON) PYTHON = $(shell which python) endif -#first try to find OpenOCD in the path -OOCD = $(shell which openocd) -#if OpenOCD could not be found in the path, try the toolchain dir -ifeq ($(OOCD),) -ifneq ($(TOOLCHAIN),) -OOCD = $(shell if test -e $(TOOLCHAIN_DIR)/bin/openocd ; then echo $(TOOLCHAIN_DIR)/bin/openocd ; else echo "Warning: OpenOCD not found"; fi) -endif -endif - ifneq ($(BOARD_SERIAL),) OOCD_OPTIONS = -c "ft2232_serial $(BOARD_SERIAL)" endif @@ -126,17 +93,14 @@ AOBJ = $(ASRC:%.S=$(OBJDIR)/%.o) # if not, use the default STM32f103re_flash.ld ifndef LDSCRIPT ifndef $(TARGET).LDSCRIPT +$(warning Linker script for target "$(TARGET)" on board "$(BOARD)" not defined. Using stm32default.ld.) LDSCRIPT = $(SRC_ARCH)/stm32default.ld else LDSCRIPT = $($(TARGET).LDSCRIPT) +$(info Using "$($(TARGET).LDSCRIPT)" as ldscript for target "$(TARGET)".) endif endif -#UNAME = $(shell uname -s) -ifndef MULTILIB -MULTILIB = $(shell if $(CC) --print-multi-lib | grep thumb2 > /dev/null ; then echo "yes"; else echo "no"; fi) -endif - CFLAGS = -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES) -D__thumb2__ -Wall -msoft-float -O$(OPT) CFLAGS += -Wl,--gc-sections CFLAGS += -mcpu=$(MCU) -mthumb -ansi @@ -277,6 +241,7 @@ upload: $(OBJDIR)/$(TARGET).bin else ifeq ($(FLASH_MODE),JTAG) ifeq ($(BMP_PORT),) upload: $(OBJDIR)/$(TARGET).hex + @echo "Assuming luftboot bootloader: $(ASSUMING_LUFTBOOT)" @echo "Using OOCD = $(OOCD)" @echo " OOCD\t$<" $(Q)$(OOCD) -f interface/$(OOCD_INTERFACE).cfg \ diff --git a/conf/airframes/CDW/debug_i2c_lpc.xml b/conf/airframes/CDW/debug_i2c_lpc.xml index ba83581105a..a93292c82ac 100644 --- a/conf/airframes/CDW/debug_i2c_lpc.xml +++ b/conf/airframes/CDW/debug_i2c_lpc.xml @@ -42,21 +42,12 @@ - - - - + + diff --git a/conf/airframes/CDW/yapa_xsens.xml b/conf/airframes/CDW/yapa_xsens.xml index 45e715c90d6..6e0db7d71ac 100644 --- a/conf/airframes/CDW/yapa_xsens.xml +++ b/conf/airframes/CDW/yapa_xsens.xml @@ -47,17 +47,12 @@ - - - - + + diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd index d1a225ab55e..9a1e5c36b68 100644 --- a/conf/airframes/airframe.dtd +++ b/conf/airframes/airframe.dtd @@ -11,11 +11,12 @@ - + + @@ -93,6 +94,12 @@ command CDATA #REQUIRED> var CDATA #REQUIRED value CDATA #REQUIRED> + + + @@ -23,21 +24,29 @@ +
+ + + + +
+ + - + - - + +
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index c39584daec0..cfec6ba1a04 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -22,6 +22,7 @@ + diff --git a/conf/boards/classix.makefile b/conf/boards/classix.makefile index 055ec54e921..efdfbdc3bc7 100644 --- a/conf/boards/classix.makefile +++ b/conf/boards/classix.makefile @@ -32,3 +32,28 @@ $(TARGET).ARCHDIR = $(ARCH) # Battery Voltage fbw.CFLAGS += -DUSE_AD0 +# +# default LED configuration +# +ifndef RADIO_CONTROL_LED +RADIO_CONTROL_LED = none +endif + +ifndef BARO_LED +BARO_LED = none +endif + +ifndef AHRS_ALIGNER_LED +AHRS_ALIGNER_LED = none +endif + +ifndef GPS_LED +GPS_LED = 2 +endif + +ifndef SYS_TIME_LED +SYS_TIME_LED = none +endif + + + diff --git a/conf/boards/lisa_l_1.1.makefile b/conf/boards/lisa_l_1.1.makefile index 0a5b55373b8..9a6521e594d 100644 --- a/conf/boards/lisa_l_1.1.makefile +++ b/conf/boards/lisa_l_1.1.makefile @@ -11,6 +11,7 @@ BOARD=lisa_l BOARD_VERSION=1.0 BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" +$(TARGET).LDSCRIPT=$(SRC_ARCH)/lisa-l.ld NO_LUFTBOOT=1 # ----------------------------------------------------------------------- diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 2a19499303b..4423d5d81d0 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -230,6 +230,7 @@ jsbsim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c ifeq ($(BOARD),classix) fbw.CFLAGS += -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE fbw.srcs += $(SRC_FIXEDWING)/link_mcu.c mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c + ap_srcs += $(SRC_FIRMWARE)/fbw_downlink.c ap.CFLAGS += -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 ap.srcs += $(SRC_FIXEDWING)/link_mcu.c mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c else diff --git a/conf/firmwares/subsystems/fixedwing/control.makefile b/conf/firmwares/subsystems/fixedwing/control.makefile index 440dbdeda0b..44ca3e90e2b 100644 --- a/conf/firmwares/subsystems/fixedwing/control.makefile +++ b/conf/firmwares/subsystems/fixedwing/control.makefile @@ -4,3 +4,6 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/guidance_v.c + +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\" + diff --git a/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile b/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile index a4d5e1aa9a2..b867d966cd8 100644 --- a/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile +++ b/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile @@ -5,3 +5,5 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v.c +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\" + diff --git a/conf/firmwares/subsystems/fixedwing/control_energy.makefile b/conf/firmwares/subsystems/fixedwing/control_energy.makefile new file mode 100644 index 00000000000..84119585f93 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/control_energy.makefile @@ -0,0 +1,9 @@ +# Hey Emacs, this is a -*- makefile -*- + +# Standard fixed wing control loops + + +$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/energy_ctrl.c + +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/energy_ctrl.h\" + diff --git a/conf/firmwares/subsystems/fixedwing/control_new.makefile b/conf/firmwares/subsystems/fixedwing/control_new.makefile index d10475f3611..0df9d74f6fb 100644 --- a/conf/firmwares/subsystems/fixedwing/control_new.makefile +++ b/conf/firmwares/subsystems/fixedwing/control_new.makefile @@ -4,3 +4,6 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v_n.c +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\" + + diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index 599d470f91a..4b3a2800925 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -34,6 +34,14 @@ ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP +endif + +ifeq ($(TARGET), fbw) + +# when compiling FBW only, the settings need to know the AHRS_TYPE + +fbw.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\" + endif diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml new file mode 100644 index 00000000000..1a0aeafbc51 --- /dev/null +++ b/conf/flight_plans/versatile_airspeed.xml @@ -0,0 +1,189 @@ + + + +
+#include "firmwares/fixedwing/guidance/energy_ctrl.h" +#include "subsystems/datalink/datalink.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index 36c179be5d4..a02ef10a2ca 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -110,6 +110,7 @@ + diff --git a/conf/modules/airspeed_adc.xml b/conf/modules/airspeed_adc.xml index 0617cc95aa0..bcd84e8d2a5 100644 --- a/conf/modules/airspeed_adc.xml +++ b/conf/modules/airspeed_adc.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/modules/digital_cam.xml b/conf/modules/digital_cam.xml index c601070e3d9..c84947f986b 100644 --- a/conf/modules/digital_cam.xml +++ b/conf/modules/digital_cam.xml @@ -22,6 +22,8 @@ Digital camera control (trigger using led) + +
diff --git a/conf/modules/digital_cam_i2c.xml b/conf/modules/digital_cam_i2c.xml index 9b54bde8f9f..2ecfec8836e 100644 --- a/conf/modules/digital_cam_i2c.xml +++ b/conf/modules/digital_cam_i2c.xml @@ -2,7 +2,9 @@ - + Trigger Digital Camera Using I2C connected remote microcontroller + +
diff --git a/conf/modules/digital_cam_servo.xml b/conf/modules/digital_cam_servo.xml index 76aff07ab39..141ca80fa08 100644 --- a/conf/modules/digital_cam_servo.xml +++ b/conf/modules/digital_cam_servo.xml @@ -22,6 +22,8 @@ Digital camera control (trigger using servo) + +
diff --git a/conf/settings/control/ctl_adaptive.xml b/conf/settings/control/ctl_adaptive.xml index bf7de6aabc0..6077e0cd82d 100644 --- a/conf/settings/control/ctl_adaptive.xml +++ b/conf/settings/control/ctl_adaptive.xml @@ -7,18 +7,18 @@ - - + + - - - - - - + + + + + + diff --git a/conf/settings/control/ctl_basic.xml b/conf/settings/control/ctl_basic.xml index 01ed2d336de..f2ea91b0f30 100644 --- a/conf/settings/control/ctl_basic.xml +++ b/conf/settings/control/ctl_basic.xml @@ -7,8 +7,8 @@ - - + + diff --git a/conf/settings/control/ctl_energy.xml b/conf/settings/control/ctl_energy.xml new file mode 100644 index 00000000000..4c2e865c524 --- /dev/null +++ b/conf/settings/control/ctl_energy.xml @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/control/ctl_new.xml b/conf/settings/control/ctl_new.xml index c2ad7035dfd..49016b696c5 100644 --- a/conf/settings/control/ctl_new.xml +++ b/conf/settings/control/ctl_new.xml @@ -7,18 +7,18 @@ - - + + - - - - - - + + + + + + diff --git a/conf/settings/control/ctl_new_airspeed.xml b/conf/settings/control/ctl_new_airspeed.xml index abc29de69d3..2b2e9422dbc 100644 --- a/conf/settings/control/ctl_new_airspeed.xml +++ b/conf/settings/control/ctl_new_airspeed.xml @@ -7,18 +7,18 @@ - - + + - - - - - - + + + + + + diff --git a/conf/settings/estimation/ac_char.xml b/conf/settings/estimation/ac_char.xml new file mode 100644 index 00000000000..81d4d0f3a58 --- /dev/null +++ b/conf/settings/estimation/ac_char.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/modules/dc.xml b/conf/settings/modules/dc.xml index 7763e2392fc..a32aa51664e 100644 --- a/conf/settings/modules/dc.xml +++ b/conf/settings/modules/dc.xml @@ -16,7 +16,7 @@ - + diff --git a/conf/settings/modules/enose.xml b/conf/settings/modules/enose.xml index 9da88c26dee..b4abd356a84 100644 --- a/conf/settings/modules/enose.xml +++ b/conf/settings/modules/enose.xml @@ -4,8 +4,8 @@ - - + + diff --git a/conf/settings/modules/photogrammetry_calculator.xml b/conf/settings/modules/photogrammetry_calculator.xml index f5622bbaa5d..082c975451b 100644 --- a/conf/settings/modules/photogrammetry_calculator.xml +++ b/conf/settings/modules/photogrammetry_calculator.xml @@ -15,9 +15,9 @@ - - - + + + diff --git a/conf/settings/modules/poles.xml b/conf/settings/modules/poles.xml index e164da3f078..56751801b3b 100644 --- a/conf/settings/modules/poles.xml +++ b/conf/settings/modules/poles.xml @@ -5,7 +5,7 @@ - + diff --git a/conf/units.xml b/conf/units.xml index 559f2e1a2dd..b15a5145051 100644 --- a/conf/units.xml +++ b/conf/units.xml @@ -1,10 +1,6 @@ - - - - @@ -15,4 +11,8 @@ + + + + diff --git a/sw/airborne/arch/lpc21/lpcusb/Makefile b/sw/airborne/arch/lpc21/lpcusb/Makefile index 556a4cd6cf8..271f6441ae3 100644 --- a/sw/airborne/arch/lpc21/lpcusb/Makefile +++ b/sw/airborne/arch/lpc21/lpcusb/Makefile @@ -4,49 +4,26 @@ LIBNAME = libusbstack PKG_NAME = target DATE = $$(date +%Y%m%d) -# Tool definitions -# try to find the paparazzi multilib toolchain -TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib ~/sat -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) - # -# found the new paparazzi toolchain, use it +# find compiler toolchain # -ifneq ($(TOOLCHAIN),) -TOOLCHAIN_DIR=$(shell dirname $(TOOLCHAIN)) -GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin -GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib -GCC_BIN_PREFIX=$(GCC_BIN_DIR)/arm-none-eabi -CC = $(GCC_BIN_PREFIX)-gcc -LD = $(GCC_BIN_PREFIX)-ld -v -AR = $(GCC_BIN_PREFIX)-ar -AS = $(GCC_BIN_PREFIX)-as -CP = $(GCC_BIN_PREFIX)-objcopy -OD = $(GCC_BIN_PREFIX)-objdump +include $(PAPARAZZI_SRC)/conf/Makefile.arm-common # -# If we can't find the toolchain (in /opt/paparazzi/arm-multilib or ~/sat) then try picking up the compilers from the path +# if the new arm-none-eabi multilib compiler was not found try the old arm-elf one # -else - -# see if we can find the new arm-none-eabi, otherwise use the older arm-elf -HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc) -ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),) -GCC_PREFIX = arm-elf -else -GCC_PREFIX = arm-none-eabi -GCC_LIB_DIR=$(shell dirname `which arm-none-eabi-gcc`)/../arm-none-eabi/lib +ifeq ($(CC),) +CC = $(shell which arm-elf-gcc) +LD = $(shell which arm-elf-gcc) +AR = $(shell which arm-elf-ar) +CP = $(shell which arm-elf-objcopy) +DMP = $(shell which arm-elf-objdump) +NM = $(shell which arm-elf-nm) +SIZE = $(shell which arm-elf-size) endif -CC = $(shell which $(GCC_PREFIX)-gcc) -LD = $(shell which $(GCC_PREFIX)-ld) -v -AR = $(shell which $(GCC_PREFIX)-ar) -AS = $(shell which $(GCC_PREFIX)-as) -CP = $(shell which $(GCC_PREFIX)-objcopy) -OD = $(shell which $(GCC_PREFIX)-objdump) -endif # Define some other programs and commands. -RM = rm TAR = tar CFLAGS = -I./ -I../ -c -W -Wall -Os -g -mcpu=arm7tdmi diff --git a/sw/airborne/arch/lpc21/test/bootloader/Makefile b/sw/airborne/arch/lpc21/test/bootloader/Makefile index b477d3ba956..ae619bc8d92 100644 --- a/sw/airborne/arch/lpc21/test/bootloader/Makefile +++ b/sw/airborne/arch/lpc21/test/bootloader/Makefile @@ -18,53 +18,9 @@ LINKFILE_RAM = lpc2148-ram.ld # # try to find the paparazzi multilib toolchain # -TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib ~/sat -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) +include $(PAPARAZZI_SRC)/conf/Makefile.arm-common -# -# found the new paparazzi toolchain, use it -# -ifneq ($(TOOLCHAIN),) -TOOLCHAIN_DIR=$(shell dirname $(TOOLCHAIN)) -GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin -GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib -GCC_BIN_PREFIX=$(GCC_BIN_DIR)/arm-none-eabi -CC = $(GCC_BIN_PREFIX)-gcc -LD = $(GCC_BIN_PREFIX)-ld -v -AR = $(GCC_BIN_PREFIX)-ar -AS = $(GCC_BIN_PREFIX)-as -CP = $(GCC_BIN_PREFIX)-objcopy -OD = $(GCC_BIN_PREFIX)-objdump -NM = $(GCC_BIN_PREFIX)-nm -SIZE = $(GCC_BIN_PREFIX)-size - -# -# If we can't find the toolchain (in /opt/paparazzi/arm-multilib or ~/sat) then try picking up the compilers from the path -# -else - -# see if we can find the new arm-none-eabi, otherwise use the older arm-elf -HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc) -ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),) -GCC_PREFIX = arm-elf -else -GCC_PREFIX = arm-none-eabi -GCC_LIB_DIR=$(shell dirname `which arm-none-eabi-gcc`)/../arm-none-eabi/lib -endif - -CC = $(shell which $(GCC_PREFIX)-gcc) -LD = $(shell which $(GCC_PREFIX)-ld) -v -AR = $(shell which $(GCC_PREFIX)-ar) -AS = $(shell which $(GCC_PREFIX)-as) -CP = $(shell which $(GCC_PREFIX)-objcopy) -OD = $(shell which $(GCC_PREFIX)-objdump) -NM = $(shell which $(GCC_PREFIX)-nm) -SIZE = $(shell which $(GCC_PREFIX)-size) -endif - -RM = rm -TAR = tar - -MULTILIB = $(shell if $(CC) --print-multi-lib | grep thumb2 > /dev/null ; then echo "yes"; else echo "no"; fi) +TAR = tar CFLAGS = -I./ -c -W -Wall -Os -g -mcpu=arm7tdmi -mthumb-interwork $(ALLFLAGS) # -DDEBUG diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 8673cb1e54e..e0e50639e31 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -29,11 +29,19 @@ static inline void bmp085_read_reg16(uint8_t addr) I2CTransceive(i2c2, baro_trans, BMP085_ADDR, 1, 2); } -static inline int16_t bmp085_read_reg16_blocking(uint8_t addr) +static inline int16_t bmp085_read_reg16_blocking(uint8_t addr, uint32_t timeout) { + uint32_t time = 0; + bmp085_read_reg16(addr); - while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning); + while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning) { + if ((time == timeout) && (timeout != 0)) { + return 0; /* Timeout of the i2c read */ + } else { + time++; + } + } return ((baro_trans.buf[0] << 8) | baro_trans.buf[1]); } @@ -46,17 +54,17 @@ static inline void bmp085_read_reg24(uint8_t addr) static void bmp085_baro_read_calibration(void) { - calibration.ac1 = bmp085_read_reg16_blocking(0xAA); // AC1 - calibration.ac2 = bmp085_read_reg16_blocking(0xAC); // AC2 - calibration.ac3 = bmp085_read_reg16_blocking(0xAE); // AC3 - calibration.ac4 = bmp085_read_reg16_blocking(0xB0); // AC4 - calibration.ac5 = bmp085_read_reg16_blocking(0xB2); // AC5 - calibration.ac6 = bmp085_read_reg16_blocking(0xB4); // AC6 - calibration.b1 = bmp085_read_reg16_blocking(0xB6); // B1 - calibration.b2 = bmp085_read_reg16_blocking(0xB8); // B2 - calibration.mb = bmp085_read_reg16_blocking(0xBA); // MB - calibration.mc = bmp085_read_reg16_blocking(0xBC); // MC - calibration.md = bmp085_read_reg16_blocking(0xBE); // MD + calibration.ac1 = bmp085_read_reg16_blocking(0xAA, 10000); // AC1 + calibration.ac2 = bmp085_read_reg16_blocking(0xAC, 10000); // AC2 + calibration.ac3 = bmp085_read_reg16_blocking(0xAE, 10000); // AC3 + calibration.ac4 = bmp085_read_reg16_blocking(0xB0, 10000); // AC4 + calibration.ac5 = bmp085_read_reg16_blocking(0xB2, 10000); // AC5 + calibration.ac6 = bmp085_read_reg16_blocking(0xB4, 10000); // AC6 + calibration.b1 = bmp085_read_reg16_blocking(0xB6, 10000); // B1 + calibration.b2 = bmp085_read_reg16_blocking(0xB8, 10000); // B2 + calibration.mb = bmp085_read_reg16_blocking(0xBA, 10000); // MB + calibration.mc = bmp085_read_reg16_blocking(0xBC, 10000); // MC + calibration.md = bmp085_read_reg16_blocking(0xBE, 10000); // MD } void baro_init(void) { diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index ea76821409d..b6bec10c9c2 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -92,7 +92,12 @@ }) -#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint); +#ifndef USE_AIRSPEED +#define PERIODIC_SEND_DESIRED(_trans, _dev) { float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);} +#else +#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint); +#endif + #if USE_INFRARED #define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &contrast); } diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c new file mode 100644 index 00000000000..f721a9ef1f5 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -0,0 +1,318 @@ +/* + * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/fixedwing/guidance/guidance_v.c + * Vertical control using total energy control for fixed wing vehicles. + * + + + ================================================= + Energy: + ------ + E = mgh + 1/2mV^2 + Edot / V = (gamma + Vdot/g) * W + + equilibrium + + Vdot / g = Thrust/W - Drag/W - sin(gamma) + with: Drag/Weight = (Cl/Cd)^-1 + + -glide angle: Vdot = 0, T=0 ==> gamma = Cd/Cl + -level flight: Vdot = 0, gamma=0 ==> W/T = Cl/Cd + ================================================= + + Strategy: thrust = path + acceleration[g] (total energy) + pitch = path - acceleration[g] (energy balance) + + Pseudo-Control Unit = dimensionless acceleration [g] + + - pitch <-> pseudocontrol: sin(Theta) steers Vdot in [g] + - throttle <-> pseudocontrol: motor characteristic as function of V x throttle steeds VDot + + */ + +#pragma message "CAUTION! Using TOTAL ENERGY CONTROLLER: Experimental!" + +#include "firmwares/fixedwing/guidance/energy_ctrl.h" +#include "estimator.h" +#include "subsystems/nav.h" +#include "generated/airframe.h" +#include "firmwares/fixedwing/autopilot.h" +#include "subsystems/ahrs.h" +#include "subsystems/imu.h" + +/////// DEFAULT GUIDANCE_V NECESSITIES ////// + +/* mode */ +uint8_t v_ctl_mode = V_CTL_MODE_MANUAL; +uint8_t v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; +uint8_t v_ctl_auto_throttle_submode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; +float v_ctl_auto_throttle_sum_err = 0; +float v_ctl_auto_airspeed_controlled = 0; +float v_ctl_auto_groundspeed_setpoint = 0; + +#ifdef LOITER_TRIM +#error "Energy Controller can not accep Loiter Trim" +#endif +//#ifdef V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE +//#error + +/////// ACTUALLY USED STUFF ////// + +/* outer loop */ +float v_ctl_altitude_setpoint; +float v_ctl_altitude_pre_climb; ///< Path Angle +float v_ctl_altitude_pgain; +float v_ctl_airspeed_pgain; +float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low + +float v_ctl_auto_airspeed_setpoint; ///< in meters per second + +float v_ctl_max_climb = 2; +float v_ctl_max_acceleration = 0.5; + +/* inner loop */ +float v_ctl_climb_setpoint; + +/* "auto throttle" inner loop parameters */ +float v_ctl_desired_acceleration; + +float v_ctl_auto_throttle_cruise_throttle; +float v_ctl_auto_throttle_nominal_cruise_throttle; +float v_ctl_auto_throttle_nominal_cruise_pitch; +float v_ctl_auto_throttle_climb_throttle_increment; +float v_ctl_auto_throttle_pitch_of_vz_pgain; + +float v_ctl_auto_throttle_of_airspeed_pgain; +float v_ctl_auto_throttle_of_airspeed_igain; +float v_ctl_auto_pitch_of_airspeed_pgain; +float v_ctl_auto_pitch_of_airspeed_igain; +float v_ctl_auto_pitch_of_airspeed_dgain; + +float v_ctl_energy_total_pgain; +float v_ctl_energy_total_igain; + +float v_ctl_energy_diff_pgain; +float v_ctl_energy_diff_igain; + + +pprz_t v_ctl_throttle_setpoint; +pprz_t v_ctl_throttle_slewed; + + +///////////////////////////////////////////////// +// Automatically found airplane characteristics + +float ac_char_climb_pitch = 0.0f; +float ac_char_climb_max = 0.0f; +int ac_char_climb_count = 0; +float ac_char_descend_pitch = 0.0f; +float ac_char_descend_max = 0.0f; +int ac_char_descend_count = 0; +float ac_char_cruise_throttle = 0.0f; +float ac_char_cruise_pitch = 0.0f; +int ac_char_cruise_count = 0; + +static void ac_char_average(float* last, float new, int count) +{ + *last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count); +} + +static void ac_char_update(float throttle, float pitch, float climb, float accelerate) +{ + if ((accelerate > -0.02) && (accelerate < 0.02)) + { + if (throttle >= 1.0f) + { + ac_char_climb_count++; + ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count ); + ac_char_average(&ac_char_climb_max , estimator_z_dot, ac_char_climb_count ); + } + else if (throttle <= 0.0f) + { + ac_char_descend_count++; + ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count ); + ac_char_average(&ac_char_descend_max , estimator_z_dot , ac_char_descend_count ); + } + else if ((climb > -0.125) && (climb < 0.125)) + { + ac_char_cruise_count++; + ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count ); + ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count ); + } + } +} + + +void v_ctl_init( void ) { + /* mode */ + v_ctl_mode = V_CTL_MODE_MANUAL; + + /* outer loop */ + v_ctl_altitude_setpoint = 0.; + v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN; + v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; + + /* inner loops */ + v_ctl_climb_setpoint = 0.; + + /* "auto throttle" inner loop parameters */ + v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; + v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT; + v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN; + + v_ctl_throttle_setpoint = 0; +} + +/** + * outer loop + * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode + */ + +void v_ctl_altitude_loop( void ) +{ + // Imput Checks + if (v_ctl_auto_airspeed_setpoint <= 0.0f) v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; + + // Altitude Controller + v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z; + float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ; + BoundAbs(sp, v_ctl_max_climb); + + float incr = sp - v_ctl_climb_setpoint; + BoundAbs(incr, 2.0 / 4.0); + v_ctl_climb_setpoint += incr; +} + + +/** + * auto throttle inner loop + * \brief + */ + +const float dt = 0.01f; + +float lp_vdot[5]; + +static float low_pass_vdot(float v); +static float low_pass_vdot(float v) +{ + lp_vdot[4] += (v - lp_vdot[4]) / 3; + lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3; + lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3; + lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3; + lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3; + + return lp_vdot[0]; +} + +void v_ctl_climb_loop( void ) +{ + // Airspeed outerloop: positive means we need to accelerate + float speed_error = v_ctl_auto_airspeed_setpoint - estimator_airspeed; + + // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration + v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f; + BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration); + + // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration +#ifndef SITL + struct FloatVect3 accel_float = {0,0,0}; + ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); + float vdot = ( accel_float.x / 9.81f - sin(ahrs_float.ltp_to_imu_euler.theta) ); +#else + float vdot = 0; +#endif + + // Acceleration Error: positive means UAV needs to accelerate: needs extra energy + float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot ); + + // Flight Path Outerloop: positive means needs to climb more: needs extra energy + float gamma_err = (v_ctl_climb_setpoint - estimator_z_dot) / v_ctl_auto_airspeed_setpoint; + + // Total Energy Error: positive means energy should be added + float en_tot_err = gamma_err + vdot_err; + + // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up + float en_dis_err = gamma_err - vdot_err; + + // Auto Cruise Throttle + if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) + { + v_ctl_auto_throttle_nominal_cruise_throttle += + v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt + + en_tot_err * v_ctl_energy_total_igain * dt; + if (v_ctl_auto_throttle_nominal_cruise_throttle < 0.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 0.0f; + else if (v_ctl_auto_throttle_nominal_cruise_throttle > 1.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 1.0f; + } + + // Total Controller + float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle + + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + + v_ctl_auto_throttle_of_airspeed_pgain * speed_error + + v_ctl_energy_total_pgain * en_tot_err; + + + if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f)) + { + // If your energy supply is not sufficient, then neglect the climb requirement + en_dis_err = -vdot_err; + } + + /* pitch pre-command */ + if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) + { + v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt + + v_ctl_energy_diff_igain * en_dis_err * dt; + Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); + } + float v_ctl_pitch_of_vz = + + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain + - v_ctl_auto_pitch_of_airspeed_pgain * speed_error + + v_ctl_auto_pitch_of_airspeed_dgain * vdot + + v_ctl_energy_diff_pgain * en_dis_err + + v_ctl_auto_throttle_nominal_cruise_pitch; + + nav_pitch = v_ctl_pitch_of_vz; + + ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration); + + v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ); +} + + +#ifdef V_CTL_THROTTLE_SLEW_LIMITER +#define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER)) +#endif + +#ifndef V_CTL_THROTTLE_SLEW +#define V_CTL_THROTTLE_SLEW 1. +#endif + +/** \brief Computes slewed throttle from throttle setpoint + called at 20Hz + */ +void v_ctl_throttle_slew( void ) { + pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed; + BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ)); + v_ctl_throttle_slewed += diff_throttle; +} diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.h b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.h new file mode 100644 index 00000000000..c3bb8b49a62 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.h @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2012 TUDelft + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/fixedwing/guidance/energy_ctrl.h + * Vertical control using total energy control for fixed wing vehicles. + * + */ + +#ifndef FW_V_CTL_ENERGY_H +#define FW_V_CTL_ENERGY_H + +#include "firmwares/fixedwing/guidance/guidance_common.h" + +/* outer loop */ +// extern float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low +extern float v_ctl_altitude_setpoint; ///< in meters above MSL +extern float v_ctl_altitude_pre_climb; ///< Path Angle +extern float v_ctl_altitude_pgain; +extern float v_ctl_airspeed_pgain; + +extern float v_ctl_auto_airspeed_setpoint; ///< in meters per second + +extern float v_ctl_max_climb; +extern float v_ctl_max_acceleration; + +/* "auto throttle" inner loop parameters */ +extern float v_ctl_desired_acceleration; + +extern float v_ctl_auto_throttle_nominal_cruise_throttle; +extern float v_ctl_auto_throttle_nominal_cruise_pitch; +extern float v_ctl_auto_throttle_climb_throttle_increment; +extern float v_ctl_auto_throttle_pitch_of_vz_pgain; + +extern float v_ctl_auto_throttle_of_airspeed_pgain; +extern float v_ctl_auto_throttle_of_airspeed_igain; +extern float v_ctl_auto_pitch_of_airspeed_pgain; +extern float v_ctl_auto_pitch_of_airspeed_igain; +extern float v_ctl_auto_pitch_of_airspeed_dgain; + +extern float v_ctl_energy_total_pgain; +extern float v_ctl_energy_total_igain; + +extern float v_ctl_energy_diff_pgain; +extern float v_ctl_energy_diff_igain; + +///////////////////////////////////////////////// +// Automatically found airplane characteristics + +extern float ac_char_climb_pitch; +extern float ac_char_climb_max; +extern float ac_char_descend_pitch; +extern float ac_char_descend_max; +extern float ac_char_cruise_throttle; +extern float ac_char_cruise_pitch; + +#endif /* FW_V_CTL_H */ diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h new file mode 100644 index 00000000000..78aff51e961 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/fixedwing/guidance/guidance_v.h + * Vertical control for fixed wing vehicles. + * + */ + +#ifndef FW_V_CTL_COMMON_H +#define FW_V_CTL_COMMON_H + + +#include +#include "paparazzi.h" + +/* Vertical mode */ +#define V_CTL_MODE_MANUAL 0 +#define V_CTL_MODE_AUTO_THROTTLE 1 +#define V_CTL_MODE_AUTO_CLIMB 2 +#define V_CTL_MODE_AUTO_ALT 3 +#define V_CTL_MODE_NB 4 +extern uint8_t v_ctl_mode; + +extern float v_ctl_climb_setpoint; +extern uint8_t v_ctl_climb_mode; + +#define V_CTL_CLIMB_MODE_AUTO_THROTTLE 0 +#define V_CTL_CLIMB_MODE_AUTO_PITCH 1 + +extern uint8_t v_ctl_auto_throttle_submode; +#define V_CTL_AUTO_THROTTLE_STANDARD 0 +#define V_CTL_AUTO_THROTTLE_AGRESSIVE 1 +#define V_CTL_AUTO_THROTTLE_BLENDED 2 + +// Needed for telemetry +extern float v_ctl_auto_throttle_sum_err; + +// Needed for course loop gain +extern float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low + +// Old airspeed code wants: +extern float v_ctl_auto_airspeed_controlled; +extern float v_ctl_auto_groundspeed_setpoint; + +extern float v_ctl_auto_throttle_cruise_throttle; +extern pprz_t v_ctl_throttle_setpoint; +extern pprz_t v_ctl_throttle_slewed; + +extern void v_ctl_init( void ); +extern void v_ctl_altitude_loop( void ); +extern void v_ctl_climb_loop ( void ); + +/** Computes throttle_slewed from throttle_setpoint */ +extern void v_ctl_throttle_slew( void ); + +#define guidance_v_SetCruiseThrottle(_v) { \ + v_ctl_auto_throttle_cruise_throttle = (_v ? _v : v_ctl_auto_throttle_nominal_cruise_throttle); \ + Bound(v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle, v_ctl_auto_throttle_max_cruise_throttle); \ +} + +#define guidance_v_SetAutoThrottleIgain(_v) { \ + v_ctl_auto_throttle_igain = _v; \ + v_ctl_auto_throttle_sum_err = 0; \ + } + + + +#endif + diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index 1f591dce319..f68409d1eb9 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -109,8 +109,6 @@ float v_ctl_auto_groundspeed_sum_err; #define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0 // altitude gain multiplier while in aggressive climb mode #endif -#pragma message "CAUTION! ALL control gains have to be positive now!" - #ifndef V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION #define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION 1.0f #endif @@ -235,6 +233,7 @@ void v_ctl_climb_loop ( void ) { v_ctl_climb_auto_throttle_loop(); break; #ifdef V_CTL_AUTO_PITCH_PGAIN +#pragma message "AUTO PITCH Enabled!" case V_CTL_CLIMB_MODE_AUTO_PITCH: v_ctl_climb_auto_pitch_loop(); break; @@ -302,7 +301,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { f_throttle = controlled_throttle; v_ctl_auto_throttle_sum_err += err; BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); - nav_pitch += v_ctl_pitch_of_vz; + nav_pitch = v_ctl_pitch_of_vz; #if defined AGR_CLIMB break; } /* switch submode */ diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 5edff3ad84c..65ae6080ea7 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -111,8 +111,6 @@ float v_ctl_auto_groundspeed_sum_err; #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100 #endif -#pragma message "CAUTION! ALL control gains have to be positive now!" - void v_ctl_init( void ) { /* mode */ v_ctl_mode = V_CTL_MODE_MANUAL; diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index ca32e3c37d2..6f22bfeca7c 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -58,7 +58,7 @@ #include "firmwares/fixedwing/autopilot.h" #include "estimator.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" +#include CTRL_TYPE_H #include "subsystems/nav.h" #include "generated/flight_plan.h" #ifdef TRAFFIC_INFO @@ -81,11 +81,6 @@ #include "gpio.h" #include "led.h" -#if defined RADIO_CONTROL -#pragma message "CAUTION! radio control roll channel input has been changed to follow aerospace sign conventions.\n You will have to change your radio control xml file to get a positive value when banking right!" -#endif - - #if USE_AHRS #if USE_IMU @@ -474,10 +469,33 @@ void navigation_task( void ) { #endif if (lateral_mode >=LATERAL_MODE_COURSE) h_ctl_course_loop(); /* aka compute nav_desired_roll */ - if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) - v_ctl_climb_loop(); + + // climb_loop(); //4Hz + } + energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours) +} + + +#if USE_AHRS +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP +volatile uint8_t new_ins_attitude = 0; +#endif +#endif + +void attitude_loop( void ) { + +#if USE_INFRARED + ahrs_update_infrared(); +#endif /* USE_INFRARED */ + + if (pprz_mode >= PPRZ_MODE_AUTO2) + { if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) v_ctl_throttle_setpoint = nav_throttle_setpoint; + else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) + { + v_ctl_climb_loop(); + } #if defined V_CTL_THROTTLE_IDLE Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ); @@ -495,19 +513,6 @@ void navigation_task( void ) { if (kill_throttle || (!estimator_flight_time && !launch)) v_ctl_throttle_setpoint = 0; } - energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours) -} - - -#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP -volatile uint8_t new_ins_attitude = 0; -#endif - -void attitude_loop( void ) { - -#if USE_INFRARED - ahrs_update_infrared(); -#endif /* USE_INFRARED */ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ v_ctl_throttle_slew(); @@ -595,8 +600,10 @@ void monitor_task( void ) { void event_task_ap( void ) { #ifndef SINGLE_MCU +#if defined USE_I2C0 || defined USE_I2C1 || defined USE_I2C2 i2c_event(); #endif +#endif #if USE_AHRS && USE_IMU ImuEvent(on_gyro_event, on_accel_event, on_mag_event); diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 6fd9d48cdad..a901fdfdd11 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -131,6 +131,8 @@ void event_task_fbw( void) { if (inter_mcu_received_ap) { inter_mcu_received_ap = FALSE; inter_mcu_event_task(); + command_roll_trim = ap_state->command_roll_trim; + command_pitch_trim = ap_state->command_pitch_trim; if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) { fbw_mode = FBW_MODE_AUTO; } diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 502d51ccd94..8c41aa28e61 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -1,6 +1,4 @@ /* - * Paparazzi $Id: fw_h_ctl.c 3603 2009-07-01 20:06:53Z hecto $ - * * Copyright (C) 2009-2010 The Paparazzi Team * * This file is part of paparazzi. @@ -35,7 +33,7 @@ #include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" +#include CTRL_TYPE_H #include "firmwares/fixedwing/autopilot.h" @@ -116,8 +114,6 @@ inline static void h_ctl_pitch_loop( void ); #define H_CTL_COURSE_DGAIN 0. #endif -#pragma message "CAUTION! ALL control gains have to be positive now!" - // Some default roll gains // H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe #ifndef H_CTL_ROLL_RATE_GAIN diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h index 4a7c082f22e..80d8fec7646 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h @@ -1,6 +1,4 @@ /* - * Paparazzi $Id: fw_h_ctl.h 3784 2009-07-24 14:55:54Z poine $ - * * Copyright (C) 2009 ENAC * * This file is part of paparazzi. @@ -49,12 +47,12 @@ extern float h_ctl_pitch_of_roll; #define H_CTL_ROLL_SUM_ERR_MAX (MAX_PPRZ/2.) #define H_CTL_PITCH_SUM_ERR_MAX (MAX_PPRZ/2.) -#define fw_h_ctl_a_SetRollIGain(_gain) { \ +#define stabilization_adaptive_SetRollIGain(_gain) { \ h_ctl_roll_sum_err = 0.; \ h_ctl_roll_igain = _gain; \ } -#define fw_h_ctl_a_SetPitchIGain(_gain) { \ +#define stabilization_adaptive_SetPitchIGain(_gain) { \ h_ctl_pitch_sum_err = 0.; \ h_ctl_pitch_igain = _gain; \ } diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index d8739b8f946..93e08c31a57 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -34,11 +34,9 @@ #include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" +#include CTRL_TYPE_H #include "firmwares/fixedwing/autopilot.h" -#pragma message "CAUTION! ALL control gains have to be positive now!" - /* outer loop parameters */ float h_ctl_course_setpoint; /* rad, CW/north */ float h_ctl_course_pre_bank; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h index ec0ad75fef9..e50a0ad1a60 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h @@ -90,7 +90,7 @@ extern float h_ctl_lo_throttle_roll_rate_pgain; extern float h_ctl_roll_rate_igain; extern float h_ctl_roll_rate_dgain; -#define fw_h_ctl_SetRollRatePGain(v) { h_ctl_hi_throttle_roll_rate_pgain = v; h_ctl_lo_throttle_roll_rate_pgain = v; } +#define stabilization_attitude_SetRollRatePGain(v) { h_ctl_hi_throttle_roll_rate_pgain = v; h_ctl_lo_throttle_roll_rate_pgain = v; } #endif extern void h_ctl_init( void ); diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 26ad8018eb8..9d7e2a37c4e 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -38,11 +38,6 @@ #include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/actuators.h" -#if defined RADIO_CONTROL -#include "subsystems/radio_control.h" -#pragma message "CAUTION! RadioControl roll and yaw channel inputs have been reversed to follow aerospace sign conventions.\n You will have to change your radio control xml file to get a positive value when pushing roll stick right and a positive value when pushing yaw stick right!" -#endif - #include "subsystems/imu.h" #include "subsystems/gps.h" diff --git a/sw/airborne/inter_mcu.h b/sw/airborne/inter_mcu.h index 925fbe3c9ab..9361dfe0fd8 100644 --- a/sw/airborne/inter_mcu.h +++ b/sw/airborne/inter_mcu.h @@ -64,6 +64,8 @@ struct fbw_state { struct ap_state { pprz_t commands[COMMANDS_NB]; + pprz_t command_roll_trim; + pprz_t command_pitch_trim; }; // Status bits from FBW to AUTOPILOT diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.c b/sw/airborne/modules/cartography/photogrammetry_calculator.c index 8f722a39b3a..02dc12c7150 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.c +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.c @@ -40,9 +40,13 @@ #define PHOTOGRAMMETRY_SIDELAP 50 #endif +#ifndef PHOTOGRAMMETRY_RESOLUTION +#define PHOTOGRAMMETRY_RESOLUTION 50 +#endif + // Flightplan Paramters -int photogrammetry_sweep_angle = 0; // in rad +float photogrammetry_sweep_angle = 0; // in rad int photogrammetry_sidestep = 0; int photogrammetry_triggerstep = 0; @@ -71,10 +75,10 @@ void init_photogrammetry_calculator(void) photogrammetry_height_max = PHOTOGRAMMETRY_HEIGHT_MAX; photogrammetry_radius_min = PHOTOGRAMMETRY_RADIUS_MIN; - photogrammetry_calculator_update(); + photogrammetry_calculator_update_camera2flightplan(); } -void photogrammetry_calculator_update(void) +void photogrammetry_calculator_update_camera2flightplan(void) { // Photogrammetry Goals @@ -98,4 +102,19 @@ void photogrammetry_calculator_update(void) photogrammetry_triggerstep = viewing_ratio_height * photogrammetry_height * (1.0f - photogrammetry_overlap_f); } +void photogrammetry_calculator_update_flightplan2camera(void) +{ + // Linear Projection Camera Model + float viewing_ratio_height = ((float) PHOTOGRAMMETRY_SENSOR_HEIGHT) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH); + float viewing_ratio_width = ((float) PHOTOGRAMMETRY_SENSOR_WIDTH) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH); + float pixel_projection_width = viewing_ratio_width / ((float)PHOTOGRAMMETRY_PIXELS_WIDTH); + + // Resolution <-> Height + photogrammetry_resolution = photogrammetry_height * 1000.0f * pixel_projection_width; + + // Overlap <-> track width + photogrammetry_sidelap = 100.0f - photogrammetry_sidestep / viewing_ratio_width / photogrammetry_height * 100.0f; + photogrammetry_overlap = 100.0f - photogrammetry_triggerstep / viewing_ratio_height / photogrammetry_height * 100.0f; +} + diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.h b/sw/airborne/modules/cartography/photogrammetry_calculator.h index a906474646e..6b16748b7b8 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.h +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.h @@ -83,7 +83,7 @@ Add to flightplan // Flightplan Variables -extern int photogrammetry_sweep_angle; +extern float photogrammetry_sweep_angle; extern int photogrammetry_sidestep; extern int photogrammetry_triggerstep; extern int photogrammetry_height; @@ -99,34 +99,53 @@ extern int photogrammetry_overlap; extern int photogrammetry_resolution; void init_photogrammetry_calculator(void); -void photogrammetry_calculator_update(void); +void photogrammetry_calculator_update_camera2flightplan(void); +void photogrammetry_calculator_update_flightplan2camera(void); -// Update Parameters on Settings Change +// Update Flightplan on Camera Change #define photogrammetry_calculator_UpdateSideLap(X) { \ photogrammetry_sidelap = X; \ - photogrammetry_calculator_update(); \ + photogrammetry_calculator_update_camera2flightplan(); \ } #define photogrammetry_calculator_UpdateOverLap(X) { \ photogrammetry_overlap = X; \ - photogrammetry_calculator_update(); \ + photogrammetry_calculator_update_camera2flightplan(); \ } #define photogrammetry_calculator_UpdateResolution(X) { \ photogrammetry_resolution = X; \ - photogrammetry_calculator_update(); \ + photogrammetry_calculator_update_camera2flightplan(); \ } +// Update Camera on Flightplan Change +#define photogrammetry_calculator_UpdateHeight(X) { \ + photogrammetry_height = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ +} + +#define photogrammetry_calculator_UpdateSideStep(X) { \ + photogrammetry_sidestep = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ +} + +#define photogrammetry_calculator_UpdateTriggerStep(X) { \ + photogrammetry_triggerstep = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ +} + + // Flightplan Routine Wrappers #define PhotogrammetryCalculatorPolygonSurvey(_WP, _COUNT) { \ WaypointAlt(WP__BASELEG) = photogrammetry_height + GROUND_ALT; \ - int _ang = 90 - photogrammetry_sweep_angle; \ + WaypointAlt(_WP) = photogrammetry_height + GROUND_ALT; \ + int _ang = 90 - DegOfRad(photogrammetry_sweep_angle); \ if (_ang > 90) _ang -= 180; if (_ang < -90) _ang += 180; \ InitializePolygonSurvey((_WP), (_COUNT), 2*photogrammetry_sidestep, _ang); \ } #define PhotogrammetryCalculatorPolygonSurveyADV(_WP, _COUNT) { \ - init_poly_survey_adv((_WP), (_COUNT), photogrammetry_sweep_angle, \ + init_poly_survey_adv((_WP), (_COUNT), DegOfRad(photogrammetry_sweep_angle), \ photogrammetry_sidestep, photogrammetry_triggerstep, \ photogrammetry_radius_min, photogrammetry_height + GROUND_ALT); \ } diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c index 112ca78f6d5..0350714b7e9 100644 --- a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c @@ -40,6 +40,11 @@ #include "messages.h" #include "subsystems/datalink/downlink.h" +// In I2C mode we can not inline this function: +void dc_send_command(uint8_t cmd) +{ + atmega_i2c_cam_ctrl_send(cmd); +} static struct i2c_transaction atmega_i2c_cam_ctrl_trans; diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h index e825df002bc..aaccdbb295b 100644 --- a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h @@ -32,12 +32,6 @@ void atmega_i2c_cam_ctrl_periodic(void); void atmega_i2c_cam_ctrl_event(void); void atmega_i2c_cam_ctrl_send(uint8_t cmd); -// In I2C mode we can not inline this function: -static inline void dc_send_command(uint8_t cmd) -{ - atmega_i2c_cam_ctrl_send(cmd); -} - // Allow commands to be set by datalink #define ParseCameraCommand() { \ { \ diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h index 2ae73b2c6c9..fe47e1865c6 100644 --- a/sw/airborne/modules/digital_cam/dc.h +++ b/sw/airborne/modules/digital_cam/dc.h @@ -100,7 +100,7 @@ typedef enum { } dc_command_type; /* Send Command To Camera */ -static inline void dc_send_command(uint8_t cmd); +extern void dc_send_command(uint8_t cmd); /* Auotmatic Digital Camera Photo Triggering */ typedef enum { diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.c b/sw/airborne/modules/digital_cam/led_cam_ctrl.c index d76fcce68e9..8928381f48a 100644 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.c @@ -26,5 +26,36 @@ uint8_t dc_timer; +/* Command The Camera */ +void dc_send_command(uint8_t cmd) +{ + dc_timer = DC_SHUTTER_DELAY; + switch (cmd) + { + case DC_SHOOT: + DC_PUSH(DC_SHUTTER_LED); +#ifndef DC_SHOOT_ON_BUTTON_RELEASE + dc_send_shot_position(); +#endif + break; +#ifdef DC_ZOOM_IN_LED + case DC_TALLER: + DC_PUSH(DC_ZOOM_IN_LED); + break; +#endif +#ifdef DC_ZOOM_OUT_LED + case DC_WIDER: + DC_PUSH(DC_ZOOM_OUT_LED); + break; +#endif +#ifdef DC_POWER_LED + case DC_ON: + DC_PUSH(DC_POWER_LED); + break; +#endif + default: + break; + } +} diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.h b/sw/airborne/modules/digital_cam/led_cam_ctrl.h index ac15e2a4c5c..421d44b9621 100644 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.h @@ -77,40 +77,16 @@ static inline void led_cam_ctrl_init(void) #error DC: Please specify at least a SHUTTER LED #endif -/* Command The Camera */ -static inline void dc_send_command(uint8_t cmd) -{ - dc_timer = DC_SHUTTER_DELAY; - switch (cmd) - { - case DC_SHOOT: - DC_PUSH(DC_SHUTTER_LED); - dc_send_shot_position(); - break; -#ifdef DC_ZOOM_IN_LED - case DC_TALLER: - DC_PUSH(DC_ZOOM_IN_LED); - break; -#endif -#ifdef DC_ZOOM_OUT_LED - case DC_WIDER: - DC_PUSH(DC_ZOOM_OUT_LED); - break; -#endif -#ifdef DC_POWER_LED - case DC_POWER: - DC_PUSH(DC_POWER_LED); - break; -#endif - default: - break; - } -} - /* 4Hz Periodic */ static inline void led_cam_ctrl_periodic( void ) { +#ifdef DC_SHOOT_ON_BUTTON_RELEASE + if (dc_timer==1) { + dc_send_shot_position(); + } +#endif + if (dc_timer) { dc_timer--; } else { diff --git a/sw/airborne/modules/digital_cam/servo_cam_ctrl.c b/sw/airborne/modules/digital_cam/servo_cam_ctrl.c index c4a8b0ad21e..c5a073ef18a 100644 --- a/sw/airborne/modules/digital_cam/servo_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/servo_cam_ctrl.c @@ -25,6 +25,37 @@ // Button Timer uint8_t dc_timer; +/* Command The Camera */ +void dc_send_command(uint8_t cmd) +{ + dc_timer = DC_SHUTTER_DELAY; + switch (cmd) + { + case DC_SHOOT: + DC_PUSH(DC_SHUTTER_SERVO); +#ifndef DC_SHOOT_ON_BUTTON_RELEASE + dc_send_shot_position(); +#endif + break; +#ifdef DC_ZOOM_IN_SERVO + case DC_TALLER: + DC_PUSH(DC_ZOOM_IN_SERVO); + break; +#endif +#ifdef DC_ZOOM_OUT_SERVO + case DC_WIDER: + DC_PUSH(DC_ZOOM_OUT_SERVO); + break; +#endif +#ifdef DC_POWER_SERVO + case DC_ON: + DC_PUSH(DC_POWER_SERVO); + break; +#endif + default: + break; + } +} diff --git a/sw/airborne/modules/digital_cam/servo_cam_ctrl.h b/sw/airborne/modules/digital_cam/servo_cam_ctrl.h index f0d0c20c834..9d8da3a9223 100644 --- a/sw/airborne/modules/digital_cam/servo_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/servo_cam_ctrl.h @@ -49,7 +49,7 @@ // Include Servo and airframe servo channels #include "std.h" -#include "commands.h" +#include "inter_mcu.h" #include "generated/airframe.h" extern uint8_t dc_timer; @@ -63,8 +63,8 @@ static inline void servo_cam_ctrl_init(void) dc_timer = 0; } -#define DC_PUSH(X) commands[X] = -MAX_PPRZ; -#define DC_RELEASE(X) commands[X] = MAX_PPRZ; +#define DC_PUSH(X) ap_state->commands[X] = -MAX_PPRZ; +#define DC_RELEASE(X) ap_state->commands[X] = MAX_PPRZ; #ifndef DC_SHUTTER_DELAY #define DC_SHUTTER_DELAY 2 /* 4Hz -> 0.5s */ @@ -74,40 +74,16 @@ static inline void servo_cam_ctrl_init(void) #error DC: Please specify at least a SHUTTER SERVO #endif -/* Command The Camera */ -static inline void dc_send_command(uint8_t cmd) -{ - dc_timer = DC_SHUTTER_DELAY; - switch (cmd) - { - case DC_SHOOT: - DC_PUSH(DC_SHUTTER_SERVO); - dc_send_shot_position(); - break; -#ifdef DC_ZOOM_IN_SERVO - case DC_TALLER: - DC_PUSH(DC_ZOOM_IN_SERVO); - break; -#endif -#ifdef DC_ZOOM_OUT_SERVO - case DC_WIDER: - DC_PUSH(DC_ZOOM_OUT_SERVO); - break; -#endif -#ifdef DC_POWER_SERVO - case DC_POWER: - DC_PUSH(DC_POWER_SERVO); - break; -#endif - default: - break; - } -} - /* 4Hz Periodic */ static inline void servo_cam_ctrl_periodic( void ) { +#ifdef DC_SHOOT_ON_BUTTON_RELEASE + if (dc_timer==1) { + dc_send_shot_position(); + } +#endif + if (dc_timer) { dc_timer--; } else { diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 92caf543d68..cb1349d7a4b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -39,6 +39,7 @@ #include "led.h" +#if FLOAT_DCM_SEND_DEBUG // FIXME Debugging Only #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -46,6 +47,7 @@ #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" +#endif #ifdef AHRS_UPDATE_FW_ESTIMATOR @@ -156,6 +158,12 @@ void ahrs_init(void) { high_accel_done = FALSE; high_accel_flag = FALSE; #endif + + ahrs_impl.gps_speed = 0; + ahrs_impl.gps_acceleration = 0; + ahrs_impl.gps_course = 0; + ahrs_impl.gps_course_valid = FALSE; + ahrs_impl.gps_age = 100; } void ahrs_align(void) @@ -213,9 +221,33 @@ void ahrs_propagate(void) compute_ahrs_representations(); } -void ahrs_update_accel(void) +void ahrs_update_gps(void) { + static float last_gps_speed_3d = 0; + +#if USE_GPS + if (gps.fix == GPS_FIX_3D) { + ahrs_impl.gps_age = 0; + ahrs_impl.gps_speed = gps.speed_3d/100.; + + if(gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 0.5 m/s + ahrs_impl.gps_course = ((float)gps.course)/1.e7; + ahrs_impl.gps_course_valid = TRUE; + } else { + ahrs_impl.gps_course_valid = FALSE; + } + } else { + ahrs_impl.gps_age = 100; + } +#endif + + ahrs_impl.gps_acceleration += ( ((ahrs_impl.gps_speed - last_gps_speed_3d)*4.0f) - ahrs_impl.gps_acceleration) / 5.0f; + last_gps_speed_3d = ahrs_impl.gps_speed; +} + +void ahrs_update_accel(void) +{ ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); // DCM filter uses g-force as positive @@ -225,12 +257,21 @@ void ahrs_update_accel(void) accel_float.z = -accel_float.z; -#if USE_GPS - if (gps.fix == GPS_FIX_3D) { //Remove centrifugal acceleration. - accel_float.y += gps.speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ - accel_float.z -= gps.speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY - } + ahrs_impl.gps_age ++; + if (ahrs_impl.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration +#if USE_AHRS_GPS_ACCELERATIONS +#pragma message "AHRS_FLOAT_DCM uses GPS acceleration." + accel_float.x += ahrs_impl.gps_acceleration; // Longitudinal acceleration #endif + accel_float.y += ahrs_impl.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ + accel_float.z -= ahrs_impl.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY + } + else + { + ahrs_impl.gps_speed = 0; + ahrs_impl.gps_acceleration = 0; + ahrs_impl.gps_age = 100; + } Drift_correction(); } @@ -281,18 +322,16 @@ void ahrs_update_mag(void) ltp_mag.x = MAG_Heading_X; ltp_mag.y = MAG_Heading_Y; +#if FLOAT_DCM_SEND_DEBUG // Downlink RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, <p_mag.x, <p_mag.y, <p_mag.z)); +#endif // Magnetic Heading // MAG_Heading = atan2(imu.mag.y, -imu.mag.x); #endif } -void ahrs_update_gps(void) { - -} - void Normalize(void) { float error=0; @@ -448,12 +487,12 @@ void Drift_correction(void) Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I -#elif USE_GPS // Use GPS Ground course to correct yaw gyro drift +#else // Use GPS Ground course to correct yaw gyro drift - if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s - float ground_course = ((float)gps.course)/1.e7 - M_PI; //This is the runaway direction of you "plane" in rad - float COGX = cosf(ground_course); //Course overground X axis - float COGY = sinf(ground_course); //Course overground Y axis + if (ahrs_impl.gps_course_valid) { + float course = ahrs_impl.gps_course - M_PI; //This is the runaway direction of you "plane" in rad + float COGX = cosf(course); //Course overground X axis + float COGY = sinf(course); //Course overground Y axis errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index 8bb7381109b..31f82f60e6d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -34,6 +34,12 @@ struct AhrsFloatDCM { */ struct FloatQuat body_to_imu_quat; struct FloatRMat body_to_imu_rmat; + + float gps_speed; + float gps_acceleration; + float gps_course; + bool_t gps_course_valid; + uint8_t gps_age; }; extern struct AhrsFloatDCM ahrs_impl; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index eff2aa00111..213c32291ae 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -161,6 +161,8 @@ void ahrs_update_accel(void) { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)}; struct Int32Vect3 residual; + struct Int32Vect3 pseudo_gravity_measurement; + if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame @@ -181,23 +183,29 @@ void ahrs_update_accel(void) { INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ - struct Int32Vect3 corrected_gravity; - INT32_VECT3_DIFF(corrected_gravity, imu.accel, acc_c_imu); - - /* compute the residual of gravity vector in imu frame */ - INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2); + INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); } else { - INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2); + VECT3_COPY(pseudo_gravity_measurement, imu.accel); } + /* compute the residual of the pseudo gravity vector in imu frame */ + INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); + int32_t inv_weight; if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration norm */ + + /* FIR filtered pseudo_gravity_measurement */ + static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, 7); + VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); + VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, 8); + int32_t acc_norm; - INT32_VECT3_NORM(acc_norm, imu.accel); + INT32_VECT3_NORM(acc_norm, filtered_gravity_measurement); const int32_t acc_norm_d = ABS(ACCEL_BFP_OF_REAL(9.81)-acc_norm); - inv_weight = Chop(6*acc_norm_d/ACCEL_BFP_OF_REAL(9.81), 1, 6); + inv_weight = Chop(50*acc_norm_d/ACCEL_BFP_OF_REAL(9.81), 1, 50); } else { inv_weight = 1; @@ -218,9 +226,9 @@ void ahrs_update_accel(void) { // ahrs_impl.high_rez_bias.q += residual.y*3; // ahrs_impl.high_rez_bias.r += residual.z*3; - ahrs_impl.high_rez_bias.p += residual.x/inv_weight; - ahrs_impl.high_rez_bias.q += residual.y/inv_weight; - ahrs_impl.high_rez_bias.r += residual.z/inv_weight; + ahrs_impl.high_rez_bias.p += residual.x/(2*inv_weight); + ahrs_impl.high_rez_bias.q += residual.y/(2*inv_weight); + ahrs_impl.high_rez_bias.r += residual.z/(2*inv_weight); /* */ diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c index 3a09d9cbe49..41c84397e1a 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin2.c +++ b/sw/airborne/subsystems/imu/imu_aspirin2.c @@ -105,6 +105,13 @@ static void mpu_configure(void) { aspirin2_mpu60x0.length = 2; + /////////////////// + // Reset the MPU + mpu_set( MPU60X0_REG_USER_CTRL, + (1 << 2) | // Trigger a FIFO_RESET + (1 << 1) | // Trigger a I2C_MST_RESET + (1 << 0) ); // Trigger a SIG_COND_RESET + /////////////////// // Configure power: diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index 5d1465b41a4..a00afe617a4 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -33,7 +33,6 @@ #include "subsystems/nav.h" #include "subsystems/gps.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" #include "firmwares/fixedwing/autopilot.h" #include "inter_mcu.h" #include "subsystems/navigation/traffic_info.h" @@ -236,6 +235,24 @@ static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t w return FALSE; } +static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide ) { + + float x_0 = waypoints[wp_td].x - waypoints[wp_af].x; + float y_0 = waypoints[wp_td].y - waypoints[wp_af].y; + float h_0 = waypoints[wp_td].a - waypoints[wp_af].a; + + /* Unit vector from AF to TD */ + float d = sqrt(x_0*x_0+y_0*y_0); + float x_1 = x_0 / d; + float y_1 = y_0 / d; + + waypoints[wp_af].x = waypoints[wp_td].x + x_1 * h_0 * glide; + waypoints[wp_af].y = waypoints[wp_td].y + y_1 * h_0 * glide; + waypoints[wp_af].a = waypoints[wp_af].a; + + return FALSE; +} + /* For a landing UPWIND. Computes Top Of Descent waypoint from Touch Down and Approach Fix diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 2a846490141..31ea51d0637 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -35,7 +35,9 @@ #include "std.h" #include "paparazzi.h" #include "state.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" +#ifdef CTRL_TYPE_H +#include CTRL_TYPE_H +#endif #include "subsystems/navigation/nav_survey_rectangle.h" #include "subsystems/navigation/common_flight_plan.h" #include "subsystems/navigation/common_nav.h" diff --git a/sw/airborne/test/ahrs/Makefile b/sw/airborne/test/ahrs/Makefile index 8451f7bbbb7..9603fb946c4 100644 --- a/sw/airborne/test/ahrs/Makefile +++ b/sw/airborne/test/ahrs/Makefile @@ -70,10 +70,14 @@ ifndef USE_GPS_HEADING USE_GPS_HEADING = 0 endif +ifndef FREQUENCY +FREQENCY = 512 +endif + AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE) -AHRS_CFLAGS += -DPERIODIC_FREQUENCY=512 -AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +AHRS_CFLAGS += -DPERIODIC_FREQUENCY=$(FREQUENCY) +AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(FREQUENCY) ifeq ($(PROPAGATE_LOW_PASS_RATES), 1) AHRS_CFLAGS += -DAHRS_PROPAGATE_LOW_PASS_RATES @@ -93,6 +97,9 @@ endif ifeq ($(DISABLE_MAG_UPDATE), 1) AHRS_CFLAGS += -DDISABLE_MAG_UPDATE endif +ifeq ($(USE_AHRS_GPS_ACCELERATIONS), 1) +AHRS_CFLAGS += -DUSE_AHRS_GPS_ACCELERATIONS +endif AHRS_SRCS += ../../subsystems/ahrs.c \ @@ -113,10 +120,6 @@ AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_lkf_quat.c endif ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR) AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" -AHRS_CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0 -AHRS_CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0 -AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 -AHRS_CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c endif ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR2) diff --git a/sw/airborne/test/ahrs/ahrs_on_synth.c b/sw/airborne/test/ahrs/ahrs_on_synth.c index 075894ad189..faf9ff1f292 100644 --- a/sw/airborne/test/ahrs/ahrs_on_synth.c +++ b/sw/airborne/test/ahrs/ahrs_on_synth.c @@ -42,6 +42,9 @@ static void traj_coordinated_circle_update(void); static void traj_stop_stop_x_init(void); static void traj_stop_stop_x_update(void); +static void traj_bungee_takeoff_init(void); +static void traj_bungee_takeoff_update(void); + struct traj traj[] = { {.name="static", .desc="blaa", .init_fun=traj_static_static_init, .update_fun=traj_static_static_update}, @@ -58,7 +61,9 @@ struct traj traj[] = { {.name="coordinated circle", .desc="blaa2", .init_fun=traj_coordinated_circle_init, .update_fun=traj_coordinated_circle_update}, {.name="stop stop x", .desc="blaa2", - .init_fun=traj_stop_stop_x_init, .update_fun=traj_stop_stop_x_update} + .init_fun=traj_stop_stop_x_init, .update_fun=traj_stop_stop_x_update}, + {.name="bungee", .desc="blaa2", + .init_fun=traj_bungee_takeoff_init, .update_fun=traj_bungee_takeoff_update} }; @@ -133,21 +138,24 @@ void aos_init(int traj_nb) { #ifdef AHRS_PROPAGATE_LOW_PASS_RATES printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n"); #endif -#ifdef AHRS_MAG_UPDATE_YAW_ONLY +#if AHRS_MAG_UPDATE_YAW_ONLY printf("# AHRS_MAG_UPDATE_YAW_ONLY\n"); #endif -#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN +#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n"); #endif -#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC +#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n"); #endif #ifdef PERFECT_SENSORS printf("# PERFECT_SENSORS\n"); #endif -#ifdef AHRS_USE_GPS_HEADING +#if AHRS_USE_GPS_HEADING printf("# AHRS_USE_GPS_HEADING\n"); #endif +#if USE_AHRS_GPS_ACCELERATIONS + printf("# USE_AHRS_GPS_ACCELERATIONS\n"); +#endif printf("# tajectory : %s\n", aos.traj->name); @@ -192,6 +200,12 @@ void aos_compute_sensors(void) { ahrs_impl.ltp_vel_norm = FLOAT_VECT3_NORM(aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = TRUE; #endif +#if AHRS_TYPE == AHRS_TYPE_FCR + ahrs_impl.gps_speed = FLOAT_VECT3_NORM(aos.ltp_vel); + ahrs_impl.gps_age = 0; + ahrs_update_gps(); + //RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration)); +#endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel)); ahrs_impl.ltp_vel_norm_valid = TRUE; @@ -237,6 +251,10 @@ void aos_run(void) { float heading = aos.heading_meas; #endif +#if AHRS_TYPE == AHRS_TYPE_FCR + ahrs_impl.gps_course = aos.heading_meas; + ahrs_impl.gps_course_valid = TRUE; +#else if (aos.time > 10) { if (!ahrs_impl.heading_aligned) { ahrs_realign_heading(heading); @@ -244,6 +262,8 @@ void aos_run(void) { RunOnceEvery(100,ahrs_update_heading(heading)); } } +#endif + #endif // AHRS_USE_GPS_HEADING #ifndef DISABLE_ALIGNEMENT @@ -445,3 +465,34 @@ static void traj_stop_stop_x_update(void){ FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); } + +static void traj_bungee_takeoff_init(void) { + + aos.traj->te = 40.; + EULERS_ASSIGN(aos.ltp_to_imu_euler, 0, RadOfDeg(10), 0); + FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); + +} + +static void traj_bungee_takeoff_update(void) { + const float initial_bungee_accel = 20.0; // in m/s^2 + const float start = 5; + const float duration = 2; + + struct FloatVect3 accel = {0, 0, 0}; //acceleration in imu x-direction in m/s^2 + + if (aos.time > start && aos.time < start+duration) { + accel.x = initial_bungee_accel * (1 - (aos.time - start) / duration); + } + else { + accel.x = 0; + } + + struct FloatQuat imu2ltp; + QUAT_INVERT(imu2ltp, aos.ltp_to_imu_quat); + FLOAT_QUAT_VMULT(aos.ltp_accel, imu2ltp, accel); + + FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt); + FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt); + +} diff --git a/sw/airborne/test/ahrs/ahrs_utils.py b/sw/airborne/test/ahrs/ahrs_utils.py index 03dfd19e3df..b6636d25845 100644 --- a/sw/airborne/test/ahrs/ahrs_utils.py +++ b/sw/airborne/test/ahrs/ahrs_utils.py @@ -84,15 +84,16 @@ def run_simulation(ahrs_type, build_opt, traj_nb): -def plot_simulation_results(plot_true_state, lsty, sim_res): +def plot_simulation_results(plot_true_state, lsty, type, sim_res): print "Plotting Results" # f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True) subplot(3,3,1) - plt.plot(sim_res.time, sim_res.phi_ahrs, lsty) + plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=type) ylabel('degres') title('phi') + legend() subplot(3,3,2) plot(sim_res.time, sim_res.theta_ahrs, lsty) @@ -155,4 +156,4 @@ def plot_simulation_results(plot_true_state, lsty, sim_res): def show_plot(): - plt.show(); + plt.show(); diff --git a/sw/airborne/test/ahrs/compare_ahrs.py b/sw/airborne/test/ahrs/compare_ahrs.py index 2dbedcd54fd..69fafbac495 100755 --- a/sw/airborne/test/ahrs/compare_ahrs.py +++ b/sw/airborne/test/ahrs/compare_ahrs.py @@ -1,6 +1,5 @@ #! /usr/bin/env python -# $Id$ # Copyright (C) 2011 Antoine Drouin # # This file is part of Paparazzi. @@ -25,47 +24,57 @@ def main(): -# traj_nb = 0; # static -# traj_nb = 1; # sine orientation -# traj_nb = 2; # sine X quad -# traj_nb = 3; # step_phi -# traj_nb = 4; # step_phi_2nd_order -# traj_nb = 5; # step_bias - traj_nb = 6; # coordinated circle -# traj_nb = 7; # stop stop X quad +# traj_nb = 0; # static +# traj_nb = 1; # sine orientation +# traj_nb = 2; # sine X quad +# traj_nb = 3; # step_phi +# traj_nb = 4; # step_phi_2nd_order +# traj_nb = 5; # step_bias +# traj_nb = 6; # coordinated circle +# traj_nb = 7; # stop stop X quad + traj_nb = 8; # bungee takeoff - build_opt1 = []; -# build_opt1 = ["Q="]; -# build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]; - build_opt1 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; -# build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=0"]; - build_opt1 += ["DISABLE_MAG_UPDATE=1"]; - build_opt1 += ["USE_GPS_HEADING=1"]; -# build_opt1 = ["MAG_UPDATE_YAW_ONLY=1"]; -# ahrs_type1 = "FCQ"; -# ahrs_type1 = "FCR2"; -# ahrs_type1 = "FLQ"; - ahrs_type1 = "ICQ"; - sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb) -# import code; code.interact(local=locals()) + build_opt1 = []; +# build_opt1 = ["Q="]; + build_opt1 += ["FREQUENCY=120"]; +# build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]; + build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; + build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; +# build_opt1 += ["DISABLE_MAG_UPDATE=1"]; +# build_opt1 += ["USE_GPS_HEADING=1"]; +# build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"]; - build_opt2 = []; - build_opt2 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; -# build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; - build_opt2 += ["DISABLE_MAG_UPDATE=1"]; - build_opt2 += ["USE_GPS_HEADING=1"]; -# build_opt2 = ["MAG_UPDATE_YAW_ONLY=0"]; -# build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]; -# build_opt2 = build_opt1; -# ahrs_type2 = "FLQ"; - ahrs_type2 = "FCQ"; -# ahrs_type2 = "ICQ"; -# ahrs_type2 = ahrs_type1; - sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb) +# ahrs_type1 = "FCQ"; +# ahrs_type1 = "FCR2"; +# ahrs_type1 = "FLQ"; + ahrs_type1 = "ICQ"; +# ahrs_type1 = "FCR"; - ahrs_utils.plot_simulation_results(False, 'b', sim_res1) - ahrs_utils.plot_simulation_results(True, 'g', sim_res2) - ahrs_utils.show_plot() + sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb) +# import code; code.interact(local=locals()) + + build_opt2 = []; + build_opt2 += ["FREQUENCY=120"]; + build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; + build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; + build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"]; +# build_opt2 += ["DISABLE_MAG_UPDATE=1"]; +# build_opt2 += ["USE_GPS_HEADING=1"]; +# build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"]; +# build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]; +# build_opt2 = build_opt1; + +# ahrs_type2 = "FLQ"; +# ahrs_type2 = "FCR2"; +# ahrs_type2 = "ICQ"; + ahrs_type2 = "FCR"; +# ahrs_type2 = ahrs_type1; + + sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb) + + ahrs_utils.plot_simulation_results(False, 'b', ahrs_type1, sim_res1) + ahrs_utils.plot_simulation_results(True, 'g', ahrs_type2, sim_res2) + ahrs_utils.show_plot() if __name__ == "__main__": - main() + main() diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth.c b/sw/airborne/test/ahrs/run_ahrs_on_synth.c index b15e942d2bf..a0d45107a7a 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth.c @@ -71,7 +71,7 @@ static void report(void) { DegOfRad(bias.q), DegOfRad(bias.r)); #endif -#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ +#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p), DegOfRad(ahrs_impl.gyro_bias.q), DegOfRad(ahrs_impl.gyro_bias.r)); diff --git a/sw/ext/Makefile b/sw/ext/Makefile index a414feafb1e..0980fb8e017 100644 --- a/sw/ext/Makefile +++ b/sw/ext/Makefile @@ -30,14 +30,14 @@ EXT_DIR=$(PAPARAZZI_SRC)/sw/ext PREFIX = arm-none-eabi # -# if we can find arm-none-eabi-gcc in the path, overwrite PREFIX with explicit path +# if we can't find arm-none-eabi-gcc in the path, overwrite PREFIX with explicit path # HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc) ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),) -# not found in PATH, check paparazzi toolchains +# not found in PATH, check paparazzi toolchain - TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib /opt/paparazzi/stm32 -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) + TOOLCHAIN=$(shell find -L /opt/paparazzi/arm-multilib -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1) ifneq ($(TOOLCHAIN),) TOOLCHAIN_DIR=$(shell dirname $(TOOLCHAIN)) PREFIX = $(TOOLCHAIN_DIR)/bin/arm-none-eabi diff --git a/sw/in_progress/python/attitude_viz.py b/sw/in_progress/python/attitude_viz.py index 1e50753a810..695f3ef661f 100755 --- a/sw/in_progress/python/attitude_viz.py +++ b/sw/in_progress/python/attitude_viz.py @@ -13,6 +13,8 @@ import pygame import time +import platform +import os _NAME = 'attitude_viz' @@ -127,7 +129,8 @@ def DrawVehicle(self, name): glColor3f(0.0, 0.0, 0.0) glTranslate(-wingspan, -0.2, thickness + 0.01) glScale(0.004, 0.004, 0.004) - glutStrokeString(GLUT_STROKE_ROMAN, name) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) glPopMatrix() glPushMatrix() @@ -138,7 +141,8 @@ def DrawVehicle(self, name): glTranslate(wingspan, -0.2, -0.01 - thickness) glScale(0.004, 0.004, 0.004) glRotate(180, 0, 1, 0) - glutStrokeString(GLUT_STROKE_ROMAN, name) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) glPopMatrix() if self.display_list is None: @@ -188,7 +192,8 @@ def DrawBar(self, name, value): glColor3f(0, 0, 0) glTranslate(-bar_length, -0.09, 0.02) glScale(0.0015, 0.0015, 0.0015) - glutStrokeString(GLUT_STROKE_ROMAN, name) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) glPopMatrix() glColor3f(0.92, 0.92, 0.92) glPushMatrix() @@ -244,7 +249,13 @@ def __init__(self): lambda x, z: z ) - IvyStart("") + if os.getenv('IVY_BUS') != None: + IvyStart(os.getenv('IVY_BUS')) + else: + if platform.system() == 'Darwin': + IvyStart("224.255.255.255:2010") + else: + IvyStart() # list of all message names messages = [] diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 00892d003ef..efcf53a8f5d 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -173,9 +173,11 @@ let scale_of_units = fun ?auto from_unit to_unit -> (* will raise Xml.No_attribute if not a valid attribute *) let f = Xml.attrib u "from" and t = Xml.attrib u "to" - and a = match auto with - | Some a -> a - | None -> String.lowercase (ExtXml.attrib_or_default u "auto" "") in + and a = try Some (Xml.attrib u "auto") with _ -> None in + let a = match auto, a with + | Some _, None | None, None -> "" (* No auto conversion *) + | Some t, Some _ | None, Some t -> String.lowercase t (* param auto is used before attribute *) + in if (f = from_unit || a = "display") && (t = to_unit || a = "code") then true else false ) (Xml.children units_xml) in (* return coef, raise Failure if coef is not a numerical value *) diff --git a/sw/tools/Makefile b/sw/tools/Makefile index 8dc8a8607a3..83603a21a76 100644 --- a/sw/tools/Makefile +++ b/sw/tools/Makefile @@ -30,7 +30,7 @@ OCAMLNETCMA=$(shell ocamlfind query -r -a-format -predicates byte netstring) OCAMLLEX=ocamllex OCAMLYACC=ocamlyacc -all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_xsens.out gen_modules.out find_free_msg_id.out gen_srtm.out +all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_xsens.out gen_modules.out find_free_msg_id.out gen_srtm.out mergelogs FP_CMO = fp_proc.cmo gen_flight_plan.cmo ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%) @@ -44,6 +44,9 @@ gen_flight_plan.cmo : fp_proc.cmi gen_common.cmo : gen_common.cmi +mergelogs: mergelogs.c + gcc mergelogs.c -o mergelogs + %.out : %.ml gen_common.ml Makefile @echo OC $< $(Q)$(OCAMLC) $(INCLUDES) -o $@ unix.cma str.cma ivy-ocaml.cma xml-light.cma lib-pprz.cma gen_common.cmo $< @@ -72,4 +75,4 @@ gen_common.cmo : gen_common.cmi $(Q)$(OCAMLYACC) $< clean: - rm -f *.cm* *.out *~ fp_parser.ml fp_parser.mli + rm -f *.cm* *.out *~ fp_parser.ml fp_parser.mli mergelogs diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml index aa58e5ad542..458f6073542 100644 --- a/sw/tools/gen_airframe.ml +++ b/sw/tools/gen_airframe.ml @@ -171,6 +171,13 @@ let parse_command_laws = fun command -> and value = a "value" in let v = preprocess_value value "values" "COMMAND" in printf " int16_t _var_%s = %s;\\\n" var v + | "ratelimit" -> + let var = a "var" + and value = a "value" + and rate_min = a "rate_min" + and rate_max = a "rate_max" in + let v = preprocess_value value "values" "COMMAND" in + printf " static int16_t _var_%s = 0; _var_%s = Chop((%s) - (_var_%s), (%s), (%s));\\\n" var var v var rate_min rate_max | "define" -> parse_element "" command | _ -> xml_error "set|let" diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 1e7e0f28f0e..df1012965be 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -81,9 +81,14 @@ let print_dl_settings = fun settings -> let v = ExtXml.attrib s "var" in begin try - let h = ExtXml.attrib s "handler" and - m = ExtXml.attrib s "module" in - lprintf "case %d: %s_%s( _value ); _value = %s; break;\\\n" !idx (Filename.basename m) h v + let h = ExtXml.attrib s "handler" in + begin + try + let m = ExtXml.attrib s "module" in + lprintf "case %d: %s_%s( _value ); _value = %s; break;\\\n" !idx (Filename.basename m) h v + with + ExtXml.Error e -> prerr_endline (sprintf "Error: You need to specify the module attribute for setting %s to use the handler %s" v h); exit 1 + end; with ExtXml.Error e -> lprintf "case %d: %s = _value; break;\\\n" !idx v end; diff --git a/sw/tools/mergelogs.c b/sw/tools/mergelogs.c new file mode 100644 index 00000000000..cffca507013 --- /dev/null +++ b/sw/tools/mergelogs.c @@ -0,0 +1,89 @@ +#include +#include + + +char buf[200]; + +float get_time(char* str) +{ + float time = 0.0f; + sscanf(str,"%f", &time); + return time; +} + +void add_time(char* str, float offset, FILE* fp) +{ + float time = 0.0f; + char* c = strchr(str, ' '); + if (c == 0) + return; + + sscanf(str,"%f", &time); + fprintf(fp, "%.3f%s", time+offset, c); +} + +int merge(char* fn1, char* fn2) +{ + FILE* f1 = fopen(fn1,"r+w"); + FILE* f2 = fopen(fn2,"r"); + int res = 0; + char str[200]; + float offset = 0.0f; + int firstline = 1; + + if ((f1 > 0) && (f2 > 0)) + { + // + printf("Searching End Time in File '%s'\n",fn1); + + while(fgets(str,sizeof(str),f1) != NULL) + { + } + + printf("Last Line:\n%s\n",str); + offset = get_time(str); + printf("Last Time Stamp: %f\n", offset); + + while(fgets(str,sizeof(str),f2) != NULL) + { + // Find initial time + //if (firstline) + //{ + // firstline = 0; + // offset -= get_time(str); + //} + + // Change Time and add to first file: + add_time(str, offset, f1); + } + + } + else + { + fprintf(stderr,"Failed to open file\n"); + res = -1; + } + + if (f1 > 0) + fclose(f1); + if (f2 > 0) + fclose(f2); + + return -1; +} + + + + +int main(int argc, char** argv) +{ + if (argc < 3) + { + fprintf(stderr, "Program needs 2 logfile names to merge\n"); + return -1; + } + + printf("Merging '%s' and '%s'\n",argv[1], argv[2]); + + return merge(argv[1],argv[2]); +}