diff --git a/.gitignore b/.gitignore
index 6a59db1e0e..52a96bc151 100644
--- a/.gitignore
+++ b/.gitignore
@@ -34,6 +34,8 @@
# JetBrains (PyCharm, etc) IDE project files
.idea
+# Vagrant VM files
+/.vagrant
/var
/dox
diff --git a/Vagrantfile b/Vagrantfile
new file mode 100644
index 0000000000..1328fdc04d
--- /dev/null
+++ b/Vagrantfile
@@ -0,0 +1,34 @@
+# -*- mode: ruby -*-
+# vi: set ft=ruby :
+
+# Vagrantfile API/syntax version. Don't touch unless you know what you're doing!
+VAGRANTFILE_API_VERSION = "2"
+
+Vagrant.configure(VAGRANTFILE_API_VERSION) do |config|
+
+ config.vm.box = "precise32"
+ config.vm.box_url = "http://files.vagrantup.com/precise32.box"
+
+ config.ssh.forward_agent = true
+ config.ssh.forward_x11 = true
+
+ # Share an additional folder to the guest VM. The first argument is
+ # the path on the host to the actual folder. The second argument is
+ # the path on the guest to mount the folder. And the optional third
+ # argument is a set of non-required options.
+ config.vm.synced_folder ".", "/home/vagrant/paparazzi"
+ # disable the default /vagrant share
+ config.vm.synced_folder ".", "/vagrant", disabled: true
+
+ config.vm.provider :virtualbox do |vb|
+ # Uncomment to boot with gui instead of headless mode
+ #vb.gui = true
+
+ # Use VBoxManage to customize the VM. For example to change memory:
+ vb.customize ["modifyvm", :id, "--memory", "2048"]
+ end
+
+ # install paparazzi packages in VM
+ config.vm.provision :shell, :path => "sw/extras/bootstrap_vm.sh"
+
+end
diff --git a/conf/Makefile.geode b/conf/Makefile.geode
index eb63bdbeaf..fb3d7ee745 100644
--- a/conf/Makefile.geode
+++ b/conf/Makefile.geode
@@ -58,7 +58,11 @@ COBJGEODE = $(SRCGEODE:%.c=$(OBJDIR)/%.o)
all: build
-build: elf
+build: $(OBJDIR) elf
+
+$(OBJDIR):
+ @echo CREATING object dir $(OBJDIR)
+ @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
elf: $(OBJDIR)/$(TARGET).elf
@@ -77,7 +81,7 @@ load upload program: $(OBJDIR)/$(TARGET).elf
$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) -c $(CFLAGS) $< -o $@
+ $(Q)$(CC) -MMD -c $(CFLAGS) $< -o $@
# Listing of phony targets.
.PHONY : all build elf clean clean_list
@@ -86,11 +90,8 @@ $(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
# Dependencies
#
-$(OBJDIR)/.depend:
- @echo DEPEND $@
- @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
- $(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
endif
diff --git a/conf/Makefile.jsbsim b/conf/Makefile.jsbsim
index 3b591724c1..a2e00922f0 100644
--- a/conf/Makefile.jsbsim
+++ b/conf/Makefile.jsbsim
@@ -49,12 +49,15 @@ $(TARGET).srcsnd = $(notdir $($(TARGET).srcs))
$(TARGET).objso = $($(TARGET).srcs:%.c=$(OBJDIR)/%.o)
$(TARGET).objs = $($(TARGET).objso:%.S=$(OBJDIR)/%.o)
-all compile: check_jsbsim $(OBJDIR)/simsitl
+all compile: check_jsbsim $(OBJDIR) $(OBJDIR)/simsitl
check_jsbsim:
@echo Paparazzi jsbsim package found: $(JSBSIM_PKG)
+$(OBJDIR):
+ @echo CREATING object dir $(OBJDIR)
+ @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
$(OBJDIR)/simsitl : $($(TARGET).objs)
@echo LD $@
@@ -80,23 +83,22 @@ $(OBJDIR)/%.s: %.cpp
$(OBJDIR)/%.o: %.c $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) $(CFLAGS) -c -o $@ $<
+ $(Q)$(CC) -MMD $(CFLAGS) -c -o $@ $<
$(OBJDIR)/%.o: %.cpp $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) $(CFLAGS) -c -o $@ $<
+ $(Q)$(CC) -MMD $(CFLAGS) -c -o $@ $<
.PHONY: all compile check_jsbsim
+
#
# Dependencies
#
-$(OBJDIR)/.depend:
- @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
- @echo DEPEND $@
- $(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
+
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
endif
diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21
index b2fb117e9d..80fd9c2e16 100644
--- a/conf/Makefile.lpc21
+++ b/conf/Makefile.lpc21
@@ -169,7 +169,11 @@ ALL_ASFLAGS = -mcpu=$(MCU) $(THUMB_IW) -I. -x assembler-with-cpp $(ASFLAGS)
# Default target.
all: printcommands sizebefore build sizeafter
-build: elf hex lss sym
+build: $(OBJDIR) elf hex lss sym
+
+$(OBJDIR):
+ @echo CREATING object dir $(OBJDIR)
+ @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
elf: $(OBJDIR)/$(TARGET).elf
hex: $(OBJDIR)/$(TARGET).hex
@@ -228,11 +232,11 @@ endif
$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) -c $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@
+ $(Q)$(CC) -MMD -c $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@
$(OBJDIR)/%.o : $(SRC_ARCH)/lpcusb/%.c $(OBJDIR)/../Makefile.ac
@echo CC $@
- $(Q)$(CC) -c $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@
+ $(Q)$(CC) -MMD -c $(ALL_CFLAGS) $(CONLYFLAGS) $< -o $@
# grab files in var/$(AIRCRAFT)/$(TARGET) aka $(OBJDIR)
@@ -260,11 +264,8 @@ $(AOBJARM) : $(OBJDIR)/%.o : $(SRC_ARCH)/%.S
# Dependencies
#
-$(OBJDIR)/.depend:
- @echo DEPEND $@
- @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
- $(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
endif
diff --git a/conf/Makefile.omap b/conf/Makefile.omap
index 9cc359d7cd..876099cbec 100644
--- a/conf/Makefile.omap
+++ b/conf/Makefile.omap
@@ -42,7 +42,7 @@ CFLAGS += -fno-short-enums
# CFLAGS += -malignment-traps
CFLAGS += -Wall -Wcast-qual -Wimplicit -Wcast-align
CFLAGS += -Wpointer-arith -Wswitch
-CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
+CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
#-Wno-unused-result
#CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))
#CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
@@ -71,7 +71,11 @@ OBJ_CPP_OMAP = $(SRC_CPP_OMAP:%.cpp=$(OBJDIR)/%.o)
all: build
-build: elf
+build: $(OBJDIR) elf
+
+$(OBJDIR):
+ @echo CREATING object dir $(OBJDIR)
+ @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
elf: $(OBJDIR)/$(TARGET).elf
@@ -166,13 +170,13 @@ endif
$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) -c $(CFLAGS) $< -o $@
+ $(Q)$(CC) -MMD -c $(CFLAGS) $< -o $@
# Compile: create object files from C++ source files
$(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac
@echo CXX $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CXX) -c $(CXXFLAGS) $< -o $@
+ $(Q)$(CXX) -MMD -c $(CXXFLAGS) $< -o $@
# Listing of phony targets.
@@ -182,13 +186,8 @@ $(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac
# Dependencies
#
-$(OBJDIR)/.depend:
- @echo DEPEND $@
- @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
- $(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
ifneq ($(MAKECMDGOALS),clean)
-ifneq ($(MAKECMDGOALS),erase)
--include $(OBJDIR)/.depend
-endif
+-include $(DEPS)
endif
diff --git a/conf/Makefile.pentium-m b/conf/Makefile.pentium-m
index ada3f1046d..04f4771137 100644
--- a/conf/Makefile.pentium-m
+++ b/conf/Makefile.pentium-m
@@ -67,7 +67,11 @@ COBJGEODE = $(SRCGEODE:%.c=$(OBJDIR)/%.o)
all: build
-build: elf
+build: $(OBJDIR) elf
+
+$(OBJDIR):
+ @echo CREATING object dir $(OBJDIR)
+ @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
elf: $(OBJDIR)/$(TARGET).elf
@@ -92,7 +96,7 @@ endif
$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) -c $(CFLAGS) $< -o $@
+ $(Q)$(CC) -MMD -c $(CFLAGS) $< -o $@
# Listing of phony targets.
.PHONY : all build elf clean clean_list
@@ -101,11 +105,8 @@ $(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
# Dependencies
#
-$(OBJDIR)/.depend:
- @echo DEPEND $@
- @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
- $(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
endif
diff --git a/conf/Makefile.sim b/conf/Makefile.sim
index 40657cc54b..14b015a00c 100644
--- a/conf/Makefile.sim
+++ b/conf/Makefile.sim
@@ -83,7 +83,11 @@ $(TARGET).srcsnd = $(notdir $($(TARGET).srcs))
$(TARGET).objso = $($(TARGET).srcs:%.c=$(OBJDIR)/%.o)
$(TARGET).objs = $($(TARGET).objso:%.S=$(OBJDIR)/%.o)
-all compile: $(OBJDIR)/simsitl
+all compile: $(OBJDIR) $(OBJDIR)/simsitl
+
+$(OBJDIR):
+ @echo CREATING object dir $(OBJDIR)
+ @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
# shared library of the C autopilot part
autopilot.so : $($(TARGET).objs)
@@ -125,23 +129,21 @@ $(OBJDIR)/%.s: %.cpp
$(OBJDIR)/%.o: %.c $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) $(CFLAGS) -c -o $@ $<
+ $(Q)$(CC) -MMD $(CFLAGS) -c -o $@ $<
$(OBJDIR)/%.o: %.cpp $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) $(CFLAGS) -c -o $@ $<
+ $(Q)$(CC) -MMD $(CFLAGS) -c -o $@ $<
.PHONY: all compile
#
# Dependencies
#
-$(OBJDIR)/.depend:
- @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
- @echo DEPEND $@
- $(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
+
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
endif
diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32
index fa39dbb7cf..ba400c1769 100644
--- a/conf/Makefile.stm32
+++ b/conf/Makefile.stm32
@@ -178,9 +178,13 @@ ODFLAGS = -S
# Default target.
all: printcommands sizebefore build sizeafter
-build: elf bin hex
+build: $(OBJDIR) elf bin hex
# lss sym
+$(OBJDIR):
+ @echo CREATING object dir $(OBJDIR)
+ @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
+
elf: $(OBJDIR)/$(TARGET).elf
bin: $(OBJDIR)/$(TARGET).bin
hex: $(OBJDIR)/$(TARGET).hex
@@ -219,7 +223,7 @@ sym: $(OBJDIR)/$(TARGET).sym
$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
@echo CC $@
$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
- $(Q)$(CC) -c $(CFLAGS) $< -o $@
+ $(Q)$(CC) -MMD -c $(CFLAGS) $< -o $@
# Assemble: create object files from assembler source files. ARM/Thumb
$(AOBJ) : $(OBJDIR)/%.o : %.S
@@ -237,11 +241,8 @@ include $(PAPARAZZI_SRC)/conf/Makefile.stm32-upload
# Dependencies
#
-$(OBJDIR)/.depend:
- @echo DEPEND $@
- @test -d $(OBJDIR) || mkdir -p $(OBJDIR)
- $(Q)$(CC) -MM -MG $(THUMB) $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
endif
diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml
index 6735c16ec3..8fd1b7b542 100644
--- a/conf/airframes/examples/stm32f4_discovery_test.xml
+++ b/conf/airframes/examples/stm32f4_discovery_test.xml
@@ -29,6 +29,12 @@
+
+
+
+
+
+
diff --git a/conf/firmwares/booz2_common.makefile b/conf/firmwares/booz2_common.makefile
deleted file mode 100644
index a0c8c0e0d5..0000000000
--- a/conf/firmwares/booz2_common.makefile
+++ /dev/null
@@ -1,12 +0,0 @@
-
-
-
-SRC_BOOZ=booz
-SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
-SRC_BOOZ_TEST=$(SRC_BOOZ)/test
-
-SRC_BOOZ_PRIV=booz_priv
-
-CFG_BOOZ=$(PAPARAZZI_SRC)/conf/firmwares/
-
-BOOZ_INC = -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
diff --git a/conf/firmwares/booz2_test_progs.makefile b/conf/firmwares/booz2_test_progs.makefile
deleted file mode 100644
index 1d6ca84e93..0000000000
--- a/conf/firmwares/booz2_test_progs.makefile
+++ /dev/null
@@ -1,485 +0,0 @@
-#
-# $id$
-#
-# Copyright (C) 2008 Antoine Drouin (poinix@gmail.com)
-#
-# 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.
-#
-#
-
-include $(PAPARAZZI_SRC)/conf/firmwares/booz2_common.makefile
-
-#
-# test_led : blinks all leds
-#
-test_led.ARCHDIR = $(ARCH)
-
-test_led.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-test_led.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_led.srcs += $(SRC_BOOZ_TEST)/booz2_test_led.c
-test_led.CFLAGS += -DUSE_LED
-test_led.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_led.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-#
-# test_downlink : sends a TIME message
-#
-test_downlink.ARCHDIR = $(ARCH)
-
-test_downlink.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-test_downlink.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_downlink.srcs += $(SRC_BOOZ_TEST)/booz2_test_subsystems/datalink/downlink.c
-test_downlink.CFLAGS += -DUSE_LED
-test_downlink.CFLAGS += -DPERIODIC_FREQUENCY='10.' -DSYS_TIME_LED=1
-test_downlink.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_downlink.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_downlink.srcs += mcu_periph/uart.c
-test_downlink.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_downlink.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_downlink.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-
-#
-# test_max1168 : samples gyros and mags, sends values
-#
-test_max1168.ARCHDIR = $(ARCH)
-
-test_max1168.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_max1168.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_max1168.srcs += $(SRC_BOOZ_TEST)/booz2_test_max1168.c
-test_max1168.CFLAGS += -DUSE_LED
-test_max1168.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_max1168.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_max1168.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_max1168.srcs += mcu_periph/uart.c
-test_max1168.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_max1168.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_max1168.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_max1168.CFLAGS += -DMAX1168_EOC_VIC_SLOT=11
-test_max1168.srcs += peripherals/max1168.c \
- $(SRC_ARCH)/peripherals/max1168_arch.c
-
-
-
-#
-# tunnel hw
-#
-tunnel.ARCHDIR = $(ARCH)
-
-tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-tunnel.srcs += $(SRC_BOOZ_TEST)/booz2_tunnel.c
-tunnel.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-tunnel.CFLAGS += -DUSE_LED
-tunnel.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-tunnel.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
-
-tunnel.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
-tunnel.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-tunnel.srcs += mcu_periph/uart.c
-tunnel.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-
-#
-# tunnel bit banging
-#
-tunnel_bb.ARCHDIR = $(ARCH)
-
-tunnel_bb.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-tunnel_bb.srcs += $(SRC_BOOZ_TEST)/booz2_tunnel_bb.c
-tunnel_bb.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-tunnel_bb.CFLAGS += -DUSE_LED
-tunnel_bb.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-
-#
-# usb tunnels
-#
-usb_tunnel_0.ARCHDIR = $(ARCH)
-usb_tunnel_0.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DPERIPHERALS_AUTO_INIT
-usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED
-usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c
-usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c
-usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c
-usb_tunnel_0.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-usb_tunnel_0.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
-
-usb_tunnel_1.ARCHDIR = $(ARCH)
-usb_tunnel_1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-usb_tunnel_1.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 -DPERIPHERALS_AUTO_INIT
-usb_tunnel_1.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED
-usb_tunnel_1.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c
-usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c
-usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c
-usb_tunnel_1.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
-
-
-#
-# test GPS
-#
-test_gps.ARCHDIR = $(ARCH)
-
-test_gps.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_gps.srcs += $(SRC_BOOZ_TEST)/booz2_test_gps.c
-test_gps.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_gps.CFLAGS += -DUSE_LED
-test_gps.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_gps.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_gps.srcs += mcu_periph/uart.c
-test_gps.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_gps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_gps.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_gps.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
-test_gps.CFLAGS += -DGPS_LINK=UART0 -DGPS_LED=2
-test_gps.srcs += $(SRC_BOOZ)/booz2_gps.c
-
-
-
-
-
-#
-# test modem
-#
-test_modem.ARCHDIR = $(ARCH)
-
-test_modem.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-test_modem.srcs += $(SRC_BOOZ_TEST)/booz2_test_modem.c
-test_modem.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_modem.CFLAGS += -DUSE_LED
-test_modem.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_modem.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_modem.srcs += mcu_periph/uart.c
-test_modem.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_modem.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_modem.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-#test_modem.CFLAGS += -DBOOZ_ANALOG_BARO_LED=2 -DBOOZ_ANALOG_BARO_PERIOD='CPU_TICKS_OF_SEC((1./100.))'
-#test_modem.srcs += $(BOOZ_PRIV)/booz_analog_baro.c
-
-
-#
-# test USB telemetry
-#
-test_usb.ARCHDIR = $(ARCH)
-
-test_usb.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-test_usb.srcs += $(SRC_BOOZ_TEST)/booz2_test_usb.c
-test_usb.CFLAGS += -DPERIODIC_FREQUENCY='512.'
-# -DSYS_TIME_LED=1
-test_usb.CFLAGS += -DUSE_LED
-test_usb.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-test_usb.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
-
-#test_usb.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-#test_usb.srcs += mcu_periph/uart.c
-#test_usb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-#test_usb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-#test_usb.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_usb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
-test_usb.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_usb.srcs += subsystems/datalink/downlink.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
-# $(SRC_FIRMWARE)/datalink.c
-test_usb.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
-test_usb.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
-
-
-
-
-
-
-#
-# test AMI
-#
-test_ami.ARCHDIR = $(ARCH)
-
-test_ami.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-test_ami.srcs += $(SRC_BOOZ_TEST)/booz2_test_ami.c
-test_ami.CFLAGS += -DPERIODIC_FREQUENCY='50.' -DSYS_TIME_LED=1
-test_ami.CFLAGS += -DUSE_LED
-test_ami.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_ami.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_ami.srcs += mcu_periph/uart.c
-test_ami.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_ami.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_ami.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_ami.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_BUF_LEN=16
-test_ami.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_ami.CFLAGS += -DUSE_AMI601
-test_ami.srcs += AMI601.c
-
-
-#
-# test crista
-#
-test_crista.ARCHDIR = $(ARCH)
-
-test_crista.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_crista.srcs += $(SRC_BOOZ_TEST)/booz2_test_crista.c
-test_crista.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_crista.CFLAGS += -DUSE_LED
-test_crista.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_crista.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600
-test_crista.srcs += mcu_periph/uart.c
-test_crista.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_crista.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART0
-test_crista.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_crista.CFLAGS += -DFLOAT_T=float -DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\"
-test_crista.srcs += $(SRC_BOOZ)/booz2_imu.c
-test_crista.srcs += $(SRC_BOOZ)/booz2_imu_crista.c $(SRC_BOOZ_ARCH)/booz2_imu_crista_hw.c
-
-
-
-
-#
-# test IMU b2
-#
-test_imu_b2.ARCHDIR = $(ARCH)
-
-test_imu_b2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_imu_b2.srcs += $(SRC_BOOZ_TEST)/booz2_test_imu_b2.c
-test_imu_b2.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_imu_b2.CFLAGS += -DUSE_LED
-test_imu_b2.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_imu_b2.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_imu_b2.srcs += mcu_periph/uart.c
-test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_imu_b2.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_imu_b2.srcs += $(SRC_BOOZ)/booz_trig_int.c
-
-test_imu_b2.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\"
-test_imu_b2.srcs += $(SRC_BOOZ)/booz2_imu_b2.c $(SRC_BOOZ_ARCH)/booz2_imu_b2_hw.c
-test_imu_b2.CFLAGS += -DMAX1168_EOC_VIC_SLOT=11
-test_imu_b2.srcs += $(SRC_BOOZ)/booz2_max1168.c $(SRC_BOOZ_ARCH)/booz2_max1168_hw.c
-test_imu_b2.CFLAGS += -DFLOAT_T=float
-test_imu_b2.srcs += $(SRC_BOOZ)/booz2_imu.c
-
-
-#
-# test rc spektrum
-#
-
-test_rc_spektrum.ARCHDIR = $(ARCH)
-
-test_rc_spektrum.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) $(BOOZ_CFLAGS)
-test_rc_spektrum.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_rc_spektrum.srcs += $(SRC_BOOZ_TEST)/booz2_test_radio_control.c
-test_rc_spektrum.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_rc_spektrum.CFLAGS += -DUSE_LED
-test_rc_spektrum.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-#test_rc_spektrum.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-#test_rc_spektrum.srcs += mcu_periph/uart.c
-#test_rc_spektrum.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-#test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-#test_rc_spektrum.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
-test_rc_spektrum.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_rc_spektrum.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
-test_rc_spektrum.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
-test_rc_spektrum.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
-
-test_rc_spektrum.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_LED=1
-test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"booz_radio_control_spektrum.h\"
-test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"booz_radio_control_spektrum_dx7se.h\"
-test_rc_spektrum.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200
-test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_LINK=UART0
-test_rc_spektrum.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
- $(SRC_SUBSYSTEMS)/radio_control_spektrum.c \
- mcu_periph/uart.c \
- $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-#
-# test rc ppm
-#
-
-test_rc_ppm.ARCHDIR = $(ARCH)
-
-test_rc_ppm.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) $(BOOZ_CFLAGS)
-test_rc_ppm.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_rc_ppm.srcs += $(SRC_BOOZ_TEST)/booz2_test_radio_control.c
-test_rc_ppm.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_rc_ppm.CFLAGS += -DUSE_LED
-test_rc_ppm.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-#test_rc_ppm.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-#test_rc_ppm.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-#test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-#test_rc_ppm.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
-test_rc_ppm.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_rc_ppm.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
-test_rc_ppm.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
-test_rc_ppm.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
-
-test_rc_ppm.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_LED=1
-test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"booz_radio_control_ppm.h\"
-test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_PPM
-test_rc_ppm.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
- $(SRC_BOOZ)/$(IMPL)/booz_radio_control_ppm.c \
- $(SRC_BOOZ)/$(IMPL)/$(ARCH)/booz_radio_control_ppm_arch.c \
-
-
-#
-# test MC
-#
-test_mc.ARCHDIR = $(ARCH)
-
-test_mc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_mc.srcs += $(SRC_BOOZ_TEST)/booz2_test_mc.c
-test_mc.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_mc.CFLAGS += -DUSE_LED
-test_mc.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_mc.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_mc.srcs += mcu_periph/uart.c
-test_mc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_mc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_mc.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_mc.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
-test_mc.srcs += $(SRC_BOOZ_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
-test_mc.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
-test_mc.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-
-#
-# test BUSS BLDC
-#
-test_buss_bldc.ARCHDIR = $(ARCH)
-
-test_buss_bldc.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_buss_bldc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_buss_bldc.srcs += $(SRC_BOOZ_TEST)/booz2_test_buss_bldc.c
-test_buss_bldc.CFLAGS += -DUSE_LED
-test_buss_bldc.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_buss_bldc.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_buss_bldc.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_buss_bldc.srcs += mcu_periph/uart.c
-test_buss_bldc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_buss_bldc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_buss_bldc.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_buss_bldc.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
-test_buss_bldc.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-
-#
-# test asctec BLMC
-#
-test_amc.ARCHDIR = $(ARCH)
-
-test_amc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_amc.srcs += $(SRC_BOOZ_TEST)/booz2_test_amc.c
-test_amc.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_amc.CFLAGS += -DUSE_LED
-test_amc.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-test_amc.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_amc.srcs += mcu_periph/uart.c
-test_amc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_amc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_amc.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-test_amc.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=UART1
-test_amc.srcs += $(SRC_FIRMWARE)/datalink.c
-
-test_amc.CFLAGS += -DACTUATORS=\"actuators_asctec_twi_blmc_hw.h\"
-test_amc.srcs += $(SRC_BOOZ_ARCH)/actuators_asctec_twi_blmc_hw.c actuators.c
-test_amc.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
-test_amc.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_amc.CFLAGS += -DFLOAT_T=float
-#-DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\"
-
-
-
-#
-# test_mkk_bldc :
-#
-test_mkk_bldc.ARCHDIR = $(ARCH)
-
-test_mkk_bldc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-test_mkk_bldc.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_mkk_bldc.srcs += $(SRC_BOOZ_TEST)/booz2_test_buss_bldc_hexa.c
-test_mkk_bldc.CFLAGS += -DUSE_LED
-test_mkk_bldc.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_mkk_bldc.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-test_mkk_bldc.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
-test_mkk_bldc.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-#
-# test 24 bits baro
-#
-test_baro_24.ARCHDIR = $(ARCH)
-
-test_baro_24.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) $(BOOZ_CFLAGS)
-test_baro_24.srcs += $(SRC_BOOZ_TEST)/booz2_test_baro_24.c
-test_baro_24.CFLAGS += -DPERIODIC_FREQUENCY='5.' -DSYS_TIME_LED=1
-test_baro_24.CFLAGS += -DUSE_LED
-test_baro_24.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
-
-
-test_baro_24.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_baro_24.srcs += mcu_periph/uart.c
-test_baro_24.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_baro_24.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART1
-test_baro_24.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-
-test_baro_24.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_BUF_LEN=16
-test_baro_24.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_baro_24.srcs += $(SRC_BOOZ)/booz2_baro_24.c
-
-#
-# test_coders : blinks all leds
-#
-test_coder.ARCHDIR = $(ARCH)
-
-test_coder.CFLAGS += -DBOARD_CONFIG=\"boards/olimex_lpc_h2148.h\" $(BOOZ_CFLAGS)
-test_coder.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_coder.srcs += $(SRC_BOOZ_TEST)/booz2_test_coder.c
-test_coder.CFLAGS += -DUSE_LED
-test_coder.CFLAGS += -DPERIODIC_FREQUENCY='512.' -DSYS_TIME_LED=1
-test_coder.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c
diff --git a/conf/firmwares/lisa_passthrough.makefile b/conf/firmwares/lisa_passthrough.makefile
index c0b5896080..fad7f90c4e 100644
--- a/conf/firmwares/lisa_passthrough.makefile
+++ b/conf/firmwares/lisa_passthrough.makefile
@@ -5,8 +5,6 @@
#
SRC_ARCH=arch/$(ARCH)
-SRC_BOOZ=booz
-SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
SRC_LISA=lisa
SRC_LISA_ARCH=$(SRC_LISA)/arch/$(ARCH)
SRC_CSC=csc
@@ -20,7 +18,7 @@ CFG_LISA_PASSTHROUGH = $(PAPARAZZI_SRC)/conf/firmwares/subsystems/lisa_passthrou
stm_passthrough.ARCHDIR = stm32
-stm_passthrough.CFLAGS += -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(SRC_LISA_ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD) -I$(SRC_ROTOR_ARCH) -I$(SRC_IMU_ARCH)
+stm_passthrough.CFLAGS += -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(SRC_LISA_ARCH) -I$(SRC_BOARD) -I$(SRC_ROTOR_ARCH) -I$(SRC_IMU_ARCH)
stm_passthrough.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
stm_passthrough.CFLAGS += -DPERIPHERALS_AUTO_INIT
stm_passthrough.srcs = $(SRC_LISA)/lisa_stm_passthrough_main.c
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index 7cec052725..f6c793f0a1 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -24,17 +24,12 @@
CFG_SHARED=$(PAPARAZZI_SRC)/conf/firmwares/subsystems/shared
CFG_ROTORCRAFT=$(PAPARAZZI_SRC)/conf/firmwares/subsystems/rotorcraft
-SRC_BOOZ_TEST=$(SRC_BOOZ)/test
-SRC_BOOZ_PRIV=booz_priv
-
SRC_BOARD=boards/$(BOARD)
SRC_FIRMWARE=firmwares/rotorcraft
SRC_SUBSYSTEMS=subsystems
SRC_ARCH=arch/$(ARCH)
-CFG_BOOZ=$(PAPARAZZI_SRC)/conf/firmwares/
-
ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD)
diff --git a/conf/firmwares/subsystems/lisa_passthrough/radio_control_joby.makefile b/conf/firmwares/subsystems/lisa_passthrough/radio_control_joby.makefile
deleted file mode 100644
index 7441f953df..0000000000
--- a/conf/firmwares/subsystems/lisa_passthrough/radio_control_joby.makefile
+++ /dev/null
@@ -1,13 +0,0 @@
-
-
-# Radio control
-stm_passthrough.CFLAGS += -DRADIO_CONTROL
-stm_passthrough.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/booz_radio_control_joby.h\"
-stm_passthrough.CFLAGS += -DRADIO_CONTROL_JOBY_MODEL_H=\"radio_control/booz_radio_control_joby_9ch.h\"
-stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
- $(SRC_BOOZ)/radio_control/booz_radio_control_joby.c
-stm_passthrough.CFLAGS += -DRADIO_CONTROL_LED=6
-stm_passthrough.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B115200
-stm_passthrough.CFLAGS += -DRADIO_CONTROL_LINK=UART3
-
-
diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile
new file mode 100644
index 0000000000..cf1a70674b
--- /dev/null
+++ b/conf/firmwares/test_progs.makefile
@@ -0,0 +1,153 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Copyright (C) 2013 The Paparazzi Team
+#
+# 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.
+#
+
+################################################################################
+#
+#
+# Common test programs for the all LPC21 and STM32 boards
+#
+################################################################################
+
+
+SRC_ARCH=arch/$(ARCH)
+SRC_BOARD=boards/$(BOARD)
+SRC_SUBSYSTEMS=subsystems
+SRC_MODULES=modules
+
+CFG_SHARED=$(PAPARAZZI_SRC)/conf/firmwares/subsystems/shared
+
+
+#
+# common test
+#
+# configuration
+# SYS_TIME_LED
+# MODEM_PORT
+# MODEM_BAUD
+#
+PERIODIC_FREQUENCY ?= 512
+
+COMMON_TEST_CFLAGS = -I$(SRC_BOARD) -DBOARD_CONFIG=$(BOARD_CFG)
+COMMON_TEST_CFLAGS += -DPERIPHERALS_AUTO_INIT
+COMMON_TEST_SRCS = mcu.c $(SRC_ARCH)/mcu_arch.c
+COMMON_TEST_SRCS += $(SRC_ARCH)/mcu_periph/gpio_arch.c
+COMMON_TEST_CFLAGS += -DUSE_SYS_TIME
+ifneq ($(SYS_TIME_LED),none)
+ COMMON_TEST_CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+endif
+COMMON_TEST_CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
+COMMON_TEST_SRCS += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
+
+COMMON_TEST_CFLAGS += -DUSE_LED
+COMMON_TEST_SRCS += $(SRC_ARCH)/led_hw.c
+
+COMMON_TELEMETRY_CFLAGS = -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+COMMON_TELEMETRY_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+COMMON_TELEMETRY_SRCS = mcu_periph/uart.c
+COMMON_TELEMETRY_SRCS += $(SRC_ARCH)/mcu_periph/uart_arch.c
+COMMON_TELEMETRY_SRCS += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
+
+#COMMON_TEST_SRCS += math/pprz_trig_int.c
+
+
+
+#
+# test sys_time
+#
+ifeq ($(BOARD), lisa_m)
+ifeq ($(BOARD_VERSION), 2.0)
+LED_DEFINES = -DLED_BLUE=3 -DLED_RED=4 -DLED_GREEN=5
+endif
+endif
+LED_DEFINES ?= -DLED_RED=2 -DLED_GREEN=3
+
+test_sys_time_timer.ARCHDIR = $(ARCH)
+test_sys_time_timer.CFLAGS += $(COMMON_TEST_CFLAGS) $(LED_DEFINES)
+test_sys_time_timer.srcs += $(COMMON_TEST_SRCS)
+test_sys_time_timer.srcs += $(SRC_AIRBORNE)/test/mcu_periph/test_sys_time_timer.c
+
+test_sys_time_usleep.ARCHDIR = $(ARCH)
+test_sys_time_usleep.CFLAGS += $(COMMON_TEST_CFLAGS) $(LED_DEFINES)
+test_sys_time_usleep.srcs += $(COMMON_TEST_SRCS)
+test_sys_time_usleep.srcs += $(SRC_AIRBORNE)/test/mcu_periph/test_sys_time_usleep.c
+
+
+#
+# test_telemetry : Sends ALIVE telemetry messages
+#
+# configuration
+# MODEM_PORT :
+# MODEM_BAUD :
+#
+test_telemetry.ARCHDIR = $(ARCH)
+test_telemetry.CFLAGS += $(COMMON_TEST_CFLAGS)
+test_telemetry.srcs += $(COMMON_TEST_SRCS)
+test_telemetry.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
+test_telemetry.srcs += $(COMMON_TELEMETRY_SRCS)
+test_telemetry.srcs += test/test_telemetry.c
+
+
+#
+# test ms2100 mag
+#
+test_ms2100.ARCHDIR = $(ARCH)
+test_ms2100.CFLAGS += $(COMMON_TEST_CFLAGS)
+test_ms2100.srcs += $(COMMON_TEST_SRCS)
+test_ms2100.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
+test_ms2100.srcs += $(COMMON_TELEMETRY_SRCS)
+
+test_ms2100.srcs += test/peripherals/test_ms2100.c
+test_ms2100.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c
+test_ms2100.CFLAGS += -DUSE_SPI -DSPI_MASTER
+test_ms2100.srcs += mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c
+ifeq ($(ARCH), lpc21)
+test_ms2100.CFLAGS += -DUSE_SPI1
+test_ms2100.CFLAGS += -DUSE_SPI_SLAVE1
+test_ms2100.CFLAGS += -DMS2100_SLAVE_IDX=1
+test_ms2100.CFLAGS += -DMS2100_SPI_DEV=spi1
+test_ms2100.CFLAGS += -DMS2100_DRDY_VIC_SLOT=12
+else ifeq ($(ARCH), stm32)
+test_ms2100.CFLAGS += -DUSE_SPI2
+test_ms2100.CFLAGS += -DUSE_SPI_SLAVE4
+test_ms2100.CFLAGS += -DMS2100_SLAVE_IDX=4
+test_ms2100.CFLAGS += -DMS2100_SPI_DEV=spi2
+endif
+
+
+#
+# test lis302dl accelerometer
+#
+test_lis302dl.ARCHDIR = $(ARCH)
+test_lis302dl.CFLAGS += $(COMMON_TEST_CFLAGS)
+test_lis302dl.srcs += $(COMMON_TEST_SRCS)
+test_lis302dl.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
+test_lis302dl.srcs += $(COMMON_TELEMETRY_SRCS)
+
+test_lis302dl.srcs += test/peripherals/test_lis302dl_spi.c
+test_lis302dl.srcs += peripherals/lis302dl_spi.c
+test_lis302dl.CFLAGS += -DUSE_SPI -DSPI_MASTER
+test_lis302dl.srcs += mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c
+
+test_lis302dl.CFLAGS += -DUSE_SPI1
+test_lis302dl.CFLAGS += -DUSE_SPI_SLAVE2
+test_lis302dl.CFLAGS += -DLIS302DL_SLAVE_IDX=2
+test_lis302dl.CFLAGS += -DLIS302DL_SPI_DEV=spi1
diff --git a/conf/modules/gps_ubx_ucenter.xml b/conf/modules/gps_ubx_ucenter.xml
index 1f2880dfad..b5db81e04e 100644
--- a/conf/modules/gps_ubx_ucenter.xml
+++ b/conf/modules/gps_ubx_ucenter.xml
@@ -24,9 +24,8 @@ Warning: you still need to tell the driver
-
- ap.CFLAGS += -DGPS_UBX_UCENTER=\"modules/gps/gps_ubx_ucenter.c\"
-
+
+
diff --git a/conf/settings/estimation/ahrs_float_mlkf.xml b/conf/settings/estimation/ahrs_float_mlkf.xml
new file mode 100644
index 0000000000..68cf7b0012
--- /dev/null
+++ b/conf/settings/estimation/ahrs_float_mlkf.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c
index 14f39300ec..2cb1636698 100644
--- a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c
@@ -89,6 +89,21 @@ void gpio_enable_clock(uint32_t port) {
case GPIOD:
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPDEN);
break;
+ case GPIOE:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPEEN);
+ break;
+ case GPIOF:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPFEN);
+ break;
+ case GPIOG:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPGEN);
+ break;
+ case GPIOH:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPHEN);
+ break;
+ case GPIOI:
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPIEN);
+ break;
default:
break;
};
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
index db20493eda..b8554767f1 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
@@ -82,6 +82,54 @@ PRINT_CONFIG_MSG("Using TIM2 for PPM input.")
#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
#endif
+#elif USE_PPM_TIM3
+
+PRINT_CONFIG_MSG("Using TIM3 for PPM input.")
+
+#define PPM_RCC &RCC_APB1ENR
+#define PPM_PERIPHERAL RCC_APB1ENR_TIM3EN
+#define PPM_TIMER TIM3
+
+#ifdef STM32F4
+/* Since APB prescaler != 1 :
+ * Timer clock frequency (before prescaling) is twice the frequency
+ * of the APB domain to which the timer is connected.
+ */
+#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
+#endif
+
+#elif USE_PPM_TIM4
+
+PRINT_CONFIG_MSG("Using TIM4 for PPM input.")
+
+#define PPM_RCC &RCC_APB1ENR
+#define PPM_PERIPHERAL RCC_APB1ENR_TIM4EN
+#define PPM_TIMER TIM4
+
+#ifdef STM32F4
+/* Since APB prescaler != 1 :
+ * Timer clock frequency (before prescaling) is twice the frequency
+ * of the APB domain to which the timer is connected.
+ */
+#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
+#endif
+
+#elif USE_PPM_TIM5
+
+PRINT_CONFIG_MSG("Using TIM5 for PPM input.")
+
+#define PPM_RCC &RCC_APB1ENR
+#define PPM_PERIPHERAL RCC_APB1ENR_TIM5EN
+#define PPM_TIMER TIM5
+
+#ifdef STM32F4
+/* Since APB prescaler != 1 :
+ * Timer clock frequency (before prescaling) is twice the frequency
+ * of the APB domain to which the timer is connected.
+ */
+#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
+#endif
+
#elif USE_PPM_TIM1
PRINT_CONFIG_MSG("Using TIM1 for PPM input.")
@@ -94,6 +142,19 @@ PRINT_CONFIG_MSG("Using TIM1 for PPM input.")
#define PPM_TIMER_CLK (rcc_ppre2_frequency * 2)
#endif
+#elif USE_PPM_TIM8
+
+PRINT_CONFIG_MSG("Using TIM8 for PPM input.")
+
+#define PPM_RCC &RCC_APB2ENR
+#define PPM_PERIPHERAL RCC_APB2ENR_TIM8EN
+#define PPM_TIMER TIM8
+
+#ifdef STM32F4
+#define PPM_TIMER_CLK (rcc_ppre2_frequency * 2)
+#endif
+
+
#else
#error Unknown PPM input timer configuration.
#endif
@@ -171,6 +232,59 @@ void tim2_isr(void) {
}
+
+#elif USE_PPM_TIM3
+
+void tim3_isr(void) {
+
+ if((TIM3_SR & PPM_CC_IF) != 0) {
+ timer_clear_flag(TIM3, PPM_CC_IF);
+
+ uint32_t now = timer_get_counter(TIM3) + timer_rollover_cnt;
+ DecodePpmFrame(now);
+ }
+ else if((TIM3_SR & TIM_SR_UIF) != 0) {
+ timer_rollover_cnt+=(1<<16);
+ timer_clear_flag(TIM3, TIM_SR_UIF);
+ }
+
+}
+
+#elif USE_PPM_TIM4
+
+void tim4_isr(void) {
+
+ if((TIM4_SR & PPM_CC_IF) != 0) {
+ timer_clear_flag(TIM4, PPM_CC_IF);
+
+ uint32_t now = timer_get_counter(TIM4) + timer_rollover_cnt;
+ DecodePpmFrame(now);
+ }
+ else if((TIM4_SR & TIM_SR_UIF) != 0) {
+ timer_rollover_cnt+=(1<<16);
+ timer_clear_flag(TIM4, TIM_SR_UIF);
+ }
+
+}
+
+#elif USE_PPM_TIM5
+
+void tim5_isr(void) {
+
+ if((TIM5_SR & PPM_CC_IF) != 0) {
+ timer_clear_flag(TIM5, PPM_CC_IF);
+
+ uint32_t now = timer_get_counter(TIM5) + timer_rollover_cnt;
+ DecodePpmFrame(now);
+ }
+ else if((TIM5_SR & TIM_SR_UIF) != 0) {
+ timer_rollover_cnt+=(1<<16);
+ timer_clear_flag(TIM5, TIM_SR_UIF);
+ }
+
+}
+
+
#elif USE_PPM_TIM1
#if defined(STM32F1)
@@ -193,5 +307,27 @@ void tim1_cc_isr(void) {
}
}
+#elif USE_PPM_TIM8 && defined(STM32F4)
+
+#if defined(STM32F1)
+void tim8_up_isr(void) {
+#elif defined(STM32F4)
+void tim8_up_tim13_isr(void) {
+#endif
+ if((TIM8_SR & TIM_SR_UIF) != 0) {
+ timer_rollover_cnt+=(1<<16);
+ timer_clear_flag(TIM8, TIM_SR_UIF);
+ }
+}
+
+void tim8_cc_isr(void) {
+ if((TIM8_SR & PPM_CC_IF) != 0) {
+ timer_clear_flag(TIM8, PPM_CC_IF);
+
+ uint32_t now = timer_get_counter(TIM8) + timer_rollover_cnt;
+ DecodePpmFrame(now);
+ }
+}
+
#endif /* USE_PPM_TIM1 */
diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h
index 87ced73af4..4ad9ca64a2 100755
--- a/sw/airborne/boards/stm32f4_discovery.h
+++ b/sw/airborne/boards/stm32f4_discovery.h
@@ -7,9 +7,9 @@
* PA2 = PWM7
* PA3 = PWM6
* PA4 = ADC_4 (ADC12 IN 4)+CS43L22 DAC out, capacitively coupled+100Kohm, should not interfere.
- * PA5 = FREE
- * PA6 = CANNOT BE USED
- * PA7 = FREE
+ * PA5 = SPI1 SCK if STM32F4_DISCOVERY_SPI1_FOR_LIS302
+ * PA6 = SPI1 MISO if STM32F4_DISCOVERY_SPI1_FOR_LIS302
+ * PA7 = SPI1 MOSI if STM32F4_DISCOVERY_SPI1_FOR_LIS302
* PA8 = SPECTRUM BIND
* PA9 = FREE (ONLY if usb is not active during runtime, PC0 must be high or input )
* PA10 = UART2 (Spektrum input)
@@ -73,7 +73,7 @@
* PE0 = CANNOT BE USED (Accel int output)
* PE1 = CANNOT BE USED (Accel int output)
* PE2 = SPI SLAVE 0
- * PE3 = FREE (Needs some testing as it is used for the accel spi/i2c select pin)
+ * PE3 = SPI SLAVE 2 (used for the LIS302DL accel spi/i2c select pin)
* PE4 = FREE
* PE5 = PWM4
* PE6 = PWM5
@@ -214,6 +214,15 @@
/************************************** SPI *************************************************/
/***************************************************************************************************/
+#if STM32F4_DISCOVERY_SPI1_FOR_LIS302
+#define SPI1_GPIO_AF GPIO_AF5
+#define SPI1_GPIO_PORT_SCK GPIOA
+#define SPI1_GPIO_SCK GPIO5
+#define SPI1_GPIO_PORT_MISO GPIOA
+#define SPI1_GPIO_MISO GPIO6
+#define SPI1_GPIO_PORT_MOSI GPIOA
+#define SPI1_GPIO_MOSI GPIO7
+#else
#define SPI1_GPIO_AF GPIO_AF5
#define SPI1_GPIO_PORT_SCK GPIOB
#define SPI1_GPIO_SCK GPIO3
@@ -221,7 +230,7 @@
#define SPI1_GPIO_MISO GPIO4
#define SPI1_GPIO_PORT_MOSI GPIOB
#define SPI1_GPIO_MOSI GPIO5
-
+#endif
/* CANNOT BE USED IF PWM CHANNELS 10 & 11 ARE ACTIVE !!! */
#define SPI2_GPIO_AF GPIO_AF5
@@ -245,6 +254,8 @@
#define SPI_SELECT_SLAVE0_PIN GPIO2
#define SPI_SELECT_SLAVE1_PORT GPIOE
#define SPI_SELECT_SLAVE1_PIN GPIO7
+#define SPI_SELECT_SLAVE2_PORT GPIOE
+#define SPI_SELECT_SLAVE2_PIN GPIO3
/***************************************************************************************************/
diff --git a/sw/airborne/booz/test/Makefile b/sw/airborne/booz/test/Makefile
deleted file mode 100644
index 164960f6e7..0000000000
--- a/sw/airborne/booz/test/Makefile
+++ /dev/null
@@ -1,57 +0,0 @@
-##
-# $Id$
-#
-# Copyright (C) 2008-2009 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.
-##
-
-# Quiet compilation
-Q=@
-
-CC = gcc
-CFLAGS = -std=gnu99 -Wall -I.. -I../.. -I../../test/ -I../../../include -I../../booz_priv
-LDFLAGS = -lm
-
-CFLAGS += -I../../../../var/BOOZ2_A1P
-test_mlkf: test_mlkf.c ../booz_ahrs.c ../../booz_priv/ahrs/booz_ahrs_mlkf.c ../../booz_priv/ahrs/booz_ahrs_mlkf_opt.c ../booz_imu.c ../../math/pprz_trig_int.c ./imu_dummy.c ../ahrs/booz_ahrs_aligner.c
- $(CC) $(CFLAGS) -DBOOZ_IMU_TYPE_H=\"test/imu_dummy.h\" -o $@ $^ $(LDFLAGS)
-
-test_vg_ref: test_vg_ref.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-test_vg_adpt: test_vg_adpt.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-
-test_deadband: test_deadband.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-test_scaling: test_scaling.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-TEST_ATT_CFLAGS = -DSTABILISATION_ATTITUDE_TYPE_INT \
- -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\" \
- -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_int.h\"
-
-test_att_ref: test_att_ref.c ../stabilization/booz_stabilization_attitude_ref_quat_int.c
- $(CC) $(CFLAGS) $(TEST_ATT_CFLAGS) -I/home/poine/work/savannah/paparazzi3/trunk/var/BOOZ2_A1 -o $@ $^ $(LDFLAGS)
-
-
-clean:
- $(Q)rm -f *~ test_att_ref
diff --git a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c b/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c
deleted file mode 100644
index e810b06370..0000000000
--- a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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.
- */
-
-#include
-
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
-#include "interrupt_hw.h"
-#include "subsystems/datalink/downlink.h"
-#include "subsystems/datalink/datalink.h"
-
-#include "booz2_test_buss_bldc_hexa.h"
-
-#define NB_MOTORS 6
-static const uint8_t motor_addr[] = {0x52, 0x54, 0x56, 0x58, 0x5A, 0x5C};
-uint8_t motor = 0;
-uint8_t thrust = 10;
-static bool_t i2c_done;
-
-//static uint8_t addr = 0x52; /* 1 : back right */
-//static uint8_t addr = 0x54; /* 2 : back left bad balanced */
-//static uint8_t addr = 0x56; /* 3 : center right not so well balanced */
-//static uint8_t addr = 0x58; /* 4 : center left */
-//static uint8_t addr = 0x5A; /* 5 : front right */
-//static uint8_t addr = 0x5C; /* 6 : front left */
-
-static inline void main_init( void );
-static inline void main_periodic_task( void );
-static inline void main_event_task( void );
-
-int main( void ) {
- main_init();
- while(1) {
- if (sys_time_check_and_ack_timer(0))
- main_periodic_task();
- main_event_task();
- }
- return 0;
-}
-
-static inline void main_init( void ) {
- mcu_init();
- sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
- mcu_int_enable();
-}
-
-static inline void main_periodic_task( void ) {
- i2c0_buf[0] = thrust;
- i2c0_transmit(motor_addr[motor], 1, &i2c_done);
-
- RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
-
-}
-
-static inline void main_event_task( void ) {
- DatalinkEvent();
-}
-
diff --git a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.h b/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.h
deleted file mode 100644
index ba0a5964f5..0000000000
--- a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#ifndef BOOZ2_TEST_BUSS_BLDC_HEXA_H
-#define BOOZ2_TEST_BUSS_BLDC_HEXA_H
-
-extern uint8_t motor;
-extern uint8_t thrust;
-
-#endif /* BOOZ2_TEST_BUSS_BLDC_HEXA_H */
diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c
deleted file mode 100644
index 21fdb9c021..0000000000
--- a/sw/airborne/booz/test/booz2_test_crista.c
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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.
- */
-
-#include
-
-#include "std.h"
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
-#include "led.h"
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "subsystems/datalink/downlink.h"
-
-#include "subsystems/imu.h"
-
-#include "interrupt_hw.h"
-
-
-static inline void main_init( void );
-static inline void main_periodic_task( void );
-static inline void main_event_task( void );
-
-static inline void on_imu_event(void);
-
-int main( void ) {
- main_init();
- while(1) {
- if (sys_time_check_and_ack_timer(0))
- main_periodic_task();
- main_event_task();
- }
- return 0;
-}
-
-static inline void main_init( void ) {
- mcu_init();
- sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
- led_init();
-
-/* LED_ON(4); */
-/* LED_ON(5); */
-/* LED_ON(6); */
-/* LED_ON(7); */
-
- uart0_init();
- imu_impl_init();
- imu_init();
-
- mcu_int_enable();
-}
-
-static inline void main_periodic_task( void ) {
- //#if 0
- RunOnceEvery(100, {
- // LED_TOGGLE(7);
- DOWNLINK_SEND_ALIVE(16, MD5SUM);
- });
- // uint16_t foo = ami601_status;
- // DOWNLINK_SEND_BOOT(&foo);
- //#endif
- // if (sys_time.nb_sec > 2)
-
- imu_periodic();
-}
-
-static inline void main_event_task( void ) {
-
- ImuEvent(on_imu_event);
-
-}
-
-static inline void on_imu_event(void) {
- ImuScaleGyro();
- ImuScaleAccel();
-
- // LED_TOGGLE(6);
- static uint8_t cnt;
- cnt++;
- if (cnt > 15) cnt = 0;
-
- if (cnt == 0) {
- DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_unscaled.x,
- &imu_gyro_unscaled.y,
- &imu_gyro_unscaled.z);
-
- DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_unscaled.x,
- &imu_accel_unscaled.y,
- &imu_accel_unscaled.z);
- }
- else if (cnt == 7) {
- DOWNLINK_SEND_IMU_GYRO_SCALED(&imu_gyro.x,
- &imu_gyro.y,
- &imu_gyro.z);
-
- DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu_accel.x,
- &imu_accel.y,
- &imu_accel.z);
- }
-}
diff --git a/sw/airborne/booz/test/booz2_test_gps.c b/sw/airborne/booz/test/booz2_test_gps.c
deleted file mode 100644
index 693f76e95d..0000000000
--- a/sw/airborne/booz/test/booz2_test_gps.c
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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.
- */
-
-#include
-
-#include "std.h"
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
-#include "subsystems/datalink/downlink.h"
-#include "subsystems/gps.h"
-#include "interrupt_hw.h"
-
-static inline void main_init( void );
-static inline void main_periodic_task( void );
-static inline void main_event_task( void );
-
-static void on_gps_sol(void);
-
-int main( void ) {
- main_init();
- while(1) {
- if (sys_time_check_and_ack_timer(0))
- main_periodic_task();
- main_event_task();
- }
- return 0;
-}
-
-static inline void main_init( void ) {
- mcu_init();
- sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
- led_init();
- gps_init();
- mcu_int_enable();
-}
-
-static inline void main_periodic_task( void ) {
-
- RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
- RunOnceEvery(128, { LED_PERIODIC();});
-}
-
-static inline void main_event_task( void ) {
- GpsEvent(on_gps_sol);
-
-}
-
-static void on_gps_sol(void) {
-
- DOWNLINK_SEND_GPS_INT( DefaultChannel, DefaultDevice,
- &gps.ecef_pos.x,
- &gps.ecef_pos.y,
- &gps.ecef_pos.z,
- &gps.lla_pos.lat,
- &gps.lla_pos.lon,
- &gps.lla_pos.alt,
- &gps.ecef_vel.x,
- &gps.ecef_vel.y,
- &gps.ecef_vel.z,
- &gps.pacc,
- &gps.sacc,
- &gps.tow,
- &gps.pdop,
- &gps.num_sv,
- &gps.fix);
-
-}
diff --git a/sw/airborne/booz/test/booz2_test_max1168.c b/sw/airborne/booz/test/booz2_test_max1168.c
deleted file mode 100644
index af4aa4514b..0000000000
--- a/sw/airborne/booz/test/booz2_test_max1168.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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.
- */
-
-#include
-
-#include "std.h"
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
-#include "interrupt_hw.h"
-#include "messages.h"
-#include "subsystems/datalink/downlink.h"
-
-#include "armVIC.h"
-#include "LPC21xx.h"
-#include "peripherals/max1168.h"
-
-static inline void main_init( void );
-static inline void main_periodic_task( void );
-static inline void main_event_task( void );
-
-
-static void main_init_ssp(void);
-static void SSP_ISR(void) __attribute__((naked));
-
-
-
-
-int main( void ) {
- main_init();
- while(1) {
- if (sys_time_check_and_ack_timer(0))
- main_periodic_task();
- main_event_task();
- }
- return 0;
-}
-
-static inline void main_init( void ) {
- mcu_init();
- sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
-
- main_init_ssp();
- max1168_init();
-
- mcu_int_enable();
-}
-
-static inline void main_periodic_task( void ) {
- LED_TOGGLE(3);
- max1168_read();
-}
-
-static inline void main_event_task( void ) {
- if (max1168_status == MAX1168_DATA_AVAILABLE) {
- RunOnceEvery(10, {
- DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &max1168_values[0], &max1168_values[1], &max1168_values[2]);
- DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &max1168_values[3], &max1168_values[4], &max1168_values[6]);
- DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &max1168_values[7]); });
- max1168_status = MAX1168_IDLE;
- }
-}
-
-/* SSPCR0 settings */
-#define SSP_DDS 0x0F << 0 /* data size : 16 bits */
-#define SSP_FRF 0x00 << 4 /* frame format : SPI */
-#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
-#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
-#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */
-
-/* SSPCR1 settings */
-#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
-#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
-#define SSP_MS 0x00 << 2 /* master slave mode : master */
-#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
-
-#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
-#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD )
-
-#define SSP_PINSEL1_SCK (2<<2)
-#define SSP_PINSEL1_MISO (2<<4)
-#define SSP_PINSEL1_MOSI (2<<6)
-
-#define SSP_Enable() SetBit(SSPCR1, SSE);
-#define SSP_Disable() ClearBit(SSPCR1, SSE);
-#define SSP_EnableRxi() SetBit(SSPIMSC, RXIM)
-#define SSP_DisableRxi() ClearBit(SSPIMSC, RXIM)
-#define SSP_EnableTxi() SetBit(SSPIMSC, TXIM)
-#define SSP_DisableTxi() ClearBit(SSPIMSC, TXIM)
-#define SSP_EnableRti() SetBit(SSPIMSC, RTIM);
-#define SSP_DisableRti() ClearBit(SSPIMSC, RTIM);
-#define SSP_ClearRti() SetBit(SSPICR, RTIC);
-
-#ifndef SSP_VIC_SLOT
-#define SSP_VIC_SLOT 7
-#endif
-
-
-static void main_init_ssp(void) {
-
- /* setup pins for SSP (SCK, MISO, MOSI, SSEL) */
- PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
-
- /* setup SSP */
- SSPCR0 = SSPCR0_VAL;;
- SSPCR1 = SSPCR1_VAL;
- SSPCPSR = 0x02;
-
- /* initialize interrupt vector */
- VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
- VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
- _VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
- _VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
-
-}
-
-static void SSP_ISR(void) {
- ISR_ENTRY();
-
- Max1168OnSpiInt();
-
- VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
- ISR_EXIT();
-}
diff --git a/sw/airborne/booz/test/plot_scaling.sce b/sw/airborne/booz/test/plot_scaling.sce
deleted file mode 100644
index 7b13835013..0000000000
--- a/sw/airborne/booz/test/plot_scaling.sce
+++ /dev/null
@@ -1,69 +0,0 @@
-///
-// $Id$
-//
-// Copyright (C) 2008-2009 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.
-///
-
-clear();
-
-M=fscanfMat('traj.out');
-
-value = M(:,1);
-raw_sensor_f = M(:,2);
-raw_sensor_i = M(:,3);
-scaled_sensor_f = M(:,4);
-scaled_sensor_i = M(:,5);
-time=1:length(value);
-
-
-//k = find(time > 500 & time < 550);
-k = find(value > -0.009 & value < 0.009);
-//k = 1:length(time);
-
-
-scf();
-//clf();
-
-drawlater();
-
-subplot(3,1,1);
-xtitle('real', 'time (s)','');
-plot2d(time(k), value(k), 3);
-plot2d2(time(k), scaled_sensor_i(k)/2^10, 2);
-
-legends(["float", "int", "fixed"],[5 3 2], with_box=%f, opt="lr");
-
-subplot(3,1,2);
-xtitle('raw', 'time (s)','');
-
-plot2d(time(k), raw_sensor_f(k), 3);
-plot2d2(time(k), raw_sensor_i(k), 2);
-
-legends(["float", "int", "fixed"],[5 3 2], with_box=%f, opt="lr");
-
-subplot(3,1,3);
-xtitle('scaled', 'time (s)','');
-plot2d(value(k), scaled_sensor_f(k), 3);
-plot2d2(value(k), scaled_sensor_i(k), 5);
-//plot2d(time(k), zeros(1, length(time(k))), 1);
-xgrid(1);
-
-
-drawnow();
diff --git a/sw/airborne/booz/test/plot_test_vg_adpt.sce b/sw/airborne/booz/test/plot_test_vg_adpt.sce
deleted file mode 100644
index 7e8f21cf25..0000000000
--- a/sw/airborne/booz/test/plot_test_vg_adpt.sce
+++ /dev/null
@@ -1,93 +0,0 @@
-///
-// $Id$
-//
-// Copyright (C) 2008-2009 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.
-///
-
-clear();
-
-M=fscanfMat('traj.out');
-
-time = M(:,1);
-meas = M(:,2);
-thrust = M(:,3);
-zdd_ref = M(:,4);
-ffX = M(:,5);
-ffP = M(:,6);
-ffm = M(:,7);
-ifX = M(:,8);
-ifP = M(:,9);
-
-//k = find(time > 500 & time < 550);
-//k = find(time > 1110 & time < 1168);
-k = 1:length(time);
-
-
-clf();
-
-drawlater();
-
-subplot(3,2,1);
-xtitle('X', 'time (s)','');
-plot2d(time(k), ffm(k), 4);
-plot2d(time(k), ffX(k), 3);
-plot2d(time(k), ifX(k)/2^18, 5);
-legends(["float", "int", "meas"],[3 5 4], with_box=%f, opt="lr");
-
-subplot(3,2,2);
-xtitle('P', 'time (s)','');
-plot2d(time(k), ffP(k), 3);
-plot2d(time(k), ifP(k)/2^18, 5);
-legends(["float", "int"],[3 5], with_box=%f, opt="lr");
-
-subplot(3,2,3);
-xtitle('X', 'time (s)','');
-plot2d(time(k), ffX(k)*2^18, 3);
-plot2d(time(k), ifX(k), 5);
-legends(["float", "int"],[3 5], with_box=%f, opt="lr");
-
-subplot(3,2,4);
-xtitle('P', 'time (s)','');
-plot2d(time(k), ffP(k)*2^18, 3);
-plot2d(time(k), ifP(k), 5);
-legends(["float", "int"],[3 5], with_box=%f, opt="lr");
-
-subplot(3,2,5);
-xtitle('Zdd', 'time (s)','');
-plot2d(time(k), meas(k)/2^10, 1);
-
-subplot(3,2,6);
-xtitle('Thrust', 'time (s)','');
-
-fmg = 9.81./ffX(k);
-img = 9.81./ifX(k)*2^18;
-plot2d(time(k), fmg, 3);
-plot2d(time(k), img, 5);
-
-fmzdd = zdd_ref(k)./ffX(k)./2^10;
-imzdd = zdd_ref(k)./ifX(k)./2^10*2^18;
-//plot2d(time(k), fmg - fmzdd, 3);
-//plot2d(time(k), img - imzdd, 5);
-
-//plot2d(time(k), thrust(k), 2);
-
-legends(["float", "int"],[3 5], with_box=%f, opt="lr");
-
-drawnow();
diff --git a/sw/airborne/booz/test/plot_test_vg_ref.sce b/sw/airborne/booz/test/plot_test_vg_ref.sce
deleted file mode 100644
index 0d6157248b..0000000000
--- a/sw/airborne/booz/test/plot_test_vg_ref.sce
+++ /dev/null
@@ -1,63 +0,0 @@
-///
-// $Id$
-//
-// Copyright (C) 2008-2009 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.
-///
-
-clear();
-
-M=fscanfMat('traj.out');
-
-time = M(:,1);
-fzsp = M(:,2);
-fzdsp = M(:,3);
-fz = M(:,4);
-fzd = M(:,5);
-fzdd = M(:,6);
-iz = M(:,7);
-izd = M(:,8);
-izdd = M(:,9);
-
-
-clf();
-
-drawlater();
-
-subplot(3,1,1);
-xtitle('z', 'time (s)','');
-plot2d(time, fzsp, 1);
-plot2d(time, fz, 2);
-plot2d(time, iz, 3);
-legends(["sp", "float", "int"],[1 2 3], with_box=%f, opt="ur");
-
-subplot(3,1,2);
-xtitle('zd', 'time (s)','');
-plot2d(time, fzdsp, 1);
-plot2d(time, fzd, 2);
-plot2d(time, izd, 3);
-legends(["float", "int"],[2 3], with_box=%f, opt="lr");
-
-subplot(3,1,3);
-xtitle('zdd', 'time (s)','');
-plot2d(time, fzdd, 2);
-plot2d(time, izdd, 3);
-legends(["float", "int"],[2 3], with_box=%f, opt="lr");
-
-drawnow();
diff --git a/sw/airborne/booz/test/test_mlkf.c b/sw/airborne/booz/test/test_mlkf.c
deleted file mode 100644
index 1a21700261..0000000000
--- a/sw/airborne/booz/test/test_mlkf.c
+++ /dev/null
@@ -1,116 +0,0 @@
-#include
-#include
-
-#include "math/pprz_algebra_double.h"
-#include "subsystems/imu.h"
-#include "subsystems/ahrs.h"
-#include "ahrs/ahrs_mlkf.h"
-
-static void read_data(const char* filename);
-static void feed_imu(int i);
-static void store_filter_output(int i);
-static void dump_output(const char* filename);
-
-#define IN_FILE "../../../simulator/scilab/q6d/data/stop_stop_state_sensors.txt"
-#define OUT_FILE "./out.txt"
-
-#define MAX_SAMPLE 15000
-struct test_sample {
- double time;
- struct DoubleQuat quat_true;
- struct DoubleRates omega_true;
- struct DoubleRates gyro;
- struct DoubleVect3 accel;
- struct DoubleVect3 mag;
-};
-static struct test_sample samples[MAX_SAMPLE];
-static int nb_samples;
-
-struct test_output {
- struct FloatQuat quat_est;
- struct FloatRates bias_est;
- struct FloatRates rate_est;
- float P[6][6];
-};
-static struct test_output output[MAX_SAMPLE];
-
-
-int main(int argc, char** argv) {
-
- read_data(IN_FILE);
-
- imu_init();
- ahrs_init();
-
- for (int i=0; itime,
- &s->quat_true.qi, &s->quat_true.qx, &s->quat_true.qy, &s->quat_true.qz,
- &s->omega_true.p, &s->omega_true.q, &s->omega_true.r,
- &s->gyro.p, &s->gyro.q, &s->gyro.r,
- &s->accel.x, &s->accel.y, &s->accel.z,
- &s->mag.x, &s->mag.y, &s->mag.z );
- nb_samples++;
- }
- while (ret == 17 && nb_samples < MAX_SAMPLE);
- nb_samples--;
- fclose(fd);
- printf("read %d points in file %s\n", nb_samples, filename);
-}
-
-
-static void feed_imu(int i) {
- if (i>0) {
- RATES_COPY(imu.gyro_prev, imu.gyro);
- }
- else {
- RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro);
- }
- RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro);
- ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel);
- MAGS_BFP_OF_REAL(imu.mag, samples[i].mag);
-}
-
-
-static void store_filter_output(int i) {
-
- QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat);
- RATES_COPY(output[i].bias_est, ahrs_mlkf.gyro_bias);
- RATES_COPY(output[i].rate_est, ahrs_float.imu_rate);
- memcpy(output[i].P, ahrs_mlkf.P, sizeof(ahrs_mlkf.P));
-
-}
-
-static void dump_output(const char* filename) {
- FILE* fd = fopen(filename, "w");
- int i;
- for (i=0; i
- *
- * 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.
- */
-
-#include
-#include
-#include
-
-#include "math/pprz_algebra_int.h"
-
-#define IMU_ACCEL_X_NEUTRAL 32081
-#define IMU_ACCEL_X_SENS -2.50411474
-#define IMU_ACCEL_X_SENS_NUM -10650
-#define IMU_ACCEL_X_SENS_DEN 4253
-
-void test_1(void);
-void test_2(void);
-void test_3(void);
-
-int main(int argc, char** argv){
-
- // test_2();
- test_3();
-
- return 0;
-}
-
-void test_1(void) {
-
- double value_f;
- for (value_f=-9.81; value_f<9.81; value_f += 0.001) {
-
- double neutral_f = (double)IMU_ACCEL_X_NEUTRAL;
- double sensitivity_f = 1./IMU_ACCEL_X_SENS;
-
- double sensor_raw_f = ACCEL_BFP_OF_REAL(value_f) * sensitivity_f + neutral_f;
- int32_t sensor_raw_i = rint(sensor_raw_f);
-
- double scaled_sensor_f = ACCEL_BFP_OF_REAL(value_f);
-#if 1
- int32_t scaled_sensor_i = ((sensor_raw_i - IMU_ACCEL_X_NEUTRAL) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN;
-#endif
-
-#if 0
- int32_t scaled_sensor_i = (sensor_raw_i * IMU_ACCEL_X_SENS_NUM / IMU_ACCEL_X_SENS_DEN) -
- (IMU_ACCEL_X_NEUTRAL * IMU_ACCEL_X_SENS_NUM / IMU_ACCEL_X_SENS_DEN);
-#endif
-
-#if 0
- const int32_t delta = (sensor_raw_i - IMU_ACCEL_X_NEUTRAL);
- int32_t scaled_sensor_i;
- if (delta > 0)
- scaled_sensor_i = ( delta * IMU_ACCEL_X_SENS_NUM ) / IMU_ACCEL_X_SENS_DEN;
- else
- scaled_sensor_i = ( delta * IMU_ACCEL_X_SENS_NUM + (IMU_ACCEL_X_SENS_DEN>>1)) / IMU_ACCEL_X_SENS_DEN;
-#endif
-
- printf("%f %f %d %f %d\n", value_f, sensor_raw_f, sensor_raw_i, scaled_sensor_f, scaled_sensor_i);
-
-
-
- }
-}
-
-void test_2(void) {
-
- int a;
- for (a=-7; a<7; a++) {
- int b = a/-2;
- int c = (a>0 ? a+1 : a-1)/-2;
- double d = rint((double)a/-2.);
- printf("%- d %- d %- d %- .1f\n", a, b, c, d);
- }
-
-}
-
-#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
-#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
-#define N_OFFSET 2
-void test_3(void) {
-
- int a;
- for (a=-(1<>N_OFFSET);
- int32_t c;
- // if ( a>=0 )
- // c = (a+(1<<(N_OFFSET-1)))>>N_OFFSET;
- // else
- // c = (a+(1<<(N_OFFSET-1))-1)>>N_OFFSET;
- c = OFFSET_AND_ROUND(a, N_OFFSET);
-
- int32_t d;
- d = OFFSET_AND_ROUND2(a, N_OFFSET);
-
- double e;
- e = (double)a/(double)(1<
- *
- * 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.
- */
-
-#define _GNU_SOURCE
-#include
-#include
-
-#include "std.h"
-#include "booz_geometry_mixed.h"
-#define GUIDANCE_V_C
-#include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
-
-
-
-#define NB_STEP 20000
-int n_dat;
-
-double time[NB_STEP];
-
-int32_t z_sp[NB_STEP];
-int32_t zd_sp[NB_STEP];
-int32_t est_z[NB_STEP];
-int32_t est_zd[NB_STEP];
-int32_t est_zdd[NB_STEP];
-int32_t ref_z[NB_STEP];
-int32_t ref_zd[NB_STEP];
-int32_t ref_zdd[NB_STEP];
-int32_t adp_inv_m[NB_STEP];
-int32_t adp_cov[NB_STEP];
-int32_t sum_err[NB_STEP];
-int32_t ff_cmd[NB_STEP];
-int32_t fb_cmd[NB_STEP];
-int32_t delta_t[NB_STEP];
-
-
-
-int32_t ifX[NB_STEP];
-int32_t ifP[NB_STEP];
-
-
-double ffX[NB_STEP];
-double ffm[NB_STEP];
-double ffP[NB_STEP];
-
-static void float_filter_init(void) {
- ffX[0] = GV_ADAPT_X0_F;
- ffP[0] = GV_ADAPT_P0_F;
-}
-
-static void float_filter_run( int i) {
-
- int prev = i>0 ? i-1 : i;
- ffX[i] = ffX[prev];
- ffP[i] = ffP[prev];
- if (delta_t[prev] == 0) return;
- ffP[i] = ffP[i] + GV_ADAPT_SYS_NOISE_F;
- ffm[i] = (9.81 - (double)est_zdd[i]/(double)(1<<10)) / (double)delta_t[prev];
- double residual = ffm[i] - ffX[i];
- double E = ffP[i] + GV_ADAPT_MEAS_NOISE_F;
- double K = ffP[i] / E;
- ffP[i] = ffP[i] - K * ffP[i];
- ffX[i] = ffX[i] + K * residual;
-}
-
-static int read_data(const char* filename) {
-
- FILE *fp = NULL;
- fp = fopen(filename, "r");
- if (fp == NULL)
- return -1;
-
- char * line = NULL;
- size_t len = 0;
- size_t read;
- n_dat = 0;
- while ((read = getline(&line, &len, fp)) != -1 && n_dat< NB_STEP) {
- if (sscanf(line, "%lf %*d VERT_LOOP %d %d %d %d %d %d %d %d %d %d %d %d %d %d",
- &time[n_dat],
- &z_sp[n_dat], &zd_sp[n_dat],
- &est_z[n_dat], &est_zd[n_dat], &est_zdd[n_dat],
- &ref_z[n_dat], &ref_zd[n_dat], &ref_zdd[n_dat],
- &adp_inv_m[n_dat], &adp_cov[n_dat], &sum_err[n_dat],
- &ff_cmd[n_dat], &fb_cmd[n_dat], &delta_t[n_dat]) == 15) n_dat++;
- }
- if (line)
- free(line);
- fclose(fp);
- return 0;
-
-}
-
-void gen_data(void) {
- int i = 0;
- n_dat = NB_STEP;
- while (i>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
- int32_t cmd_i = (BOOZ_INT_OF_FLOAT(9.81, FF_CMD_FRAC) - (ref_zdd[i]<<(FF_CMD_FRAC - IACCEL_RES))) / inv_m_i;
-
- double inv_m_f = (double)BOOZ_FLOAT_OF_INT(gv_adapt_X, GV_ADAPT_X_FRAC);
- double cmd_f = (9.81 - (double)BOOZ_FLOAT_OF_INT(ref_zdd[i], IACCEL_RES)) / inv_m_f;
-
- int32_t cmd_i_fixed;
-
- // = ((int32_t)BOOZ_INT_OF_FLOAT(9.81, IACCEL_RES) - ref_zdd[i] + (inv_m_i_fixed>>1)) / inv_m_i_fixed;
-
- if (ref_zdd[i] < BOOZ_INT_OF_FLOAT(9.81, IACCEL_RES))
- cmd_i_fixed = (BOOZ_INT_OF_FLOAT(9.81, FF_CMD_FRAC) - (ref_zdd[i]<<(FF_CMD_FRAC - IACCEL_RES)) + (inv_m_i>>1) ) / inv_m_i;
- else
- cmd_i_fixed = (BOOZ_INT_OF_FLOAT(9.81, FF_CMD_FRAC) - (ref_zdd[i]<<(FF_CMD_FRAC - IACCEL_RES)) - (inv_m_i>>1) ) / inv_m_i;
-
- printf("%d %f %d\n",cmd_i, cmd_f, cmd_i_fixed);
-}
-
-int main(int argc, char** argv) {
- // gen_data();
- read_data("09_02_15__20_45_58.data");
- printf("read %d\n", n_dat);
- gv_adapt_init();
- float_filter_init();
- int i = 0;
- while (i
- *
- * 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.
- */
-
-#include
-
-#include "booz_geometry_mixed.h"
-#define GUIDANCE_V_C
-#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
-
-#define NB_STEP 10000
-#define DT (1./GV_FREQ)
-
-double z_sp, zd_sp, z_ref, zd_ref, zdd_ref;
-
-void set_ref(double z, double zd, double zdd) {
- z_ref = z;
- zd_ref = zd;
- zdd_ref = zdd;
-}
-
-void update_ref_from_z(void) {
-
- z_ref += (zd_ref * DT);
- zd_ref += (zdd_ref * DT);
- zdd_ref = -2.*GV_ZETA*GV_OMEGA*zd_ref - GV_OMEGA*GV_OMEGA*(z_ref - z_sp);
-
- Bound(zdd_ref, GV_MIN_ZDD_F, GV_MAX_ZDD_F);
-
- if (zd_ref <= GV_MIN_ZD_F) {
- zd_ref = GV_MIN_ZD_F;
- if (zdd_ref < 0)
- zdd_ref = 0;
- }
- else if (zd_ref >= GV_MAX_ZD_F) {
- zd_ref = GV_MAX_ZD_F;
- if (zdd_ref > 0)
- zdd_ref = 0;
- }
-
-}
-
-void update_ref_from_zd(void) {
-
- z_ref += (zd_ref * DT);
- zd_ref += (zdd_ref * DT);
- zdd_ref = -1/GV_REF_THAU_F*(zd_ref - zd_sp);
-
- Bound(zdd_ref, GV_MIN_ZDD_F, GV_MAX_ZDD_F);
-
- if (zd_ref <= GV_MIN_ZD_F) {
- zd_ref = GV_MIN_ZD_F;
- if (zdd_ref < 0)
- zdd_ref = 0;
- }
- else if (zd_ref >= GV_MAX_ZD_F) {
- zd_ref = GV_MAX_ZD_F;
- if (zdd_ref > 0)
- zdd_ref = 0;
- }
-
-}
-
-void print_ref(int i) {
- double i2f_z = BOOZ_FLOAT_OF_INT( gv_z_ref, GV_Z_REF_FRAC);
- double i2f_zd = BOOZ_FLOAT_OF_INT( gv_zd_ref, GV_ZD_REF_FRAC);
- double i2f_zdd = BOOZ_FLOAT_OF_INT( gv_zdd_ref, GV_ZDD_REF_FRAC);
- printf("%f %f %f %f %f %f %f %f %f\n",
- (double)i*DT, z_sp, zd_sp,
- z_ref, zd_ref, zdd_ref,
- i2f_z, i2f_zd, i2f_zdd );
-}
-
-int32_t get_sp (int i) {
- // return BOOZ_INT_OF_FLOAT(i>512 ? -50.0 : 0, IPOS_FRAC);
- return BOOZ_INT_OF_FLOAT((i>512&&i<3072) ? -1.0 : 0, ISPEED_RES);
-}
-
-
-
-int main(int argc, char** argv) {
- gv_set_ref(0, 0, 0);
- set_ref(0., 0., 0.);
- int i = 0;
- while (i make install_shared_lib
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 70d2ea22ae..6a0ec61b45 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -36,6 +36,19 @@
#include "math/pprz_trig_int.h"
#include
+
+struct Uint8Vect3 {
+ uint8_t x;
+ uint8_t y;
+ uint8_t z;
+};
+
+struct Int8Vect3 {
+ int8_t x;
+ int8_t y;
+ int8_t z;
+};
+
struct Uint16Vect3 {
uint16_t x;
uint16_t y;
diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c
index 059d536cd5..afcea7d488 100644
--- a/sw/airborne/modules/gps/gps_ubx_ucenter.c
+++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c
@@ -28,6 +28,7 @@
*/
#include "gps_ubx_ucenter.h"
+#include "subsystems/gps/gps_ubx.h"
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.h b/sw/airborne/modules/gps/gps_ubx_ucenter.h
index 843e6c90a2..b0e93bf428 100644
--- a/sw/airborne/modules/gps/gps_ubx_ucenter.h
+++ b/sw/airborne/modules/gps/gps_ubx_ucenter.h
@@ -28,6 +28,8 @@
#ifndef GPS_UBX_UCENTER_H
#define GPS_UBX_UCENTER_H
+#include "std.h"
+
/** U-Center Variables */
#define GPS_UBX_UCENTER_CONFIG_STEPS 17
diff --git a/sw/airborne/peripherals/lis302dl.h b/sw/airborne/peripherals/lis302dl.h
new file mode 100644
index 0000000000..a71fabc96a
--- /dev/null
+++ b/sw/airborne/peripherals/lis302dl.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2013 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/lis302dl.h
+ *
+ * ST LIS302DL 3-axis accelerometer driver common interface (I2C and SPI).
+ */
+
+#ifndef LIS302DL_H
+#define LIS302DL_H
+
+/* Include address and register definition */
+#include "peripherals/lis302dl_regs.h"
+
+enum Lis302dlConfStatus {
+ LIS_CONF_UNINIT = 0,
+ LIS_CONF_WHO_AM_I = 1,
+ LIS_CONF_WHO_AM_I_OK = 2,
+ LIS_CONF_REG2 = 3,
+ LIS_CONF_REG3 = 4,
+ LIS_CONF_ENABLE = 5,
+ LIS_CONF_DONE = 6
+};
+
+struct Lis302dlConfig {
+ bool_t int_invert; ///< Invert Interrupt FALSE: active high, TRUE: active low
+ bool_t spi_3_wire; ///< Set 3-wire SPI mode, if FALSE: 4-wire SPI mode
+
+ /** Filtered Data Selection.
+ * FALSE: internal filter bypassed;
+ * TRUE: data from internal filter sent to output register */
+ bool_t filt_data;
+ enum Lis302dlRanges range; ///< g Range
+ enum Lis302dlRates rate; ///< Data Output Rate
+};
+
+static inline void lis302dl_set_default_config(struct Lis302dlConfig *c)
+{
+ c->int_invert = TRUE;
+ c->filt_data = FALSE;
+ c->spi_3_wire = FALSE;
+
+ c->rate = LIS302DL_RATE_100HZ;
+ c->range = LIS302DL_RANGE_2G;
+}
+
+#endif /* LIS302DL_H */
diff --git a/sw/airborne/peripherals/lis302dl_regs.h b/sw/airborne/peripherals/lis302dl_regs.h
new file mode 100644
index 0000000000..059c682cff
--- /dev/null
+++ b/sw/airborne/peripherals/lis302dl_regs.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2013 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/lis302dl_regs.h
+ *
+ * ST LIS302DL 3-axis accelerometer register definitions.
+ */
+
+#ifndef LIS302DL_REGS_H
+#define LIS302DL_REGS_H
+
+/* Registers */
+#define LIS302DL_REG_WHO_AM_I 0x0F
+#define LIS302DL_REG_CTRL_REG1 0x20
+#define LIS302DL_REG_CTRL_REG2 0x21
+#define LIS302DL_REG_CTRL_REG3 0x22
+#define LIS302DL_REG_STATUS 0x27
+#define LIS302DL_REG_OUTX 0x29
+#define LIS302DL_REG_OUTY 0x2B
+#define LIS302DL_REG_OUTZ 0x2D
+
+/** LIS302DL device identifier contained in LIS302DL_REG_WHO_AM_I */
+#define LIS302DL_WHO_AM_I 0x3B
+
+enum Lis302dlRates {
+ LIS302DL_RATE_100HZ = 0,
+ LIS302DL_RATE_400HZ = 1
+};
+
+enum Lis302dlRanges {
+ LIS302DL_RANGE_2G = 0,
+ LIS302DL_RANGE_8G = 1
+};
+
+
+#endif /* LIS302DL_REGS_H */
diff --git a/sw/airborne/peripherals/lis302dl_spi.c b/sw/airborne/peripherals/lis302dl_spi.c
new file mode 100644
index 0000000000..55348f41e0
--- /dev/null
+++ b/sw/airborne/peripherals/lis302dl_spi.c
@@ -0,0 +1,174 @@
+/*
+ * Copyright (C) 2013 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/lis302dl_spi.c
+ *
+ * Driver for LIS302DL 3-axis accelerometer from ST using SPI.
+ */
+
+#include "peripherals/lis302dl_spi.h"
+
+void lis302dl_spi_init(struct Lis302dl_Spi *lis, struct spi_periph *spi_p, uint8_t slave_idx)
+{
+ /* set spi_peripheral */
+ lis->spi_p = spi_p;
+
+ /* configure spi transaction */
+ lis->spi_trans.cpol = SPICpolIdleHigh;
+ lis->spi_trans.cpha = SPICphaEdge2;
+ lis->spi_trans.dss = SPIDss8bit;
+ lis->spi_trans.bitorder = SPIMSBFirst;
+ lis->spi_trans.cdiv = SPIDiv64;
+
+ lis->spi_trans.select = SPISelectUnselect;
+ lis->spi_trans.slave_idx = slave_idx;
+ lis->spi_trans.output_length = 2;
+ lis->spi_trans.input_length = 8;
+ // callback currently unused
+ lis->spi_trans.before_cb = NULL;
+ lis->spi_trans.after_cb = NULL;
+ lis->spi_trans.input_buf = &(lis->rx_buf[0]);
+ lis->spi_trans.output_buf = &(lis->tx_buf[0]);
+
+ /* set inital status: Success or Done */
+ lis->spi_trans.status = SPITransDone;
+
+ /* set default LIS302DL config options */
+ lis302dl_set_default_config(&(lis->config));
+
+ lis->initialized = FALSE;
+ lis->data_available = FALSE;
+ lis->init_status = LIS_CONF_UNINIT;
+}
+
+
+static void lis302dl_spi_write_to_reg(struct Lis302dl_Spi *lis, uint8_t _reg, uint8_t _val) {
+ lis->spi_trans.output_length = 2;
+ lis->spi_trans.input_length = 0;
+ lis->tx_buf[0] = _reg;
+ lis->tx_buf[1] = _val;
+ spi_submit(lis->spi_p, &(lis->spi_trans));
+}
+
+// Configuration function called once before normal use
+static void lis302dl_spi_send_config(struct Lis302dl_Spi *lis)
+{
+ uint8_t reg_val = 0;
+
+ switch (lis->init_status) {
+ case LIS_CONF_WHO_AM_I:
+ /* query device id */
+ lis->spi_trans.output_length = 1;
+ lis->spi_trans.input_length = 2;
+ /* set read bit then reg address */
+ lis->tx_buf[0] = (1<<7 | LIS302DL_REG_WHO_AM_I);
+ if (spi_submit(lis->spi_p, &(lis->spi_trans)))
+ lis->init_status++;
+ break;
+ case LIS_CONF_REG2:
+ /* set SPI mode, Filtered Data Selection */
+ reg_val = (lis->config.spi_3_wire << 7) | (lis->config.filt_data << 4);
+ lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG2, reg_val);
+ lis->init_status++;
+ break;
+ case LIS_CONF_REG3:
+ /* Interrupt active high/low */
+ lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG3, (lis->config.int_invert << 7));
+ lis->init_status++;
+ break;
+ case LIS_CONF_ENABLE:
+ /* set data rate, range, enable measurement, is in standby after power up */
+ reg_val = (lis->config.rate << 7) |
+ (1 << 6) | // Power Down Control to active mode
+ (lis->config.range << 5) |
+ 0x5; // enable z,y,x axes
+ lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG1, reg_val);
+ lis->init_status++;
+ break;
+ case LIS_CONF_DONE:
+ lis->initialized = TRUE;
+ lis->spi_trans.status = SPITransDone;
+ break;
+ default:
+ break;
+ }
+}
+
+void lis302dl_spi_start_configure(struct Lis302dl_Spi *lis)
+{
+ if (lis->init_status == LIS_CONF_UNINIT) {
+ lis->init_status++;
+ if (lis->spi_trans.status == SPITransSuccess || lis->spi_trans.status == SPITransDone) {
+ lis302dl_spi_send_config(lis);
+ }
+ }
+}
+
+void lis302dl_spi_read(struct Lis302dl_Spi *lis)
+{
+ if (lis->initialized && lis->spi_trans.status == SPITransDone) {
+ lis->spi_trans.output_length = 1;
+ lis->spi_trans.input_length = 8;
+ /* set read bit and multiple byte bit, then address */
+ lis->tx_buf[0] = (1<<7|1<<6|LIS302DL_REG_STATUS);
+ spi_submit(lis->spi_p, &(lis->spi_trans));
+ }
+}
+
+void lis302dl_spi_event(struct Lis302dl_Spi *lis)
+{
+ if (lis->initialized) {
+ if (lis->spi_trans.status == SPITransFailed) {
+ lis->spi_trans.status = SPITransDone;
+ }
+ else if (lis->spi_trans.status == SPITransSuccess) {
+ // Successfull reading
+ if (bit_is_set(lis->rx_buf[1], 3)) {
+ // new xyz data available
+ lis->data.vect.x = lis->rx_buf[3];
+ lis->data.vect.y = lis->rx_buf[5];
+ lis->data.vect.z = lis->rx_buf[7];
+ lis->data_available = TRUE;
+ }
+ lis->spi_trans.status = SPITransDone;
+ }
+ }
+ else if (lis->init_status != LIS_CONF_UNINIT) { // Configuring but not yet initialized
+ switch (lis->spi_trans.status) {
+ case SPITransFailed:
+ lis->init_status--; // Retry config (TODO max retry)
+ case SPITransSuccess:
+ if (lis->init_status == LIS_CONF_WHO_AM_I_OK) {
+ if (lis->rx_buf[1] == LIS302DL_WHO_AM_I)
+ lis->init_status++;
+ else
+ lis->init_status = LIS_CONF_WHO_AM_I;
+ }
+ case SPITransDone:
+ lis->spi_trans.status = SPITransDone;
+ lis302dl_spi_send_config(lis);
+ break;
+ default:
+ break;
+ }
+ }
+}
diff --git a/sw/airborne/peripherals/lis302dl_spi.h b/sw/airborne/peripherals/lis302dl_spi.h
new file mode 100644
index 0000000000..375f81ca1d
--- /dev/null
+++ b/sw/airborne/peripherals/lis302dl_spi.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2013 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/lis302dl_spi.h
+ *
+ * Driver for LIS302DL 3-axis accelerometer from ST using SPI.
+ */
+
+#ifndef LIS302DL_SPI_H
+#define LIS302DL_SPI_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+#include "mcu_periph/spi.h"
+
+/* Include common LIS302DL options and definitions */
+#include "peripherals/lis302dl.h"
+
+
+struct Lis302dl_Spi {
+ struct spi_periph *spi_p;
+ struct spi_transaction spi_trans;
+ volatile uint8_t tx_buf[2];
+ volatile uint8_t rx_buf[8];
+ enum Lis302dlConfStatus init_status; ///< init status
+ bool_t initialized; ///< config done flag
+ volatile bool_t data_available; ///< data ready flag
+ union {
+ struct Int8Vect3 vect; ///< data vector in accel coordinate system
+ int8_t value[3]; ///< data values accessible by channel index
+ } data;
+ struct Lis302dlConfig config;
+};
+
+// Functions
+extern void lis302dl_spi_init(struct Lis302dl_Spi *lis, struct spi_periph *spi_p, uint8_t addr);
+extern void lis302dl_spi_start_configure(struct Lis302dl_Spi *lis);
+extern void lis302dl_spi_read(struct Lis302dl_Spi *lis);
+extern void lis302dl_spi_event(struct Lis302dl_Spi *lis);
+
+/// convenience function: read or start configuration if not already initialized
+static inline void lis302dl_spi_periodic(struct Lis302dl_Spi *lis) {
+ if (lis->initialized)
+ lis302dl_spi_read(lis);
+ else
+ lis302dl_spi_start_configure(lis);
+}
+
+#endif // LIS302DL_SPI_H
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
index 8cc175a819..1ae27fc631 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
@@ -50,9 +50,15 @@
#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
#endif
+#ifndef AHRS_MAG_NOISE_X
+#define AHRS_MAG_NOISE_X 0.2
+#define AHRS_MAG_NOISE_Y 0.2
+#define AHRS_MAG_NOISE_Z 0.2
+#endif
+
static inline void propagate_ref(void);
static inline void propagate_state(void);
-static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]);
+static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise);
static inline void reset_state(void);
static inline void set_body_state_from_quat(void);
@@ -93,6 +99,8 @@ void ahrs_init(void) {
{ 0., 0., 0., 0., 0., P0_b}};
memcpy(ahrs_impl.P, P0, sizeof(P0));
+ VECT3_ASSIGN(ahrs_impl.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
+
}
void ahrs_align(void) {
@@ -128,9 +136,8 @@ void ahrs_update_accel(void) {
(1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81);
const struct FloatVect3 earth_g = {0., 0., -9.81 };
const float dn = 250*fabs( ahrs_impl.lp_accel );
- const float g_noise[] = {1.+dn, 1.+dn, 1.+dn};
- // const float g_noise[] = {150., 150., 150.};
- update_state(&earth_g, &imu_g, g_noise);
+ struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
+ update_state(&earth_g, &imu_g, &g_noise);
reset_state();
}
@@ -138,8 +145,7 @@ void ahrs_update_accel(void) {
void ahrs_update_mag(void) {
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
- const float h_noise[] = { 0.1610, 0.1771, 0.2659};
- update_state(&ahrs_impl.mag_h, &imu_h, h_noise);
+ update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise);
reset_state();
}
@@ -200,7 +206,7 @@ static inline void propagate_state(void) {
/**
* Incorporate one 3D vector measurement
*/
-static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]) {
+static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) {
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
@@ -214,8 +220,12 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
MAT_MUL(3,6,6, tmp, H, ahrs_impl.P);
float S[3][3];
MAT_MUL_T(3,6,3, S, tmp, H);
- for (int i=0;i<3;i++)
- S[i][i] += noise[i];
+
+ /* add the measurement noise */
+ S[0][0] += noise->x;
+ S[1][1] += noise->y;
+ S[2][2] += noise->z;
+
float invS[3][3];
MAT_INV33(invS, S);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
index 043b519a64..5f811594c4 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
@@ -43,6 +43,8 @@ struct AhrsMlkf {
struct FloatVect3 mag_h;
+ struct FloatVect3 mag_noise;
+
struct FloatQuat gibbs_cor;
float P[6][6];
float lp_accel;
diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index 260ed71983..160042852e 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -53,30 +53,7 @@
#define UTM_HEM_SOUTH 1
-#define GpsUartSend1(c) GpsLink(Transmit(c))
-#define GpsUartSetBaudrate(_a) GpsLink(SetBaudrate(_a))
#define GpsUartRunning GpsLink(TxRunning)
-#define GpsUartSendMessage GpsLink(SendMessage)
-
-#define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; }
-#define UpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; }
-#define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); }
-
-#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); }
-#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); }
-#define UbxSend1ByAddr(x) { UbxSend1(*x); }
-#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); }
-#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); }
-
-#define UbxHeader(nav_id, msg_id, len) { \
- GpsUartSend1(UBX_SYNC1); \
- GpsUartSend1(UBX_SYNC2); \
- UbxInitCheksum(); \
- UbxSend1(nav_id); \
- UbxSend1(msg_id); \
- UbxSend2(len); \
- }
-
struct GpsUbx gps_ubx;
@@ -267,11 +244,6 @@ void gps_ubx_parse( uint8_t c ) {
return;
}
-#ifdef GPS_UBX_UCENTER
-#include GPS_UBX_UCENTER
-#endif
-
-
void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
#ifdef GPS_LINK
UbxSend_CFG_RST(bbr, reset_mode, 0x00);
diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h
index dab9de81a5..e2d1c2ce58 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.h
+++ b/sw/airborne/subsystems/gps/gps_ubx.h
@@ -134,5 +134,27 @@ extern void ubxsend_cfg_rst(uint16_t, uint8_t);
ubxsend_cfg_rst(gps_ubx.reset, CFG_RST_Reset_Controlled); \
}
+#define GpsUartSendMessage GpsLink(SendMessage)
+
+#define GpsUartSend1(c) GpsLink(Transmit(c))
+#define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; }
+#define UpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; }
+#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); }
+#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); }
+#define UbxSend1ByAddr(x) { UbxSend1(*x); }
+#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); }
+#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); }
+#define GpsUartSetBaudrate(_a) GpsLink(SetBaudrate(_a))
+
+#define UbxHeader(nav_id, msg_id, len) { \
+ GpsUartSend1(UBX_SYNC1); \
+ GpsUartSend1(UBX_SYNC2); \
+ UbxInitCheksum(); \
+ UbxSend1(nav_id); \
+ UbxSend1(msg_id); \
+ UbxSend2(len); \
+ }
+
+#define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); }
#endif /* GPS_UBX_H */
diff --git a/sw/airborne/test/Makefile b/sw/airborne/test/Makefile
index 5938ed851a..bd28c86dc4 100644
--- a/sw/airborne/test/Makefile
+++ b/sw/airborne/test/Makefile
@@ -18,23 +18,11 @@ test_geodetic: test_geodetic.c ../math/pprz_geodetic_float.c ../math/pprz_geodet
test_algebra: test_algebra.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-test_martin: test_martin.c ../math/pprz_trig_int.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
test_bla: test_bla.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-test_att: test_att.c ../math/pprz_trig_int.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-test_sqrt: test_sqrt.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-test_fmul: test_fmul.c
- $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
%.exe : %.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
clean:
- $(Q)rm -f *~ test_geodetic test_algebra *.exe
+ $(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla *.exe
diff --git a/sw/airborne/test/ahrs/ahrs_utils.py b/sw/airborne/test/ahrs/ahrs_utils.py
index 60d33d7e9a..9d56fc73a5 100644
--- a/sw/airborne/test/ahrs/ahrs_utils.py
+++ b/sw/airborne/test/ahrs/ahrs_utils.py
@@ -1,6 +1,5 @@
#! /usr/bin/env python
-# $Id$
# Copyright (C) 2011 Antoine Drouin
#
# This file is part of Paparazzi.
@@ -21,37 +20,32 @@
# Boston, MA 02111-1307, USA.
#
-#import os
-#from optparse import OptionParser
-#import scipy
-#from scipy import optimize
-import shlex, subprocess
-from pylab import *
-from array import array
-import numpy
+from __future__ import print_function
+
+import subprocess
+import numpy as np
import matplotlib.pyplot as plt
def run_simulation(ahrs_type, build_opt, traj_nb):
- print "\nBuilding ahrs"
+ print("\nBuilding ahrs")
args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_" + ahrs_type] + build_opt
- # print args
+ #print(args)
p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
outputlines = p.stdout.readlines()
p.wait()
for i in outputlines:
- print " # " + i,
- print
-
- print "Running simulation"
- print " using traj " + str(traj_nb)
+ print(" # " + i, end=' ')
+ print()
+ print("Running simulation")
+ print(" using traj " + str(traj_nb))
p = subprocess.Popen(args=["./run_ahrs_on_synth", str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
shell=False)
outputlines = p.stdout.readlines()
p.wait()
# for i in outputlines:
- # print " "+i,
- # print "\n"
+ # print(" "+i, end=' ')
+ # print("\n")
ahrs_data_type = [('time', 'float32'),
('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'),
@@ -61,92 +55,91 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'),
('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')]
- mydescr = numpy.dtype(ahrs_data_type)
+ mydescr = np.dtype(ahrs_data_type)
data = [[] for dummy in xrange(len(mydescr))]
# import code; code.interact(local=locals())
for line in outputlines:
if line.startswith("#"):
- print " " + line,
+ print(" " + line, end=' ')
else:
fields = line.strip().split(' ')
- # print fields
+ #print(fields)
for i, number in enumerate(fields):
data[i].append(number)
-
- print
+ print()
for i in xrange(len(mydescr)):
- data[i] = cast[mydescr[i]](data[i])
+ data[i] = np.cast[mydescr[i]](data[i])
- return numpy.rec.array(data, dtype=mydescr)
+ return np.rec.array(data, dtype=mydescr)
-def plot_simulation_results(plot_true_state, lsty, type, sim_res):
- print "Plotting Results"
+def plot_simulation_results(plot_true_state, lsty, label, 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, label=type)
- ylabel('degres')
- title('phi')
- legend()
+ plt.subplot(3, 3, 1)
+ plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=label)
+ plt.ylabel('degres')
+ plt.title('phi')
+ plt.legend()
- subplot(3, 3, 2)
- plot(sim_res.time, sim_res.theta_ahrs, lsty)
- title('theta')
+ plt.subplot(3, 3, 2)
+ plt.plot(sim_res.time, sim_res.theta_ahrs, lsty)
+ plt.title('theta')
- subplot(3, 3, 3)
- plot(sim_res.time, sim_res.psi_ahrs, lsty)
- title('psi')
+ plt.subplot(3, 3, 3)
+ plt.plot(sim_res.time, sim_res.psi_ahrs, lsty)
+ plt.title('psi')
- subplot(3, 3, 4)
+ plt.subplot(3, 3, 4)
plt.plot(sim_res.time, sim_res.p_ahrs, lsty)
- ylabel('degres/s')
- title('p')
+ plt.ylabel('degres/s')
+ plt.title('p')
- subplot(3, 3, 5)
+ plt.subplot(3, 3, 5)
plt.plot(sim_res.time, sim_res.q_ahrs, lsty)
- title('q')
+ plt.title('q')
- subplot(3, 3, 6)
+ plt.subplot(3, 3, 6)
plt.plot(sim_res.time, sim_res.r_ahrs, lsty)
- title('r')
+ plt.title('r')
- subplot(3, 3, 7)
+ plt.subplot(3, 3, 7)
plt.plot(sim_res.time, sim_res.bp_ahrs, lsty)
- ylabel('degres/s')
- xlabel('time in s')
- title('bp')
+ plt.ylabel('degres/s')
+ plt.xlabel('time in s')
+ plt.title('bp')
- subplot(3, 3, 8)
+ plt.subplot(3, 3, 8)
plt.plot(sim_res.time, sim_res.bq_ahrs, lsty)
- xlabel('time in s')
- title('bq')
+ plt.xlabel('time in s')
+ plt.title('bq')
- subplot(3, 3, 9)
+ plt.subplot(3, 3, 9)
plt.plot(sim_res.time, sim_res.br_ahrs, lsty)
- xlabel('time in s')
- title('br')
+ plt.xlabel('time in s')
+ plt.title('br')
if plot_true_state:
- subplot(3, 3, 1)
+ plt.subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_true, 'r--')
- subplot(3, 3, 2)
- plot(sim_res.time, sim_res.theta_true, 'r--')
- subplot(3, 3, 3)
- plot(sim_res.time, sim_res.psi_true, 'r--')
- subplot(3, 3, 4)
- plot(sim_res.time, sim_res.p_true, 'r--')
- subplot(3, 3, 5)
- plot(sim_res.time, sim_res.q_true, 'r--')
- subplot(3, 3, 6)
- plot(sim_res.time, sim_res.r_true, 'r--')
- subplot(3, 3, 7)
- plot(sim_res.time, sim_res.bp_true, 'r--')
- subplot(3, 3, 8)
- plot(sim_res.time, sim_res.bq_true, 'r--')
- subplot(3, 3, 9)
- plot(sim_res.time, sim_res.br_true, 'r--')
+ plt.subplot(3, 3, 2)
+ plt.plot(sim_res.time, sim_res.theta_true, 'r--')
+ plt.subplot(3, 3, 3)
+ plt.plot(sim_res.time, sim_res.psi_true, 'r--')
+ plt.subplot(3, 3, 4)
+ plt.plot(sim_res.time, sim_res.p_true, 'r--')
+ plt.subplot(3, 3, 5)
+ plt.plot(sim_res.time, sim_res.q_true, 'r--')
+ plt.subplot(3, 3, 6)
+ plt.plot(sim_res.time, sim_res.r_true, 'r--')
+ plt.subplot(3, 3, 7)
+ plt.plot(sim_res.time, sim_res.bp_true, 'r--')
+ plt.subplot(3, 3, 8)
+ plt.plot(sim_res.time, sim_res.bq_true, 'r--')
+ plt.subplot(3, 3, 9)
+ plt.plot(sim_res.time, sim_res.br_true, 'r--')
def show_plot():
diff --git a/sw/airborne/test/ahrs/compare_ahrs.py b/sw/airborne/test/ahrs/compare_ahrs.py
index 524af47d55..c2d3fc3686 100755
--- a/sw/airborne/test/ahrs/compare_ahrs.py
+++ b/sw/airborne/test/ahrs/compare_ahrs.py
@@ -20,8 +20,11 @@
# Boston, MA 02111-1307, USA.
#
+import os
+import sys
import ahrs_utils
+
def main():
# traj_nb = 0 # static
@@ -77,4 +80,7 @@ def main():
ahrs_utils.show_plot()
if __name__ == "__main__":
+ script_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+ if script_path != os.path.abspath(os.getcwd()):
+ sys.exit("Please run this script from " + script_path)
main()
diff --git a/sw/airborne/test/nova_test_imu.c b/sw/airborne/test/nova_test_imu.c
deleted file mode 100644
index e14b8f1c35..0000000000
--- a/sw/airborne/test/nova_test_imu.c
+++ /dev/null
@@ -1,58 +0,0 @@
-#include "std.h"
-#include "mcu.h"
-#include "interrupt_hw.h"
-#include "mcu_periph/sys_time.h"
-#include "led.h"
-
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "subsystems/datalink/downlink.h"
-
-#include "ADS8344.h"
-
-static inline void main_init( void );
-static inline void main_periodic( void );
-static inline void main_event( void );
-
-int main( void ) {
- main_init();
- while(1) {
- if (sys_time_check_and_ack_timer(0))
- main_periodic();
- main_event();
- }
- return 0;
-}
-
-static inline void main_init( void ) {
- mcu_init();
-
- sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
-
- led_init();
-
- uart1_init_tx();
-
- ADS8344_init();
-
- mcu_int_enable();
-
- ADS8344_start();
-}
-
-
-static inline void main_periodic( void ) {
- DOWNLINK_SEND_BOOT(&sys_time.nb_sec);
-
-}
-
-static inline void main_event( void ) {
- if ( ADS8344_available ) {
- float foo = ADS8344_values[0];
- float foo1 = ADS8344_values[1];
- float foo2 = ADS8344_values[2];
- DOWNLINK_SEND_IMU_GYRO(&foo, &foo1, &foo2);
- // LED_TOGGLE(2);
- ADS8344_available = FALSE;
- }
-}
diff --git a/sw/airborne/booz/test/booz2_test_usb.c b/sw/airborne/test/peripherals/test_lis302dl_spi.c
similarity index 50%
rename from sw/airborne/booz/test/booz2_test_usb.c
rename to sw/airborne/test/peripherals/test_lis302dl_spi.c
index c6f4912bc8..e08913cfa3 100644
--- a/sw/airborne/booz/test/booz2_test_usb.c
+++ b/sw/airborne/test/peripherals/test_lis302dl_spi.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2013 Felix Ruess
*
* This file is part of paparazzi.
*
@@ -19,53 +19,85 @@
* Boston, MA 02111-1307, USA.
*/
-#include
+/**
+ * @file test/peripherals/test_lis302dl_spi.c
+ *
+ * Test for LIS302DL 3-axis accelerometer from ST using SPI.
+ */
-#include "std.h"
+#include BOARD_CONFIG
#include "mcu.h"
#include "mcu_periph/sys_time.h"
+#include "mcu_periph/uart.h"
+#include "subsystems/datalink/downlink.h"
#include "led.h"
-#include "mcu_periph/usb_serial.h"
+#include "peripherals/lis302dl_spi.h"
-#include "messages.h"
-#include "subsystems/datalink/downlink.h"
+#ifndef LIS302DL_SPI_DEV
+#define LIS302DL_SPI_DEV spi2
+#endif
+PRINT_CONFIG_VAR(LIS302DL_SPI_DEV)
-#include "interrupt_hw.h"
+#ifndef LIS302DL_SPI_SLAVE_IDX
+#define LIS302DL_SPI_SLAVE_IDX SPI_SLAVE2
+#endif
+PRINT_CONFIG_VAR(LIS302DL_SPI_SLAVE_IDX)
+struct Lis302dl_Spi lis302;
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
-int main( void ) {
+int main(void) {
main_init();
+
while(1) {
if (sys_time_check_and_ack_timer(0))
main_periodic_task();
main_event_task();
}
+
return 0;
}
+
static inline void main_init( void ) {
mcu_init();
- sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
- led_init();
-
- VCOM_init();
-
mcu_int_enable();
+
+ sys_time_register_timer((1./50), NULL);
+
+ lis302dl_spi_init(&lis302, &(LIS302DL_SPI_DEV), LIS302DL_SPI_SLAVE_IDX);
}
static inline void main_periodic_task( void ) {
- RunOnceEvery(10, {
- LED_TOGGLE(1);
- DOWNLINK_SEND_ALIVE(PprzTransport,16, MD5SUM);
- });
+ RunOnceEvery(100, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM));
+
+ if (sys_time.nb_sec > 1) {
+ lis302dl_spi_periodic(&lis302);
+#if USE_LED_5
+ RunOnceEvery(10, LED_TOGGLE(5););
+#endif
+ }
}
static inline void main_event_task( void ) {
+ if (sys_time.nb_sec > 1)
+ lis302dl_spi_event(&lis302);
+ if (lis302.data_available) {
+ struct Int32Vect3 accel;
+ VECT3_COPY(accel, lis302.data.vect);
+ lis302.data_available = FALSE;
+
+ RunOnceEvery(10, {
+ DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
+ &accel.x, &accel.y, &accel.z);
+#if USE_LED_6
+ LED_TOGGLE(6);
+#endif
+ });
+ }
}
-
diff --git a/sw/airborne/test/stabilization/.gitignore b/sw/airborne/test/stabilization/.gitignore
new file mode 100644
index 0000000000..7f9b268c78
--- /dev/null
+++ b/sw/airborne/test/stabilization/.gitignore
@@ -0,0 +1,6 @@
+*.so
+algebra_int.c
+algebra_float.c
+ref_quat_float.c
+ref_quat_int.c
+build
diff --git a/sw/airborne/test/stabilization/README.md b/sw/airborne/test/stabilization/README.md
new file mode 100644
index 0000000000..4ede9e79f0
--- /dev/null
+++ b/sw/airborne/test/stabilization/README.md
@@ -0,0 +1,11 @@
+Some Cython wrappers for stabilization reference generation testing.
+
+Tested with Cython 0.19
+
+To build execute in this directory:
+
+ python setup.py build_ext --inplace
+
+Run the example script to compare float and int ref quat implementations:
+
+ ./compare_ref_quat.py
diff --git a/sw/airborne/test/stabilization/algebra_float.pxd b/sw/airborne/test/stabilization/algebra_float.pxd
new file mode 100644
index 0000000000..55791c0601
--- /dev/null
+++ b/sw/airborne/test/stabilization/algebra_float.pxd
@@ -0,0 +1,10 @@
+cimport pprz_algebra_float_c
+
+cdef class FloatEulers:
+ cdef pprz_algebra_float_c.FloatEulers *ptr
+
+cdef class FloatQuat:
+ cdef pprz_algebra_float_c.FloatQuat *ptr
+
+cdef class FloatRates:
+ cdef pprz_algebra_float_c.FloatRates *ptr
diff --git a/sw/airborne/test/stabilization/algebra_float.pyx b/sw/airborne/test/stabilization/algebra_float.pyx
new file mode 100644
index 0000000000..349ddcf0d3
--- /dev/null
+++ b/sw/airborne/test/stabilization/algebra_float.pyx
@@ -0,0 +1,118 @@
+cdef extern from "stdint.h":
+ ctypedef unsigned long int uintptr_t
+
+cimport pprz_algebra_float_c
+cimport algebra_float
+cimport numpy as np
+import numpy as np
+
+
+cdef class FloatRates:
+ # already declared in pxd
+ #cdef pprz_algebra_float_c.FloatRates *ptr
+
+ def __cinit__(self, uintptr_t p):
+ self.ptr = p
+
+ property array:
+ def __get__(self):
+ return np.asarray([self.ptr[0].p, self.ptr[0].q, self.ptr[0].r])
+ def __set__(self, np.ndarray a):
+ self.ptr[0].p = a[0]
+ self.ptr[0].q = a[1]
+ self.ptr[0].r = a[2]
+
+ property p:
+ def __get__(self):
+ return self.ptr[0].p
+ def __set__(self, v):
+ self.ptr[0].p = v
+
+ property q:
+ def __get__(self):
+ return self.ptr[0].q
+ def __set__(self, v):
+ self.ptr[0].q = v
+
+ property r:
+ def __get__(self):
+ return self.ptr[0].r
+ def __set__(self, v):
+ self.ptr[0].r = v
+
+
+cdef class FloatEulers:
+ # already declared in pxd
+ #cdef pprz_algebra_float_c.FloatEulers *ptr
+
+ def __cinit__(self, uintptr_t p):
+ self.ptr = p
+
+ property array:
+ def __get__(self):
+ return np.asarray([self.ptr[0].phi, self.ptr[0].theta,
+ self.ptr[0].psi])
+ def __set__(self, np.ndarray a):
+ self.ptr[0].phi = a[0]
+ self.ptr[0].theta = a[1]
+ self.ptr[0].psi = a[2]
+
+ property phi:
+ def __get__(self):
+ return self.ptr[0].phi
+ def __set__(self, v):
+ self.ptr[0].phi = v
+
+ property theta:
+ def __get__(self):
+ return self.ptr[0].theta
+ def __set__(self, v):
+ self.ptr[0].theta = v
+
+ property psi:
+ def __get__(self):
+ return self.ptr[0].psi
+ def __set__(self, v):
+ self.ptr[0].psi = v
+
+
+cdef class FloatQuat:
+ # already declared in pxd
+ #cdef pprz_algebra_float_c.FloatQuat *ptr
+
+ def __cinit__(self, uintptr_t p):
+ self.ptr = p
+
+ property array:
+ def __get__(self):
+ return np.asarray([self.ptr[0].qi, self.ptr[0].qx,
+ self.ptr[0].qy, self.ptr[0].qz])
+ def __set__(self, np.ndarray a):
+ self.ptr[0].qi = a[0]
+ self.ptr[0].qx = a[1]
+ self.ptr[0].qy = a[2]
+ self.ptr[0].qz = a[3]
+
+ property qi:
+ def __get__(self):
+ return self.ptr[0].qi
+ def __set__(self, v):
+ self.ptr[0].qi = v
+
+ property qx:
+ def __get__(self):
+ return self.ptr[0].qx
+ def __set__(self, v):
+ self.ptr[0].qx = v
+
+ property qy:
+ def __get__(self):
+ return self.ptr[0].qy
+ def __set__(self, v):
+ self.ptr[0].qy = v
+
+ property qz:
+ def __get__(self):
+ return self.ptr[0].qz
+ def __set__(self, v):
+ self.ptr[0].qz = v
diff --git a/sw/airborne/test/stabilization/algebra_int.pxd b/sw/airborne/test/stabilization/algebra_int.pxd
new file mode 100644
index 0000000000..c164fc8418
--- /dev/null
+++ b/sw/airborne/test/stabilization/algebra_int.pxd
@@ -0,0 +1,10 @@
+cimport pprz_algebra_int_c
+
+cdef class Int32Eulers:
+ cdef pprz_algebra_int_c.Int32Eulers *ptr
+
+cdef class Int32Quat:
+ cdef pprz_algebra_int_c.Int32Quat *ptr
+
+cdef class Int32Rates:
+ cdef pprz_algebra_int_c.Int32Rates *ptr
diff --git a/sw/airborne/test/stabilization/algebra_int.pyx b/sw/airborne/test/stabilization/algebra_int.pyx
new file mode 100644
index 0000000000..0c55f6ae85
--- /dev/null
+++ b/sw/airborne/test/stabilization/algebra_int.pyx
@@ -0,0 +1,112 @@
+cdef extern from "stdint.h":
+ ctypedef unsigned long int uintptr_t
+
+cimport pprz_algebra_int_c
+cimport algebra_int
+cimport numpy as np
+import numpy as np
+
+
+cdef class Int32Rates:
+
+ def __cinit__(self, uintptr_t p):
+ self.ptr = p
+
+ property array:
+ def __get__(self):
+ return np.asarray([self.ptr[0].p, self.ptr[0].q, self.ptr[0].r])
+ def __set__(self, np.ndarray a):
+ self.ptr[0].p = a[0]
+ self.ptr[0].q = a[1]
+ self.ptr[0].r = a[2]
+
+ property p:
+ def __get__(self):
+ return self.ptr[0].p
+ def __set__(self, v):
+ self.ptr[0].p = v
+
+ property q:
+ def __get__(self):
+ return self.ptr[0].q
+ def __set__(self, v):
+ self.ptr[0].q = v
+
+ property r:
+ def __get__(self):
+ return self.ptr[0].r
+ def __set__(self, v):
+ self.ptr[0].r = v
+
+
+cdef class Int32Eulers:
+
+ def __cinit__(self, uintptr_t p):
+ self.ptr = p
+
+ property array:
+ def __get__(self):
+ return np.asarray([self.ptr[0].phi, self.ptr[0].theta,
+ self.ptr[0].psi])
+ def __set__(self, np.ndarray a):
+ self.ptr[0].phi = a[0]
+ self.ptr[0].theta = a[1]
+ self.ptr[0].psi = a[2]
+
+ property phi:
+ def __get__(self):
+ return self.ptr[0].phi
+ def __set__(self, v):
+ self.ptr[0].phi = v
+
+ property theta:
+ def __get__(self):
+ return self.ptr[0].theta
+ def __set__(self, v):
+ self.ptr[0].theta = v
+
+ property psi:
+ def __get__(self):
+ return self.ptr[0].psi
+ def __set__(self, v):
+ self.ptr[0].psi = v
+
+
+cdef class Int32Quat:
+
+ def __cinit__(self, uintptr_t p):
+ self.ptr = p
+
+ property array:
+ def __get__(self):
+ return np.asarray([self.ptr[0].qi, self.ptr[0].qx,
+ self.ptr[0].qy, self.ptr[0].qz])
+ def __set__(self, np.ndarray a):
+ self.ptr[0].qi = a[0]
+ self.ptr[0].qx = a[1]
+ self.ptr[0].qy = a[2]
+ self.ptr[0].qz = a[3]
+
+ property qi:
+ def __get__(self):
+ return self.ptr[0].qi
+ def __set__(self, v):
+ self.ptr[0].qi = v
+
+ property qx:
+ def __get__(self):
+ return self.ptr[0].qx
+ def __set__(self, v):
+ self.ptr[0].qx = v
+
+ property qy:
+ def __get__(self):
+ return self.ptr[0].qy
+ def __set__(self, v):
+ self.ptr[0].qy = v
+
+ property qz:
+ def __get__(self):
+ return self.ptr[0].qz
+ def __set__(self, v):
+ self.ptr[0].qz = v
diff --git a/sw/airborne/test/stabilization/compare_ref_quat.py b/sw/airborne/test/stabilization/compare_ref_quat.py
new file mode 100755
index 0000000000..76fc8d3a15
--- /dev/null
+++ b/sw/airborne/test/stabilization/compare_ref_quat.py
@@ -0,0 +1,50 @@
+#! /usr/bin/env python
+
+from __future__ import division, print_function, absolute_import
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+import ref_quat_float
+import ref_quat_int
+
+steps = 512 * 2
+ref_eul_res = np.zeros((steps, 3))
+ref_quat_res = np.zeros((steps, 3))
+
+ref_quat_float.init()
+ref_quat_int.init()
+
+# reset psi and update_ref_quat_from_eulers
+ref_quat_float.enter()
+ref_quat_int.enter()
+
+q_sp = np.array([0.92387956, 0.38268346, 0., 0.])
+ref_quat_float.sp_quat.array = q_sp
+ref_quat_int.sp_quat.array = q_sp * (1 << 15)
+
+for i in range(0, steps):
+ ref_quat_float.update()
+ ref_eul_res[i, :] = ref_quat_float.ref_euler.array
+ ref_quat_int.update()
+ ref_quat_res[i, :] = ref_quat_int.ref_euler.array / (1 << 20)
+
+
+plt.figure(1)
+plt.subplot(311)
+plt.title("reference in euler angles")
+plt.plot(np.degrees(ref_eul_res[:, 0]), 'g')
+plt.plot(np.degrees(ref_quat_res[:, 0]), 'r')
+plt.ylabel("phi [deg]")
+
+plt.subplot(312)
+plt.plot(np.degrees(ref_eul_res[:, 1]), 'g')
+plt.plot(np.degrees(ref_quat_res[:, 1]), 'r')
+plt.ylabel("theta [deg]")
+
+plt.subplot(313)
+plt.plot(np.degrees(ref_eul_res[:, 2]), 'g')
+plt.plot(np.degrees(ref_quat_res[:, 2]), 'r')
+plt.ylabel("psi [deg]")
+
+plt.show()
diff --git a/sw/airborne/test/stabilization/generated/airframe.h b/sw/airborne/test/stabilization/generated/airframe.h
new file mode 100644
index 0000000000..6ed6afe37f
--- /dev/null
+++ b/sw/airborne/test/stabilization/generated/airframe.h
@@ -0,0 +1,51 @@
+/* fake generated airframe file */
+
+#ifndef AIRFRAME_H
+#define AIRFRAME_H
+
+#define PERIODIC_FREQUENCY 512
+
+#if defined STABILIZATION_ATTITUDE_TYPE_FLOAT
+
+#define SECTION_STABILIZATION_ATTITUDE 1
+#define STABILIZATION_ATTITUDE_SP_MAX_PHI 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
+
+#define STABILIZATION_ATTITUDE_REF_OMEGA_P {RadOfDeg(800)}
+#define STABILIZATION_ATTITUDE_REF_ZETA_P {0.85}
+#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_Q {RadOfDeg(800)}
+#define STABILIZATION_ATTITUDE_REF_ZETA_Q {0.85}
+#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_R {RadOfDeg(500)}
+#define STABILIZATION_ATTITUDE_REF_ZETA_R {0.85}
+#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
+#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
+
+#elif defined STABILIZATION_ATTITUDE_TYPE_INT
+
+#define SECTION_STABILIZATION_ATTITUDE 1
+#define STABILIZATION_ATTITUDE_SP_MAX_PHI 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
+
+#define STABILIZATION_ATTITUDE_REF_OMEGA_P 13.962634
+#define STABILIZATION_ATTITUDE_REF_ZETA_P 0.85
+#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_Q 13.962634
+#define STABILIZATION_ATTITUDE_REF_ZETA_Q 0.85
+#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_R 8.72664625
+#define STABILIZATION_ATTITUDE_REF_ZETA_R 0.85
+#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
+#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
+
+#endif
+
+
+#endif // AIRFRAME_H
diff --git a/sw/airborne/test/stabilization/pprz_algebra_double_c.pxd b/sw/airborne/test/stabilization/pprz_algebra_double_c.pxd
new file mode 100644
index 0000000000..e2204d25b8
--- /dev/null
+++ b/sw/airborne/test/stabilization/pprz_algebra_double_c.pxd
@@ -0,0 +1,29 @@
+cdef extern from "math/pprz_algebra_double.h":
+
+ struct DoubleVect2
+ double x
+ double y
+
+ struct DoubleVect3
+ double x
+ double y
+ double z
+
+ struct DoubleEulers:
+ double phi
+ double theta
+ double psi
+
+ struct DoubleQuat:
+ double qi
+ double qx
+ double qy
+ double qz
+
+ struct DoubleRates:
+ double p
+ double q
+ double r
+
+ struct DoubleRMat:
+ double m[3*3]
diff --git a/sw/airborne/test/stabilization/pprz_algebra_float_c.pxd b/sw/airborne/test/stabilization/pprz_algebra_float_c.pxd
new file mode 100644
index 0000000000..62e4d9090f
--- /dev/null
+++ b/sw/airborne/test/stabilization/pprz_algebra_float_c.pxd
@@ -0,0 +1,29 @@
+cdef extern from "math/pprz_algebra_float.h":
+
+ struct FloatVect2:
+ float x
+ float y
+
+ struct FloatVect3:
+ float x
+ float y
+ float z
+
+ struct FloatEulers:
+ float phi
+ float theta
+ float psi
+
+ struct FloatQuat:
+ float qi
+ float qx
+ float qy
+ float qz
+
+ struct FloatRates:
+ float p
+ float q
+ float r
+
+ struct FloatRMat:
+ float m[3*3]
diff --git a/sw/airborne/test/stabilization/pprz_algebra_int_c.pxd b/sw/airborne/test/stabilization/pprz_algebra_int_c.pxd
new file mode 100644
index 0000000000..c85698ad32
--- /dev/null
+++ b/sw/airborne/test/stabilization/pprz_algebra_int_c.pxd
@@ -0,0 +1,34 @@
+from libc.stdint cimport uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t
+
+cdef extern from "math/pprz_algebra_int.h":
+
+ struct Int32Vect2:
+ int32_t x
+ int32_t y
+
+ struct Int32Vect3:
+ int32_t x
+ int32_t y
+ int32_t z
+
+ struct Int32Eulers:
+ int32_t phi
+ int32_t theta
+ int32_t psi
+
+ struct Int32Quat:
+ int32_t qi
+ int32_t qx
+ int32_t qy
+ int32_t qz
+
+ struct Int32Rates:
+ int32_t p
+ int32_t q
+ int32_t r
+
+ struct Int32RMat:
+ int32_t m[3*3]
+
+cdef extern from "math/pprz_trig_int.h":
+ int16_t pprz_trig_int[]
diff --git a/sw/airborne/test/stabilization/ref_quat_float.pyx b/sw/airborne/test/stabilization/ref_quat_float.pyx
new file mode 100644
index 0000000000..1509ee3e87
--- /dev/null
+++ b/sw/airborne/test/stabilization/ref_quat_float.pyx
@@ -0,0 +1,60 @@
+cdef extern from "stdint.h":
+ ctypedef unsigned long int uintptr_t
+
+
+import numpy as np
+cimport numpy as np
+
+cimport pprz_algebra_float_c
+from algebra_float import FloatQuat, FloatEulers, FloatRates
+
+
+cdef extern from "stabilization/stabilization_attitude_ref_quat_float.h":
+ void stabilization_attitude_ref_init()
+ void stabilization_attitude_ref_update()
+ void stabilization_attitude_ref_enter()
+ void stabilization_attitude_ref_schedule(int idx)
+
+ pprz_algebra_float_c.FloatEulers stab_att_sp_euler
+ pprz_algebra_float_c.FloatQuat stab_att_sp_quat
+ pprz_algebra_float_c.FloatEulers stab_att_ref_euler
+ pprz_algebra_float_c.FloatQuat stab_att_ref_quat
+ pprz_algebra_float_c.FloatRates stab_att_ref_rate
+ pprz_algebra_float_c.FloatRates stab_att_ref_accel
+
+
+sp_euler = FloatEulers( &stab_att_sp_euler)
+sp_quat = FloatQuat( &stab_att_sp_quat)
+ref_euler = FloatEulers( &stab_att_ref_euler)
+ref_quat = FloatQuat( &stab_att_ref_quat)
+ref_rate = FloatRates( &stab_att_ref_rate)
+ref_accel = FloatRates( &stab_att_ref_accel)
+
+
+def init():
+ stabilization_attitude_ref_init()
+
+def enter():
+ stabilization_attitude_ref_enter()
+
+def update():
+ stabilization_attitude_ref_update()
+
+"""
+# test some different set methods
+def set_sp_euler(**kwds):
+ global stab_att_sp_euler
+ cdef pprz_algebra_float_c.FloatEulers c_sp = kwds
+ stab_att_sp_euler = c_sp
+
+def set_sp_euler(phi=0.0, theta=0.0, psi=0.0):
+ global sp_euler
+ sp_euler.phi = phi
+ sp_euler.theta = theta
+ sp_euler.psi = psi
+
+def set_sp_quat(**kwds):
+ global stab_att_sp_quat
+ cdef pprz_algebra_float_c.FloatQuat c_sp = kwds
+ stab_att_sp_quat = c_sp
+"""
diff --git a/sw/airborne/test/stabilization/ref_quat_int.pyx b/sw/airborne/test/stabilization/ref_quat_int.pyx
new file mode 100644
index 0000000000..0e4d265495
--- /dev/null
+++ b/sw/airborne/test/stabilization/ref_quat_int.pyx
@@ -0,0 +1,37 @@
+cdef extern from "stdint.h":
+ ctypedef unsigned long int uintptr_t
+
+
+cimport pprz_algebra_int_c
+from algebra_int import Int32Quat, Int32Eulers, Int32Rates
+
+
+cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h":
+ void stabilization_attitude_ref_init()
+ void stabilization_attitude_ref_update()
+ void stabilization_attitude_ref_enter()
+
+ pprz_algebra_int_c.Int32Eulers stab_att_sp_euler
+ pprz_algebra_int_c.Int32Quat stab_att_sp_quat
+ pprz_algebra_int_c.Int32Eulers stab_att_ref_euler
+ pprz_algebra_int_c.Int32Quat stab_att_ref_quat
+ pprz_algebra_int_c.Int32Rates stab_att_ref_rate
+ pprz_algebra_int_c.Int32Rates stab_att_ref_accel
+
+
+sp_euler = Int32Eulers( &stab_att_sp_euler)
+sp_quat = Int32Quat( &stab_att_sp_quat)
+ref_euler = Int32Eulers( &stab_att_ref_euler)
+ref_quat = Int32Quat( &stab_att_ref_quat)
+ref_rate = Int32Rates( &stab_att_ref_rate)
+ref_accel = Int32Rates( &stab_att_ref_accel)
+
+
+def init():
+ stabilization_attitude_ref_init()
+
+def enter():
+ stabilization_attitude_ref_enter()
+
+def update():
+ stabilization_attitude_ref_update()
diff --git a/sw/airborne/test/stabilization/setup.py b/sw/airborne/test/stabilization/setup.py
new file mode 100644
index 0000000000..f3aa9d8b0b
--- /dev/null
+++ b/sw/airborne/test/stabilization/setup.py
@@ -0,0 +1,38 @@
+from distutils.core import setup, Extension
+from Cython.Build import cythonize
+from distutils.extension import Extension
+import numpy
+
+from os import path, getenv
+
+# if PAPARAZZI_SRC not set, then assume the tree containing this
+# file is a reasonable substitute
+pprz_src = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
+pprz_airborne = path.join(pprz_src, "sw/airborne")
+
+common_inc_dirs = ["./", path.join(pprz_src, "sw/include"), pprz_airborne, numpy.get_include()]
+
+algebra_float = Extension("algebra_float", sources=['algebra_float.pyx'],
+ include_dirs=common_inc_dirs)
+
+algebra_int = Extension("algebra_int", sources=['algebra_int.pyx', path.join(pprz_airborne, 'math/pprz_trig_int.c')],
+ include_dirs=common_inc_dirs)
+
+includedirs = common_inc_dirs + [path.join(pprz_airborne, "firmwares/rotorcraft")]
+ext_quat_float = Extension("ref_quat_float",
+ sources=['ref_quat_float.pyx',
+ path.join(pprz_airborne, "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c")],
+ include_dirs=includedirs,
+ extra_compile_args=["-std=c99", "-DSTABILIZATION_ATTITUDE_TYPE_FLOAT"])
+ext_quat_int = Extension("ref_quat_int",
+ sources=['ref_quat_int.pyx',
+ path.join(pprz_airborne, 'math/pprz_trig_int.c'),
+ path.join(pprz_airborne, "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c")],
+ include_dirs=includedirs,
+ extra_compile_args=["-std=c99", "-DSTABILIZATION_ATTITUDE_TYPE_INT"])
+
+extensions = [algebra_float, algebra_int, ext_quat_float, ext_quat_int]
+
+setup(
+ ext_modules=cythonize(extensions)
+)
diff --git a/sw/extras/bootstrap_vm.sh b/sw/extras/bootstrap_vm.sh
new file mode 100644
index 0000000000..e890b35d73
--- /dev/null
+++ b/sw/extras/bootstrap_vm.sh
@@ -0,0 +1,20 @@
+#!/usr/bin/env bash
+
+export DEBIAN_FRONTEND=noninteractive
+apt-get update
+
+# install a desktop environment and login manager
+apt-get install -y xfce4 gdm
+dpkg-reconfigure gdm
+
+# default precise32 box doesn't have the add-apt-repository command, get it:
+apt-get install -y python-software-properties
+
+add-apt-repository ppa:paparazzi-uav/ppa
+add-apt-repository ppa:flixr/gcc-arm-embedded
+apt-get update
+apt-get install -y paparazzi-dev gcc-arm-none-eabi libcanberra-gtk-module
+
+
+# setup some git aliases for the vagrant user
+su -c /home/vagrant/paparazzi/sw/extras/setup_git_aliases.sh vagrant
diff --git a/sw/extras/setup_git_aliases.sh b/sw/extras/setup_git_aliases.sh
new file mode 100755
index 0000000000..923b96f4c7
--- /dev/null
+++ b/sw/extras/setup_git_aliases.sh
@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+
+git config --global color.ui auto # colors for all
+git config --global alias.st status # make `git st` work
+git config --global alias.co checkout # make `git co` work
+git config --global alias.ci commit # make `git ci` work
+git config --global alias.br branch # make `git br` work
+git config --global alias.up "pull --rebase" # make `git up` work similar to svn up
+git config --global alias.lg "log --graph --pretty=format:'%Cred%h%Creset -%C(yellow)%d%Creset %s %Cgreen(%cr) %C(bold blue)<%an>%Creset' --abbrev-commit --date=relative"
diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile
index 946ba2082a..dc832fa4ed 100644
--- a/sw/ground_segment/cockpit/Makefile
+++ b/sw/ground_segment/cockpit/Makefile
@@ -49,11 +49,11 @@ all : $(MAIN)
opt : $(MAIN).opt
-$(MAIN) : $(CMO)
+$(MAIN) : $(CMO) $(XLIBPPRZCMA)
@echo OL $@
$(Q)$(OCAMLC) $(OCAMLCFLAGS) $(INCLUDES) $(LIBS) $(LINKPKG) myGtkInit.cmo $(CMO) -o $@
-$(MAIN).opt : $(CMX)
+$(MAIN).opt : $(CMX) $(XLIBPPRZCMXA)
@echo OOL $@
$(Q)$(OCAMLOPT) $(OCAMLCFLAGS) $(INCLUDES) $(LIBSX) -package pprz.xlib,$(LABLGTK2INIT) -linkpkg $(CMX) -o $@
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 7600115d95..2949ca0ae8 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -383,6 +383,20 @@ let get_speech_name = fun af_xml def_name ->
fvalue "SPEECH_NAME" default_speech_name
with _ -> default_speech_name
+let get_icon_and_track_size = fun af_xml ->
+ (* firmware name as default if fixedwing or rotorcraft *)
+ let firmware = ExtXml.child af_xml "firmware" in
+ let firmware_name = ExtXml.attrib firmware "name" in
+ try
+ (* search AC_ICON in GCS section *)
+ let gcs_section = ExtXml.child af_xml ~select:(fun x -> Xml.attrib x "name" = "GCS") "section" in
+ let fvalue = fun name default ->
+ try ExtXml.attrib (ExtXml.child gcs_section ~select:(fun x -> ExtXml.attrib x "name" = name) "define") "value" with _ -> default in
+ match fvalue "AC_ICON" "fixedwing" with
+ | "home" -> ("home", 1) (* no track for home icon *)
+ | x -> (x, !track_size)
+ with _ -> (firmware_name, !track_size)
+
let key_press_event = fun keys do_action ev ->
try
let (modifiers, action) = List.assoc (GdkEvent.Key.keyval ev) keys in
@@ -429,7 +443,8 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
let fp_show = ac_menu_fact#add_check_item "Fligh Plan" ~active:true in
ignore (fp_show#connect#toggled (fun () -> show_mission ac_id fp_show#active));
- let track = new MapTrack.track ~size: !track_size ~name ~color:color geomap in
+ let (icon, size) = get_icon_and_track_size af_xml in
+ let track = new MapTrack.track ~size ~icon ~name ~color:color geomap in
geomap#register_to_fit (track:>MapCanvas.geographic);
let center_ac = center geomap track in
diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile
index 3159a90804..d0872d3788 100644
--- a/sw/lib/ocaml/Makefile
+++ b/sw/lib/ocaml/Makefile
@@ -59,7 +59,7 @@ SRC = fig.ml debug.ml base64.ml serial.ml ocaml_tools.ml expr_syntax.ml expr_par
CMO = $(SRC:.ml=.cmo)
CMX = $(SRC:.ml=.cmx)
-XSRC = gtk_tools.ml platform.ml wind_sock.ml gtk_papget_editor.ml gtk_papget_text_editor.ml gtk_papget_gauge_editor.ml gtk_papget_led_editor.ml papget_common.ml papget_renderer.ml papget.ml mapCanvas.ml mapWaypoints.ml mapTrack.ml mapGoogle.ml mapIGN.ml ml_gtk_drag.o xmlEdit.ml mapFP.ml
+XSRC = gtk_tools.ml platform.ml contrastLabel.ml acIcon.ml wind_sock.ml gtk_papget_editor.ml gtk_papget_text_editor.ml gtk_papget_gauge_editor.ml gtk_papget_led_editor.ml papget_common.ml papget_renderer.ml papget.ml mapCanvas.ml mapWaypoints.ml mapTrack.ml mapGoogle.ml mapIGN.ml ml_gtk_drag.o xmlEdit.ml mapFP.ml
XCMO = $(XSRC:.ml=.cmo)
XCMX = $(XSRC:.ml=.cmx)
diff --git a/sw/lib/ocaml/acIcon.ml b/sw/lib/ocaml/acIcon.ml
new file mode 100644
index 0000000000..2477d54939
--- /dev/null
+++ b/sw/lib/ocaml/acIcon.ml
@@ -0,0 +1,82 @@
+(*
+ * A Label with a contrasting outline
+ *
+ * Copyright (C) 2013 Piotr Esden-Tempski
+ *
+ * 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.
+ *
+ *)
+
+type icon = {
+ lines : float array list;
+ ellipse : float array list;
+ width: int
+}
+
+let icon_fixedwing_template = {
+ lines = [
+ [| 0.; -6.; 0.; 14.|];
+ [| -9.; 0.; 9.; 0.|];
+ [| -4.; 10.; 4.; 10.|]
+ ];
+ ellipse = [];
+ width = 4
+}
+
+let icon_rotorcraft_template = {
+ lines = [
+ [| 0.; -8.; 0.; 8.|];
+ [| -8.; 0.; 8.; 0.|];
+ [| 6.; -15.; 0.; -24.; -6.; -15.|];
+ ];
+ ellipse = [
+ [| 8.; -5.; 18.; 5.|];
+ [| -8.; -5.; -18.; 5.|];
+ [| -5.; 8.; 5.; 18.|];
+ [| -5.; -8.; 5.; -18.|];
+ ];
+ width = 2
+}
+
+let icon_home_template = {
+ lines = [
+ [| -9.; -9.; -9.; 9.; 9.; 9.; 9.; -9.|];
+ [| -12.; -7.; 0.; -15.; 12.; -7.|];
+ ];
+ ellipse = [];
+ width = 3;
+}
+
+class widget = fun ?(color="red") ?(icon_template=icon_fixedwing_template) (group:GnoCanvas.group) ->
+ let new_line width color points =
+ GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS width; `CAP_STYLE `ROUND] ~points:points group in
+ let new_ellipse width color points =
+ GnoCanvas.ellipse ~props:[`OUTLINE_COLOR color; `WIDTH_PIXELS width] ~x1:points.(0) ~y1:points.(1) ~x2:points.(2) ~y2:points.(3) group in
+ let icon_bg =
+ (List.map (fun points -> new_line (icon_template.width+2) "black" points) icon_template.lines,
+ List.map (fun points -> new_ellipse (icon_template.width+2) "black" points) icon_template.ellipse) in
+ let icon =
+ (List.map (fun points -> new_line icon_template.width color points) icon_template.lines,
+ List.map (fun points -> new_ellipse icon_template.width color points) icon_template.ellipse) in
+object(self)
+ method set_color color =
+ List.iter2 (fun segment ellipse -> segment#set [`FILL_COLOR color]; ellipse#set [`FILL_COLOR color]) (fst icon) (snd icon)
+ method set_bg_color color =
+ List.iter2 (fun segment ellipse -> segment#set [`FILL_COLOR color]; ellipse#set [`FILL_COLOR color]) (fst icon_bg) (snd icon_bg)
+end
+
diff --git a/sw/lib/ocaml/acIcon.mli b/sw/lib/ocaml/acIcon.mli
new file mode 100644
index 0000000000..6d2c1f46e3
--- /dev/null
+++ b/sw/lib/ocaml/acIcon.mli
@@ -0,0 +1,43 @@
+(*
+ * A Label with a contrasting outline
+ *
+ * Copyright (C) 2013 Piotr Esden-Tempski
+ *
+ * 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.
+ *
+ *)
+
+type icon = {
+ lines : float array list;
+ ellipse : float array list;
+ width: int
+}
+
+val icon_fixedwing_template : icon
+val icon_rotorcraft_template : icon
+val icon_home_template : icon
+
+class widget :
+ ?color : string ->
+ ?icon_template : icon ->
+ GnoCanvas.group ->
+object
+ method set_color : string -> unit
+ method set_bg_color : string -> unit
+end
+
diff --git a/sw/lib/ocaml/contrastLabel.ml b/sw/lib/ocaml/contrastLabel.ml
new file mode 100644
index 0000000000..904f05f16c
--- /dev/null
+++ b/sw/lib/ocaml/contrastLabel.ml
@@ -0,0 +1,60 @@
+(*
+ * A Label with a contrasting outline
+ *
+ * Copyright (C) 2013 Piotr Esden-Tempski
+ *
+ * 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.
+ *
+ *)
+
+(*
+ * This module creates labels with outlines by creating 9
+ * overlapping labels slightly offset from eachother. Where the 8
+ * labels in the background have a different color from last center one.
+ *)
+
+let label_offset_matrix =
+ [
+ (* X Y *)
+ ( 0., -1.); (* N *)
+ ( 0., 1.); (* S *)
+ ( 1., 0.); (* E *)
+ (-1., 0.); (* W *)
+ ( 1., -1.); (* NE *)
+ ( 1., 1.); (* SE *)
+ (-1., 1.); (* SW *)
+ (-1., -1.); (* NW *)
+ ( 0., 0.); (* Z *)
+ ]
+
+class widget = fun ?(name = "Noname") ?(size = 500) ?(bg_color = "black") ?(color = "white") x y (group:GnoCanvas.group) ->
+ let new_text offset =
+ GnoCanvas.text group ~props:[`TEXT name;
+ `X (x +. (fst offset)); `Y (y +. (snd offset));
+ `ANCHOR `SW;
+ `FILL_COLOR (if offset = (0., 0.) then color else bg_color)] in
+ let labels = List.map new_text label_offset_matrix in
+object(self)
+ method set_name s = List.iter (fun label -> label#set [`TEXT s]) labels
+ method set_x x = List.iter2 (fun label offset -> label#set [`X (x +. (fst offset))])
+ labels label_offset_matrix
+ method set_y y = List.iter2 (fun label offset -> label#set [`Y (y +. (snd offset))])
+ labels label_offset_matrix
+ method affine_absolute a = List.iter (fun label -> label#affine_absolute a) labels
+end
+
diff --git a/sw/lib/ocaml/contrastLabel.mli b/sw/lib/ocaml/contrastLabel.mli
new file mode 100644
index 0000000000..0f26172da2
--- /dev/null
+++ b/sw/lib/ocaml/contrastLabel.mli
@@ -0,0 +1,39 @@
+(*
+ * A Label with a contrasting outline
+ *
+ * Copyright (C) 2013 Piotr Esden-Tempski
+ *
+ * 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.
+ *
+ *)
+
+class widget :
+ ?name:string ->
+ ?size:int ->
+ ?bg_color:string ->
+ ?color:string ->
+ float ->
+ float ->
+ GnoCanvas.group ->
+ object
+ method set_name : string -> unit
+ method set_x : float -> unit
+ method set_y : float -> unit
+ method affine_absolute : float array -> unit
+ end
+
diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml
index 41891434d4..cb5fe9a4c3 100644
--- a/sw/lib/ocaml/mapTrack.ml
+++ b/sw/lib/ocaml/mapTrack.ml
@@ -28,6 +28,9 @@ module LL = Latlong
module G = MapCanvas
+module CL = ContrastLabel
+module ACI = AcIcon
+
let affine_pos_and_angle z xw yw angle =
let rad_angle = angle /. 180. *. acos(-1.) in
let cos_a = cos rad_angle in
@@ -50,20 +53,21 @@ type desired =
| DesiredCircle of LL.geographic*float*GnoCanvas.ellipse
| DesiredSegment of LL.geographic*LL.geographic*GnoCanvas.line
-
-class track = fun ?(name="Noname") ?(size = 500) ?(color="red") (geomap:MapCanvas.widget) ->
+class track = fun ?(name="Noname") ?(icon="fixedwing") ?(size = 500) ?(color="red") (geomap:MapCanvas.widget) ->
let group = GnoCanvas.group geomap#canvas#root in
let empty = ({LL.posn_lat=0.; LL.posn_long=0.}, GnoCanvas.line group) in
let v_empty = ({LL.posn_lat=0.; LL.posn_long=0.}, 0.0) in
let aircraft = GnoCanvas.group group
and track = GnoCanvas.group group in
- let _ac_icon =
- ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 4;`CAP_STYLE `ROUND] ~points:[|0.;-6.;0.;14.|] aircraft);
- ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 4;`CAP_STYLE `ROUND] ~points:[|-9.;0.;9.;0.|] aircraft);
- ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 4;`CAP_STYLE `ROUND] ~points:[|-4.;10.;4.;10.|] aircraft) in
- let ac_label =
- GnoCanvas.text group ~props:[`TEXT name; `X 25.; `Y 25.; `ANCHOR `SW; `FILL_COLOR color] in
+ let icon_template = match icon with
+ | "home" -> ACI.icon_home_template
+ | "rotorcraft" -> ACI.icon_rotorcraft_template
+ | "fixedwing" | _ -> ACI.icon_fixedwing_template
+ in
+ let _ac_icon = new ACI.widget ~color ~icon_template aircraft in
+ let ac_label = new CL.widget ~name ~color 25. 25. group in
+
let carrot = GnoCanvas.group group in
let _ac_carrot =
ignore (GnoCanvas.polygon ~points:[|0.;0.;-5.;-10.;5.;-10.|] ~props:[`WIDTH_UNITS 1.;`FILL_COLOR "orange"; `OUTLINE_COLOR "orange"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] carrot) in
@@ -118,7 +122,8 @@ object (self)
method track = track
method v_path = v_path
method aircraft = aircraft
- method set_label = fun s -> ac_label#set [`TEXT s]
+ method set_label = fun s ->
+ ac_label#set_name s
method clear_one = fun i ->
if segments.(i) != empty then begin
(snd segments.(i))#destroy ();
@@ -150,7 +155,8 @@ object (self)
method set_params_state = fun b ->
params_on <- b;
if not b then (* Reset to the default simple label *)
- ac_label#set [`TEXT name; `Y 25.]
+ ac_label#set_name name;
+ ac_label#set_y 25.
method set_v_params_state = fun b -> v_params_on <- b
method set_last = fun x -> last <- x
method last = last
@@ -193,7 +199,8 @@ object (self)
if params_on then begin
let last_height = self#height () in
- ac_label#set [`TEXT (sprintf "%s\n%+.0f m\n%.1f m/s" name last_height last_speed); `Y 70. ]
+ ac_label#set_name (sprintf "%s\n%+.0f m\n%.1f m/s" name last_height last_speed);
+ ac_label#set_y 70.
end;
ac_label#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.);
diff --git a/sw/lib/ocaml/mapTrack.mli b/sw/lib/ocaml/mapTrack.mli
index e6baa8d015..807f2f31ed 100644
--- a/sw/lib/ocaml/mapTrack.mli
+++ b/sw/lib/ocaml/mapTrack.mli
@@ -24,6 +24,7 @@
class track :
?name:string ->
+ ?icon:string ->
?size:int ->
?color:string ->
MapCanvas.widget ->
diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index 2a84278e59..adc7231e0b 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -26,7 +26,12 @@ module LL = Latlong
open Printf
open LL
-let s = 6.
+module CL = ContrastLabel
+
+(*
+ * Waypoint label offsets
+ *)
+let s = 6. (* x offset *)
class group = fun ?(color="red") ?(editable=true) ?(show_moved=false) (geomap:MapCanvas.widget) ->
let g = GnoCanvas.group geomap#canvas#root in
@@ -66,7 +71,7 @@ object (self)
val mutable x0 = 0.
val mutable y0 = 0.
- val label = GnoCanvas.text wpt_group ~props:[`TEXT name; `X s; `Y 0.; `ANCHOR `SW; `FILL_COLOR "green"]
+ val label = new CL.widget ~name:name ~color:"white" s 0. wpt_group
val mutable name = name (* FIXME: already in label ! *)
val mutable alt = alt
val mutable moved = None
@@ -84,7 +89,7 @@ object (self)
method set_name n =
if n <> name then begin
name <- n;
- label#set [`TEXT name]
+ label#set_name name
end
method alt = alt
method label = label
@@ -150,7 +155,7 @@ object (self)
let callback = fun _ ->
self#set_name ename#text;
alt <- ea#value;
- label#set [`TEXT name];
+ label#set_name name;
set_coordinates ();
updated ();
if wpts_group#show_moved then
diff --git a/sw/lib/ocaml/mapWaypoints.mli b/sw/lib/ocaml/mapWaypoints.mli
index 4b79805265..25d9ec5fa5 100644
--- a/sw/lib/ocaml/mapWaypoints.mli
+++ b/sw/lib/ocaml/mapWaypoints.mli
@@ -48,7 +48,7 @@ class waypoint :
method pos : Latlong.geographic
method event : GnoCanvas.item_event -> bool
method item : GnoCanvas.rect
- method label : GnoCanvas.text
+ method label : ContrastLabel.widget
method move : float -> float -> unit
method name : string
method set : ?altitude:float -> ?update:bool -> Latlong.geographic -> unit
diff --git a/sw/simulator/old_booz/booz2_sim_main.c b/sw/simulator/old_booz/booz2_sim_main.c
deleted file mode 100644
index 4161279adb..0000000000
--- a/sw/simulator/old_booz/booz2_sim_main.c
+++ /dev/null
@@ -1,449 +0,0 @@
-/*
- * Copyright (C) 2008 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.
- *
- */
-
-#include
-#include
-#include
-
-#include
-#include
-
-#include "booz_flight_model.h"
-#include "booz_sensors_model.h"
-#include "booz_wind_model.h"
-#include "booz_rc_sim.h"
-#include "firmwares/rotorcraft/battery.h"
-
-#include "main.h"
-
-
-char* fg_host = "10.31.4.107";
-unsigned int fg_port = 5501;
-char* joystick_dev = "/dev/input/js0";
-
-/* rate of the host mainloop */
-#define HOST_TIMEOUT_PERIOD 4
-struct timeval host_time_start;
-double host_time_elapsed;
-double host_time_factor = 1.;
-
-/* 250Hz <-> 4ms */
-#define SIM_DT (1./512.)
-double sim_time;
-
-#define DT_DISPLAY 0.04
-double disp_time;
-
-double booz_sim_actuators_values[] = {0., 0., 0., 0.};
-
-static void sim_run_one_step(void);
-#ifdef BYPASS_AHRS
-static void sim_overwrite_ahrs(void);
-#endif
-#ifdef BYPASS_INS
-static void sim_overwrite_ins(void);
-#endif
-static void sim_gps_feed_data(void);
-static void sim_mag_feed_data(void);
-
-static void booz2_sim_parse_options(int argc, char** argv);
-static void booz2_sim_init(void);
-static gboolean booz2_sim_periodic(gpointer data);
-static void booz2_sim_display(void);
-static void booz_sim_read_actuators(void);
-
-static void ivy_transport_init(void);
-static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]);
-static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]);
-static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]);
-
-
-static void booz2_sim_init(void) {
-
- gettimeofday (&host_time_start, NULL);
- sim_time = 0.;
- disp_time = 0.;
-
- booz_flight_model_init();
-
- booz_sensors_model_init(sim_time);
-
- booz_wind_model_init();
-
- ivy_transport_init();
-
- main_init();
-
-}
-
-#include "booz2_analog_baro.h"
-#include "imu.h"
-
-static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) {
-
- struct timeval host_time_now;
- gettimeofday (&host_time_now, NULL);
- host_time_elapsed = host_time_factor *
- ((host_time_now.tv_sec - host_time_start.tv_sec) +
- (host_time_now.tv_usec - host_time_start.tv_usec)*1e-6);
-
- while (sim_time <= host_time_elapsed) {
- sim_run_one_step();
- sim_time += SIM_DT;
- }
-
-
-
- return TRUE;
-}
-
-
-#include "subsystems/ahrs.h"
-
-static void sim_run_one_step(void) {
-
- /* read actuators positions */
- booz_sim_read_actuators();
-
- /* run our models */
- if (sim_time > 13.)
- bfm.on_ground = FALSE;
-
- booz_wind_model_run(SIM_DT);
-
- booz_flight_model_run(SIM_DT, booz_sim_actuators_values);
-
- booz_sensors_model_run(sim_time);
- battery_voltage = bfm.bat_voltage * 10;
-
- /* outputs models state */
- booz2_sim_display();
-
- /* run the airborne code */
-
- // feed a rc frame and signal event
- BoozRcSimFeed(sim_time);
- // process it
- main_event();
-
- if (booz_sensors_model_baro_available()) {
- Booz2BaroISRHandler(bsm.baro);
- main_event();
-#ifdef BYPASS_INS
- sim_overwrite_ins();
-#endif /* BYPASS_INS */
- }
- if (booz_sensors_model_gyro_available()) {
- imu_feed_data();
- main_event();
-#ifdef BYPASS_AHRS
- sim_overwrite_ahrs();
-#endif /* BYPASS_AHRS */
-#ifdef BYPASS_INS
- sim_overwrite_ins();
-#endif /* BYPASS_INS */
- }
-
- if (booz_sensors_model_gps_available()) {
- sim_gps_feed_data();
- main_event();
- }
-
- if (booz_sensors_model_mag_available()) {
- sim_mag_feed_data();
- main_event();
-#ifdef BYPASS_AHRS
- sim_overwrite_ahrs();
-#endif /* BYPASS_AHRS */
- }
-
- main_periodic();
-
-
-}
-
-
-
-
-
-#ifdef BYPASS_AHRS
-#include "booz_geometry_mixed.h"
-#include "subsystems/ahrs.h"
-static void sim_overwrite_ahrs(void) {
- ahrs.ltp_to_body_euler.phi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_X]);
- ahrs.ltp_to_body_euler.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]);
- ahrs.ltp_to_body_euler.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]);
-
- ahrs.ltp_to_body_quat.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
- ahrs.ltp_to_body_quat.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
- ahrs.ltp_to_body_quat.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
- ahrs.ltp_to_body_quat.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
-
- ahrs.body_rate.p = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]);
- ahrs.body_rate.q = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]);
- ahrs.body_rate.r = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]);
-
-}
-#endif /* BYPASS_AHRS */
-
-
-#ifdef BYPASS_INS
-#include "subsystems/ins.h"
-static void sim_overwrite_ins(void) {
- ins_position.z = BOOZ_POS_I_OF_F(bfm.pos_ltp->ve[AXIS_Z]);
- ins_speed_earth.z = BOOZ_SPEED_I_OF_F(bfm.speed_ltp->ve[AXIS_Z]);
- ins_accel_earth.z = BOOZ_ACCEL_I_OF_F(bfm.accel_ltp->ve[AXIS_Z]);
-}
-#endif /* BYPASS_INS */
-
-
-
-
-#include "booz2_gps.h"
-static void sim_gps_feed_data(void) {
- // booz2_gps_lat = bsm.gps_pos_lla.lat;
- // booz2_gps_lon = bsm.gps_pos_lla.lon;
- // speed?
- // booz2_gps_vel_n = rint(bsm.gps_speed->ve[AXIS_X] * 100.);
- // booz2_gps_vel_e = rint(bsm.gps_speed->ve[AXIS_Y] * 100.);
-
-
- booz_gps_state.ecef_pos.x = rint(bsm.gps_pos_ecef.x);
- booz_gps_state.ecef_pos.y = rint(bsm.gps_pos_ecef.y);
- booz_gps_state.ecef_pos.z = rint(bsm.gps_pos_ecef.z);
- // VECT3_COPY(booz_gps_state.ecef_pos, bsm.gps_pos_ecef);
- VECT3_COPY(booz_gps_state.ecef_speed, bsm.gps_speed_ecef);
- booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
-
-
-}
-
-#include "AMI601.h"
-static void sim_mag_feed_data(void) {
- ami601_val[IMU_MAG_X_CHAN] = bsm.mag->ve[AXIS_X];
- ami601_val[IMU_MAG_Y_CHAN] = bsm.mag->ve[AXIS_Y];
- ami601_val[IMU_MAG_Z_CHAN] = bsm.mag->ve[AXIS_Z];
- ami601_status = AMI601_DATA_AVAILABLE;
-}
-
-
-
-#define RPM_OF_RAD_S(a) ((a)*60./M_PI)
-static void booz2_sim_display(void) {
- if (sim_time >= disp_time) {
- disp_time+= DT_DISPLAY;
- // booz_flightgear_send();
- IvySendMsg("%d NPS_RPMS %f %f %f %f",
- AC_ID,
- RPM_OF_RAD_S(bfm.omega->ve[SERVO_FRONT]),
- RPM_OF_RAD_S(bfm.omega->ve[SERVO_BACK]),
- RPM_OF_RAD_S(bfm.omega->ve[SERVO_RIGHT]),
- RPM_OF_RAD_S(bfm.omega->ve[SERVO_LEFT]) );
- IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
- AC_ID,
- DegOfRad(bfm.ang_rate_body->ve[AXIS_X]),
- DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]),
- DegOfRad(bfm.ang_rate_body->ve[AXIS_Z]),
- DegOfRad(bfm.eulers->ve[AXIS_X]),
- DegOfRad(bfm.eulers->ve[AXIS_Y]),
- DegOfRad(bfm.eulers->ve[AXIS_Z]));
- IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f",
- AC_ID,
- (bfm.speed_ltp->ve[AXIS_X]),
- (bfm.speed_ltp->ve[AXIS_Y]),
- (bfm.speed_ltp->ve[AXIS_Z]),
- (bfm.pos_ltp->ve[AXIS_X]),
- (bfm.pos_ltp->ve[AXIS_Y]),
- (bfm.pos_ltp->ve[AXIS_Z]));
-#if 0
- IvySendMsg("%d NPS_WIND %f %f %f",
- AC_ID,
- bwm.velocity->ve[AXIS_X],
- bwm.velocity->ve[AXIS_Y],
- bwm.velocity->ve[AXIS_Z]);
-#endif
- IvySendMsg("%d NPS_ACCEL_LTP %f %f %f",
- AC_ID,
- bfm.accel_ltp->ve[AXIS_X],
- bfm.accel_ltp->ve[AXIS_Y],
- bfm.accel_ltp->ve[AXIS_Z]);
-
- }
-}
-
-int main ( int argc, char** argv) {
-
- booz2_sim_parse_options(argc, argv);
-
- booz2_sim_init();
-
- GMainLoop *ml = g_main_loop_new(NULL, FALSE);
-
- g_timeout_add(HOST_TIMEOUT_PERIOD, booz2_sim_periodic, NULL);
-
- g_main_loop_run(ml);
-
- return 0;
-}
-
-
-static void ivy_transport_init(void) {
- IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL);
- IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
- IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)");
- IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
- IvyStart("127.255.255.255");
-}
-
-#include "std.h"
-#include "generated/settings.h"
-#include "dl_protocol.h"
-#include "subsystems/datalink/downlink.h"
-static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]){
- uint8_t index = atoi(argv[2]);
- float value = atof(argv[3]);
- DlSetting(index, value);
- DOWNLINK_SEND_DL_VALUE(&index, &value);
- // printf("setting %d %f\n", index, value);
-}
-
-static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]){
- int block = atoi(argv[1]);
- nav_goto_block(block);
-}
-
-#include "pprz_geodetic_int.h"
-#include "stdio.h"
-static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]){
- int wp_id = atoi(argv[1]);
- //int ac_id = atoi(argv[1]);
- struct LlaCoor_i lla;
- struct EnuCoor_i enu;
- //printf("move deg %d %d %d\n",atoi(argv[3]),atoi(argv[4]),atoi(argv[5]));
- int lat = atoi(argv[3]);
- int lon = atoi(argv[4]);
- int alt = atoi(argv[5]);
- lla.lat = INT32_RAD_OF_DEG(lat);
- lla.lon = INT32_RAD_OF_DEG(lon);
- lla.alt = alt+ins_ltp_def.lla.alt;
- //printf("move rad %d %d %d (%d)\n",lla.lat,lla.lon,lla.alt,ins_ltp_def.lla.alt);
- enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
- enu.x = POS_BFP_OF_REAL(enu.x)/100;
- enu.y = POS_BFP_OF_REAL(enu.y)/100;
- enu.z = POS_BFP_OF_REAL(enu.z)/100;
- //printf("enu_of_lla %d %d %d -> %d %d %d (%f %f %f)\n",waypoints[wp_id].x,waypoints[wp_id].y,waypoints[wp_id].z,enu.x,enu.y,enu.z,
- // POS_FLOAT_OF_BFP(enu.x),POS_FLOAT_OF_BFP(enu.y),POS_FLOAT_OF_BFP(enu.z));
- VECT3_ASSIGN(waypoints[wp_id],enu.x,enu.y,enu.z);
- DOWNLINK_SEND_WP_MOVED_LTP(&wp_id, &enu.x, &enu.y, &enu.z);
- //DOWNLINK_SEND_WP_MOVED_LLA(&wp_id, &lat, &lon, &alt);
- //printf("WP_MOVED\n");
- //fflush(stdout);
-}
-
-#include "actuators.h"
-static void booz_sim_read_actuators(void) {
-
-
-// printf("actatuors %d %d %d %d\n",
-// Actuator(SERVO_FRONT), Actuator(SERVO_BACK), Actuator(SERVO_RIGHT), Actuator(SERVO_LEFT));
- int32_t ut_front = Actuator(SERVO_FRONT) - TRIM_FRONT;
- int32_t ut_back = Actuator(SERVO_BACK) - TRIM_BACK;
- int32_t ut_right = Actuator(SERVO_RIGHT) - TRIM_RIGHT;
- int32_t ut_left = Actuator(SERVO_LEFT) - TRIM_LEFT;
-#if 1
- booz_sim_actuators_values[0] = (double)ut_front / SUPERVISION_MAX_MOTOR;
- booz_sim_actuators_values[1] = (double)ut_back / SUPERVISION_MAX_MOTOR;
- booz_sim_actuators_values[2] = (double)ut_right / SUPERVISION_MAX_MOTOR;
- booz_sim_actuators_values[3] = (double)ut_left / SUPERVISION_MAX_MOTOR;
-#else
- // double foo = 0.33;
- double foo = 0.35;
- booz_sim_actuators_values[0] = foo;
- booz_sim_actuators_values[1] = foo;
- booz_sim_actuators_values[2] = foo;
- booz_sim_actuators_values[3] = foo;
-#endif
-
-
- // printf("%f %f %f %f\n", booz_sim_actuators_values[0], booz_sim_actuators_values[1], booz_sim_actuators_values[2], booz_sim_actuators_values[3]);
-
-}
-
-static void booz2_sim_parse_options(int argc, char** argv) {
-
- static const char* usage =
-"Usage: %s [options]\n"
-" Options :\n"
-" -j --js_dev joystick device\n"
-" --fg_host flight gear host\n"
-" --fg_port flight gear port\n";
-
-
- while (1) {
-
- static struct option long_options[] = {
- {"fg_host", 1, NULL, 0},
- {"fg_port", 1, NULL, 0},
- {"js_dev", 1, NULL, 0},
- {0, 0, 0, 0}
- };
- int option_index = 0;
- int c = getopt_long(argc, argv, "j:",
- long_options, &option_index);
- if (c == -1)
- break;
-
- switch (c) {
- case 0:
- switch (option_index) {
- case 0:
- fg_host = strdup(optarg); break;
- case 1:
- fg_port = atoi(optarg); break;
- case 2:
- joystick_dev = strdup(optarg); break;
- }
- break;
-
- case 'j':
- joystick_dev = strdup(optarg);
- break;
-
- default: /* $B!G(B?$B!G(B */
- printf("?? getopt returned character code 0%o ??\n", c);
- fprintf(stderr, usage, argv[0]);
- exit(EXIT_FAILURE);
- }
- }
-}
diff --git a/sw/simulator/old_booz/booz_flight_model.c b/sw/simulator/old_booz/booz_flight_model.c
deleted file mode 100644
index 5aa0029238..0000000000
--- a/sw/simulator/old_booz/booz_flight_model.c
+++ /dev/null
@@ -1,362 +0,0 @@
-#include "booz_flight_model.h"
-
-#define BFMS_X 0
-#define BFMS_Y 1
-#define BFMS_Z 2
-#define BFMS_XD 3
-#define BFMS_YD 4
-#define BFMS_ZD 5
-#define BFMS_PHI 6
-#define BFMS_THETA 7
-#define BFMS_PSI 8
-#define BFMS_P 9
-#define BFMS_Q 10
-#define BFMS_R 11
-#define BFMS_OM_B 12
-#define BFMS_OM_F 13
-#define BFMS_OM_R 14
-#define BFMS_OM_L 15
-#define BFMS_SIZE 16
-
-#define BoozFlighModelGetPos(_dest) { \
- _dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \
- _dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \
- _dest->ve[AXIS_Z] = bfm.state->ve[BFMS_Z]; \
- }
-
-#define BoozFlighModelGetSpeedLtp(_dest) { \
- _dest->ve[AXIS_X] = bfm.state->ve[BFMS_XD]; \
- _dest->ve[AXIS_Y] = bfm.state->ve[BFMS_YD]; \
- _dest->ve[AXIS_Z] = bfm.state->ve[BFMS_ZD]; \
- }
-
-#define BoozFlighModelGetAngles(_dest) { \
- _dest->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; \
- _dest->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; \
- _dest->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; \
- }
-
-#define BoozFlighModelGetRate(_dest) { \
- _dest->ve[AXIS_P] = bfm.state->ve[BFMS_P]; \
- _dest->ve[AXIS_Q] = bfm.state->ve[BFMS_Q]; \
- _dest->ve[AXIS_R] = bfm.state->ve[BFMS_R]; \
- }
-
-#define BoozFlighModelGetRPMS(_dest) { \
- _dest->ve[SERVO_BACK] = bfm.state->ve[BFMS_OM_B]; \
- _dest->ve[SERVO_FRONT] = bfm.state->ve[BFMS_OM_F]; \
- _dest->ve[SERVO_RIGHT] = bfm.state->ve[BFMS_OM_R]; \
- _dest->ve[SERVO_LEFT] = bfm.state->ve[BFMS_OM_L]; \
- }
-
-
-#include
-
-#include "booz_flight_model_params.h"
-#include "booz_flight_model_utils.h"
-#include "booz_wind_model.h"
-
-#include "6dof.h"
-
-
-struct BoozFlightModel bfm;
-
-//static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot);
-
-static void booz_flight_model_update_byproducts(void);
-static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square);
-static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square );
-static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
-
-void booz_flight_model_init( void ) {
- bfm.on_ground = TRUE;
-
- bfm.time = 0.;
- bfm.bat_voltage = BAT_VOLTAGE;
- bfm.mot_voltage = v_get(SERVOS_NB);
-
- bfm.state = v_get(BFMS_SIZE);
- v_zero(bfm.state);
-
- bfm.pos_ltp = v_get(AXIS_NB);
- bfm.speed_ltp = v_get(AXIS_NB);
- bfm.accel_ltp = v_get(AXIS_NB);
-
- bfm.speed_body = v_get(AXIS_NB);
- bfm.accel_body = v_get(AXIS_NB);
-
- bfm.eulers = v_get(AXIS_NB);
- bfm.ang_rate_body = v_get(AXIS_NB);
- bfm.ang_accel_body = v_get(AXIS_NB);
-
- bfm.dcm = m_get(AXIS_NB, AXIS_NB);
- bfm.dcm_t = m_get(AXIS_NB, AXIS_NB);
- bfm.quat = v_get(AXIS_NB);
-
- bfm.omega = v_get(SERVOS_NB);
- bfm.omega_square = v_get(SERVOS_NB);
-
-
- /* constants */
- bfm.g_ltp = v_get(AXIS_NB);
- bfm.g_ltp->ve[AXIS_X] = 0.;
- bfm.g_ltp->ve[AXIS_Y] = 0.;
- bfm.g_ltp->ve[AXIS_Z] = G;
-
- /* FIXME */
- bfm.h_ltp = v_get(AXIS_NB);
- bfm.h_ltp->ve[AXIS_X] = 1.;
- bfm.h_ltp->ve[AXIS_Y] = 0.;
- bfm.h_ltp->ve[AXIS_Z] = 1.;
-
- bfm.thrust_factor = 0.5 * RHO * PROP_AREA * C_t * PROP_RADIUS * PROP_RADIUS;
- bfm.torque_factor = 0.5 * RHO * PROP_AREA * C_q * PROP_RADIUS * PROP_RADIUS;
-
- bfm.props_moment_matrix = m_get(AXIS_NB, SERVOS_NB);
- m_zero(bfm.props_moment_matrix);
- bfm.props_moment_matrix->me[AXIS_X][SERVO_LEFT] = L * bfm.thrust_factor;
- bfm.props_moment_matrix->me[AXIS_X][SERVO_RIGHT] = -L * bfm.thrust_factor;
- bfm.props_moment_matrix->me[AXIS_Y][SERVO_BACK] = -L * bfm.thrust_factor;
- bfm.props_moment_matrix->me[AXIS_Y][SERVO_FRONT] = L * bfm.thrust_factor;
- bfm.props_moment_matrix->me[AXIS_Z][SERVO_LEFT] = bfm.torque_factor;
- bfm.props_moment_matrix->me[AXIS_Z][SERVO_RIGHT] = bfm.torque_factor;
- bfm.props_moment_matrix->me[AXIS_Z][SERVO_BACK] = -bfm.torque_factor;
- bfm.props_moment_matrix->me[AXIS_Z][SERVO_FRONT] = -bfm.torque_factor;
-
- bfm.mass = MASS;
-
- bfm.Inert = m_get(AXIS_NB, AXIS_NB);
- m_zero(bfm.Inert);
- bfm.Inert->me[AXIS_X][AXIS_X] = Ix;
- bfm.Inert->me[AXIS_Y][AXIS_Y] = Iy;
- bfm.Inert->me[AXIS_Z][AXIS_Z] = Iz;
-
- bfm.Inert_inv = m_get(AXIS_NB, AXIS_NB);
- m_zero(bfm.Inert_inv);
- bfm.Inert_inv->me[AXIS_X][AXIS_X] = 1./Ix;
- bfm.Inert_inv->me[AXIS_Y][AXIS_Y] = 1./Iy;
- bfm.Inert_inv->me[AXIS_Z][AXIS_Z] = 1./Iz;
-
-}
-
-
-#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;}
-
-void booz_flight_model_run( double dt, double* commands ) {
-
-
- int i;
- for (i=0; ive[i] = bfm.bat_voltage * commands[i];
- // rk4(motor_model_derivative, bfm.mot_omega, bfm.mot_voltage, dt);
- rk4(booz_flight_model_get_derivatives, bfm.state, bfm.mot_voltage, dt);
- /* wrap euler angles */
- WRAP( bfm.state->ve[BFMS_PHI], M_PI);
- WRAP( bfm.state->ve[BFMS_THETA], M_PI_2);
- WRAP( bfm.state->ve[BFMS_PSI], M_PI);
- booz_flight_model_update_byproducts();
- bfm.time += dt;
-}
-
-
-static void booz_flight_model_update_byproducts(void) {
-
- /* extract eulers angles from state */
- BoozFlighModelGetAngles( bfm.eulers);
- /* extract body rotational rates from state */
- BoozFlighModelGetRate( bfm.ang_rate_body);
-
- /* direct cosine matrix ( inertial to body )*/
- dcm_of_eulers(bfm.eulers, bfm.dcm);
- /* transpose of dcm ( body to inertial ) */
- m_transp(bfm.dcm, bfm.dcm_t);
- /* quaternion */
- quat_of_eulers(bfm.quat, bfm.eulers);
-
- BoozFlighModelGetPos(bfm.pos_ltp);
- /* extract speed in ltp frame from state */
- BoozFlighModelGetSpeedLtp(bfm.speed_ltp);
- /* extract prop rotational speeds from state */
- BoozFlighModelGetRPMS(bfm.omega);
- /* compute square */
- v_star(bfm.omega, bfm.omega, bfm.omega_square);
- /* compute ltp accelerations */
- static VEC *f_ltp = VNULL;
- f_ltp = v_resize(f_ltp, AXIS_NB);
- f_ltp = booz_get_forces_ltp(f_ltp , bfm.speed_ltp, bfm.dcm_t, bfm.omega_square);
- sv_mlt( 1./bfm.mass, f_ltp, bfm.accel_ltp);
- /* rotate speed and accel to body frame */
- mv_mlt(bfm.dcm, bfm.speed_ltp, bfm.speed_body);
- mv_mlt(bfm.dcm, bfm.accel_ltp, bfm.accel_body);
-
-
- /* rotational accelerations */
-
-
-}
-
-
-
-/*
- compute the sum of external forces.
- assumes that dcm and omega_square are already precomputed from X
-*/
-static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square) {
-
- // FIXME : nimporte koi !
- F = v_zero(F);
- if (!bfm.on_ground) {
-
- // propeller thrust
- static VEC *prop_thrust_body = VNULL;
- prop_thrust_body = v_resize(prop_thrust_body, AXIS_NB);
- prop_thrust_body->ve[AXIS_X] = 0;
- prop_thrust_body->ve[AXIS_Y] = 0;
- prop_thrust_body->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor;
- static VEC *prop_thrust_ltp = VNULL;
- prop_thrust_ltp = v_resize(prop_thrust_ltp, AXIS_NB);
- prop_thrust_ltp = mv_mlt(dcm_t, prop_thrust_body, prop_thrust_ltp);
- F = v_add(F, prop_thrust_ltp, F);
-
- // gravity
- F = v_mltadd(F, bfm.g_ltp, bfm.mass, F);
-
- // drag
- static VEC *airspeed_ltp = VNULL;
- airspeed_ltp = v_resize(airspeed_ltp, AXIS_NB);
- airspeed_ltp = v_sub(speed_ltp, bwm.velocity, airspeed_ltp);
- double norm_speed = v_norm2(airspeed_ltp);
- F = v_mltadd(F, airspeed_ltp, -norm_speed * C_d_body, F);
-
- }
- return F;
-}
-
-/*
- compute the sum of external moments.
- assumes that omega_square is already precomputed from X
-*/
-static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square ) {
- if (bfm.on_ground) {
- M = v_zero(M);
- }
- else {
- M = mv_mlt(bfm.props_moment_matrix, omega_square, M);
- }
- return M;
-}
-
-static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
-
- /* square of prop rotational speeds */
- static VEC *omega_square = VNULL;
- omega_square = v_resize(omega_square,SERVOS_NB);
- BoozFlighModelGetRPMS(omega_square);
- omega_square = v_star(omega_square, omega_square, omega_square);
- /* extract eulers angles from state */
- static VEC *eulers = VNULL;
- eulers = v_resize(eulers, AXIS_NB);
- BoozFlighModelGetAngles(eulers);
- /* direct cosine matrix ( inertial to body )*/
- static MAT *dcm = MNULL;
- dcm = m_resize(dcm,AXIS_NB, AXIS_NB);
- dcm = dcm_of_eulers(eulers, dcm);
- /* transpose of dcm ( body to inertial ) */
- static MAT *dcm_t = MNULL;
- dcm_t = m_resize(dcm_t,AXIS_NB, AXIS_NB);
- dcm_t = m_transp(dcm, dcm_t);
- /* extract ltp_speeds_from state */
- static VEC *speed_ltp = VNULL;
- speed_ltp = v_resize(speed_ltp, AXIS_NB);
- BoozFlighModelGetSpeedLtp(speed_ltp);
- /* extracts body rates from state */
- static VEC *rate_body = VNULL;
- rate_body = v_resize(rate_body, AXIS_NB);
- BoozFlighModelGetRate(rate_body);
-
- /* derivatives of position */
- Xdot->ve[BFMS_X] = speed_ltp->ve[AXIS_X];
- Xdot->ve[BFMS_Y] = speed_ltp->ve[AXIS_Y];
- Xdot->ve[BFMS_Z] = speed_ltp->ve[AXIS_Z];
-
- /* derivatives of speed */
- static VEC *f_ltp = VNULL;
- f_ltp = v_resize(f_ltp, AXIS_NB);
- f_ltp = booz_get_forces_ltp(f_ltp , speed_ltp, dcm_t, omega_square);
- Xdot->ve[BFMS_XD] = 1./bfm.mass * f_ltp->ve[AXIS_X];
- Xdot->ve[BFMS_YD] = 1./bfm.mass * f_ltp->ve[AXIS_Y];
- Xdot->ve[BFMS_ZD] = 1./bfm.mass * f_ltp->ve[AXIS_Z];
-
- /* derivatives of eulers */
- double sinPHI = sin(eulers->ve[EULER_PHI]);
- double cosPHI = cos(eulers->ve[EULER_PHI]);
- double cosTHETA = cos(eulers->ve[EULER_THETA]);
- double tanTHETA = tan(eulers->ve[EULER_THETA]);
- static MAT *euler_dot_of_pqr = MNULL;
- euler_dot_of_pqr = m_resize(euler_dot_of_pqr,AXIS_NB, AXIS_NB);
- euler_dot_of_pqr->me[EULER_PHI][AXIS_P] = 1.;
- euler_dot_of_pqr->me[EULER_PHI][AXIS_Q] = sinPHI*tanTHETA;
- euler_dot_of_pqr->me[EULER_PHI][AXIS_R] = cosPHI*tanTHETA;
- euler_dot_of_pqr->me[EULER_THETA][AXIS_P] = 0.;
- euler_dot_of_pqr->me[EULER_THETA][AXIS_Q] = cosPHI;
- euler_dot_of_pqr->me[EULER_THETA][AXIS_R] = -sinPHI;
- euler_dot_of_pqr->me[EULER_PSI][AXIS_P] = 0.;
- euler_dot_of_pqr->me[EULER_PSI][AXIS_Q] = sinPHI/cosTHETA;
- euler_dot_of_pqr->me[EULER_PSI][AXIS_R] = cosPHI/cosTHETA;
- static VEC *euler_dot = VNULL;
- euler_dot = v_resize(euler_dot, AXIS_NB);
- euler_dot = mv_mlt(euler_dot_of_pqr, rate_body, euler_dot);
- Xdot->ve[BFMS_PHI] = euler_dot->ve[EULER_PHI];
- Xdot->ve[BFMS_THETA] = euler_dot->ve[EULER_THETA];
- Xdot->ve[BFMS_PSI] = euler_dot->ve[EULER_PSI];
-
- /* derivatives of rates */
- /* compute external moments */
- static VEC *m_body = VNULL;
- m_body = v_resize(m_body, AXIS_NB);
- m_body = booz_get_moments_body_frame(m_body, omega_square);
- /* Newton in body frame */
- static VEC *i_omega = VNULL;
- i_omega = v_resize(i_omega, AXIS_NB);
- i_omega = mv_mlt(bfm.Inert, rate_body, i_omega);
- static VEC *omega_i_omega = VNULL;
- omega_i_omega = v_resize(omega_i_omega, AXIS_NB);
- omega_i_omega = out_prod(rate_body, i_omega, omega_i_omega);
- static VEC *m_tot = VNULL;
- m_tot = v_resize(m_tot, AXIS_NB);
- m_tot = v_sub(m_body, omega_i_omega, m_tot);
- static VEC *I_inv_m_tot = VNULL;
- I_inv_m_tot = v_resize(I_inv_m_tot, AXIS_NB);
- I_inv_m_tot = mv_mlt(bfm.Inert_inv, m_tot, I_inv_m_tot);
- Xdot->ve[BFMS_P] = I_inv_m_tot->ve[AXIS_P];
- Xdot->ve[BFMS_Q] = I_inv_m_tot->ve[AXIS_Q];
- Xdot->ve[BFMS_R] = I_inv_m_tot->ve[AXIS_R];
-
- /* derivatives of motors rpm */
- /* omega_dot = -1/THAU*omega - Kq*omega^2 + Kv/THAU * V */
- Xdot->ve[BFMS_OM_B] = -1./THAU * X->ve[BFMS_OM_B] - Kq * omega_square->ve[SERVO_BACK] + Kv/THAU * u->ve[SERVO_BACK];
- Xdot->ve[BFMS_OM_F] = -1./THAU * X->ve[BFMS_OM_F] - Kq * omega_square->ve[SERVO_FRONT] + Kv/THAU * u->ve[SERVO_FRONT];
- Xdot->ve[BFMS_OM_R] = -1./THAU * X->ve[BFMS_OM_R] - Kq * omega_square->ve[SERVO_RIGHT] + Kv/THAU * u->ve[SERVO_RIGHT];
- Xdot->ve[BFMS_OM_L] = -1./THAU * X->ve[BFMS_OM_L] - Kq * omega_square->ve[SERVO_LEFT] + Kv/THAU * u->ve[SERVO_LEFT];
-
-}
-
-
-
-
-
-
-#if 0
-static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot) {
- static VEC *temp1 = VNULL;
- static VEC *temp2 = VNULL;
- temp1 = v_resize(temp1,SERVOS_NB);
- temp2 = v_resize(temp2,SERVOS_NB);
-
- // omega_dot = -1/THAU*omega - Kq*omega^2 + Kv/THAU * V;
- temp1 = sv_mlt(-1./THAU, x, temp1); /* temp1 = -1/THAU * x */
- temp2 = v_star(x, x, temp2); /* temp2 = x^2 */
- xdot = v_mltadd(temp1, temp2, -Kq, xdot); /* xdot = temp1 - Kq*temp2 */
- xdot = v_mltadd(xdot, u, Kv/THAU, xdot); /* xdot = xdot + Kv/THAU * u */
-}
-#endif
diff --git a/sw/simulator/old_booz/booz_flight_model.h b/sw/simulator/old_booz/booz_flight_model.h
deleted file mode 100644
index 57e2d0a66c..0000000000
--- a/sw/simulator/old_booz/booz_flight_model.h
+++ /dev/null
@@ -1,65 +0,0 @@
-#ifndef BOOZ_FLIGHT_MODEL_H
-#define BOOZ_FLIGHT_MODEL_H
-
-#include
-#include "generated/airframe.h"
-
-
-
-struct BoozFlightModel {
- /* are we flying ? */
- int on_ground;
-
- double time;
- /* battery voltage in V */
- double bat_voltage;
- /* motors supply voltage in V */
- VEC* mot_voltage;
-
- /* private state : see defines in .c for fields */
- VEC* state;
-
- /* user products */
- VEC* pos_ecef;
- VEC* pos_ltp;
- VEC* speed_ltp;
- VEC* accel_ltp;
- VEC* speed_body;
- VEC* accel_body;
-
- VEC* eulers;
- VEC* ang_rate_body;
- VEC* ang_accel_body;
- MAT* dcm;
- MAT* dcm_t;
- VEC* quat;
-
- VEC* omega;
- VEC* omega_square;
-
- /* constants used in derivative computation */
- /* magnetic field in earth frame */
- VEC* h_ltp;
- /* gravitation in earth frame */
- VEC* g_ltp;
- /* propeller thrust factor */
- double thrust_factor;
- /* propeller torque factor */
- double torque_factor;
- /* Matrix used to compute the moments produced by props */
- MAT* props_moment_matrix;
- /* */
- double mass;
- /* inertia matrix */
- MAT* Inert;
- /* invert of inertia matrix */
- MAT* Inert_inv;
-};
-
-extern struct BoozFlightModel bfm;
-
-extern void booz_flight_model_init( void );
-extern void booz_flight_model_run( double t, double* commands );
-
-
-#endif /* BOOZ_FLIGHT_MODEL_H */
diff --git a/sw/simulator/old_booz/booz_flight_model_params.h b/sw/simulator/old_booz/booz_flight_model_params.h
deleted file mode 100644
index 964b9287ff..0000000000
--- a/sw/simulator/old_booz/booz_flight_model_params.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#ifndef BOOZ_FLIGHT_MODEL_PARAMS_H
-#define BOOZ_FLIGHT_MODEL_PARAMS_H
-
-/* body drag coefficient */
-#define C_d_body .0075
-/* propeller thrust aerodynamic coefficient */
-#define C_t 0.297
-/* propeller moment aerodynamic coefficient */
-#define C_q 0.0276
-/* propeller radius in m */
-#define PROP_RADIUS 0.125
-/* propeller area in m2 */
-#define PROP_AREA 0.005
-/* air density in kg/m3 */
-#define RHO 1.225
-/* gravity in m/s2 */
-#define G 9.81
-/* mass in kg */
-#define MASS 0.724
-/* inertia on x axis in kg * m2 */
-#define Ix .007
-/* inertia on y axis in kg * m2 */
-#define Iy .0073
-/* inertia on z axis in kg * m2 */
-#define Iz .0137
-/* lenght between centers of vehicle and prop in m */
-#define L 0.25
-
-/* motors parameters
-
- from http://cherokee.stanford.edu/~starmac/docs/DynamicsSummary
-
- omega_dot = -1/thau * omega - Kq * omega^2 + Kv/thau * V
-
-*/
-#define BAT_VOLTAGE 11.
-
-#define THAU 0.05
-#define Kq 0.12
-#define Kv 304.
-
-
-
-#endif /* BOOZ_FLIGHT_MODEL_PARAMS_H */
diff --git a/sw/simulator/old_booz/booz_flight_model_utils.c b/sw/simulator/old_booz/booz_flight_model_utils.c
deleted file mode 100644
index 6a648227c6..0000000000
--- a/sw/simulator/old_booz/booz_flight_model_utils.c
+++ /dev/null
@@ -1,100 +0,0 @@
-#include "booz_flight_model_utils.h"
-
-#include
-#include "6dof.h"
-
-void rk4(ode_fun f, VEC* x, VEC* u, double dt) {
-
- static VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL;
- static VEC *temp=VNULL;
-
- v1 = v_resize(v1,x->dim);
- v2 = v_resize(v2,x->dim);
- v3 = v_resize(v3,x->dim);
- v4 = v_resize(v4,x->dim);
- temp = v_resize(temp,x->dim);
-
- // MEM_STAT_REG(v1,TYPE_VEC);
- // MEM_STAT_REG(v2,TYPE_VEC);
- // MEM_STAT_REG(v3,TYPE_VEC);
- // MEM_STAT_REG(v4,TYPE_VEC);
- // MEM_STAT_REG(temp,TYPE_VEC);
-
- f(x, u, v1);
- v_mltadd(x,v1,0.5*dt,temp); /* temp = x + .5*dt*v1 */
- f(temp, u, v2);
- v_mltadd(x,v2,0.5*dt,temp); /* temp = x + .5*dt*v2 */
- f(temp,u, v3);
- v_mltadd(x,v3,dt,temp); /* temp = x + dt*v3 */
- f(temp, u, v4);
-
- /* now add: v1+2*v2+2*v3+v4 */
- v_copy(v1,temp); /* temp = v1 */
- v_mltadd(temp,v2,2.0,temp); /* temp = v1+2*v2 */
- v_mltadd(temp,v3,2.0,temp); /* temp = v1+2*v2+2*v3 */
- v_add(temp,v4,temp); /* temp = v1+2*v2+2*v3+v4 */
-
- /* adjust x */
- v_mltadd(x,temp,dt/6.0,x); /* x = x+(h/6) * temp */
-
-}
-
-
-MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ) {
-
- dcm = m_resize(dcm, 3,3);
-
- double sinPHI = sin(eulers->ve[EULER_PHI]);
- double cosPHI = cos(eulers->ve[EULER_PHI]);
- double sinTHETA = sin(eulers->ve[EULER_THETA]);
- double cosTHETA = cos(eulers->ve[EULER_THETA]);
- double sinPSI = sin(eulers->ve[EULER_PSI]);
- double cosPSI = cos(eulers->ve[EULER_PSI]);
-
- dcm->me[0][0] = cosTHETA * cosPSI;
- dcm->me[0][1] = cosTHETA * sinPSI;
- dcm->me[0][2] = -sinTHETA;
- dcm->me[1][0] = sinPHI * sinTHETA * cosPSI - cosPHI * sinPSI;
- dcm->me[1][1] = sinPHI * sinTHETA * sinPSI + cosPHI * cosPSI;
- dcm->me[1][2] = sinPHI * cosTHETA;
- dcm->me[2][0] = cosPHI * sinTHETA * cosPSI + sinPHI * sinPSI;
- dcm->me[2][1] = cosPHI * sinTHETA * sinPSI - sinPHI * cosPSI;
- dcm->me[2][2] = cosPHI * cosTHETA;
-
- return dcm;
-}
-
-
-VEC* quat_of_eulers(VEC* quat, VEC* eulers) {
-
- double phi2 = eulers->ve[EULER_PHI] / 2.0;
- double theta2 = eulers->ve[EULER_THETA] / 2.0;
- double psi2 = eulers->ve[EULER_PSI] / 2.0;
-
- double sinphi2 = sin( phi2 );
- double cosphi2 = cos( phi2 );
- double sintheta2 = sin( theta2 );
- double costheta2 = cos( theta2 );
- double sinpsi2 = sin( psi2 );
- double cospsi2 = cos( psi2 );
-
- quat->ve[QUAT_QI] = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
- quat->ve[QUAT_QX] = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
- quat->ve[QUAT_QY] = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
- quat->ve[QUAT_QZ] = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
-
- return quat;
-}
-
-
-
-VEC* out_prod( VEC* a, VEC* b, VEC* out) {
- if ( a->dim != 3 || b->dim != 3 )
- error(E_SIZES,"out_prod");
- if ( out==(VEC *)NULL || out->dim != a->dim )
- out = v_resize(out,a->dim);
- out->ve[0] = a->ve[1]*b->ve[2] - a->ve[2]*b->ve[1];
- out->ve[1] = a->ve[2]*b->ve[0] - a->ve[0]*b->ve[2];
- out->ve[2] = a->ve[0]*b->ve[1] - a->ve[1]*b->ve[0];
- return out;
-}
diff --git a/sw/simulator/old_booz/booz_flight_model_utils.h b/sw/simulator/old_booz/booz_flight_model_utils.h
deleted file mode 100644
index a53798608c..0000000000
--- a/sw/simulator/old_booz/booz_flight_model_utils.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef BOOZ_FM_UTILS_H
-#define BOOZ_FM_UTILS_H
-
-#include
-
-typedef void (*ode_fun)(VEC* x, VEC* u, VEC* xdot);
-
-extern void rk4(ode_fun f, VEC* x, VEC* u, double dt);
-
-extern MAT* dcm_of_eulers (VEC* eulers, MAT* dcm );
-extern VEC* quat_of_eulers(VEC* quat, VEC* eulers);
-
-extern VEC* out_prod( VEC* a, VEC* b, VEC* out);
-
-
-
-
-#endif /* BOOZ_FM_UTILS_H */
diff --git a/sw/simulator/old_booz/booz_flightgear.c b/sw/simulator/old_booz/booz_flightgear.c
deleted file mode 100644
index 2a58ea4529..0000000000
--- a/sw/simulator/old_booz/booz_flightgear.c
+++ /dev/null
@@ -1,88 +0,0 @@
-#include "booz_flightgear.h"
-#include "flight_gear.h"
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-
-static int fg_socket;
-static struct sockaddr_in fg_addr;
-
-void net_gui_init (struct FGNetGUI* gui);
-
-void booz_flightgear_init(const char* host, unsigned int port) {
-
- printf("connecting to %s on port %d\n", host, port);
-
- int so_reuseaddr = 1;
- struct protoent * pte = getprotobyname("UDP");
- fg_socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
- setsockopt(fg_socket, SOL_SOCKET, SO_REUSEADDR,
- &so_reuseaddr, sizeof(so_reuseaddr));
-
- fg_addr.sin_family = PF_INET;
- fg_addr.sin_port = htons(port);
- fg_addr.sin_addr.s_addr = inet_addr(host);
-
-}
-
-
-void booz_flightgear_send() {
-
- const double earth_radius = 6372795.;
-
- double lat = 0.656480 + asin((bfm.state->ve[BFMS_X] - 90)/earth_radius);
- double lon = -2.135537 + asin((bfm.state->ve[BFMS_Y] - 45)/earth_radius);
-
- struct FGNetGUI gui;
- net_gui_init(&gui);
-
- gui.latitude = lat;
- gui.longitude = lon;
- gui.altitude = 1.1 - bfm.state->ve[BFMS_Z];
-
- gui.phi = bfm.state->ve[BFMS_PHI];
- gui.theta = bfm.state->ve[BFMS_THETA];
- gui.psi = bfm.state->ve[BFMS_PSI];
-
- gui.cur_time += (unsigned long)bfm.time;
-
- if (sendto(fg_socket, (char*)(&gui), sizeof(gui), 0,
- (struct sockaddr*)&fg_addr, sizeof(fg_addr)) == -1)
- printf("error sending\n");
-}
-
-void net_gui_init (struct FGNetGUI* gui) {
- gui->version = FG_NET_GUI_VERSION;
- gui->latitude = 0.656480;
- gui->longitude = -2.135537;
- gui->altitude = 0.807609;
- gui->agl = 1.111652;
-
- gui->phi = 0.;
- gui->theta = 0.;
- gui->psi = 5.20;
-
- gui->vcas = 0.;
- gui->climb_rate = 0.;
-
- gui->num_tanks = 1;
- gui->fuel_quantity[0] = 0.;
-
- gui->cur_time = 3198060679ul;
- gui->warp = 1122474394ul;
-
- gui->ground_elev = 0.;
-
- gui->tuned_freq = 125.65;
- gui->nav_radial = 90.;
- gui->in_range = 1;
- gui->dist_nm = 10.;
- gui->course_deviation_deg = 0.;
- gui->gs_deviation_deg = 0.;
-}
diff --git a/sw/simulator/old_booz/booz_flightgear.h b/sw/simulator/old_booz/booz_flightgear.h
deleted file mode 100644
index bbb5ea3033..0000000000
--- a/sw/simulator/old_booz/booz_flightgear.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef BOOZ_FLIGHTGEAR_H
-#define BOOZ_FLIGHTGEAR_H
-
-#include "booz_flight_model.h"
-
-extern void booz_flightgear_init(const char* host, unsigned int port);
-extern void booz_flightgear_send();
-
-#endif /* BOOZ_FLIGHTGEAR_H */
diff --git a/sw/simulator/old_booz/booz_joystick.c b/sw/simulator/old_booz/booz_joystick.c
deleted file mode 100644
index 731c7a5305..0000000000
--- a/sw/simulator/old_booz/booz_joystick.c
+++ /dev/null
@@ -1,84 +0,0 @@
-#include "booz_joystick.h"
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-double booz_joystick_value[JS_NB_AXIS];
-//const double booz_joystick_neutral[JS_NB_AXIS] = { 112., 113., 112., 112., 112., 112., 30.};
-//const double booz_joystick_max[JS_NB_AXIS] = { 1., 224., 224., 224., 224., 224., 223.};
-//const double booz_joystick_min[JS_NB_AXIS] = { 224., 1., 1., 1., 1., 1., 30.};
-
-const double booz_joystick_neutral[JS_NB_AXIS] = { 113., 113., 112., 112., 112., 112., 30.};
-const double booz_joystick_max[JS_NB_AXIS] = { 46., 178., 224., 224., 224., 179., 223.};
-const double booz_joystick_min[JS_NB_AXIS] = { 179., 46., 1., 1., 1., 46., 30.};
-
-
-
-static gboolean on_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
-
-void booz_joystick_init(const char* device) {
- int i;
- for (i=0; i
-
-#include "booz_r250.h"
-
-/* set the following if you trust rand(), otherwise the minimal standard
- generator is used
-*/
-/* #define TRUST_RAND */
-
-
-#ifndef TRUST_RAND
-#include "booz_randlcg.h"
-#endif
-
-/* defines to allow for 16 or 32 bit integers */
-#define BITS 31
-
-
-#if WORD_BIT == 32
-#ifndef BITS
-#define BITS 32
-#endif
-#else
-#ifndef BITS
-#define BITS 16
-#endif
-#endif
-
-#if BITS == 31
-#define MSB 0x40000000L
-#define ALL_BITS 0x7fffffffL
-#define HALF_RANGE 0x20000000L
-#define STEP 7
-#endif
-
-#if BITS == 32
-#define MSB 0x80000000L
-#define ALL_BITS 0xffffffffL
-#define HALF_RANGE 0x40000000L
-#define STEP 7
-#endif
-
-#if BITS == 16
-#define MSB 0x8000
-#define ALL_BITS 0xffff
-#define HALF_RANGE 0x4000
-#define STEP 11
-#endif
-
-static unsigned int r250_buffer[ 250 ];
-static int r250_index;
-
-#ifdef NO_PROTO
-void r250_init(sd)
-int seed;
-#else
-void r250_init(int sd)
-#endif
-{
- int j, k;
- unsigned int mask, msb;
-
-#ifdef TRUST_RAND
-
-#if BITS == 32 || BITS == 31
- srand48( sd );
-#else
- srand( sd );
-#endif
-
-
-#else
- set_seed( sd );
-#endif
-
- r250_index = 0;
- for (j = 0; j < 250; j++) /* fill r250 buffer with BITS-1 bit values */
-#ifdef TRUST_RAND
-#if BITS == 32 || BITS == 31
- r250_buffer[j] = (unsigned int)lrand48();
-#else
- r250_buffer[j] = rand();
-#endif
-#else
- r250_buffer[j] = randlcg();
-#endif
-
-
- for (j = 0; j < 250; j++) /* set some MSBs to 1 */
-#ifdef TRUST_RAND
- if ( rand() > HALF_RANGE )
- r250_buffer[j] |= MSB;
-#else
- if ( randlcg() > HALF_RANGE )
- r250_buffer[j] |= MSB;
-#endif
-
-
- msb = MSB; /* turn on diagonal bit */
- mask = ALL_BITS; /* turn off the leftmost bits */
-
- for (j = 0; j < BITS; j++)
- {
- k = STEP * j + 3; /* select a word to operate on */
- r250_buffer[k] &= mask; /* turn off bits left of the diagonal */
- r250_buffer[k] |= msb; /* turn on the diagonal bit */
- mask >>= 1;
- msb >>= 1;
- }
-
-}
-
-unsigned int r250() /* returns a random unsigned integer */
-{
- register int j;
- register unsigned int new_rand;
-
- if ( r250_index >= 147 )
- j = r250_index - 147; /* wrap pointer around */
- else
- j = r250_index + 103;
-
- new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
- r250_buffer[ r250_index ] = new_rand;
-
- if ( r250_index >= 249 ) /* increment pointer for next time */
- r250_index = 0;
- else
- r250_index++;
-
- return new_rand;
-
-}
-
-
-double dr250() /* returns a random double in range 0..1 */
-{
- register int j;
- register unsigned int new_rand;
-
- if ( r250_index >= 147 )
- j = r250_index - 147; /* wrap pointer around */
- else
- j = r250_index + 103;
-
- new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
- r250_buffer[ r250_index ] = new_rand;
-
- if ( r250_index >= 249 ) /* increment pointer for next time */
- r250_index = 0;
- else
- r250_index++;
-
- return (double)new_rand / ALL_BITS;
-
-}
-
-#ifdef MAIN
-
-/* test driver prints out either NMR_RAND values or a histogram */
-
-#include
-
-#define NMR_RAND 5000
-#define MAX_BINS 500
-
-#ifdef NO_PROTO
-void main(argc, argv)
-int argc;
-char **argv;
-#else
-void main(int argc, char **argv)
-#endif
-{
- int j,k,nmr_bins,seed;
- int bins[MAX_BINS];
- double randm, bin_inc;
- double bin_limit[MAX_BINS];
-
- if ( argc != 3 )
- {
- printf("Usage -- %s nmr_bins seed\n", argv[0]);
- exit(1);
- }
-
- nmr_bins = atoi( argv[1] );
- if ( nmr_bins > MAX_BINS )
- {
- printf("ERROR -- maximum number of bins is %d\n", MAX_BINS);
- exit(1);
- }
-
- seed = atoi( argv[2] );
-
- r250_init( seed );
-
- if ( nmr_bins < 1 ) /* just print out the numbers */
- {
- for (j = 0; j < NMR_RAND; j++)
- printf("%f\n", dr250() );
- exit(0);
- }
-
- bin_inc = 1.0 / nmr_bins;
- for (j = 0; j < nmr_bins; j++) /* initialize bins to zero */
- {
- bins[j] = 0;
- bin_limit[j] = (j + 1) * bin_inc;
- }
-
- bin_limit[nmr_bins-1] = 1.0e7; /* make sure all others are in last bin */
-
- for (j = 0; j < NMR_RAND; j++)
- {
- randm = r250() / (double)ALL_BITS;
- for (k = 0; k < nmr_bins; k++)
- if ( randm < bin_limit[k] )
- {
- bins[k]++;
- break;
- }
- }
-
-
- for (j = 0; j < nmr_bins; j++)
- printf("%d\n", bins[j]);
-
-}
-
-#endif
-
diff --git a/sw/simulator/old_booz/booz_r250.h b/sw/simulator/old_booz/booz_r250.h
deleted file mode 100644
index 8455e534b8..0000000000
--- a/sw/simulator/old_booz/booz_r250.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/* r250.h prototypes for r250 random number generator,
-
- Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
- Shift-Register Sequence Random Number Generator",
- Journal of Computational Physics, V.40
-
- also:
-
- see W.L. Maier, DDJ May 1991
-
-
-*/
-
-#ifndef _R250_H_
-#define _R250_H_ 1.2
-
-void r250_init(int seed);
-unsigned int r250( void );
-double dr250( void );
-
-#endif
diff --git a/sw/simulator/old_booz/booz_randlcg.c b/sw/simulator/old_booz/booz_randlcg.c
deleted file mode 100644
index 31170292e1..0000000000
--- a/sw/simulator/old_booz/booz_randlcg.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/* rndlcg Linear Congruential Method, the "minimal standard generator"
- Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
-
-*/
-
-#include
-#include
-
-#define ALL_BITS 0xffffffff
-
-static long int quotient = LONG_MAX / 16807L;
-static long int my_remainder = LONG_MAX % 16807L;
-
-static long int seed_val = 1L;
-
-long set_seed(long int sd)
-{
- return seed_val = sd;
-}
-
-long get_seed()
-{
- return seed_val;
-}
-
-
-unsigned long int randlcg() /* returns a random unsigned integer */
-{
- if ( seed_val <= quotient )
- seed_val = (seed_val * 16807L) % LONG_MAX;
- else
- {
- long int high_part = seed_val / quotient;
- long int low_part = seed_val % quotient;
-
- long int test = 16807L * low_part - my_remainder * high_part;
-
- if ( test > 0 )
- seed_val = test;
- else
- seed_val = test + LONG_MAX;
-
- }
-
- return seed_val;
-}
-
-
-
-
-
diff --git a/sw/simulator/old_booz/booz_randlcg.h b/sw/simulator/old_booz/booz_randlcg.h
deleted file mode 100644
index 7093ff3cfa..0000000000
--- a/sw/simulator/old_booz/booz_randlcg.h
+++ /dev/null
@@ -1,19 +0,0 @@
-
-/* randlcg.h prototypes for the minimal standard random number generator,
-
- Linear Congruential Method, the "minimal standard generator"
- Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
-
-
- rcsid: @(#)randlcg.h 1.1 15:48:09 11/21/94 EFC
-
-*/
-
-#ifndef _RANDLCG_H_
-#define _RANDLCG_H_ 1.1
-
-long set_seed(long);
-long get_seed(long);
-unsigned long int randlcg();
-
-#endif
diff --git a/sw/simulator/old_booz/booz_rc_sim.h b/sw/simulator/old_booz/booz_rc_sim.h
deleted file mode 100644
index 7a6522d189..0000000000
--- a/sw/simulator/old_booz/booz_rc_sim.h
+++ /dev/null
@@ -1,33 +0,0 @@
-#ifndef BOOZ_RC_SIM_H
-#define BOOZ_RC_SIM_H
-
-#include "subsystems/radio_control.h"
-
-#define MODE_SWITCH_MANUAL 1900
-#define MODE_SWITCH_AUTO1 1500
-#define MODE_SWITCH_AUTO2 1200
-
-#define BoozRcSimFeed(_time) { \
- /* starts motors */ \
- if (_time < 0.93) { \
- ppm_pulses[RADIO_YAW] = 1500 - 0.5 * (2050-950); \
- ppm_pulses[RADIO_THROTTLE] = 1223 + 0.0 * (2050-1223); \
- ppm_pulses[RADIO_MODE] = MODE_SWITCH_MANUAL; \
- } \
- else if (_time < 3.5) { \
- ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \
- ppm_pulses[RADIO_THROTTLE] = 1223 + 0.4 * (2050-1223); \
- ppm_pulses[RADIO_MODE] = MODE_SWITCH_MANUAL; \
- } \
- else { \
- ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \
- ppm_pulses[RADIO_THROTTLE] = 1223 + 0.99 * (2050-1223); \
- ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO2; \
- } \
- ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
- ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
- ppm_valid = TRUE; \
- }
-
-
-#endif /* BOOZ_RC_SIM_H */
diff --git a/sw/simulator/old_booz/booz_sensors_model.c b/sw/simulator/old_booz/booz_sensors_model.c
deleted file mode 100644
index 8b4ac35086..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model.c
+++ /dev/null
@@ -1,64 +0,0 @@
-#include "booz_sensors_model.h"
-#include BSM_PARAMS
-
-#include
-
-#include "std.h"
-#include "booz_flight_model.h"
-#include "booz_flight_model_utils.h"
-
-struct BoozSensorsModel bsm;
-
-extern void booz_sensors_model_accel_init(double time);
-extern void booz_sensors_model_accel_run(double time);
-
-extern void booz_sensors_model_gyro_init(double time);
-extern void booz_sensors_model_gyro_run(double time);
-
-extern void booz_sensors_model_mag_init(double time);
-extern void booz_sensors_model_mag_run(double time);
-
-extern void booz_sensors_model_rangemeter_init(double time);
-extern void booz_sensors_model_rangemeter_run(double time);
-
-extern void booz_sensors_model_baro_init(double time);
-extern void booz_sensors_model_baro_run(double time);
-
-extern void booz_sensors_model_gps_init(double time);
-extern void booz_sensors_model_gps_run(double time);
-
-
-void booz_sensors_model_init(double time) {
-
- VEC* tmp_eulers = v_get(AXIS_NB);
- tmp_eulers->ve[EULER_PHI] = BSM_BODY_TO_IMU_PHI;
- tmp_eulers->ve[EULER_THETA] = BSM_BODY_TO_IMU_THETA;
- tmp_eulers->ve[EULER_PSI] = BSM_BODY_TO_IMU_PSI;
- bsm.body_to_imu = m_get(AXIS_NB, AXIS_NB);
- dcm_of_eulers (tmp_eulers, bsm.body_to_imu );
-
- booz_sensors_model_accel_init(time);
- booz_sensors_model_gyro_init(time);
- booz_sensors_model_mag_init(time);
- booz_sensors_model_rangemeter_init(time);
- booz_sensors_model_baro_init(time);
- booz_sensors_model_gps_init(time);
-
-}
-
-void booz_sensors_model_run(double time) {
-
- booz_sensors_model_accel_run(time);
- booz_sensors_model_gyro_run(time);
- booz_sensors_model_mag_run(time);
- booz_sensors_model_rangemeter_run(time);
- booz_sensors_model_baro_run(time);
- booz_sensors_model_gps_run(time);
-
-}
-
-
-
-
-
-
diff --git a/sw/simulator/old_booz/booz_sensors_model.h b/sw/simulator/old_booz/booz_sensors_model.h
deleted file mode 100644
index ee33e998a4..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model.h
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- * Copyright (C) 2008 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.
- *
- */
-
-#ifndef BOOZ_SENSORS_MODEL_H
-#define BOOZ_SENSORS_MODEL_H
-
-#include
-#include
-
-#include "pprz_algebra_float.h"
-#include "pprz_geodetic_double.h"
-#include "booz_geometry_int.h"
-#include "6dof.h"
-#include "std.h"
-
-extern void booz_sensors_model_init(double time);
-extern void booz_sensors_model_run( double time);
-extern bool_t booz_sensors_model_accel_available();
-extern bool_t booz_sensors_model_gyro_available();
-extern bool_t booz_sensors_model_mag_available();
-extern bool_t booz_sensors_model_baro_available();
-extern bool_t booz_sensors_model_gps_available();
-
-
-struct BoozSensorsModel {
-
- /* our imu alignement */
- MAT* body_to_imu;
-
- /* Accelerometer */
- VEC* accel;
- unsigned int accel_resolution;
- MAT* accel_sensitivity;
- VEC* accel_neutral;
- VEC* accel_noise_std_dev;
- VEC* accel_bias;
- double accel_next_update;
- int accel_available;
-
-
- /* Gyrometer */
- VEC* gyro;
- unsigned int gyro_resolution;
- MAT* gyro_sensitivity;
- VEC* gyro_neutral;
- VEC* gyro_noise_std_dev;
- VEC* gyro_bias_initial;
- VEC* gyro_bias_random_walk_std_dev;
- VEC* gyro_bias_random_walk_value;
- double gyro_next_update;
- int gyro_available;
-
-
- /* Magnetometer */
- VEC* mag;
- MAT* mag_imu_to_sensor;
- MAT* mag_sensitivity;
- VEC* mag_neutral;
- VEC* mag_noise_std_dev;
- double mag_next_update;
- int mag_available;
-
-
- /* Rangemeter */
- double rangemeter;
- double rangemeter_next_update;
- int rangemeter_available;
-
-
- /* Barometer */
- double baro;
- unsigned int baro_resolution;
- double baro_next_update;
- int baro_available;
-
-
- /* GPS */
- /* state */
- VEC* gps_speed;
- VEC* gps_speed_noise_std_dev;
- GSList* gps_speed_history;
- VEC* gps_pos;
- VEC* gps_pos_noise_std_dev;
- VEC* gps_pos_bias_initial;
- VEC* gps_pos_bias_random_walk_std_dev;
- VEC* gps_pos_bias_random_walk_value;
- GSList* gps_pos_history;
- /* outputs */
- struct LtpDef_d gps_ltp_def;
- struct EcefCoor_d gps_speed_ecef;
- struct EcefCoor_d gps_pos_ecef;
- double gps_pos_utm_north;
- double gps_pos_utm_east;
- double gps_pos_utm_alt;
- // struct Pprz_double_lla gps_pos_lla;
- double gps_speed_course;
- double gps_speed_gspeed;
- double gps_speed_climb;
- double gps_next_update;
- int gps_available;
-
-};
-
-extern struct BoozSensorsModel bsm;
-
-
-#endif /* BOOZ_SENSORS_MODEL_H */
diff --git a/sw/simulator/old_booz/booz_sensors_model_accel.c b/sw/simulator/old_booz/booz_sensors_model_accel.c
deleted file mode 100644
index 77d834bf2f..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_accel.c
+++ /dev/null
@@ -1,100 +0,0 @@
-#include "booz_sensors_model.h"
-
-#include BSM_PARAMS
-#include "booz_sensors_model_utils.h"
-#include "booz_flight_model.h"
-#include "booz_flight_model_utils.h"
-
-
-bool_t booz_sensors_model_accel_available() {
- if (bsm.accel_available) {
- bsm.accel_available = FALSE;
- return TRUE;
- }
- return FALSE;
-}
-
-
-void booz_sensors_model_accel_init(double time) {
-
- bsm.accel = v_get(AXIS_NB);
- bsm.accel->ve[AXIS_X] = 0.;
- bsm.accel->ve[AXIS_Y] = 0.;
- bsm.accel->ve[AXIS_Z] = 0.;
- bsm.accel_resolution = BSM_ACCEL_RESOLUTION;
-
- bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
- m_zero(bsm.accel_sensitivity);
- bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX;
- bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY;
- bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ;
-
- bsm.accel_neutral = v_get(AXIS_NB);
- bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X;
- bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y;
- bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z;
-
- bsm.accel_noise_std_dev = v_get(AXIS_NB);
- bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X;
- bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y;
- bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z;
-
- bsm.accel_bias = v_get(AXIS_NB);
- bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X;
- bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y;
- bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z;
-
- bsm.accel_next_update = time;
- bsm.accel_available = FALSE;
-
-}
-
-void booz_sensors_model_accel_run( double time ) {
- if (time < bsm.accel_next_update)
- return;
-
- static VEC* accel_ltp = VNULL;
- accel_ltp = v_resize(accel_ltp, AXIS_NB);
- /* substract gravity to acceleration in ltp frame */
- accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp);
- /* convert to body frame */
- static VEC* accel_body = VNULL;
- accel_body = v_resize(accel_body, AXIS_NB);
- mv_mlt(bfm.dcm, accel_ltp, accel_body);
- /* convert to imu frame */
- static VEC* accel_imu = VNULL;
- accel_imu = v_resize(accel_imu, AXIS_NB);
- mv_mlt(bsm.body_to_imu, accel_body, accel_imu);
-
-
-
- // printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
-
- /* compute accel reading */
- mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel);
- v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
-
- /* compute accel error readings */
- static VEC *accel_error = VNULL;
- accel_error = v_resize(accel_error, AXIS_NB);
- accel_error = v_zero(accel_error);
- /* add a gaussian noise */
- accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
- /* constant bias */
- accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
- /* scale to adc units FIXME : should use full adc gain ? sum ? */
- accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
- accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
- accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
- /* add per accel error reading */
- bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
- /* round signal to account for adc discretisation */
- RoundSensor(bsm.accel);
- /* saturation */
- BoundSensor(bsm.accel, 0, bsm.accel_resolution);
-
- // printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
- bsm.accel_next_update += BSM_ACCEL_DT;
- bsm.accel_available = TRUE;
-}
-
diff --git a/sw/simulator/old_booz/booz_sensors_model_baro.c b/sw/simulator/old_booz/booz_sensors_model_baro.c
deleted file mode 100644
index e16e3d1c12..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_baro.c
+++ /dev/null
@@ -1,42 +0,0 @@
-#include "booz_sensors_model.h"
-
-#include BSM_PARAMS
-#include "booz_sensors_model_utils.h"
-#include "booz_flight_model.h"
-
-bool_t booz_sensors_model_baro_available() {
- if (bsm.baro_available) {
- bsm.baro_available = FALSE;
- return TRUE;
- }
- return FALSE;
-}
-
-
-void booz_sensors_model_baro_init( double time ) {
- bsm.baro = 0.;
-
- bsm.baro_next_update = time;
- bsm.baro_available = FALSE;
-
-}
-
-void booz_sensors_model_baro_run( double time ) {
- if (time < bsm.baro_next_update)
- return;
-
- if (time < 12.5)
- bsm.baro = 840;
- else {
- double z = bfm.pos_ltp->ve[AXIS_Z] + get_gaussian_noise()*BSM_BARO_NOISE_STD_DEV;
- // double p = ( z / 0.084 ) + BSM_BARO_QNH;
- // double baro_reading = p * BSM_BARO_SENSITIVITY;
- double baro_reading = BSM_BARO_QNH + z * BSM_BARO_SENSITIVITY;
-
- /* FIXME : add noise and random walk */
- baro_reading = rint(baro_reading);
- bsm.baro = baro_reading;
- }
- bsm.baro_next_update += BSM_BARO_DT;
- bsm.baro_available = TRUE;
-}
diff --git a/sw/simulator/old_booz/booz_sensors_model_gps.c b/sw/simulator/old_booz/booz_sensors_model_gps.c
deleted file mode 100644
index c77d2e47c4..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_gps.c
+++ /dev/null
@@ -1,151 +0,0 @@
-#include "booz_sensors_model.h"
-
-#include BSM_PARAMS
-#include "booz_sensors_model_utils.h"
-#include "booz_flight_model.h"
-#include "pprz_geodetic_double.h"
-
-bool_t booz_sensors_model_gps_available() {
- if (bsm.gps_available) {
- bsm.gps_available = FALSE;
- return TRUE;
- }
- return FALSE;
-}
-
-
-
-
-void booz_sensors_model_gps_init( double time ) {
-
- bsm.gps_speed = v_get(AXIS_NB);
- v_zero(bsm.gps_speed);
-
- bsm.gps_speed_noise_std_dev = v_get(AXIS_NB);
- bsm.gps_speed_noise_std_dev->ve[AXIS_X] = BSM_GPS_SPEED_NOISE_STD_DEV;
- bsm.gps_speed_noise_std_dev->ve[AXIS_Y] = BSM_GPS_SPEED_NOISE_STD_DEV;
- bsm.gps_speed_noise_std_dev->ve[AXIS_Z] = BSM_GPS_SPEED_NOISE_STD_DEV;
-
- bsm.gps_speed_history = NULL;
-
- bsm.gps_pos = v_get(AXIS_NB);
- v_zero(bsm.gps_pos);
-
- bsm.gps_pos_noise_std_dev = v_get(AXIS_NB);
- bsm.gps_pos_noise_std_dev->ve[AXIS_X] = BSM_GPS_POS_NOISE_STD_DEV;
- bsm.gps_pos_noise_std_dev->ve[AXIS_Y] = BSM_GPS_POS_NOISE_STD_DEV;
- bsm.gps_pos_noise_std_dev->ve[AXIS_Z] = BSM_GPS_POS_NOISE_STD_DEV;
-
- bsm.gps_pos_bias_initial = v_get(AXIS_NB);
- bsm.gps_pos_bias_initial->ve[AXIS_X] = BSM_GPS_POS_BIAS_INITIAL_X;
- bsm.gps_pos_bias_initial->ve[AXIS_Y] = BSM_GPS_POS_BIAS_INITIAL_Y;
- bsm.gps_pos_bias_initial->ve[AXIS_Z] = BSM_GPS_POS_BIAS_INITIAL_Z;
-
- bsm.gps_pos_bias_random_walk_std_dev = v_get(AXIS_NB);
- bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_X] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X;
- bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Y] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y;
- bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Z] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z;
-
- bsm.gps_pos_bias_random_walk_value = v_get(AXIS_NB);
- bsm.gps_pos_bias_random_walk_value->ve[AXIS_X] = bsm.gps_pos_bias_initial->ve[AXIS_X];
- bsm.gps_pos_bias_random_walk_value->ve[AXIS_Y] = bsm.gps_pos_bias_initial->ve[AXIS_Y];
- bsm.gps_pos_bias_random_walk_value->ve[AXIS_Z] = bsm.gps_pos_bias_initial->ve[AXIS_Z];
-
- bsm.gps_pos_history = NULL;
-
- /* Toulouse */
- struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
- // struct EcefCoor_d ref_coor = { 6368189. , 0., 0.};
-
- ltp_def_from_ecef_d(&bsm.gps_ltp_def, &ref_coor);
-
- bsm.gps_next_update = time;
- bsm.gps_available = FALSE;
-
-}
-
-/* We store our positions like the fdm and convert to UTM and funcky course, gspeed, climb */
-void booz_sensors_model_gps_run( double time) {
- if (time < bsm.gps_next_update)
- return;
-
- /*
- * simulate speed sensor
- */
- static VEC *cur_speed_reading = VNULL;
- cur_speed_reading = v_resize(cur_speed_reading, AXIS_NB);
- v_copy(bfm.speed_ltp, cur_speed_reading);;
- /* add a gaussian noise */
- cur_speed_reading = v_add_gaussian_noise(cur_speed_reading, bsm.gps_speed_noise_std_dev,
- cur_speed_reading);
- UpdateSensorLatency(time, cur_speed_reading, bsm.gps_speed_history, BSM_GPS_SPEED_LATENCY, bsm.gps_speed);
-
- /* course, gspeed, climb convertion */
- bsm.gps_speed_course = DegOfRad(atan2(bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_X]))*10.;
- bsm.gps_speed_course = rint(bsm.gps_speed_course);
- bsm.gps_speed_gspeed = sqrt(bsm.gps_speed->ve[AXIS_X] * bsm.gps_speed->ve[AXIS_X] +
- bsm.gps_speed->ve[AXIS_Y] * bsm.gps_speed->ve[AXIS_Y]) * 100.;
- bsm.gps_speed_gspeed = rint(bsm.gps_speed_gspeed);
- bsm.gps_speed_climb = rint(-bsm.gps_speed->ve[AXIS_Z] * 100);
-
-
- /*
- * simulate position sensor
- */
-
- /* compute position error */
- static VEC *pos_error = VNULL;
- pos_error = v_resize(pos_error, AXIS_NB);
- pos_error = v_zero(pos_error);
- /* add a gaussian noise */
- pos_error = v_add_gaussian_noise(pos_error, bsm.gps_pos_noise_std_dev, pos_error);
- /* update random walk bias */
- bsm.gps_pos_bias_random_walk_value =
- v_update_random_walk(bsm.gps_pos_bias_random_walk_value,
- bsm.gps_pos_bias_random_walk_std_dev, BSM_GPS_DT,
- bsm.gps_pos_bias_random_walk_value);
- /* add it */
- pos_error = v_add(pos_error, bsm.gps_pos_bias_random_walk_value, pos_error);
- /* sum true pos and error reading */
- static VEC *cur_pos_reading = VNULL;
- cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB);
- v_add(bfm.pos_ltp, pos_error, cur_pos_reading);
- /* store that for later and retrieve a previously stored data */
- UpdateSensorLatency(time, cur_pos_reading, &bsm.gps_pos_history, BSM_GPS_POS_LATENCY, bsm.gps_pos);
-
- /* UTM conversion */
- bsm.gps_pos_utm_north = bsm.gps_pos->ve[AXIS_X] * 100. + BSM_GPS_POS_INITIAL_UTM_EAST;
- bsm.gps_pos_utm_north = rint(bsm.gps_pos_utm_north);
- bsm.gps_pos_utm_east = bsm.gps_pos->ve[AXIS_Y] * 100. + BSM_GPS_POS_INITIAL_UTM_NORTH;
- bsm.gps_pos_utm_east = rint(bsm.gps_pos_utm_east);
- bsm.gps_pos_utm_alt = bsm.gps_pos->ve[AXIS_Z] * 100. + BSM_GPS_POS_INITIAL_UTM_ALT;
- bsm.gps_pos_utm_alt = rint(bsm.gps_pos_utm_alt);
-
- /* LLA conversion */
-
-#if 0
-#define LAT0 40.
-#define LON0 1.
-#define GROUND_ALT 180.
-
- bsm.gps_pos_lla.lat = (bsm.gps_pos->ve[AXIS_X] * 9e-6 + LAT0) * 1e7;
- bsm.gps_pos_lla.lat = rint(bsm.gps_pos_lla.lat);
- bsm.gps_pos_lla.lon = (bsm.gps_pos->ve[AXIS_Y] * 9e-6 + LON0) * 1e7;
- bsm.gps_pos_lla.lon = rint(bsm.gps_pos_lla.lon);
- bsm.gps_pos_lla.alt = (bsm.gps_pos->ve[AXIS_Z] + GROUND_ALT)* 100.;
- bsm.gps_pos_lla.alt = rint(bsm.gps_pos_lla.alt);
-#endif
-
- /* ECEF Conversion */
- struct NedCoor_d pos_ned = {bsm.gps_pos->ve[AXIS_X], bsm.gps_pos->ve[AXIS_Y], bsm.gps_pos->ve[AXIS_Z]};
- ecef_of_ned_point_d(&bsm.gps_pos_ecef, &bsm.gps_ltp_def, &pos_ned);
- VECT3_SMUL(bsm.gps_pos_ecef, bsm.gps_pos_ecef, (double)1e2);
- struct NedCoor_d speed_ned = {bsm.gps_speed->ve[AXIS_X], bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_Z]};
- ecef_of_ned_vect_d(&bsm.gps_speed_ecef, &bsm.gps_ltp_def , &speed_ned);
- VECT3_SMUL(bsm.gps_speed_ecef, bsm.gps_speed_ecef, (double)1e2);
-
- bsm.gps_next_update += BSM_GPS_DT;
- bsm.gps_available = TRUE;
-
-}
-
diff --git a/sw/simulator/old_booz/booz_sensors_model_gyro.c b/sw/simulator/old_booz/booz_sensors_model_gyro.c
deleted file mode 100644
index f8f607ca0e..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_gyro.c
+++ /dev/null
@@ -1,102 +0,0 @@
-#include "booz_sensors_model.h"
-
-#include BSM_PARAMS
-#include "booz_sensors_model_utils.h"
-#include "booz_flight_model.h"
-
-bool_t booz_sensors_model_gyro_available() {
- if (bsm.gyro_available) {
- bsm.gyro_available = FALSE;
- return TRUE;
- }
- return FALSE;
-}
-
-void booz_sensors_model_gyro_init(double time) {
-
- bsm.gyro = v_get(AXIS_NB);
- bsm.gyro->ve[AXIS_P] = 0.;
- bsm.gyro->ve[AXIS_Q] = 0.;
- bsm.gyro->ve[AXIS_R] = 0.;
- bsm.gyro_resolution = BSM_GYRO_RESOLUTION;
-
- bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
- m_zero(bsm.gyro_sensitivity);
- bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = BSM_GYRO_SENSITIVITY_PP;
- bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = BSM_GYRO_SENSITIVITY_QQ;
- bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = BSM_GYRO_SENSITIVITY_RR;
-
- bsm.gyro_neutral = v_get(AXIS_NB);
- bsm.gyro_neutral->ve[AXIS_P] = BSM_GYRO_NEUTRAL_P;
- bsm.gyro_neutral->ve[AXIS_Q] = BSM_GYRO_NEUTRAL_Q;
- bsm.gyro_neutral->ve[AXIS_R] = BSM_GYRO_NEUTRAL_R;
-
- bsm.gyro_noise_std_dev = v_get(AXIS_NB);
- bsm.gyro_noise_std_dev->ve[AXIS_P] = BSM_GYRO_NOISE_STD_DEV_P;
- bsm.gyro_noise_std_dev->ve[AXIS_Q] = BSM_GYRO_NOISE_STD_DEV_Q;
- bsm.gyro_noise_std_dev->ve[AXIS_R] = BSM_GYRO_NOISE_STD_DEV_R;
-
- bsm.gyro_bias_initial = v_get(AXIS_NB);
- bsm.gyro_bias_initial->ve[AXIS_P] = BSM_GYRO_BIAS_INITIAL_P;
- bsm.gyro_bias_initial->ve[AXIS_Q] = BSM_GYRO_BIAS_INITIAL_Q;
- bsm.gyro_bias_initial->ve[AXIS_R] = BSM_GYRO_BIAS_INITIAL_R;
-
- bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB);
- bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P;
- bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q;
- bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R;
-
- bsm.gyro_bias_random_walk_value = v_get(AXIS_NB);
- bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P];
- bsm.gyro_bias_random_walk_value->ve[AXIS_Q] = bsm.gyro_bias_initial->ve[AXIS_Q];
- bsm.gyro_bias_random_walk_value->ve[AXIS_R] = bsm.gyro_bias_initial->ve[AXIS_R];
-
- bsm.gyro_next_update = time;
- bsm.gyro_available = FALSE;
-
-}
-
-void booz_sensors_model_gyro_run( double time ) {
- if (time < bsm.gyro_next_update)
- return;
-
- /* rotate to IMU frame */
- /* convert to imu frame */
- static VEC* rate_imu = VNULL;
- rate_imu = v_resize(rate_imu, AXIS_NB);
- mv_mlt(bsm.body_to_imu, bfm.ang_rate_body, rate_imu);
- /* compute gyros readings */
- bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_imu, bsm.gyro);
- bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
-
- /* compute gyro error readings */
- static VEC *gyro_error = VNULL;
- gyro_error = v_resize(gyro_error, AXIS_NB);
- gyro_error = v_zero(gyro_error);
- /* add a gaussian noise */
- gyro_error = v_add_gaussian_noise(gyro_error, bsm.gyro_noise_std_dev, gyro_error);
- /* update random walk bias */
- bsm.gyro_bias_random_walk_value =
- v_update_random_walk(bsm.gyro_bias_random_walk_value,
- bsm.gyro_bias_random_walk_std_dev, BSM_GYRO_DT,
- bsm.gyro_bias_random_walk_value);
- gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error);
- /* scale to adc units FIXME : should use full adc gain ? sum ? */
- gyro_error->ve[AXIS_P] =
- gyro_error->ve[AXIS_P] * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
- gyro_error->ve[AXIS_Q] =
- gyro_error->ve[AXIS_Q] * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
- gyro_error->ve[AXIS_R] =
- gyro_error->ve[AXIS_R] * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
- /* add per gyro error reading */
- bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro);
- /* round signal to account for adc discretisation */
- RoundSensor(bsm.gyro);
- /* saturation */
- BoundSensor(bsm.gyro, 0, bsm.gyro_resolution);
-
- bsm.gyro_next_update += BSM_GYRO_DT;
- bsm.gyro_available = TRUE;
-}
-
-
diff --git a/sw/simulator/old_booz/booz_sensors_model_mag.c b/sw/simulator/old_booz/booz_sensors_model_mag.c
deleted file mode 100644
index 4ad6b65157..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_mag.c
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "booz_sensors_model.h"
-
-#include BSM_PARAMS
-#include "booz_sensors_model_utils.h"
-#include "booz_flight_model.h"
-#include "booz_flight_model_utils.h"
-
-bool_t booz_sensors_model_mag_available() {
- if (bsm.mag_available) {
- bsm.mag_available = FALSE;
- return TRUE;
- }
- return FALSE;
-}
-
-
-void booz_sensors_model_mag_init( double time ) {
-
- bsm.mag = v_get(AXIS_NB);
- bsm.mag->ve[AXIS_X] = 0.;
- bsm.mag->ve[AXIS_Y] = 0.;
- bsm.mag->ve[AXIS_Z] = 0.;
- // bsm.mag_resolution = BSM_MAG_RESOLUTION;
-
- bsm.mag_imu_to_sensor = m_get(AXIS_NB, AXIS_NB);
- VEC* tmp_eulers = v_get(AXIS_NB);
- tmp_eulers->ve[EULER_PHI] = BSM_MAG_IMU_TO_SENSOR_PHI;
- tmp_eulers->ve[EULER_THETA] = BSM_MAG_IMU_TO_SENSOR_THETA;
- tmp_eulers->ve[EULER_PSI] = BSM_MAG_IMU_TO_SENSOR_PSI;
- dcm_of_eulers (tmp_eulers, bsm.mag_imu_to_sensor );
-
- bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
- m_zero(bsm.mag_sensitivity);
- bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = BSM_MAG_SENSITIVITY_XX;
- bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_MAG_SENSITIVITY_YY;
- bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_MAG_SENSITIVITY_ZZ;
-
- bsm.mag_neutral = v_get(AXIS_NB);
- bsm.mag_neutral->ve[AXIS_X] = BSM_MAG_NEUTRAL_X;
- bsm.mag_neutral->ve[AXIS_Y] = BSM_MAG_NEUTRAL_Y;
- bsm.mag_neutral->ve[AXIS_Z] = BSM_MAG_NEUTRAL_Z;
-
- bsm.mag_noise_std_dev = v_get(AXIS_NB);
- bsm.mag_noise_std_dev->ve[AXIS_X] = BSM_MAG_NOISE_STD_DEV_X;
- bsm.mag_noise_std_dev->ve[AXIS_Y] = BSM_MAG_NOISE_STD_DEV_Y;
- bsm.mag_noise_std_dev->ve[AXIS_Z] = BSM_MAG_NOISE_STD_DEV_Z;
-
- bsm.mag_next_update = time;
- bsm.mag_available = FALSE;
-
-}
-
-void booz_sensors_model_mag_run( double time ) {
- if (time < bsm.mag_next_update)
- return;
-
- /* rotate h to body frame */
- static VEC *h_body = VNULL;
- h_body = v_resize(h_body, AXIS_NB);
- mv_mlt(bfm.dcm, bfm.h_ltp, h_body);
- /* rotate to imu frame */
- static VEC *h_imu = VNULL;
- h_imu = v_resize(h_imu, AXIS_NB);
- mv_mlt(bsm.body_to_imu, h_body, h_imu);
- /* rotate to sensor frame */
- static VEC *h_sensor = VNULL;
- h_sensor = v_resize(h_sensor, AXIS_NB);
- mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor);
-
- mv_mlt(bsm.mag_sensitivity, h_sensor, bsm.mag);
- v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
-
- /* compute mag error readings */
- static VEC *mag_error = VNULL;
- mag_error = v_resize(mag_error, AXIS_NB);
- /* add hard iron now ? */
- mag_error = v_zero(mag_error);
- /* add a gaussian noise */
- mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
-
- mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];
- mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];
- mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
-
- /* add error */
- v_add(bsm.mag, mag_error, bsm.mag);
-
- // printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
- // printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
- /* round signal to account for adc discretisation */
- RoundSensor(bsm.mag);
-
- bsm.mag_next_update += BSM_MAG_DT;
- bsm.mag_available = TRUE;
-}
-
diff --git a/sw/simulator/old_booz/booz_sensors_model_params_booz2.h b/sw/simulator/old_booz/booz_sensors_model_params_booz2.h
deleted file mode 100644
index 57dad7195f..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_params_booz2.h
+++ /dev/null
@@ -1,145 +0,0 @@
-/*
- * Copyright (C) 2008 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.
- *
- */
-
-#ifndef BOOZ_SENSORS_MODEL_PARAMS_H
-#define BOOZ_SENSORS_MODEL_PARAMS_H
-
-/*
- * Accelerometer
- */
-#define BSM_ACCEL_RESOLUTION (65536)
-/* ms-2 */
-#define BSM_ACCEL_SENSITIVITY_XX 1250.
-#define BSM_ACCEL_SENSITIVITY_YY 1250.
-#define BSM_ACCEL_SENSITIVITY_ZZ 1250.
-#define BSM_ACCEL_NEUTRAL_X 32768
-#define BSM_ACCEL_NEUTRAL_Y 32768
-#define BSM_ACCEL_NEUTRAL_Z 32768
-/* m2s-4 */
-//#define BSM_ACCEL_NOISE_STD_DEV_X 0
-//#define BSM_ACCEL_NOISE_STD_DEV_Y 0
-//#define BSM_ACCEL_NOISE_STD_DEV_Z 0
-
-#define BSM_ACCEL_NOISE_STD_DEV_X 5e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Y 5e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Z 5e-1
-
-/* ms-2 */
-#define BSM_ACCEL_BIAS_X 0
-#define BSM_ACCEL_BIAS_Y 0
-#define BSM_ACCEL_BIAS_Z 0
-/* s */
-#define BSM_ACCEL_DT (1./1000.)
-
-
-
-/*
- * Gyrometer
- */
-#define BSM_GYRO_RESOLUTION 65536
-
-#define BSM_GYRO_SENSITIVITY_PP (-4541.3261)
-#define BSM_GYRO_SENSITIVITY_QQ (-4651.1628)
-#define BSM_GYRO_SENSITIVITY_RR ( 4752.8517)
-
-#define BSM_GYRO_NEUTRAL_P 32768
-#define BSM_GYRO_NEUTRAL_Q 32768
-#define BSM_GYRO_NEUTRAL_R 32768
-
-#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
-#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
-#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
-
-#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
-#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)
-#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0)
-
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
-
-#define BSM_GYRO_DT (1./1000.)
-
-
-
-/*
- * Magnetometer
- */
-#define BSM_MAG_RESOLUTION 4096
-
-#define BSM_MAG_SENSITIVITY_XX (680)
-#define BSM_MAG_SENSITIVITY_YY (680)
-#define BSM_MAG_SENSITIVITY_ZZ (680)
-
-#define BSM_MAG_NEUTRAL_X 0
-#define BSM_MAG_NEUTRAL_Y 0
-#define BSM_MAG_NEUTRAL_Z 0
-
-#define BSM_MAG_NOISE_STD_DEV_X 0
-#define BSM_MAG_NOISE_STD_DEV_Y 0
-#define BSM_MAG_NOISE_STD_DEV_Z 0
-
-//#define BSM_MAG_NOISE_STD_DEV_X 2e-2
-//#define BSM_MAG_NOISE_STD_DEV_Y 2e-2
-//#define BSM_MAG_NOISE_STD_DEV_Z 2e-2
-
-#define BSM_MAG_DT (1./1000.)
-
-
-/*
- * Range meter
- */
-#define BSM_RANGEMETER_RESOLUTION (1024)
-#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
-#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY)
-#define BSM_RANGEMETER_DT (1./20.)
-
-
-/*
- * Barometer
- */
-#define BSM_BARO_QNH 900.
-#define BSM_BARO_SENSITIVITY 10.
-#define BSM_BARO_DT (1./10.)
-
-
-/*
- * GPS
- */
-#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1
-#define BSM_GPS_SPEED_LATENCY 0.25
-
-#define BSM_GPS_POS_NOISE_STD_DEV 3e-1
-#define BSM_GPS_POS_BIAS_INITIAL_X 1e-1
-#define BSM_GPS_POS_BIAS_INITIAL_Y -1e-1
-#define BSM_GPS_POS_BIAS_INITIAL_Z -5e-1
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-1
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-1
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-1
-#define BSM_GPS_POS_LATENCY 0.25
-#define BSM_GPS_POS_INITIAL_UTM_EAST 37728000
-#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300
-#define BSM_GPS_POS_INITIAL_UTM_ALT 15200
-
-#define BSM_GPS_DT (1./4.)
-
-#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */
diff --git a/sw/simulator/old_booz/booz_sensors_model_params_booz2_a1.h b/sw/simulator/old_booz/booz_sensors_model_params_booz2_a1.h
deleted file mode 100644
index 352a6b0d01..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_params_booz2_a1.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- * Copyright (C) 2008 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.
- *
- */
-
-#ifndef BOOZ_SENSORS_MODEL_PARAMS_H
-#define BOOZ_SENSORS_MODEL_PARAMS_H
-
-#include "generated/airframe.h"
-
-
-#define BSM_BODY_TO_IMU_PHI RadOfDeg(4.)
-#define BSM_BODY_TO_IMU_THETA RadOfDeg(3.)
-//#define BSM_BODY_TO_IMU_PHI RadOfDeg(0.)
-//#define BSM_BODY_TO_IMU_THETA RadOfDeg(0.)
-#define BSM_BODY_TO_IMU_PSI RadOfDeg(0.)
-
-/*
- * Accelerometer
- */
-#define BSM_ACCEL_RESOLUTION (65536)
-/* ms-2 */
-/* aka 2^10/ACCEL_X_SENS */
-#define BSM_ACCEL_SENSITIVITY_XX -408.92695
-#define BSM_ACCEL_SENSITIVITY_YY -412.69325
-#define BSM_ACCEL_SENSITIVITY_ZZ -407.32522
-#define BSM_ACCEL_NEUTRAL_X 32081
-#define BSM_ACCEL_NEUTRAL_Y 33738
-#define BSM_ACCEL_NEUTRAL_Z 32441
-/* m2s-4 */
-//#define BSM_ACCEL_NOISE_STD_DEV_X 0
-//#define BSM_ACCEL_NOISE_STD_DEV_Y 0
-//#define BSM_ACCEL_NOISE_STD_DEV_Z 0
-
-#define BSM_ACCEL_NOISE_STD_DEV_X 1.e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Z 1.1e-1
-
-/* ms-2 */
-#define BSM_ACCEL_BIAS_X 0
-#define BSM_ACCEL_BIAS_Y 0
-#define BSM_ACCEL_BIAS_Z 0
-/* s */
-#define BSM_ACCEL_DT (1./512.)
-
-
-
-/*
- * Gyrometer
- */
-#define BSM_GYRO_RESOLUTION 65536
-
-/* 2^12/GYRO_X_SENS */
-#define BSM_GYRO_SENSITIVITY_PP ( 4055.)
-#define BSM_GYRO_SENSITIVITY_QQ (-4055.)
-#define BSM_GYRO_SENSITIVITY_RR (-4055.)
-
-#define BSM_GYRO_NEUTRAL_P 33924
-#define BSM_GYRO_NEUTRAL_Q 33417
-#define BSM_GYRO_NEUTRAL_R 32809
-
-//#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
-//#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
-//#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
-
-#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
-#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
-#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
-
-#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
-#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)
-#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0)
-
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
-
-#define BSM_GYRO_DT (1./512.)
-
-
-
-/*
- * Magnetometer
- */
-//#define BSM_MAG_RESOLUTION 65536
-
-#define BSM_MAG_IMU_TO_SENSOR_PHI 0.
-#define BSM_MAG_IMU_TO_SENSOR_THETA 0.
-#define BSM_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.)
-
-#define BSM_MAG_SENSITIVITY_XX (1.*(1<<11)/-4.94075530)
-#define BSM_MAG_SENSITIVITY_YY (1.*(1<<11)/ 5.10207664)
-#define BSM_MAG_SENSITIVITY_ZZ (1.*(1<<11)/-4.90788848)
-
-#define BSM_MAG_NEUTRAL_X 2358
-#define BSM_MAG_NEUTRAL_Y 2362
-#define BSM_MAG_NEUTRAL_Z 2119
-
-//#define BSM_MAG_NOISE_STD_DEV_X 0
-//#define BSM_MAG_NOISE_STD_DEV_Y 0
-//#define BSM_MAG_NOISE_STD_DEV_Z 0
-
-#define BSM_MAG_NOISE_STD_DEV_X 2e-3
-#define BSM_MAG_NOISE_STD_DEV_Y 2e-3
-#define BSM_MAG_NOISE_STD_DEV_Z 2e-3
-
-#define BSM_MAG_DT (1./100.)
-
-
-/*
- * Range meter
- */
-#define BSM_RANGEMETER_RESOLUTION (1024)
-#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
-#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY)
-#define BSM_RANGEMETER_DT (1./20.)
-
-
-/*
- * Barometer
- */
-/* m */
-/* aka 2^8/INS_BARO_SENS */
-#define BSM_BARO_QNH 900.
-#define BSM_BARO_SENSITIVITY 17.066667
-#define BSM_BARO_DT (1./100.)
-#define BSM_BARO_NOISE_STD_DEV 5.e-2
-
-/*
- * GPS
- */
-
-#ifdef GPS_PERFECT
-
-#define BSM_GPS_SPEED_NOISE_STD_DEV 0.
-#define BSM_GPS_SPEED_LATENCY 0.
-#define BSM_GPS_POS_NOISE_STD_DEV 0.001
-#define BSM_GPS_POS_BIAS_INITIAL_X 0.
-#define BSM_GPS_POS_BIAS_INITIAL_Y 0.
-#define BSM_GPS_POS_BIAS_INITIAL_Z 0.
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
-#define BSM_GPS_POS_LATENCY 0.
-
-#else
-
-#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1
-#define BSM_GPS_SPEED_LATENCY 0.25
-#define BSM_GPS_POS_NOISE_STD_DEV 1e-1
-#define BSM_GPS_POS_BIAS_INITIAL_X 0e-1
-#define BSM_GPS_POS_BIAS_INITIAL_Y -0e-1
-#define BSM_GPS_POS_BIAS_INITIAL_Z -0e-1
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
-#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
-#define BSM_GPS_POS_LATENCY 0.25
-
-#endif /* GPS_PERFECT */
-
-#define BSM_GPS_POS_INITIAL_UTM_EAST 37728000
-#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300
-#define BSM_GPS_POS_INITIAL_UTM_ALT 15200
-
-#define BSM_GPS_DT (1./4.)
-
-#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */
diff --git a/sw/simulator/old_booz/booz_sensors_model_rangemeter.c b/sw/simulator/old_booz/booz_sensors_model_rangemeter.c
deleted file mode 100644
index e1a8ed7d7d..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_rangemeter.c
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "booz_sensors_model.h"
-
-#include BSM_PARAMS
-#include "booz_sensors_model_utils.h"
-#include "booz_flight_model.h"
-
-bool_t booz_sensors_model_rangemeter_available() {
- if (bsm.rangemeter_available) {
- bsm.rangemeter_available = FALSE;
- return TRUE;
- }
- return FALSE;
-}
-
-
-void booz_sensors_model_rangemeter_init(double time) {
-
- bsm.rangemeter = 0.;
-
- bsm.rangemeter_next_update = time;
- bsm.rangemeter_available = FALSE;
-
-}
-
-
-void booz_sensors_model_rangemeter_run( double time ) {
- if (time < bsm.rangemeter_next_update)
- return;
-
- /* compute dist from ground */
- double dz = bfm.pos_ltp->ve[AXIS_Z];
- if (dz > 0.) dz = 0.;
- double dx = dz * tan(bfm.eulers->ve[EULER_THETA]);
- double dy = dz * tan(bfm.eulers->ve[EULER_PHI]);
- double dist = sqrt( dx*dx + dy*dy + dz*dz);
- dist *= BSM_RANGEMETER_SENSITIVITY;
- /* add gaussian noise */
-
- if (dist > BSM_RANGEMETER_MAX_RANGE)
- dist = BSM_RANGEMETER_MAX_RANGE;
- dist = rint(dist);
- bsm.rangemeter = dist;
-
- bsm.rangemeter_next_update += BSM_RANGEMETER_DT;
- bsm.rangemeter_available = TRUE;
-
-}
diff --git a/sw/simulator/old_booz/booz_sensors_model_utils.c b/sw/simulator/old_booz/booz_sensors_model_utils.c
deleted file mode 100644
index 29ba3f2530..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_utils.c
+++ /dev/null
@@ -1,94 +0,0 @@
-#include "booz_sensors_model_utils.h"
-
-#include "6dof.h"
-#include
-
-void UpdateSensorLatency(double time, VEC* cur_reading, GSList **history,
- double latency, VEC* sensor_reading) {
- /* add new reading */
- struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1);
- cur_read->time = time;
- cur_read->value = v_get(AXIS_NB);
- v_copy(cur_reading, cur_read->value);
- *history = g_slist_prepend(*history, cur_read);
- /* remove old readings */
- GSList* last = g_slist_last(*history);
- while (last &&
- ((struct BoozDatedSensor*)last->data)->time < time - latency) {
- *history = g_slist_remove_link(*history, last);
- v_free(((struct BoozDatedSensor*)last->data)->value);
- g_free((struct BoozDatedSensor*)last->data);
- g_slist_free(last);
- last = g_slist_last(*history);
- }
- /* update sensor */
- v_copy(((struct BoozDatedSensor*)last->data)->value, sensor_reading);
-}
-
-#define THAU 5.
-VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out) {
-
- static VEC *drw = VNULL;
- drw = v_resize(drw, AXIS_NB);
- v_get_gaussian_noise(std_dev, drw);
-
- static VEC *tmp = VNULL;
- tmp = v_resize(tmp, AXIS_NB);
- v_copy(in, tmp);
- sv_mlt((-1./THAU), tmp, tmp);
- v_add(drw, tmp, drw);
- sv_mlt(dt, drw, drw);
- v_add(drw, in, out);
- return out;
-}
-
-VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) {
- static VEC *tmp = VNULL;
- tmp = v_resize(tmp, in->dim);
- unsigned int i;
- for (i=0; idim; i++)
- tmp->ve[i] = get_gaussian_noise() * std_dev->ve[i];
- v_add(in, tmp, out);
- return out;
-}
-
-VEC* v_get_gaussian_noise(VEC* std_dev, VEC* out) {
-
- unsigned int i;
- for (i=0; idim; i++)
- out->ve[i] = get_gaussian_noise() * std_dev->ve[i];
- return out;
-
-}
-
-/*
- http://www.taygeta.com/random/gaussian.html
-*/
-
-#include "booz_r250.h"
-
-double get_gaussian_noise(void) {
-
- double x1;
- static int nb_call = 0;
- static double x2, w;
- if (nb_call==0) r250_init(0);
- nb_call++;
- if (nb_call%2) {
- do {
- x1 = 2.0 * dr250() - 1.0;
- x2 = 2.0 * dr250() - 1.0;
- w = x1 * x1 + x2 * x2;
- } while ( w >= 1.0 );
-
- w = sqrt( (-2.0 * log( w ) ) / w );
- return x1 * w;
- }
- else
- return x2 * w;
-}
-
-
-
-
-
diff --git a/sw/simulator/old_booz/booz_sensors_model_utils.h b/sw/simulator/old_booz/booz_sensors_model_utils.h
deleted file mode 100644
index c4cb8278fd..0000000000
--- a/sw/simulator/old_booz/booz_sensors_model_utils.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef BOOZ_SENSORS_MODEL_UTILS_H
-#define BOOZ_SENSORS_MODEL_UTILS_H
-
-#include
-#include
-
-struct BoozDatedSensor {
- VEC* value;
- double time;
-};
-
-extern void UpdateSensorLatency(double time, VEC* cur_reading, GSList **history,
- double latency, VEC* sensor_reading);
-extern VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out);
-extern VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
-extern VEC* v_get_gaussian_noise(VEC* std_dev, VEC* out);
-extern double get_gaussian_noise();
-
-#define RoundSensor(_sensor) { \
- _sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \
- _sensor->ve[AXIS_Y] = rint(_sensor->ve[AXIS_Y]); \
- _sensor->ve[AXIS_Z] = rint(_sensor->ve[AXIS_Z]); \
- }
-
-#define BoundSensor(_sensor, _min, _max) { \
- if ( _sensor->ve[AXIS_X] < _min) _sensor->ve[AXIS_X] = _min; \
- if ( _sensor->ve[AXIS_X] > _max) _sensor->ve[AXIS_X] = _max; \
- if ( _sensor->ve[AXIS_Y] < _min) _sensor->ve[AXIS_Y] = _min; \
- if ( _sensor->ve[AXIS_Y] > _max) _sensor->ve[AXIS_Y] = _max; \
- if ( _sensor->ve[AXIS_Z] < _min) _sensor->ve[AXIS_Z] = _min; \
- if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \
- }
-
-
-
-#endif /* BOOZ_SENSORS_MODEL_UTILS_H */
diff --git a/sw/simulator/old_booz/booz_wind_model.c b/sw/simulator/old_booz/booz_wind_model.c
deleted file mode 100644
index 6aaa633e71..0000000000
--- a/sw/simulator/old_booz/booz_wind_model.c
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "booz_wind_model.h"
-
-#include "6dof.h"
-#include "booz_flight_model_utils.h"
-
-#if 0
-static void booz_wind_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
-#endif
-
-struct BoozWindModel bwm;
-
-#define BWM_STATE_SIZE 6
-
-#define BWM_WM 1.5
-#define BWM_ZETA 5e-5
-#define BWM_STD_DEV 3.3
-#define BWM_MAX_X 3.
-#define BWM_MAX_Y 3.
-#define BWM_MAX_Z 3.
-
-void booz_wind_model_init( void ) {
-
- bwm.velocity = v_get(AXIS_NB);
- v_zero(bwm.velocity);
- bwm.velocity->ve[AXIS_X] = INIT_WIND_X;
- bwm.velocity->ve[AXIS_Y] = INIT_WIND_Y;
- bwm.velocity->ve[AXIS_Z] = INIT_WIND_Z;
-
-
- bwm.state = v_get(BWM_STATE_SIZE);
- v_zero(bwm.state);
-
-}
-
-
-void booz_wind_model_run( double dt __attribute__ ((unused))) {
-
-
-#if 0
- static VEC *u = VNULL;
- u = v_resize(u, AXIS_NB);
- u = v_rand(u);
- static VEC *one = VNULL;
- one = v_resize(one, AXIS_NB);
- one = v_ones(one);
- u = v_mltadd(one, u, -2., u);
- u = sv_mlt((BWM_STD_DEV * BWM_STD_DEV), u, u);
-
- rk4(booz_wind_model_get_derivatives, bwm.state, u, dt);
-
- bwm.velocity->ve[AXIS_X] = bwm.state->ve[0] * BWM_WM * BWM_WM * BWM_MAX_X;
- bwm.velocity->ve[AXIS_Y] = bwm.state->ve[2] * BWM_WM * BWM_WM * BWM_MAX_Y;
- bwm.velocity->ve[AXIS_Z] = bwm.state->ve[4] * BWM_WM * BWM_WM * BWM_MAX_Z;
-#endif
-
-
-}
-
-
-#if 0
-
-static void booz_wind_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
-
- Xdot->ve[0] = X->ve[1];
- Xdot->ve[1] = -2. * BWM_ZETA * BWM_WM * X->ve[1] -
- BWM_WM * BWM_WM * X->ve[0] + u->ve[AXIS_X];
- Xdot->ve[2] = X->ve[3];
- Xdot->ve[3] = -2. * BWM_ZETA * BWM_WM * X->ve[3] -
- BWM_WM * BWM_WM * X->ve[2] + u->ve[AXIS_Y];
- Xdot->ve[4] = X->ve[5];
- Xdot->ve[5] = -2. * BWM_ZETA * BWM_WM * X->ve[5] -
- BWM_WM * BWM_WM * X->ve[4] + u->ve[AXIS_Z];
-
-}
-
-#endif
diff --git a/sw/simulator/old_booz/booz_wind_model.h b/sw/simulator/old_booz/booz_wind_model.h
deleted file mode 100644
index 49e65a65a8..0000000000
--- a/sw/simulator/old_booz/booz_wind_model.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef BOOZ_WIND_MODEL_H
-#define BOOZ_WIND_MODEL_H
-
-#include
-
-struct BoozWindModel {
-
- /* velocity in earth tp frame */
- VEC* velocity;
-
- /* internal state */
- VEC* state;
-
-};
-
-extern struct BoozWindModel bwm;
-
-extern void booz_wind_model_init( void );
-extern void booz_wind_model_run( double dt );
-
-#endif /* BOOZ_WIND_MODEL_H */
diff --git a/sw/simulator/old_booz/tests/Makefile b/sw/simulator/old_booz/tests/Makefile
deleted file mode 100644
index 4fbd86e779..0000000000
--- a/sw/simulator/old_booz/tests/Makefile
+++ /dev/null
@@ -1,76 +0,0 @@
-#
-# NPS
-#
-#JSBSIM = /usr/local
-
-Q=@
-
-#CC = g++
-#CFLAGS = -Wall -I$(JSBSIM)/include/JSBSim -I../include
-#LDFLAGS = -L$(JSBSIM)/lib -lJSBSim
-#CFLAGS += -I/usr/include/meschach -I/usr/local/include/
-#LDFLAGS += -lmeschach -L/usr/lib
-#CFLAGS += `pkg-config glib-2.0 --cflags`
-#LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lglibivy -lpcre
-
-#test1: nps_test1.c nps_jsbsim.c
-# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-#test2: nps_test2.c
-# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-#test_nps_fdm: test_nps_fdm.c nps_fdm.c
-# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-
-JSBSIM = /home/violato/enac/programs/install_jsbsim
-
-CFLAGS = -Wall \
- -I.. \
- -I../../../var/BOOZ2_A1 \
- -I../../airborne \
- -I../../include \
- -I$(JSBSIM)/include/JSBSim \
- `pkg-config glib-2.0 --cflags` \
-
-LDFLAGS = -lm \
- -lglibivy \
- -L$(JSBSIM)/lib -lJSBSim \
- `pkg-config glib-2.0 --libs` \
-
-SIMDIR = ..
-
-#
-#
-#
-
-TEST_FDM_SRCS = nps_test_fdm.c \
- $(SIMDIR)/nps_fdm_jsbsim.c \
-
-
-nps_test_fdm : $(TEST_FDM_SRCS)
- g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-
-#
-#
-#
-
-TEST_SENSORS_SRCS = test_sensors.c \
- $(SIMDIR)/booz_flight_model.c \
- $(SIMDIR)/booz_flight_model_utils.c \
- $(SIMDIR)/booz_sensors_model.c \
- $(SIMDIR)/booz_sensors_model_utils.c \
- $(SIMDIR)/booz_sensors_model_accel.c \
- $(SIMDIR)/booz_sensors_model_gyro.c \
- $(SIMDIR)/booz_sensors_model_mag.c \
- $(SIMDIR)/booz_sensors_model_rangemeter.c \
- $(SIMDIR)/booz_sensors_model_baro.c \
- $(SIMDIR)/booz_sensors_model_gps.c \
-
-test_sensors : $(TEST_SENSORS_SRCS)
- gcc $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-clean:
- $(Q)rm -f *.o
-
diff --git a/sw/simulator/old_booz/tests/test_fdm.c b/sw/simulator/old_booz/tests/test_fdm.c
deleted file mode 100644
index bb7cb44156..0000000000
--- a/sw/simulator/old_booz/tests/test_fdm.c
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "booz_flight_model.h"
-
-
-#define DT_FDM (1./250.)
-#define NB_ITER 100
-
-int main(int argc, char** argv) {
-
- double actuators_values[] = {0.6, 0.6, 0.6, 0.6};
- double time = 0.;
-
- booz_flight_model_init();
-
- bfm.on_ground = FALSE;
-
- int i;
- for (i=0; ive[BFMS_Z]);
- }
-
- return 0;
-}
diff --git a/sw/simulator/old_booz/tests/test_sensors.c b/sw/simulator/old_booz/tests/test_sensors.c
deleted file mode 100644
index f712bc9876..0000000000
--- a/sw/simulator/old_booz/tests/test_sensors.c
+++ /dev/null
@@ -1,32 +0,0 @@
-#include "booz_flight_model.h"
-#include "booz_sensors_model.h"
-
-
-#define DT_FDM (1./250.)
-#define NB_ITER 100
-
-int main(int argc, char** argv) {
-
- double actuators_values[] = {0.6, 0.6, 0.6, 0.6};
- double time = 0.;
-
- booz_flight_model_init();
- booz_sensors_model_init(time);
-
- bfm.on_ground = FALSE;
-
- int i;
- for (i=0; ive[AXIS_Z], bsm.gps_speed->ve[AXIS_Z]);
- if (booz_sensors_model_accel_available())
- printf("%f \t %f \t %f \t %f\n", time, bsm.accel->ve[AXIS_X], bsm.accel->ve[AXIS_Y], bsm.accel->ve[AXIS_Z]);
- }
-
- return 0;
-}
diff --git a/sw/simulator/sim_ac_booz.c b/sw/simulator/sim_ac_booz.c
deleted file mode 100644
index acc08ccd2d..0000000000
--- a/sw/simulator/sim_ac_booz.c
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * Copyright (C) 2008 Gautier Hattenberger
- *
- * 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.
- *
- */
-
-#include
-#include "sim_ac_jsbsim.h"
-#include "firmwares/fixedwing/main_ap.h"
-#include "firmwares/fixedwing/main_fbw.h"
-
-using namespace JSBSim;
-
-//static void sim_gps_feed_data(void);
-//static void sim_ir_feed_data(void);
-
-
-void airborne_run_one_step(void) {
- // SEE sim_run_one_step
-}
-
-void sim_autopilot_init(void) {
- init_fbw();
- init_ap();
-}
-
-void autopilot_periodic_task(void) {
- periodic_task_ap();
- periodic_task_fbw();
-}
-
-void autopilot_event_task(void) {
- event_task_ap();
- event_task_fbw();
-}
-
-void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) {
-
- FGPropertyManager* cur_node;
- double cur_value;
- char buf[64];
- const char* state[] = {"front_motor",
- "back_motor",
- "right_motor",
- "left_motor"};
-
- for (int i=0; i<4; i++) {
- sprintf(buf,"fcs/%s",state[i]);
- FDMExec->GetPropertyManager()->SetDouble(buf,0.5);
- cur_node = FDMExec->GetPropertyManager()->GetNode(buf);
- cur_value = cur_node->getDoubleValue();
- cout << state[i] << " " << cur_value << endl;
- }
-}
-
-void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) {
-
- FGPropertyManager* cur_node;
- double cur_value, factor=1;
- char buf[64];
- const char* state[] = {"sim-time-sec",
- "lat-gc-deg","long-gc-deg","h-sl-ft",
- "u-fps","v-fps","w-fps",
- "theta-deg","phi-deg","psi-true-deg",
- "p-rad_sec","q-rad_sec","r-rad_sec"};
- int i=0;
-
- cur_node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec");
- cur_value = cur_node->getDoubleValue();
- cout << state[i] << cur_value << endl;
-
- for (i=1; i<12+1; i++) {
- sprintf(buf,"ic/%s",state[i]);
- if (strstr(state[i],"rad_")!=NULL) factor=RAD2DEG;
- if (strstr(state[i],"fps")!=NULL || strstr(state[i],"ft")!=NULL) factor=FT2M;
- cur_node = FDMExec->GetPropertyManager()->GetNode(buf);
- cur_value = factor*(cur_node->getDoubleValue());
- cout << state[i] << " " << cur_value << endl;
- factor = 1;
- }
-
-}
diff --git a/sw/supervision/paparazzicenter.glade b/sw/supervision/paparazzicenter.glade
index e68aa24f77..3f71df82a9 100644
--- a/sw/supervision/paparazzicenter.glade
+++ b/sw/supervision/paparazzicenter.glade
@@ -151,6 +151,17 @@
+
+
+
-
-
- True
- True
- False
- Add a target in the combo list
- False
-
-
- True
- False
- 0
- 0
-
-
- True
- False
- 2
-
-
- True
- False
- gtk-add
-
-
- False
- False
- 0
-
-
-
-
- True
- False
- New Target
- True
-
-
- False
- False
- 1
-
-
-
-
-
-
-
-
- False
- False
- 1
-
-
False
diff --git a/sw/supervision/paparazzicenter.ml b/sw/supervision/paparazzicenter.ml
index 966475d7c6..8d949b2be9 100644
--- a/sw/supervision/paparazzicenter.ml
+++ b/sw/supervision/paparazzicenter.ml
@@ -190,8 +190,6 @@ let () =
gui#button_clean#misc#set_sensitive false;
gui#button_build#misc#set_sensitive false;
- AC.ac_combo_handler gui ac_combo target_combo;
-
(* Change the buffer of the text view to attach a tag_table *)
let background_tags =
List.map (fun color ->
@@ -235,6 +233,8 @@ let () =
let end_mark = gui#console#buffer#create_mark end_iter in
gui#console#scroll_mark_onscreen (`MARK end_mark) in
+ AC.ac_combo_handler gui ac_combo target_combo log;
+
AC.build_handler ~file gui ac_combo target_combo log;
let session_combo, execute_session = CP.supervision ~file gui log ac_combo target_combo in
diff --git a/sw/supervision/pc_aircraft.ml b/sw/supervision/pc_aircraft.ml
index 1537747e69..a25cedaff0 100644
--- a/sw/supervision/pc_aircraft.ml
+++ b/sw/supervision/pc_aircraft.ml
@@ -174,7 +174,7 @@ let first_word = fun s ->
(** Parse Airframe File for Targets **)
-let parse_ac_targets = fun target_combo ac_file ->
+let parse_ac_targets = fun target_combo ac_file (log:string->unit) ->
let strings = ref [] in
let count = ref 0 in
let (store, column) = Gtk_tools.combo_model target_combo in
@@ -182,7 +182,7 @@ let parse_ac_targets = fun target_combo ac_file ->
(** Clear ComboBox
**)
(try
- let af_xml = Xml.parse_file (Env.paparazzi_src // "conf" // ac_file) in
+ let af_xml = Xml.parse_file (Env.paparazzi_home // "conf" // ac_file) in
List.iter (fun tag ->
if ExtXml.tag_is tag "firmware" then begin
begin try
@@ -215,11 +215,11 @@ let parse_ac_targets = fun target_combo ac_file ->
(**
Gtk_tools.combo (!strings) target_combo
**)
- with _ -> ())
+ with _ -> log (sprintf "Error while parsing targets from file %s\n" ac_file))
(* Link A/C to airframe & flight_plan labels *)
-let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo ->
+let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo (log:string->unit) ->
(* build tree for settings *)
let tree_set = Gtk_tools.tree gui#tree_settings in
let model = Gtk_tools.tree_model tree_set in
@@ -249,7 +249,7 @@ let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo ->
current_color := gui_color;
gui#entry_ac_id#set_text ac_id;
(Gtk_tools.combo_widget target_combo)#misc#set_sensitive true;
- parse_ac_targets target_combo (ExtXml.attrib aircraft "airframe");
+ parse_ac_targets target_combo (ExtXml.attrib aircraft "airframe") log;
with
Not_found ->
gui#label_airframe#set_text "";
@@ -295,6 +295,18 @@ let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo ->
in
ignore (gui#delete_ac_menu_item#connect#activate ~callback);
+ (* New Target button *)
+ let callback = fun _ ->
+ match GToolbox.input_string ~title:"New Target" ~text:"tunnel" "New build target ?" with
+ None -> ()
+ | Some s ->
+ let (store, column) = Gtk_tools.combo_model target_combo in
+ let row = store#append () in
+ store#set ~row ~column s;
+ (Gtk_tools.combo_widget target_combo)#set_active_iter (Some row)
+ in
+ ignore (gui#menu_item_new_target#connect#activate ~callback);
+
(* GUI color *)
let callback = fun _ ->
let csd = GWindow.color_selection_dialog ~show:true () in
@@ -365,18 +377,6 @@ let build_handler = fun ~file gui ac_combo (target_combo:Gtk_tools.combo) (log:s
(* if target is sim or nps, deactivate the upload button *)
gui#button_upload#misc#set_sensitive (target <> "sim" && target <> "nps"));
- (* New Target button *)
- let callback = fun _ ->
- match GToolbox.input_string ~title:"New Target" ~text:"tunnel" "New build target ?" with
- None -> ()
- | Some s ->
- let (store, column) = Gtk_tools.combo_model target_combo in
- let row = store#append () in
- store#set ~row ~column s;
- (Gtk_tools.combo_widget target_combo)#set_active_iter (Some row)
- in
- ignore (gui#button_new_target#connect#clicked ~callback);
-
(* Clean button *)
let callback = fun () ->
Utils.command ~file gui log (Gtk_tools.combo_value ac_combo) "clean_ac" in
diff --git a/sw/tools/calibration/calibrate_gyro.py b/sw/tools/calibration/calibrate_gyro.py
index 77aac9f076..fd28767ccd 100755
--- a/sw/tools/calibration/calibrate_gyro.py
+++ b/sw/tools/calibration/calibrate_gyro.py
@@ -23,14 +23,16 @@
#
# calibrate gyrometers using turntable measurements
#
+
+from __future__ import print_function, division
+
from optparse import OptionParser
import os
import sys
-import re
-import scipy
-from scipy import linspace, polyval, polyfit, sqrt, stats, randn
-from pylab import *
+from scipy import linspace, polyval, stats
+
+import matplotlib.pyplot as plt
import calibration_utils
@@ -94,7 +96,7 @@ def main():
print("Was looking for IMU_TURNTABLE from id: "+str(options.tt_id)+" and IMU_GYRO_RAW from id: "+str(options.ac_id)+" in file "+filename)
sys.exit(1)
if options.verbose:
- print("found "+str(len(samples))+" records")
+ print("found "+str(len(samples))+" records")
if options.axis == 'p':
axis_idx = 1
@@ -120,21 +122,21 @@ def main():
ovl_omega = linspace(1, 7.5, 10)
ovl_adc = polyval([a_s, b_s], ovl_omega)
- title('Linear Regression Example')
- subplot(3, 1, 1)
- plot(samples[:, 1])
- plot(samples[:, 2])
- plot(samples[:, 3])
- legend(['p', 'q', 'r'])
+ plt.title('Linear Regression Example')
+ plt.subplot(3, 1, 1)
+ plt.plot(samples[:, 1])
+ plt.plot(samples[:, 2])
+ plt.plot(samples[:, 3])
+ plt.legend(['p', 'q', 'r'])
- subplot(3, 1, 2)
- plot(samples[:, 0])
+ plt.subplot(3, 1, 2)
+ plt.plot(samples[:, 0])
- subplot(3, 1, 3)
- plot(samples[:, 0], samples[:, axis_idx], 'b.')
- plot(ovl_omega, ovl_adc, 'r')
+ plt.subplot(3, 1, 3)
+ plt.plot(samples[:, 0], samples[:, axis_idx], 'b.')
+ plt.plot(ovl_omega, ovl_adc, 'r')
- show()
+ plt.show()
if __name__ == "__main__":
diff --git a/sw/tools/calibration/calibrate_mag_current.py b/sw/tools/calibration/calibrate_mag_current.py
index 6779f0f9a1..3f26fc3901 100755
--- a/sw/tools/calibration/calibrate_mag_current.py
+++ b/sw/tools/calibration/calibrate_mag_current.py
@@ -22,11 +22,10 @@
import sys
import os
from optparse import OptionParser
-import scipy
-from scipy import optimize
import calibration_utils
+
def main():
usage = "usage: %prog [options] log_filename.data" + "\n" + "Run %prog --help to list the options."
parser = OptionParser(usage)
@@ -40,6 +39,7 @@ def main():
action="store_true", dest="verbose")
(options, args) = parser.parse_args()
options.sensor = "MAG"
+
if len(args) != 1:
parser.error("incorrect number of arguments")
else:
@@ -48,6 +48,7 @@ def main():
else:
print(args[0] + " not found")
sys.exit(1)
+
ac_ids = calibration_utils.get_ids_in_log(filename)
if options.ac_id is None:
if len(ac_ids) == 1:
@@ -57,7 +58,6 @@ def main():
if options.verbose:
print("Using aircraft id "+options.ac_id)
-
if not filename.endswith(".data"):
parser.error("Please specify a *.data log file")
if options.verbose:
diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py
index 4fc13aab6c..4ef66bc675 100644
--- a/sw/tools/calibration/calibration_utils.py
+++ b/sw/tools/calibration/calibration_utils.py
@@ -1,5 +1,4 @@
-# $Id$
# Copyright (C) 2010 Antoine Drouin
#
# This file is part of Paparazzi.
@@ -20,17 +19,20 @@
# Boston, MA 02111-1307, USA.
#
+from __future__ import print_function, division
+
import re
-import scipy
-from scipy import linalg
-from scipy import stats
-from pylab import *
+import numpy as np
+from numpy import sin, cos
+from scipy import linalg, stats
+
+import matplotlib
+import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
-#
-# returns available ac_id from a log
-#
+
def get_ids_in_log(filename):
+ """Returns available ac_id from a log."""
f = open(filename, 'r')
ids = []
pattern = re.compile("\S+ (\S+)")
@@ -40,15 +42,14 @@ def get_ids_in_log(filename):
break
m = re.match(pattern, line)
if m:
- id = m.group(1)
- if not id in ids:
- ids.append(id)
+ ac_id = m.group(1)
+ if not ac_id in ids:
+ ids.append(id)
return ids
-#
-# extracts raw sensor measurements from a log
-#
+
def read_log(ac_id, filename, sensor):
+ """Extracts raw sensor measurements from a log."""
f = open(filename, 'r')
pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW (\S+) (\S+) (\S+)")
list_meas = []
@@ -59,12 +60,11 @@ def read_log(ac_id, filename, sensor):
m = re.match(pattern, line)
if m:
list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4))])
- return scipy.array(list_meas)
+ return np.array(list_meas)
+
-#
-# extracts raw magnetometer and current measurements from a log
-#
def read_log_mag_current(ac_id, filename):
+ """Extracts raw magnetometer and current measurements from a log."""
f = open(filename, 'r')
pattern = re.compile("(\S+) "+ac_id+" IMU_MAG_CURRENT_CALIBRATION (\S+) (\S+) (\S+) (\S+)")
list_meas = []
@@ -75,59 +75,52 @@ def read_log_mag_current(ac_id, filename):
m = re.match(pattern, line)
if m:
list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4)), float(m.group(5))])
- return scipy.array(list_meas)
+ return np.array(list_meas)
+
-#
-# select only non-noisy data
-#
def filter_meas(meas, window_size, noise_threshold):
+ """Select only non-noisy data."""
filtered_meas = []
filtered_idx = []
for i in range(window_size, len(meas)-window_size):
- noise = meas[i-window_size:i+window_size,:].std(axis=0)
- if linalg.norm(noise) < noise_threshold:
- filtered_meas.append(meas[i,:])
+ noise = meas[i-window_size:i+window_size, :].std(axis=0)
+ if linalg.norm(noise) < noise_threshold:
+ filtered_meas.append(meas[i, :])
filtered_idx.append(i)
- return scipy.array(filtered_meas), filtered_idx
+ return np.array(filtered_meas), filtered_idx
-#
-# initial boundary based calibration
-#
def get_min_max_guess(meas, scale):
- max_meas = meas[:,:].max(axis=0)
- min_meas = meas[:,:].min(axis=0)
+ """Initial boundary based calibration."""
+ max_meas = meas[:, :].max(axis=0)
+ min_meas = meas[:, :].min(axis=0)
n = (max_meas + min_meas) / 2
sf = 2*scale/(max_meas - min_meas)
- return scipy.array([n[0], n[1], n[2], sf[0], sf[1], sf[2]])
+ return np.array([n[0], n[1], n[2], sf[0], sf[1], sf[2]])
-#
-# scale the set of measurements
-#
def scale_measurements(meas, p):
+ """Scale the set of measurements."""
l_comp = []
l_norm = []
- for m in meas[:,]:
+ for m in meas[:, ]:
sm = (m - p[0:3])*p[3:6]
l_comp.append(sm)
l_norm.append(linalg.norm(sm))
- return scipy.array(l_comp), scipy.array(l_norm)
+ return np.array(l_comp), np.array(l_norm)
+
-#
-# calculate linear coefficient of magnetometer-current relation
-#
def estimate_mag_current_relation(meas):
+ """Calculate linear coefficient of magnetometer-current relation."""
coefficient = []
- for i in range(0,3):
- gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:,3], meas[:,i])
+ for i in range(0, 3):
+ gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:, 3], meas[:, i])
coefficient.append(gradient)
return coefficient
-#
-# print xml for airframe file
-#
+
def print_xml(p, sensor, res):
+ """Print xml for airframe file."""
print("")
print("")
print("")
@@ -137,64 +130,60 @@ def print_xml(p, sensor, res):
print("")
-
-#
-# plot calibration results
-#
def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref):
- subplot(3, 1, 1)
- plot(measurements[:, 0])
- plot(measurements[:, 1])
- plot(measurements[:, 2])
- plot(flt_idx, flt_meas[:, 0], 'ro')
- plot(flt_idx, flt_meas[:, 1], 'ro')
- plot(flt_idx, flt_meas[:, 2], 'ro')
- xlabel('time (s)')
- ylabel('ADC')
- title('Raw sensors')
+ """Plot calibration results."""
+ plt.subplot(3, 1, 1)
+ plt.plot(measurements[:, 0])
+ plt.plot(measurements[:, 1])
+ plt.plot(measurements[:, 2])
+ plt.plot(flt_idx, flt_meas[:, 0], 'ro')
+ plt.plot(flt_idx, flt_meas[:, 1], 'ro')
+ plt.plot(flt_idx, flt_meas[:, 2], 'ro')
+ plt.xlabel('time (s)')
+ plt.ylabel('ADC')
+ plt.title('Raw sensors')
- subplot(3, 2, 3)
- plot(cp0[:, 0])
- plot(cp0[:, 1])
- plot(cp0[:, 2])
- plot(-sensor_ref*scipy.ones(len(flt_meas)))
- plot(sensor_ref*scipy.ones(len(flt_meas)))
+ plt.subplot(3, 2, 3)
+ plt.plot(cp0[:, 0])
+ plt.plot(cp0[:, 1])
+ plt.plot(cp0[:, 2])
+ plt.plot(-sensor_ref*np.ones(len(flt_meas)))
+ plt.plot(sensor_ref*np.ones(len(flt_meas)))
- subplot(3, 2, 4)
- plot(np0)
- plot(sensor_ref*scipy.ones(len(flt_meas)))
+ plt.subplot(3, 2, 4)
+ plt.plot(np0)
+ plt.plot(sensor_ref*np.ones(len(flt_meas)))
- subplot(3, 2, 5)
- plot(cp1[:, 0])
- plot(cp1[:, 1])
- plot(cp1[:, 2])
- plot(-sensor_ref*scipy.ones(len(flt_meas)))
- plot(sensor_ref*scipy.ones(len(flt_meas)))
+ plt.subplot(3, 2, 5)
+ plt.plot(cp1[:, 0])
+ plt.plot(cp1[:, 1])
+ plt.plot(cp1[:, 2])
+ plt.plot(-sensor_ref*np.ones(len(flt_meas)))
+ plt.plot(sensor_ref*np.ones(len(flt_meas)))
- subplot(3, 2, 6)
- plot(np1)
- plot(sensor_ref*scipy.ones(len(flt_meas)))
+ plt.subplot(3, 2, 6)
+ plt.plot(np1)
+ plt.plot(sensor_ref*np.ones(len(flt_meas)))
# if we want to have another plot we only draw the figure (non-blocking)
# also in matplotlib before 1.0.0 there is only one call to show possible
if block:
- show()
+ plt.show()
else:
- draw()
+ plt.draw()
+
-#
-# plot mag measurements in 3D
-#
def plot_mag_3d(measured, calibrated, p):
+ """Plot magnetometer measurements on 3D sphere."""
# set up points for sphere and ellipsoid wireframes
- u = r_[0:2 * pi:20j]
- v = r_[0:pi:20j]
- wx = outer(cos(u), sin(v))
- wy = outer(sin(u), sin(v))
- wz = outer(ones(size(u)), cos(v))
- ex = p[0] * ones(size(u)) + outer(cos(u), sin(v)) / p[3]
- ey = p[1] * ones(size(u)) + outer(sin(u), sin(v)) / p[4]
- ez = p[2] * ones(size(u)) + outer(ones(size(u)), cos(v)) / p[5]
+ u = np.r_[0:2 * np.pi:20j]
+ v = np.r_[0:np.pi:20j]
+ wx = np.outer(cos(u), sin(v))
+ wy = np.outer(sin(u), sin(v))
+ wz = np.outer(np.ones(np.size(u)), cos(v))
+ ex = p[0] * np.ones(np.size(u)) + np.outer(cos(u), sin(v)) / p[3]
+ ey = p[1] * np.ones(np.size(u)) + np.outer(sin(u), sin(v)) / p[4]
+ ez = p[2] * np.ones(np.size(u)) + np.outer(np.ones(np.size(u)), cos(v)) / p[5]
# measurements
mx = measured[:, 0]
@@ -214,14 +203,14 @@ def plot_mag_3d(measured, calibrated, p):
rect_l = [left, bottom, width, height]
rect_r = [left/2+0.5, bottom, width, height]
- fig = figure(figsize=figaspect(0.5))
+ fig = plt.figure(figsize=plt.figaspect(0.5))
if matplotlib.__version__.startswith('0'):
ax = Axes3D(fig, rect=rect_l)
else:
ax = fig.add_subplot(1, 2, 1, position=rect_l, projection='3d')
# plot measurements
ax.scatter(mx, my, mz)
- hold(True)
+ plt.hold(True)
# plot line from center to ellipsoid center
ax.plot([0.0, p[0]], [0.0, p[1]], [0.0, p[2]], color='black', marker='+', markersize=10)
# plot ellipsoid
@@ -232,9 +221,9 @@ def plot_mag_3d(measured, calibrated, p):
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5 * (mx.max() + mx.min())
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5 * (my.max() + my.min())
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5 * (mz.max() + mz.min())
- # uncomment following both lines to test the fake bounding box:
- #for xb, yb, zb in zip(Xb, Yb, Zb):
- # ax.plot([xb], [yb], [zb], 'r*')
+ # add the fake bounding box:
+ for xb, yb, zb in zip(Xb, Yb, Zb):
+ ax.plot([xb], [yb], [zb], 'w')
ax.set_title('MAG raw with fitted ellipsoid and center offset')
ax.set_xlabel('x')
@@ -246,7 +235,7 @@ def plot_mag_3d(measured, calibrated, p):
else:
ax = fig.add_subplot(1, 2, 2, position=rect_r, projection='3d')
ax.plot_wireframe(wx, wy, wz, color='grey', alpha=0.5)
- hold(True)
+ plt.hold(True)
ax.scatter(cx, cy, cz)
ax.set_title('MAG calibrated on unit sphere')
@@ -256,13 +245,13 @@ def plot_mag_3d(measured, calibrated, p):
ax.set_xlim3d(-1, 1)
ax.set_ylim3d(-1, 1)
ax.set_zlim3d(-1, 1)
- show()
+ plt.show()
+
-#
-# read a turntable log
-# return an array which first column is turnatble and next 3 are gyro
-#
def read_turntable_log(ac_id, tt_id, filename, _min, _max):
+ """ Read a turntable log.
+ return an array which first column is turnatble and next 3 are gyro
+ """
f = open(filename, 'r')
pattern_g = re.compile("(\S+) "+str(ac_id)+" IMU_GYRO_RAW (\S+) (\S+) (\S+)")
pattern_t = re.compile("(\S+) "+str(tt_id)+" IMU_TURNTABLE (\S+)")
@@ -276,10 +265,6 @@ def read_turntable_log(ac_id, tt_id, filename, _min, _max):
if m:
last_tt = float(m.group(2))
m = re.match(pattern_g, line)
- if m and last_tt and last_tt > _min and last_tt < _max:
+ if m and last_tt and _min < last_tt < _max:
list_tt.append([last_tt, float(m.group(2)), float(m.group(3)), float(m.group(4))])
- return scipy.array(list_tt)
-
-#
-#
-#
+ return np.array(list_tt)