Merge remote-tracking branch 'paparazzi/master' into telemetry

.gitignore

@@ -34,6 +34,8 @@
 # JetBrains (PyCharm, etc) IDE project files
 .idea
 
+# Vagrant VM files
+/.vagrant
 
 /var
 /dox

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

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:
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
-	@echo DEPEND $@
-	@test -d $(OBJDIR) || mkdir -p $(OBJDIR)
-	$(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
 ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
 endif

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)
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
-	@echo DEPEND $@
-	$(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
 ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
 endif

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:
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
-	@echo DEPEND $@
-	@test -d $(OBJDIR) || mkdir -p $(OBJDIR)
-	$(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
 ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
 endif

conf/Makefile.omap

@@ -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:
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
-	@echo DEPEND $@
-	@test -d $(OBJDIR) || mkdir -p $(OBJDIR)
-	$(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
 ifneq ($(MAKECMDGOALS),clean)
-ifneq ($(MAKECMDGOALS),erase)
+-include $(DEPS)
--include $(OBJDIR)/.depend
-endif
 endif

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:
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
-	@echo DEPEND $@
-	@test -d $(OBJDIR) || mkdir -p $(OBJDIR)
-	$(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
 ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
 endif

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)
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
-	@echo DEPEND $@
-	$(Q)$(CC) -MM -MG $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
 ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
 endif

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:
+DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d))
-	@echo DEPEND $@
-	@test -d $(OBJDIR) || mkdir -p $(OBJDIR)
-	$(Q)$(CC) -MM -MG $(THUMB) $(CFLAGS) $($(TARGET).srcs) | sed 's|\([^\.]*\.o\)|$(OBJDIR)/\1|' > $@
 
 ifneq ($(MAKECMDGOALS),clean)
--include $(OBJDIR)/.depend
+-include $(DEPS)
 endif

conf/airframes/examples/stm32f4_discovery_test.xml

@@ -29,6 +29,12 @@
     <subsystem name="ins"/>
   </firmware>
 
+  <firmware name="test_progs">
+    <target name="test_lis302dl" board="stm32f4_discovery">
+      <define name="STM32F4_DISCOVERY_SPI1_FOR_LIS302"/>
+    </target>
+  </firmware>
+
   <modules main_freq="512">
     <load name="baro_ets.xml">
       <define name="BARO_ETS_SCALE" value="40.0"/>

conf/firmwares/booz2_common.makefile

@@ -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)

conf/firmwares/booz2_test_progs.makefile

@@ -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

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

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)
 
 

conf/firmwares/subsystems/lisa_passthrough/radio_control_joby.makefile

@@ -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
-
-

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

conf/modules/gps_ubx_ucenter.xml

@@ -24,9 +24,8 @@ Warning: you still need to tell the driver
   <init fun="gps_ubx_ucenter_init()"/>
   <periodic fun="gps_ubx_ucenter_periodic()" start="gps_ubx_ucenter_init()" freq="4." autorun="TRUE"/>
   <makefile target="ap">
-    <raw>
+    <file name="gps_ubx_ucenter.c"/>
-      ap.CFLAGS += -DGPS_UBX_UCENTER=\"modules/gps/gps_ubx_ucenter.c\"
+    <define name="GPS_UBX_UCENTER"/>
-    </raw>
   </makefile>
 </module>
 

conf/settings/estimation/ahrs_float_mlkf.xml

@@ -0,0 +1,13 @@
+<!DOCTYPE settings SYSTEM "../settings.dtd">
+
+<settings>
+  <dl_settings>
+
+    <dl_settings NAME="AHRS">
+       <dl_setting var="ahrs_impl.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
+       <dl_setting var="ahrs_impl.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
+       <dl_setting var="ahrs_impl.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
+    </dl_settings>
+
+  </dl_settings>
+</settings>

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;
   };

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 */
 

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
+ * PA5  = SPI1 SCK if STM32F4_DISCOVERY_SPI1_FOR_LIS302
- * PA6  = CANNOT BE USED
+ * PA6  = SPI1 MISO if STM32F4_DISCOVERY_SPI1_FOR_LIS302
- * PA7  = FREE
+ * 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
 
 
 /***************************************************************************************************/

sw/airborne/booz/test/Makefile

@@ -1,57 +0,0 @@
-##
-# $Id$
-#
-# Copyright (C) 2008-2009 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.
-##
-
-# 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

sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c

@@ -1,76 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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 <inttypes.h>
-
-#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();
-}
-

sw/airborne/booz/test/booz2_test_buss_bldc_hexa.h

@@ -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 */

sw/airborne/booz/test/booz2_test_crista.c

@@ -1,117 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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 <inttypes.h>
-
-#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);
-  }
-}

sw/airborne/booz/test/booz2_test_gps.c

@@ -1,85 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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 <inttypes.h>
-
-#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);
-
-}

sw/airborne/booz/test/booz2_test_max1168.c

@@ -1,141 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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 <inttypes.h>
-
-#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();
-}

sw/airborne/booz/test/plot_scaling.sce

@@ -1,69 +0,0 @@
-///
-// $Id$
-//  
-// Copyright (C) 2008-2009 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. 
-///
-
-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();

sw/airborne/booz/test/plot_test_vg_adpt.sce

@@ -1,93 +0,0 @@
-///
-// $Id$
-//  
-// Copyright (C) 2008-2009 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. 
-///
-
-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();

sw/airborne/booz/test/plot_test_vg_ref.sce

@@ -1,63 +0,0 @@
-///
-// $Id$
-//  
-// Copyright (C) 2008-2009 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. 
-///
-
-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();

sw/airborne/booz/test/test_mlkf.c

@@ -1,116 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-
-#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; i<nb_samples; i++) {
-    feed_imu(i);
-    ahrs_propagate();
-    ahrs_update_accel();
-    ahrs_update_mag();
-    store_filter_output(i);
-  }
-  dump_output(OUT_FILE);
-
-  return 0;
-}
-
-
-static void read_data(const char* filename) {
-  FILE* fd = fopen(filename, "r");
-  nb_samples = 0;
-  int ret = 0;
-  do {
-    struct test_sample* s = &samples[nb_samples];
-    ret = fscanf(fd, "%lf [%lf %lf %lf %lf] [%lf %lf %lf] [%lf %lf %lf] [%lf %lf %lf] [%lf %lf %lf]",
-		 &s->time,
-		 &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<nb_samples; i++) {
-    fprintf(fd, "%.16f [%.16f %.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f %.16f %.16f %.16f]\n",
-	    samples[i].time,
-	    output[i].quat_est.qi, output[i].quat_est.qx, output[i].quat_est.qy, output[i].quat_est.qz, // quaternion
-	    output[i].rate_est.p, output[i].rate_est.q, output[i].rate_est.r,                           // omega
-	    output[i].bias_est.p, output[i].bias_est.q, output[i].bias_est.r,                           // bias
-            output[i].P[0][0], output[i].P[1][1], output[i].P[2][2],                                    // covariance
-            output[i].P[3][3], output[i].P[4][4], output[i].P[5][5] );
-  }
-  fclose(fd);
-  printf("wrote %d points in file %s\n", nb_samples, filename);
-}

sw/airborne/booz/test/test_mlkf.sce

@@ -1,121 +0,0 @@
-clear();
-clearglobal();
-
-
-exec('../../../simulator/scilab/q6d/q6d_fdm.sci');
-exec('../../../simulator/scilab/q6d/q6d_algebra.sci');
-
-global ahrs_quat;
-global ahrs_omega;
-global ahrs_bias;
-global ahrs_cov;
-
-function read_ahrs_output(filename)
-  global ahrs_quat;
-  global ahrs_omega;
-  global ahrs_bias;
-  global ahrs_cov;
-  ahrs_quat  = zeros(4,length(time));
-  ahrs_omega = zeros(3,length(time));
-  ahrs_bias  = zeros(3,length(time));
-  ahrs_cov   = zeros(6,length(time));
-  i=1;
-  u=mopen(filename, 'r');
-  while meof(u) == 0 & i <= length(time)
-    line = mgetl(u, 1);
-    if line ~= []
-      [nb_scan, t, qi, qx, qy, qz, p, q, r, bp, bq, br, c11, c22, c33, c44, c55, c66] = ...
-	  msscanf(1, line, '%f [%f %f %f %f] [%f %f %f] [%f %f %f] [%f %f %f %f %f %f]');
-      if nb_scan == 17
-	ahrs_quat(:,i)  = [qi qx qy qz]';
-	ahrs_omega(:,i) = [p q r]';
-	ahrs_bias(:,i)  = [bp bq br]';
-	ahrs_cov(:,i)   = [c11 c22 c33 c44 c55 c66]';
-	i = i+1;
-      end
-    end
-  end
-  mclose(u);
-endfunction
-
-function display_ahrs_state()
-  clf();
-  f=get("current_figure");
-  f.figure_name="AHRS state";
-
-  eul_fdm  = zeros(3,length(time));
-  eul_ahrs = zeros(3,length(time));
-  
-  for i=1:length(time)
-    eul_fdm(:,i)  = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,i));
-    eul_ahrs(:,i) = euler_of_quat(ahrs_quat(:,i));
-  end
-  
-  subplot(4,3,1);
-  plot2d(time, deg_of_rad(eul_ahrs(1,:)), 3);
-  plot2d(time, deg_of_rad(eul_fdm(1,:)), 2);
-  legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul"); 
-  xtitle('Phi');
-
-  subplot(4,3,2);
-  plot2d(time, deg_of_rad(eul_ahrs(2,:)), 3);
-  plot2d(time, deg_of_rad(eul_fdm(2,:)), 2);
-  legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul"); 
-  xtitle('Theta');
-  
-  subplot(4,3,3);
-  plot2d(time, deg_of_rad(eul_ahrs(3,:)), 3);
-  plot2d(time, deg_of_rad(eul_fdm(3,:)), 2);
-  legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul"); 
-  xtitle('Psi');
-  
-  subplot(4,3,4);
-  plot2d(time, deg_of_rad(ahrs_omega(1,:)), 3);
-  plot2d(time, deg_of_rad(fdm_state(FDM_SP,:)), 2);
-  xtitle('p');
-  
-  subplot(4,3,5);
-  plot2d(time, deg_of_rad(ahrs_omega(2,:)), 3);
-  plot2d(time, deg_of_rad(fdm_state(FDM_SQ,:)), 2);
-  xtitle('q');
-
-  subplot(4,3,6);
-  plot2d(time, deg_of_rad(ahrs_omega(3,:)), 3);
-  plot2d(time, deg_of_rad(fdm_state(FDM_SR,:)), 2);
-  xtitle('r');
-
-  subplot(4,3,7);
-  plot2d(time, deg_of_rad(ahrs_bias(1,:)), 3);
-  xtitle('bp');
-  
-  subplot(4,3,8);
-  plot2d(time, deg_of_rad(ahrs_bias(2,:)), 3);
-  xtitle('bq');
-
-  subplot(4,3,9);
-  plot2d(time, deg_of_rad(ahrs_bias(3,:)), 3);
-  xtitle('br');
- 
-  subplot(4,3,10);
-
-  subplot(4,3,11);
-  plot2d(time, ahrs_cov(1,:),3);
-  plot2d(time, ahrs_cov(2,:),3);
-  plot2d(time, ahrs_cov(3,:),3);
-  
-  legends(["ahrs_c", "ahrs_s"],[3 4], with_box=%f, opt="ul"); 
-  xtitle('cov angles'); 
-  
-  subplot(4,3,12);
-  plot2d(time, ahrs_cov(4,:),3);
-  plot2d(time, ahrs_cov(5,:),3);
-  plot2d(time, ahrs_cov(6,:),3);
- 
-  xtitle('cov bias'); 
-  
-endfunction
-
-load('../../../simulator/scilab/q6d/data/stop_stop_state_sensors.dat', 'time', 'fdm_state', 'sensor_gyro', 'sensor_accel', 'sensor_mag');
-read_ahrs_output('out.txt');
-
-display_ahrs_state();

sw/airborne/booz/test/test_scaling.c

@@ -1,117 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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 <stdio.h>
-#include <math.h>
-#include <inttypes.h>
-
-#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); a<=(1<<N_OFFSET); a++) {
-   int32_t b = (a>>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<<N_OFFSET);
-
-   printf("%- d %- d %- d %- d %.1f\n", a, b, c, d, e);
- }
-}

sw/airborne/booz/test/test_vg_adpt.c

@@ -1,173 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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.
- */
-
-#define _GNU_SOURCE
-#include <stdio.h>
-#include <stdlib.h>
-
-#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<n_dat) {
-    long int r = random();
-    double noise = 2. * 2. * ((double)r / (double)RAND_MAX - 0.5);
-    //    printf("%ld %f\n", r, noise);
-    est_zdd[i] = 0 + noise;
-    delta_t[i] = 85;
-    i++;
-  }
-}
-
-void dump_res(void) {
- int i = 0;
-  while (i<n_dat) {
-    printf("%f %d %d %d %f %f %f %d %d\n", time[i], est_zdd[i], delta_t[i], ref_zdd[i], ffX[i], ffP[i], ffm[i], ifX[i], ifP[i]);
-    i++;
-  }
-
-}
-
-#define FF_CMD_FRAC 18
-void test_command(int i) {
-
-  const int32_t inv_m_i =  gv_adapt_X>>(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<n_dat) {
-    Bound(delta_t[i], 1, 200);
-    gv_adapt_run(est_zdd[i], delta_t[i]);
-    ifX[i] = gv_adapt_X;
-    ifP[i] = gv_adapt_P;
-    float_filter_run(i);
-    //    test_command(i);
-    i++;
-  }
-
-  dump_res();
-
-  return 0;
-}

sw/airborne/booz/test/test_vg_ref.c

@@ -1,116 +0,0 @@
-/*
- * Copyright (C) 2008-2009 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 <stdio.h>
-
-#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<NB_STEP) {
-    int32_t sp_i = get_sp(i);
-    //    gv_update_ref_from_z_sp(sp_i);
-    gv_update_ref_from_zd_sp(sp_i);
-    //    z_sp = BOOZ_FLOAT_OF_INT(sp_i, IPOS_FRAC);
-    //    update_ref_from_z();
-    zd_sp = BOOZ_FLOAT_OF_INT(sp_i, ISPEED_RES);
-    update_ref_from_zd();
-    print_ref(i);
-    i++;
-  }
-
-
-  return 0;
-}

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h

@@ -45,9 +45,6 @@ extern struct Int32Eulers stabilization_att_sum_err;
 extern int32_t stabilization_att_fb_cmd[COMMANDS_NB];
 extern int32_t stabilization_att_ff_cmd[COMMANDS_NB];
 
-// common so it can be used for downlink/debug
-extern struct Int32Eulers stab_att_sp_euler;  ///< with #INT32_ANGLE_FRAC
-
 #define stabilization_attitude_common_int_SetKiPhi(_val) {	\
     stabilization_gains.i.x = _val;             \
     stabilization_att_sum_err.phi = 0;          \

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h

@@ -27,6 +27,4 @@
 
 #include "math/pprz_algebra_int.h"
 
-extern struct Int32Quat   stab_att_sp_quat;   ///< with #INT32_QUAT_FRAC
-
 #endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h

@@ -29,8 +29,8 @@
 
 #include "math/pprz_algebra_int.h"
 
+extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
 extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
-extern struct Int32Quat   stab_att_ref_quat;  ///< with #INT32_QUAT_FRAC
 extern struct Int32Rates  stab_att_ref_rate;  ///< with #REF_RATE_FRAC
 extern struct Int32Rates  stab_att_ref_accel; ///< with #REF_ACCEL_FRAC
 

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h

@@ -32,6 +32,9 @@
 
 #include "stabilization_attitude_ref_int.h"
 
+extern struct Int32Quat   stab_att_sp_quat;  ///< with #INT32_QUAT_FRAC
+extern struct Int32Quat   stab_att_ref_quat;  ///< with #INT32_QUAT_FRAC
+
 void stabilization_attitude_ref_enter(void);
 
 #endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */

sw/airborne/math/Makefile

@@ -8,14 +8,15 @@ CC= gcc
 CFLAGS= -fpic
 INCLUDES= -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/sw/airborne
 
-# build in ../../../var/build/math
+ifeq ($(PAPARAZZI_HOME),)
-ifndef BUILDDIR
+PAPARAZZI_HOME=$(PAPARAZZI_SRC)
-BUILDDIR=../../../var/build/math
 endif
 
-ifndef PREFIX
+# build in $(PAPARAZZI_HOME)/var/build/math
-PREFIX=/usr
+BUILDDIR ?= $(PAPARAZZI_HOME)/var/build/math
-endif
+
+PREFIX ?= /usr/local
+
 LIB_INSTALLDIR=${PREFIX}/lib
 INCLUDE_INSTALLDIR=${PREFIX}/include/pprz
 PKGCONFIG_INSTALLDIR=${PREFIX}/lib/pkgconfig
@@ -48,3 +49,4 @@ $(BUILDDIR)/%.o: %.c
 clean:
 	$(Q)rm -f $(BUILDDIR)/*.o $(BUILDDIR)/$(LIBNAME).so
 
+.PHONY: all clean shared_lib install_shared_lib

sw/airborne/math/README

@@ -11,11 +11,11 @@ HOWTO install a shared library to use in other projects
 2. Install library: in this folder, type
   make install_shared_lib
 
-  the default install dir is /usr
+  the default install dir is /usr/local
   and will install files in
-  /usr/lib
+  /usr/local/lib
-  /usr/lib/pkgconfig
+  /usr/local/lib/pkgconfig
-  /usr/include/pprz
+  /usr/local/include/pprz
 
   to change the install dir: PREFIX=<your_install_dir> make install_shared_lib
 

sw/airborne/math/pprz_algebra_int.h

@@ -36,6 +36,19 @@
 #include "math/pprz_trig_int.h"
 #include <stdlib.h>
 
+
+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;

sw/airborne/modules/gps/gps_ubx_ucenter.c

@@ -28,6 +28,7 @@
  */
 
 #include "gps_ubx_ucenter.h"
+#include "subsystems/gps/gps_ubx.h"
 
 //////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////

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
 

sw/airborne/peripherals/lis302dl.h

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@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.
+ */
+
+/**
+ * @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 */

sw/airborne/peripherals/lis302dl_regs.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@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.
+ */
+
+/**
+ * @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 */

sw/airborne/peripherals/lis302dl_spi.c

@@ -0,0 +1,174 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@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.
+ */
+
+/**
+ * @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;
+    }
+  }
+}

sw/airborne/peripherals/lis302dl_spi.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@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.
+ */
+
+/**
+ * @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

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};
+  struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
-  //  const float g_noise[] = {150., 150., 150.};
+  update_state(&earth_g, &imu_g, &g_noise);
-  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, &ahrs_impl.mag_noise);
-  update_state(&ahrs_impl.mag_h, &imu_h, h_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);